Batchplan: Plan batches of robot motions, use for execution or training data

Quick start · Docs · API · Benchmarks · Examples · How it works

batchplan is a Rust library for collision-aware motion planning, in batches. One call plans thousands of motions, each in its own scene: IK, a collision-free path, and a trajectory timed within the robot's limits. A typical problem plans in 0.17 ms on one CPU core, faster than VAMP, with paths that keep clear of the real geometry more often than any planner we compared. IK, trajectory optimization and distance grids run on any GPU that wgpu supports (Vulkan, Metal or DX12), with no CUDA, no ROCm and no ROS, or on the CPU with the same results.

It is built for robot learning at scale: planned motions become LeRobot datasets that train as they are. Two reBot arms hand blocks to each other over and around dividers in 512 scenes in 1.4–1.7 s on an M4 Pro or a Ryzen AI Max+ 395, on the GPU or the CPU.

[dependencies] batchplan = "0.2"

Cargo feature Default Adds

gpu on GPU devices through wgpu. Without it, batchplan is CPU-only and has no GPU dependencies.

lerobot off LeRobot v3.0 export (Arrow, Parquet).

usd off Robots and scenes from OpenUSD.

batchplan builds on stable Rust 1.90 or later (1.96 with usd), with no build flags to set: its collision checks use the widest SIMD instructions the CPU has, chosen when the program runs (SSE2 up to AVX-512 on x86-64, NEON on ARM).

The paths below point at test robots in this repository's assets/; use your own robot's file instead.

use batchplan::glam::{Quat, Vec3}; use batchplan::{ Device, Error, IkOptions, IkProblem, Obstacle, PlanOptions, PlanProblem, Pose, Robot, RobotOptions, Trajectory, World, };

