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

The fastest motion planning in the West.

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.

• Plan: batched IK; RRT-Connect, shortcutting and smoothing on SIMD collision checks; smooth B-spline paths, optionally optimized with L-BFGS; time-optimal timing within velocity, acceleration and jerk limits; replanning from a moving state; and controlled stops along the path.
• Robots: load from URDF, MJCF or OpenUSD, including mimic joints, closed loops, continuous joints and ball or floating joints, or assemble several, such as two arms. Collision spheres are fitted automatically. IK takes a target for each hand at once, and plans can hold one arm still while another moves.
• Worlds: boxes, spheres, cylinders, capsules and signed distance grids, built from meshes, point clouds, depth images or frames fused over time.
• Data: reaches with recoveries from mistakes, pick-and-place episodes and handovers between two arms, with grippers and task strings, plus states perturbed from them to start reinforcement learning from. They export as LeRobot v3.0 datasets (arm by arm, as ALOHA's) or NumPy arrays;scripts/replay_mujoco.py replays them in MuJoCo and adds camera videos.
• Embeddable: CPU-only builds, threads you own, time budgets, allocation-free sampling and typed errors, for use inside a robot's control process.

[dependencies]
batchplan = "0.2"

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(())
}
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(),
    )
}
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

• The fastest of these planners: 0.17 ms per problem and 19,902 problems/s on an M4 Pro CPU, ahead of VAMP on the same spheres.
• Clear of the real robot most often: 85–93% of its paths clear the Panda's meshes; no other planner's reach 82%.
• Solves the most in every comparison, with shorter paths than every other sampling-based planner, and, with two optimization seeds, than cuRobo.

Apple M4 Pro CPU, 14 cores.

¹ 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.

| 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. |

| 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. |

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).

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 Franka Panda is from franka_ros and NVlabs/curobo (Apache-2.0). Its collision spheres in assets/franka/panda_collision.json are converted from cuRobo'sfranka.yml (Apache-2.0, © NVIDIA).
• The UR5e is from Universal Robots' ROS 2 description (BSD-3-Clause).
• The Robotiq 2F-85 is from PickNik's ros2_robotiq_gripper (BSD-3-Clause).
• The SO-101 is from TheRobotStudio's SO-ARM100 (Apache-2.0).

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).