Motion planning computes a collision-free route from a robot's current configuration to a goalSource 9, a sequence of the joint angles that kinematics maps to poses. Sampling-based planners explore configurations and the connections between them; trajectory optimizers improve a complete candidate trajectory against an objectiveSource 7 Source 8.

A white two-armed PR2 robot stands at a round red table in an office, reaching toward small objects, with a whiteboard and computer monitors behind it.
A Willow Garage PR2 in 2010. The TrajOpt benchmark below planned 198 arm problems and 96 full-body problems for this robot in simulated MoveIt scenes.
Photo: Steve Jurvetson / Wikimedia Commons. Licence: CC BY 2.0.

Drag the slider to step through the search, or press "Grow the tree" to replay it.

Rapidly-exploring random tree (RRT)
Random branches feel their way around obstacles to the goal
Adjust more

iteration 288 / 288nodes 289goal reached at iteration 288path length 124.2 units

Thin lines: the treeLit: the first branch to reach the goal288 branches · path found

Illustrative: one run on a made-up map

Rather than check every route, the robot's search grows random branches into open space until one reaches the goal.
How this was made

Authored fixed-seed scene. Each sampling attempt selects the goal with probability 1.5% and otherwise samples uniformly; steps are capped at 2 world units. Each accepted step grows from the tree node nearest a random sample, so the tree spreads into open space. Every iteration adds one node; with this seed a branch first reaches the goal at iteration 288, and the path it gives measures 124.2 units.

The RRT tree is at iteration 288 of 288 with 289 nodes and status goal reached at iteration 288; the highlighted start-to-goal path measures 124.2 units after a branch first reached the goal at iteration 288.

Current RRT tree state
iteration
288 / 288
nodes
289
status
goal reached at iteration 288
path length
124.2 units
1998
RRT introduced
Iowa State TR 98-11; date in LaValle's bibliography
2009
CHOMP
covariant gradient refinement
2011
RRT* analysis
Karaman and Frazzoli; conditional theorem
2D
demo scene below
100 by 64 world, 5 obstacles

Configuration space

Lozano-Pérez's 1983 reformulation moves planning from the workspace into configuration space. A configuration qq is one complete posture, the vector of joint values. The set C\mathcal{C} of all of them is 2D for a point robot in the plane and 7-dimensional for a 7-DoF armSource 1.

The configuration-space obstacle Cobs\mathcal{C}_{obs} holds every configuration where the robot intersects an obstacle; the rest is free spaceSource 1:

C=Cobs∪Cfree,Cfree=C∖Cobs\mathcal{C} = \mathcal{C}_{obs} \cup \mathcal{C}_{free}, \qquad \mathcal{C}_{free} = \mathcal{C} \setminus \mathcal{C}_{obs}

Shrinking the robot to a point and growing each obstacle by its shape reduces collision checking to point membership. Planning becomes the search for a continuous curve in Cfree\mathcal{C}_{free} from qstartq_{start} to qgoalq_{goal}Source 1.

Explicit boundary or solid models of Cobs\mathcal{C}_{obs} are difficult to build, though semi-algebraic models for chains and trees can be generated automatically. The basic semi-algebraic mover problem is PSPACE-hard when the number of degrees of freedom is unboundedSource 9. Hardness holds for a planar linkage with multiple links and for a multi-arm robot among 3D polyhedra; Canny's roadmap method is singly exponentialSource 11.

Sampling-based methods skip building Cobs\mathcal{C}_{obs} by querying a collision checker and validating whole local paths, though closed-chain constraints make sampling difficultSource 9.

Sampling-based planning

The probabilistic roadmap (PRM) of Kavraki, Svestka, Latombe, and Overmars samples configurations uniformly, keeps the collision-free ones as nodes and adds extra nodes where a heuristic marks the space as difficult. A local planner links nearby nodes, and queries become graph searches, so one roadmap serves many start-goal pairsSource 2.

LaValle introduced the rapidly-exploring random tree in Iowa State Computer Science Technical Report 98-11, October 1998Source 3 Source 9. Each iteration samples a state, finds the nearest vertex under a chosen metric, picks a control input and integrates over a fixed time interval, keeping vertices and edge paths in free space. For dynamics x˙=f(x,u)\dot{x}=f(x,u), the Euler step isSource 3

xnew≈x+f(x,u)Δtx_{new} \approx x + f(x,u)\Delta t

with starting state xx and input uu; the report recommends a higher-order integrator such as Runge-KuttaSource 3. LaValle's 2006 book drops the step size: each sample connects to the nearest point on the tree's swath, the union of its edge paths, splitting an edge when neededSource 9.