fn main() -> Result<(), Error> { // Any URDF, MJCF or USD robot. Collision spheres are fitted when it loads. let robot = Robot::load( "assets/ur5e/ur_description/urdf/ur5e.urdf", &RobotOptions::default(), )?; let device = Device::gpu(&robot).or_else(|_| Device::cpu(&robot))?;

// Scenes are uploaded once. Problems name their scene by index. let table = Obstacle::cuboid(Vec3::new(0.0, 0.0, -0.02), Vec3::new(1.0, 1.0, 0.02)); let worlds = device.upload(&[World { obstacles: vec![table], }])?;

// IK for a batch of gripper poses... let down = Quat::from_rotation_x(std::f32::consts::PI); let goals: Vec<IkProblem> = [Vec3::new(0.4, 0.2, 0.3), Vec3::new(0.3, -0.3, 0.4)] .into_iter() .map(|position| IkProblem::new(0, Pose::new(position, down))) .collect(); let ik = batchplan::solve_ik(&device, &worlds, &goals, &IkOptions::default())?;

// ...then a collision-free motion to every solution, in one call. let problems: Vec<PlanProblem> = ik .solved() .map(|s| PlanProblem::new(s.problem.world, robot.default_q(), s.solution)) .collect(); let plans = batchplan::plan(&device, &worlds, &problems, &PlanOptions::default())?;

// Time each path within the robot's velocity, acceleration and jerk limits. for s in plans.solved() { let trajectory = Trajectory::try_new(&robot, s.solution, 1.0)?; let samples = trajectory.sample(100.0)?; // 100 Hz, for a controller println!( "motion {}: {:.2} s, {} samples", s.index, trajectory.duration(), samples.len() ); } Ok(()) }

Make a training dataset

use batchplan::datagen::DemoOptions; use batchplan::glam::{Quat, Vec3}; use batchplan::lerobot::ExportOptions; use batchplan::{Device, Error, IkProblem, Pose, Robot, RobotOptions, World, datagen, lerobot};

fn main() -> Result<(), Error> { let robot = Robot::load( "assets/ur5e/ur_description/urdf/ur5e.urdf", &RobotOptions::default(), )?; let device = Device::gpu(&robot).or_else(|_| Device::cpu(&robot))?; let worlds = device.upload(&[World::default()])?;

// Reaches to 64 goals, plus recoveries from states perturbed along them. let down = Quat::from_rotation_x(std::f32::consts::PI); let goals: Vec<IkProblem> = (0..64) .map(|i| { Vec3::new( 0.3 + 0.03 * (i % 8) as f32, -0.25 + 0.06 * (i / 8) as f32, 0.3, ) }) .map(|position| IkProblem::new(0, Pose::new(position, down))) .collect(); let demos = datagen::demonstrations(&device, &worlds, &goals, &DemoOptions::default())?;

// A LeRobot v3.0 dataset (cargo feature lerobot), ready for lerobot-train. lerobot::export( "data/reaches", &robot, &worlds, &demos, &ExportOptions::default(), ) }

Plan around what a depth camera sees

use batchplan::glam::{Quat, Vec3}; use batchplan::{ DepthImage, Device, Error, Intrinsics, Obstacle, Occlusion, Pose, Robot, SdfGrid, SdfOptions, World, Worlds, };

/// Turns one depth frame into a world to plan in, leaving the arm itself out. fn world_from_camera( device: &Device, robot: &Robot, depth: &[f32], q: &[f32], ) -> Result<Worlds, Error> { let image = DepthImage { depth, // metres, row by row width: 640, intrinsics: Intrinsics { fx: 525.0, fy: 525.0, cx: 319.5, cy: 239.5, }, // 1.5 m above the table, looking down (OpenCV axes: z forward, y down). camera: Pose::new( Vec3::new(0.5, 0.0, 1.5), Quat::from_rotation_x(std::f32::consts::PI), ), }; let arm = image.robot_mask(robot, q, 0.04)?; // the arm is not an obstacle to itself // What objects hide is solid; what only the arm hides is free, or it would wall the arm in. let behind = Occlusion::OccupiedExceptRobotShadow; let grid = SdfGrid::from_depth(device, &image, Some(&arm), behind, &SdfOptions::default())?; device.upload(&[World { obstacles: vec![Obstacle::from(grid)], }]) }

To fuse frames over time, use an OccupancyMap (Worlds and perception).

Each library planned the same 2,600 Panda problems from MotionBenchMaker and MπNets, and one checker judged every path, batchplan's included. Solved means the path reaches the goal within the joint limits without touching the model it was planned on; clear on the meshes checks it against the Panda's meshes, the real robot. Setup and per-set results · Run them yourself

¹ Median over the 1,557 problems all five solve. ² As batchplan's searches do; VAMP's default is 100,000. ³ VAMP's library default; the other rows step 1 rad, its Panda setting. ⁴ With VAMP's collision checker and a 5 s limit.

cuRobo on an NVIDIA T4 GPU with 8 Xeon threads; batchplan on the M4 Pro². The 1,300 problems whose scenes are only boxes, which both libraries load exactly.

¹ Median over the 1,092 problems all four solve. ² The T4 machine has not been available since batchplan's previous version, which planned there in 0.64 ms and 2,152 problems/s. ³ PlanOptions::seeds, off by default: the searched path and the straight line, optimized. ⁴ Each call's time over its problems.

AMD Ryzen AI Max+ 395 CPU, 32 threads. batchplan plans on the spheres it ships with; every path is judged on the meshes.

¹ Median over the 1,334 problems all three solve.

Planning runs on CPU threads on either device; IK and trajectory optimization also run on the GPU. Each call here solves IK and plans a batch of tabletop grasps, end to end, in solved problems per second:

A GPU call costs about 10 ms, so one problem at a time runs 9–15 times faster on the CPU; from a few hundred problems per call the GPU overtakes it, and at 4,096 it solves 2.3–3.4 times as many.

VAMP, from Python, or for its asymptotically optimal planners (AORRTC, FCIT*) and roadmaps (PRM) on the same fast checks.

cuRobo, for model-predictive control and NVIDIA's perception stack (nvblox), if you have an NVIDIA GPU.

MoveIt 2, for ROS 2: perception, controllers, grasping and the rest of a robot's stack around the planner.

OMPL, for its range of planners, such as asymptotically optimal and kinodynamic ones.

Where to find things

You want to use it

API Every type and function, by task, with the guide for each.

Planning IK, searching and smoothing paths, keeping clear of obstacles, optional trajectory optimization, goals a turn away, timing, replanning mid-motion, and the options worth knowing.

Robots URDF, MJCF and OpenUSD; joint types, mimic joints and closed loops; collision spheres; holding objects; checking against meshes.

Worlds and perception Obstacles, scenes, distance grids, depth images, masking the robot, and fused occupancy maps.

Training data Reaches with recoveries, pick-and-place, and exporting to NumPy or LeRobot.

Devices GPU or CPU, which GPUs work, and running inside a robot's control process.

Benchmarks Against VAMP, OMPL, RoboPlan and MoveIt 2, GPU against CPU, spheres against meshes, and how to reproduce them.

You are working on it

Contributing Building and testing, the style guide the code follows, and what a change needs.

How it works The design rules, the CPU/GPU twin, collision, per-robot kernels, and how the optimizer runs on the device.

Verification Running the tests, and what each one pins down.

CLAUDE.md Working rules and the architecture in full detail, for people and coding agents.

Examples

Command What it does

scripts/fetch_rebot.sh && cargo run --release --example handover Two reBot B601 arms hand blocks to each other in 512 scenes, three in four divided down the middle: the bimanual benchmark. --features lerobot and -- 16 --lerobot data/handover write a LeRobot dataset, and scripts/replay_mujoco.py data/handover data/handover_video adds three camera videos.

cargo run --release --example bench -- 512 IK and planning for 512 cluttered tabletop scenes, GPU against CPU.

cargo run --release --example datagen -- data/demo 512 20 Reaches and recoveries in 512 scenes, as .npy arrays at 20 fps.

cargo run --release --features lerobot --example datagen -- --pick-place --lerobot data/pick 512 20 Pick-and-place episodes as a LeRobot dataset.

cargo run --release --example depth Builds a distance grid from a rendered depth image and plans around it.

cargo run --release --example control_loop -- 10 A planner thread feeding a 50 Hz control loop.

scripts/fetch_benchmark.sh && cargo run --release --example benchmark The 2,600 MotionBenchMaker and MπNets problems, on every device.

demos/ shows four of them live in Rerun: two reBot arms handing blocks over, Panda pick-and-place around obstacles, planning around what depth cameras see, and replanning mid-motion.

batchplan-ros is a ROS 2 Jazzy node that speaks MoveIt's messages without needing MoveIt: a MoveGroup action and an IK service, with the scene in each request. Several goals in one request are planned in one batched call, and the shortest path wins. It comes with a browser demo of a simulated Panda (pixi run demo, on Linux or macOS) and an optional MoveIt planner plugin, through which batchplan plans the stock Panda demo's requests in 4–5 ms, against OMPL's 14–15 ms.

Spheres. Planning sees the robot as spheres. MeshModel checks finished plans against its meshes on the CPU, as a filter.

Joints. Each closed loop must have exactly one actuated joint. Ball and floating joints are expanded into Euler angles, which lose a direction of motion where the middle angle reaches ±90°.

Timing. Trajectories end at rest. Time-optimal timing respects jerk limits but does not minimize jerk.

Small batches on a GPU. A GPU call costs about 10 ms however small the batch, so below a few hundred problems per call the CPU is faster (Batches and the GPU). Planning itself runs on CPU threads on either device.

Pick-and-place and handovers. Episodes grasp upright boxes from above and have no recoveries. Camera videos come from replaying them in MuJoCo, kinematically: no physics checks the grasps.

Maturity. batchplan is at version 0.2, and its interface may change. It is tested on AMD (Vulkan) and Apple (Metal) GPUs; on NVIDIA, only earlier versions ran (on a T4).

License

Licensed under either of Apache License, Version 2.0 or MIT license, at your option.

The test robots under assets/ keep their own licences, listed with their sources and changes in assets/README.md:

The reBot B601 arms that examples/handover/ uses are not in the repository: scripts/fetch_rebot.sh downloads them from Seeed Studio's reBot-DevArm at a pinned commit (CERN-OHL-W-2.0).