Nearest-neighbor selection gives RRT a Voronoi bias, because frontier vertices own larger Voronoi regions. The report leaves convergence rates openSource 3. LaValle and Kuffner's kinodynamic version steers with controls instead of straight linesSource 4. Drag the shelf in the figure below and the tree of random safe steps regrows around it to the goal.

How does a robot get around obstacles?
A robot's gripper must reach the can on the far shelf. Going straight there, it hits the shelf in between.
A robot's gripper must reach the can on the far shelf. Going straight there, it hits the shelf in between.
A robot rarely knows the way around an obstacle in advance, but random safe steps grown into a tree find one.
How this was made
The steps in words
  1. A robot's gripper must reach the can on the far shelf. Going straight there, it hits the shelf in between.
  2. Pick a random point. Step a short way toward it from the closest spot already reached. Keep the step only if it hits nothing.
  3. Repeat thousands of times. The kept steps branch out into a tree that creeps into every open gap.
  4. When a branch reaches the goal, follow it back to the start. Then cut every corner where a straight shortcut is safe.
  5. Drag the shelf: the tree regrows. A fast version of this rapidly-exploring random tree plans a typical arm path in 40 millionths of a second.

Kuffner and LaValle's RRT-Connect grows two trees, rooted at the start and the goalSource 12. In the six manipulation problems of a 2023 comparison, it had the best overall success rateSource 13.

Probabilistic completeness means the chance of finding an existing solution approaches one as sampling continues, with no finite-budget guaranteeSource 9. Karaman and Frazzoli prove it for simplified PRM (sPRM) and geometric RRT on X=(0,1)dX=(0,1)^d, d≥2d\geq2, assuming a solution with clearance δ>0\delta>0, uniform independent samples, a fixed sPRM radius, capped RRT steering and straight-line collision tests. A 1-nearest-neighbor sPRM is not probabilistically completeSource 5.

OMPL 2.0.0, released April 6, 2026, added VAMP as a SIMD backend for planning speeds up to 25 kHzSource 15; VAMP's authors report gains of more than 500x from fine-grained parallelism on 7 to 14 DoF robotsSource 14. The library implements PRM and RRT, includes benchmarking tools and plugs into external collision checkers and visualizersSource 10.

Optimality: RRT*

Karaman and Frazzoli's 2011 analysis separates feasibility from cost. In an obstacle-free Euclidean example with steering distance at least the domain diameter, RRT's best cost converges almost surely to a suboptimal value. Their PRM result concerns a forest-building version that rejects connections inside a connected component. Fixed-radius sPRM, which allows them, is asymptotically optimal at greater computational costSource 5.

RRT* adds least-cost parent selection and rewiring. A new vertex takes the cheapest collision-free parent among nearby vertices, and neighbors reattach to it only when that lowers their cost. With nn vertices, the connection radius isSource 5

rn=min⁡ ⁣{η,  γ(log⁡nn)1/d},r_n = \min\!\left\{\eta,\; \gamma\left(\frac{\log n}{n}\right)^{1/d}\right\},

where η>0\eta>0 caps the steering distance and d≥2d\geq2 is the dimension. Of the paper's two sufficient bounds on γ\gamma, the conservative one is γ>4(μ(Xfree)/ζd)1/d\gamma>4(\mu(X_{free})/\zeta_d)^{1/d}, with free-space volume μ(Xfree)\mu(X_{free}) and unit-ball volume ζd\zeta_dSource 5.

The result assumes a bounded Euclidean domain, uniform independent samples and straight-line connections, which excludes differential constraints. Cost must be positive, monotone under concatenation and at most a constant times path length. The optimum must have weak clearance, deforming continuously into paths with positive clearance, with cost continuous near it in a bounded-variation normSource 5:

P ⁣(lim⁡n→∞cn=c∗)=1.\mathbb{P}\!\left(\lim_{n\to\infty}c_n=c^*\right)=1.

Here cnc_n is the best cost after nn iterations, so a finite budget can return a suboptimal path. The non-optimality analysis assumes that optimal paths traverse a set of states of measure zero. Expected processing work is of order nlog⁡nn\log n, within a constant factor of RRT, though RRT* makes O(log⁡n)O(\log n) collision checks per iteration to RRT's oneSource 5.

After a first solution, Informed RRT* skips routes irrelevant to the query by sampling only the domain's intersection withSource 6

X^f={x:∥x−xstart∥2+∥x−xgoal∥2≤cbest}.\hat{X}_f=\left\{x:\lVert x-x_{start}\rVert_2+\lVert x-x_{goal}\rVert_2\leq c_{best}\right\}.

This prolate hyperspheroid has the start and goal as foci and the best path length cbestc_{best} as transverse diameter. It contains every state on a strictly improving path, but its states need not be collision-free, so collision checks remain. Over 100 runs per variation with 60 seconds of refinement, it refined faster than RRT*, and it gains nothing once the set covers the domainSource 6.

Trajectory optimization

Trajectory optimization makes the trajectory the decision variable, with smoothness, clearance and task terms in the objective or constraints. Neither the objective nor an initial guess guarantees a collision-free result. CHOMP and TrajOpt both plan from infeasible guesses as well as refine existing trajectoriesSource 7 Source 8. Graphs of Convex Sets (GCS) poses planning as a mixed-integer program with a very tight convex relaxation, so in most cases a cheap rounding suffices for globally optimal trajectoriesSource 16.

CHOMP: covariant gradient descent

CHOMP combines an obstacle cost with an environment-independent prior on trajectory dynamics. In the discrete form, ξ\xi holds the interior waypoints, KdK_d computes finite differences and ede_d accounts for the fixed endpointsSource 7:

U(ξ)=fprior(ξ)+fobs(ξ),fprior(ξ)=12∑d=1Dwd∥Kdξ+ed∥2.\mathcal U(\xi)=f_{\mathrm{prior}}(\xi)+f_{\mathrm{obs}}(\xi), \qquad f_{\mathrm{prior}}(\xi)=\frac12\sum_{d=1}^{D} w_d\lVert K_d\xi+e_d\rVert^2.

The first-derivative term penalizes squared velocity; higher-order terms penalize other dynamics. The obstacle cost integrates a signed-distance penalty over the robot's body, weighted by arc length so that moving faster through a costly region cannot lower it. Covariant descent uses the inverse smoothness metric to spread each gradient step smoothly along the trajectorySource 7.

On the first six joints of a seven-DoF Barrett WAM, CHOMP solved 99 of 105 problems from 15 endpoint configurations. On LittleDog it ran inside a footstep controller with a separate footstep planner and a ZMP preview controller seeding the trunk. Trajectories were computed before execution because worst-case optimization took about 1.5 times as long as executionSource 7.

The paper also tests a Hamiltonian Monte Carlo variant and records finite-time local-minimum failuresSource 7; the 2013 journal CHOMP uses Hamiltonian Monte Carlo against high-cost local minima and for probabilistic completeness, and respects hard trajectory constraintsSource 17.

TrajOpt: penalties, trust regions, and collision limits

TrajOpt solves a sequence of convex subproblems, with a kinematic objective summing squared displacements between successive waypoints. Nonlinear constraints enter an ℓ1\ell_1 penalty whose coefficients grow in an outer loop as needed; linear constraints are imposed directly. With signed distance sd⁡\operatorname{sd} positive for separated objects, the collision penalty isSource 8

max⁡ ⁣(dsafe−sd⁡, 0).\max\!\left(d_{\mathrm{safe}}-\operatorname{sd},\,0\right).

It is nonzero inside the safety margin. Pairs out to dcheck>dsafed_{\mathrm{check}}>d_{\mathrm{safe}} enter the linearized subproblem, including some with zero current penalty. A box trust region bounds each step, expanding when actual improvement matches the local model and shrinking otherwiseSource 8.

Waypoint checks can miss collisions between waypoints, so TrajOpt penalizes a swept volume: the convex hull of endpoint shapes, inflated for rotation. The rotation bound adds darc=rϕ2/8d_{\mathrm{arc}}=r\phi^2/8 to the safety margin, a correction omitted in experiments because it was well under 1 cm. The swept-volume gradient is not guaranteed accurate in 3D, and the implementation used Bullet with convex hulls of robot meshes and penalized self-collisions only at discrete timesSource 8.

A simulated black Atlas humanoid robot stands at a table in a grey virtual warehouse, holding a red hose toward a green valve wheel on a wall.
A 2013 Defense Department rendering of a simulated Atlas connecting a hose, a DARPA Robotics Challenge task. TrajOpt planned statically stable walking for this 34-DoF humanoid.
Photo: U.S. Department of Defense / Wikimedia Commons. Licence: Public domain.

What the TrajOpt benchmark measured

The 2013 benchmark used four simulated MoveIt scenes with fixed endpoints: 198 seven-DoF PR2 arm problems with 11 timesteps and 96 eighteen-DoF full-body problems with 41. OMPL ran with default parameters and MoveIt's default smoother, and path length was normalized by the shortest path any planner found. Both tables give historical 2013 timingsSource 8.

Arm methodSuccess fractionAverage time (s)Average normalized length
TrajOpt0.840.201.2
TrajOpt, multiple initializations0.990.321.2
OMPL RRTConnect0.971.21.6
OMPL LBKPIECE0.963.11.7
CHOMP0.663.12.4
CHOMP, multiple initializations0.856.02.6

On the arm, single-initialization TrajOpt solved fewer problems than either OMPL planner and multi-initialization TrajOpt the most. The tested CHOMP implementation came from its authorsSource 8.

Full-body methodSuccess fractionAverage time (s)Average normalized length
TrajOpt0.632.11.08
TrajOpt, multiple initializations0.847.61.09
OMPL RRTConnect0.5318.01.5
OMPL LBKPIECE0.5018.71.5

The full-body comparison omits CHOMP, for which the authors lacked documentation or data. The introduction gives every planner ten seconds, while the experiments specify three seconds per CHOMP initialization and thirty seconds for full-body OMPLSource 8.

cuRobo runs many parallel optimizer seeds on GPUs and solves difficult motion generation problems within 50 ms on average, 60x faster than state-of-the-art trajectory optimizationSource 18.

Initial guesses and the two-stage pattern

CHOMP and TrajOpt both need an initial trajectory and can stall in bad local minima. In CHOMP's tests, straight-line seeds often beat RRT seeds on the objective where collision removal succeeded; in TrajOpt's, multiple initializations changed success ratesSource 7 Source 8.

Ratliff and colleagues describe PRM and RRT as commonly deployed in two phases: find a feasible path, then remove redundant or jerky motionSource 7. Shortcutting, a cheap second stage, repeatedly picks two trajectory points and tries to replace the segment between them with a shorter collision-free oneSource 19.

Where this meets the learned stack

Motion Policy Networks trained on over 3 million motion planning problems in over 500,000 environmentsSource 23. A 2026 survey finds that neural motion planners often struggle to generalize to unseen, out-of-distribution planning settingsSource 30.

Deep Reactive Policy replaced AIT* with cuRobo as its expert planner because GPU acceleration let it scale to 10 million expert trajectories. Its authors report that AIT* achieved 0% success on all dynamic tasks, and cuRobo fell to 3.00% when the goal was temporarily obstructedSource 24.

Ariadne, from Carnegie Mellon's Silong Yong, Guanya Shi and colleagues (October 2026, accepted at NeurIPS 2026), trains only on valid states, with no trajectories, and builds a path by following the geometry of the learned state manifold. On DualKUKA, where two 7-DoF arms must avoid obstacles and each other, it averaged 53.3% success across three environments, against 44.7% for RRT, 31.7% for a trajectory-trained Diffuser and 53.2% for PBDMP, which also receives the scene layout. Ariadne learns one planner per sceneSource 31.

Receding-horizon model-predictive control keeps optimization inside the feedback loopSource 25; reward design and the MPC debate weighs it against learned policies. Diffusion Policy represents a visuomotor policy as a conditional denoising diffusion process run with receding-horizon controlSource 26. Control and state estimation track the resulting trajectories and locate the robot.

The runtime handoff contract

A deployable planner needs the robot model, current state, collision geometry, allowed contacts, attached objects, joint and timing limits and a controller that can execute the result, and a path checked against a stale scene is unsafe. Time-Optimal Trajectory Generation (TOTG) computes the time-optimal trajectory that exactly follows a differentiable joint-space path within velocity and acceleration boundsSource 27, and MoveIt 2 has recommended it for time parameterization since January 2023Source 20. Because TOTG sets no jerk limits, Ruckig, a real-time jerk-limited generatorSource 28, can smooth its outputSource 21.

The cuRoboV2 authors note that fast planners output physically unexecutable trajectories, and report 99.7% success under a 3 kg payload against 72 to 77% for baselinesSource 29.

The ROS 2 guide maps these artifacts onto the runtime graph. In hybrid systems a learned component proposes a goal, cost, waypoint or action chunk, and a planner or command gate checks what the model cannot certify. Validators fail too: MoveIt Pro 10.1.0 fixed a shortcut stage that let paths pass through obstacles between collision checks, and a ValidateTrajectory bug that checked the wrong robot configuration and risked passing a colliding trajectory as collision-freeSource 22.