Path Planning (A*, RRT, PRM, Lattice) — Robotics Reference

See also (Tier 3 family index): Path-Planning Algorithm Zoo

Scope. This note covers the geometric layer of robot motion planning: given a start configuration ${\mathbf{q}}_s$, a goal configuration ${\mathbf{q}}_g$ (or goal region), and a description of the obstacle field, compute a collision-free, kinematically feasible path $\mathbf{\sigma }:[0,1]\to {\mathcal{C}}_{\text{free}}$. Time parameterisation — turning that path into $\mathbf{q}(t),\dot{\mathbf{q}}(t),\ddot{\mathbf{q}}(t)$ for a controller — lives in the sibling note [[Robotics/trajectory-generation]]. The configuration space itself, and the FK/IK needed to build it, is [[Robotics/kinematics-dh]].

1. At a glance

A path is a continuous curve in configuration space; a trajectory is a path plus time. This note is about producing the curve. Two main algorithmic families dominate, plus a third for vehicles and a fourth for post-smoothing:

Family	Representative algorithms	Suits
Grid / discrete search	Dijkstra, A*, Theta*, JPS, D*, D* Lite, ARA*, AD*	2D/3D mobile robots on occupancy grids; ≤4 DoF
Sampling-based	PRM, RRT, RRT*, RRT-Connect, Informed RRT*, BIT*, FMT*, SST	6+ DoF manipulators, drones; kinodynamic problems
State-lattice	Hybrid-A*, motion-primitive lattices	Non-holonomic ground vehicles, Reeds-Shepp cars
Trajectory optimisation	CHOMP, STOMP, TrajOpt, GPMP2	Post-smoothing or single-shot smooth motion through clutter

Why path planning is its own discipline: the obstacle field lives in the workspace $\mathcal{W}\subset {\mathbb{R}}^3$, but the planner has to search in configuration space $\mathcal{C}\subset {\mathbb{R}}^n$ where the robot has $n$ degrees of freedom. For a 6-DoF arm $n=6$; for a humanoid $n\ge 30$. Configuration-space obstacles are not directly representable — collision is checked on demand by transforming the robot’s geometry into the workspace at each candidate $\mathbf{q}$ and running collision detection. The whole algorithmic taxonomy below is about how to search ${\mathcal{C}}_{\text{free}}$ efficiently without ever materialising it explicitly.

First ask before picking a planner:

1. How many DoF? ≤3 → grid (A* on an occupancy map). ≥5 → sampling-based (RRT-Connect, BIT*). Lattice for cars.
2. Single-query or multi-query? One pick-and-place pose → RRT-Connect / BIT*. Re-using the same scene for many queries → PRM (preprocess once).
3. Kinodynamic constraints? Differential constraints (Ackermann steering, drone dynamics) → Hybrid-A* or SST or kinodynamic RRT.
4. Constraint manifold? “Keep the cup upright” (5-DoF task constraint on a 7-DoF arm) → CBiRRT (Berenson 2009) with projection onto the manifold.
5. Replanning under changing costs? Static map → A*. Map updates as the robot drives → D* Lite (Koenig 2002) or ARA* / AD*.
6. Optimality required? A* with admissible heuristic is optimal; RRT is not; RRT* / BIT* are asymptotically optimal (the bound tightens with samples). Anytime variants give a quick suboptimal answer and improve on demand.

The whole field is open source and well-implemented: OMPL (Şucan, Moll, Kavraki 2012) provides 40+ sampling-based planners; MoveIt 2 wraps OMPL + CHOMP + STOMP + TrajOpt for manipulators; Nav2 provides A*, Theta*, and Hybrid-A* for mobile bases; Autoware and Apollo wrap Hybrid-A* + lattice planners for autonomous vehicles.

2. First principles

Configuration space and free space

The configuration space $\mathcal{C}$ is the space of all generalised coordinates that uniquely describe the robot’s pose. For a planar mobile robot $\mathcal{C}=SE(2)={\mathbb{R}}^2\times S^1$. For a serial $n$-DoF arm $\mathcal{C}=T^n$ (a product of circles for revolute joints, with $\mathbb{R}$-factors for prismatic). For a free-flying drone $\mathcal{C}=SE(3)={\mathbb{R}}^3\times SO(3)$.

Given workspace obstacles $\mathcal{O}\subset \mathcal{W}$, the C-obstacle of obstacle $O_i$ under robot geometry $\mathcal{A}(\mathbf{q})$ is

${\mathcal{C}}_{O_i}=\{\mathbf{q}\in \mathcal{C}:\mathcal{A}(\mathbf{q})\cap O_i\ne \varnothing \}$

Free space is ${\mathcal{C}}_{\text{free}}=\mathcal{C}\setminus {\bigcup }_i{\mathcal{C}}_{O_i}$. Computing ${\mathcal{C}}_{O_i}$ explicitly is in general intractable (PSPACE-hard for polytopal obstacles in general, Reif 1979; LaValle 2006 ch. 7). The whole zoo of planners exists because we cannot afford to materialise ${\mathcal{C}}_{\text{free}}$ — we can only sample it via collision queries.

Completeness and optimality

Planners are graded on what they guarantee:

Property	Definition	Examples
Complete	Finds a path if one exists, in finite time	Dijkstra, A*, BFS (on a finite graph)
Resolution-complete	Complete at chosen discretisation; finer resolution → finds finer solutions	Grid A*, lattice planners
Probabilistically complete	$P(\text{success})\to 1$ as samples $\to \infty$	RRT, PRM, RRT-Connect
Optimal	Returns the global minimum-cost path	Dijkstra, A* (with admissible $h$)
Asymptotically optimal	Cost of returned solution $\to$ optimal as samples $\to \infty$	RRT*, PRM*, BIT*, FMT*
Suboptimal but bounded	Cost $\le (1+ϵ)$ optimal	ARA*, AD*, anytime RRT*

There is a time/quality trade: RRT gives a feasible path fast, RRT* gives an asymptotically optimal one slowly. Most production stacks chain them — first solve with RRT-Connect, then post-smooth with shortcut + B-spline or feed into TrajOpt for local refinement.

Heuristics for grid search

A* (Hart, Nilsson, Raphael 1968) ranks open-set nodes by $f(n)=g(n)+h(n)$ where $g$ is the cost from start to $n$ and $h$ is an estimate of the cost from $n$ to goal. The algorithm is optimal iff $h$ is admissible (never over-estimates the true cost-to-go) and consistent ($h(n)\le c(n,n^{\prime })+h(n^{\prime })$ for all neighbours $n^{\prime }$).

Standard heuristics on a 2D grid:

Heuristic	Formula	Admissible for
Manhattan	$	\Delta x
Chebyshev	$\max(	\Delta x
Octile	$\max +(\sqrt{2}-1)\min$	8-connected, $\sqrt{2}$ diagonal cost
Euclidean	$\sqrt{\Delta x^2+\Delta y^2}$	Any-angle motion (Theta*, JPS+)

For a 6-DoF arm, the natural heuristic is the C-space Euclidean distance $\Vert {\mathbf{q}}_g-\mathbf{q}{\Vert }_2$, which is admissible if joint-space distance lower-bounds true path length under the chosen edge cost. In practice, learned heuristics (graph neural networks trained on past plans) reduce A* node expansions by 5–20× on standard benchmarks (Qureshi MPNet 2019).

Collision detection — the bottleneck

Every planner spends 80–95 % of its runtime in collision checks. The standard pipeline:

1. Broad phase — bounding-volume hierarchy (BVH) or spatial hashing rules out distant pairs in $O(\log N)$ per pair.
2. Narrow phase — exact collision between two convex meshes via GJK (Gilbert, Johnson, Keerthi 1988) for distance, EPA (Expanding Polytope Algorithm) for penetration depth. Non-convex meshes are pre-decomposed into convex pieces.
3. Continuous collision detection (CCD) for swept volumes between configurations ${\mathbf{q}}_1,{\mathbf{q}}_2$; needed to catch tunnelling on long edges. ROS/MoveIt uses FCL (Pan, Manocha 2012); Drake uses Hydroelastic; Bullet, ODE, and PhysX implement variants.

The cost is $\mathcal{O}({\text{links}}^2)$ for self-collision plus $\mathcal{O}(\text{links}\times \text{obstacle-tris})$ for environment collision. A 7-DoF arm checking against a 50k-triangle scene runs ~10 µs per pose on a 2024 x86 with FCL. The planning algorithm chooses how many of these checks to do — A* on a fine grid does millions; RRT does thousands.

Sampling distributions

Sampling-based planners differ in how they generate candidate configurations:

• Uniform in $\mathcal{C}$ — RRT default; bias-toward-goal 5–10 %.
• Halton / Sobol quasi-random — PRM* variants; more uniform coverage, log-factor improvement.
• Gaussian near obstacles — biases samples to narrow passages (Boor, Overmars 1999).
• Bridge — samples pairs near obstacles, connects midpoint if it’s free (Hsu 2003).
• Informed ellipsoidal — after first solution of cost $c$, only sample inside the prolate ellipsoid with foci at start/goal and sum-distance $c$ (Gammell 2014).
• Task-space — sample end-effector poses in $SE(3)$ and use IK to lift to $\mathcal{C}$ (workspace-biased sampling; Shkolnik 2009).
• Learned — diffusion-model or VAE-trained samplers (Janner, Du 2022 MPC-Diffuser).

Distance metrics in configuration space

Sampling-based planners need a notion of “nearest” — and the choice of metric materially affects performance. For a serial revolute arm the natural metric is joint-space Euclidean weighted by joint range: $d({\mathbf{q}}_1,{\mathbf{q}}_2)=\sqrt{{\sum }_i{w}_i(q_{1,i}-q_{2,i}{)}^2}$, with $w_i$ proportional to $1/\Delta q_i^{\max }$ so each joint contributes comparably. Bare unweighted Euclidean over-weights wrist joints (which travel further per radian than shoulder joints in arc-length terms) and produces “twitchy” paths.

For an Ackermann vehicle, the Reeds-Shepp metric — the closed-form length of the optimal forward-and-reverse path — is the right “as-the-car-drives” notion of distance. Planners that use Euclidean instead waste samples chasing kinematically infeasible neighbours.

For a free-flying drone in $SE(3)$, a weighted sum of position Euclidean and rotational geodesic on $SO(3)$ is standard: $d=\Vert {\mathbf{p}}_1-{\mathbf{p}}_2\Vert +\lambda \arccos \left(2⟨{\mathbf{q}}_1,{\mathbf{q}}_2{⟩}^2-1\right)$ with $\lambda$ tuned to balance translation vs rotation (typical $\lambda =0.5$ m/rad when “30° of rotation” feels equivalent to “15 cm of translation”). The choice of $\lambda$ alone changes which neighbour RRT picks and therefore the entire tree topology — pin it for reproducibility.

Steering functions

RRT-style planners need a steering function $\text{steer}({\mathbf{q}}_a,{\mathbf{q}}_b,ϵ)\to {\mathbf{q}}_{\text{new}}$ that produces a feasible motion of length at most $ϵ$ from ${\mathbf{q}}_a$ toward ${\mathbf{q}}_b$. For holonomic systems this is straight-line interpolation; for non-holonomic and kinodynamic systems it is a two-point boundary-value problem (BVP) — generally hard. Three options in practice:

• Closed-form for known systems: Reeds-Shepp (Ackermann), Dubins (forward-only Ackermann), Lyapunov controllers (unicycle).
• Numerical optimisation: solve a small TrajOpt or LQR with terminal cost. Adds 1–10 ms per steer call.
• Forward simulation only: integrate dynamics under a few sampled inputs and pick the input bringing closest to ${\mathbf{q}}_b$. SST uses this; cheap but lossy.

A solid heuristic: closed-form steer when available, forward-simulation otherwise; reserve numerical BVP for offline trajectory optimisation, not online sampling.

The narrow-passage problem

The single most studied failure mode in sampling-based planning is the narrow-passage problem. If ${\mathcal{C}}_{\text{free}}$ contains a corridor of measure $ϵ$ that connects two large regions, the probability that a uniform sample lands in the corridor is $ϵ/\text{vol}(\mathcal{C})$. With $ϵ$ small (e.g., a 5 mm gap in a 1 m workspace), expected sample count to find the corridor is $1/ϵ\approx 200$, but actual planner success rate drops faster than $1/ϵ$ because two samples must land in the corridor and be locally connectable. Gaussian, bridge-test, and obstacle-based sampling (OBPRM) all attack this; BIT*‘s heuristic-guided implicit RGG is the most effective single fix.

Edge collision-checking — lazy vs eager

A second algorithmic axis cuts across the planner zoo: when to evaluate edge collision. Eager planners (classical RRT, PRM) check each edge before adding it to the tree/graph. Lazy planners (Lazy PRM, LBKPIECE, BIT*) add edges optimistically and only check on the candidate solution path; if a check fails, the edge is removed and the search restarts from the cached state. Lazy collision is asymptotically free when most edges are not on the optimal path — a property that holds in cluttered scenes — and typically gives 2–10× speedup in industrial benchmarks.

3. Algorithm families in depth

3.1 Grid and discrete search

BFS — uninformed; on an unweighted grid produces the optimal path; $O(V+E)$.

Dijkstra — uninformed; weighted graph; uses a min-priority-queue keyed on $g$; $O((V+E)\log V)$ with a binary heap.

A* — informed; keyed on $f=g+h$. Same complexity bound as Dijkstra in the worst case, but with a tight $h$ expands ~$O(b^d)$ vs Dijkstra’s full graph, where $b$ is branching factor and $d$ is solution depth.

Theta* (Daniel, Nash, Koenig 2010) — any-angle on a grid. Instead of constraining children to grid neighbours, the parent pointer is re-routed to the grandparent if line-of-sight exists. Produces straighter, shorter paths than 8-connected A* with the same node count.

JPS — Jump Point Search (Harabor, Grastien 2011) — on a uniform 8-connected grid, exploits symmetry to expand only “jump points” where the path direction must change. 10–30× faster than A* on standard benchmarks (Sturtevant Moving AI maps); used in many AAA games.

D* (Stentz 1995) and D* Lite (Koenig, Likhachev 2002) — incremental replanners. When edge costs change (a new obstacle appears in the costmap), only the affected region is re-expanded, reusing the prior search tree. D* Lite is the modern variant; runs in ROS 2 Nav2 as SmacPlanner with a D* backend.

ARA* / AD* (Likhachev 2003) — Anytime Repairing A* / Anytime D*. Return a quick suboptimal path with bound $ϵ$, then improve toward $ϵ=1$ while time remains. Used in DARPA Urban Challenge replanning.

3.2 Sampling-based — single-query

RRT — Rapidly-exploring Random Tree (LaValle 1998 CS-TR-98-11). Grow a tree from ${\mathbf{q}}_s$:

loop until goal-reached or max-iters:
    q_rand ← uniform_sample(C)        (with 5–10 % prob, q_rand ← q_goal)
    q_near ← nearest_in_tree(q_rand)
    q_new  ← steer(q_near, q_rand, ε) (step of length ε)
    if collision_free(q_near, q_new):
        add edge (q_near, q_new)

Voronoi-bias property: the next sample tends to fall in the largest unexplored Voronoi region, which drives exponential coverage of ${\mathcal{C}}_{\text{free}}$. Probabilistically complete; not asymptotically optimal — the first feasible path is kept as-is and rarely matches the geodesic.

RRT-Connect (Kuffner, LaValle 2000 ICRA) — bidirectional. Grow two trees, one from start, one from goal; greedily extend each toward the other; meet in the middle. Typical 5–20× speedup over plain RRT; default in OMPL and MoveIt for arms.

RRT* (Karaman, Frazzoli 2011 IJRR) — asymptotically optimal. On each insertion, check all neighbours within a shrinking radius $r_n\propto (\log n/n{)}^{1/d}$ and rewire: connect ${\mathbf{q}}_{\text{new}}$ via the cheapest neighbour, and check whether any neighbour would be cheaper via ${\mathbf{q}}_{\text{new}}$. Cost of returned path converges to optimal as $n\to \infty$ a.s.

Informed RRT* (Gammell, Srinivasa, Barfoot 2014 IROS) — once a path of cost $c_{\text{best}}$ is found, restrict subsequent sampling to the prolate hyperspheroid with foci at start/goal and major-axis $c_{\text{best}}$. Outside that ellipsoid, no sample can improve the solution. Dramatic speedup in high dimensions.

BIT* — Batch Informed Trees (Gammell, Srinivasa, Barfoot 2015 ICRA) — combines RRT*‘s asymptotic optimality with A*-style heuristic ordering. Samples a batch of $n$ points, builds a lazy implicit RGG, and runs heuristic search with on-demand edge collision checks. Currently the strongest single-query asymptotically-optimal planner for 6–14 DoF arms.

Kinodynamic RRT / SST — Sparse Stable Trees (Li, Littlefield, Bekris 2016) — for systems with differential constraints (drone, car). Steer using forward simulation under the dynamics rather than linear interpolation; SST adds pruning of dominated nodes for asymptotic near-optimality.

3.3 Sampling-based — multi-query

PRM — Probabilistic Roadmap (Kavraki, Svestka, Latombe, Overmars 1996). Two phases:

1. Preprocessing (offline): sample $n$ free configurations, connect each to its $k$ nearest neighbours if the local path is collision-free. Result: a roadmap graph $G=(V,E)$.
2. Query (online): connect ${\mathbf{q}}_s,{\mathbf{q}}_g$ to the roadmap, run Dijkstra / A* on the graph.

Justified when the scene is static and many queries will be issued (e.g. an industrial cell that runs the same factory layout for months). Variants: PRM* (Karaman 2011) uses radius-based connections for asymptotic optimality; Lazy PRM (Bohlin, Kavraki 2000) defers edge collision checks to query time.

FMT* — Fast Marching Tree (Janson, Schmerling, Pavone 2015 IJRR). Batch sampling; wave-front expansion (like fast-marching method for Eikonal PDEs) from start through the sample set. Single-shot asymptotic optimality without rewiring; faster than RRT* in moderate dimensions for the same sample count.

3.4 State lattices and Hybrid-A* for vehicles

A car cannot drive sideways. Non-holonomic constraints — $\dot{x}\sin \theta -\dot{y}\cos \theta =0$ for an Ackermann vehicle — mean that the set of immediately reachable poses from $(x,y,\theta )$ is a curve in $SE(2)$, not a disk. Grid A* expanding to 8 neighbours does not respect this.

State-lattice planner (Pivtoraiko, Knepper, Kelly 2009 JFR): pre-compute a small library of dynamically feasible motion primitives — e.g., 21 short arcs of length ~1 m at various steering angles. Each primitive is a (Δx, Δy, Δθ) at a specific (x, y, θ) bin. Search the lattice with A* using a workspace-Dijkstra heuristic.

Hybrid-A* (Dolgov, Thrun, Montemerlo, Diebel 2010 IJRR — Stanford’s “Junior” for DARPA Urban Challenge) — continuous (x, y, θ) state but discrete grid for closed-set lookup. Two heuristics taken element-wise max:

• $h_1$ = workspace 2D Dijkstra to goal (cheap; ignores heading and non-holonomy)
• $h_2$ = Reeds-Shepp path length to goal (Reeds-Shepp 1990, the optimal car path between two poses with forward + reverse motion); ignores obstacles

Combining the two is provably admissible and tighter than either alone. Periodically, the planner attempts a Reeds-Shepp shortcut straight to the goal; if collision-free, terminate. Used in ROS 2 Nav2 (SmacPlanner with motion_model: "Reeds-Shepp") and in Autoware’s freespace_planner.

3.5 Trajectory optimisation as a planner

When the topological homotopy class is given (e.g., “reach forward through the gap”), local trajectory optimisation can replace combinatorial search.

CHOMP — Covariant Hamiltonian Optimisation for Motion Planning (Ratliff, Zucker, Bagnell, Srinivasa 2009 ICRA). Gradient descent on a cost functional combining a smoothness term $\int \Vert \ddot{\mathbf{q}}{\Vert }^{2}dt$ and an obstacle term using a precomputed signed distance field (SDF) of the workspace. Converges in tens of iterations; produces visibly smooth motion through clutter; can get stuck in homotopy classes — multiple seeds advised.

STOMP — Stochastic Trajectory Optimisation for Motion Planning (Kalakrishnan, Chitta, Theodorou, Pastor, Schaal 2011 ICRA). Same setting as CHOMP but uses a gradient-free Monte Carlo update: sample $K$ noisy trajectories, weight by exp(-cost), update mean. Handles non-differentiable cost terms (e.g., binary collision) directly. Slightly slower per iteration than CHOMP but more robust.

TrajOpt (Schulman et al 2014 IJRR). Sequential convex optimisation; collision constraints linearised at each iterate using FCL. Handles inequality constraints natively; consistently outperforms CHOMP/STOMP in head-to-head benchmarks on cluttered scenes. Used in industrial pick-and-place stacks (e.g., Kindred / Ocado warehouse robotics).

GPMP2 — Gaussian-Process Motion Planning (Mukadam, Yan, Boots 2018 IJRR). Treats the trajectory as a sample from a GP; uses factor graphs (iSAM2 backend) for inference. Particularly fast for online replanning in repetitive tasks.

3.5b Neural and learning-based motion planning

Three threads in the 2018–2026 literature have produced working planners:

• Learned heuristics for grid search. A CNN or GNN trained on (state, goal, obstacle map) → cost-to-go replaces the geometric heuristic in A*. Best-known: MotionNet (Qureshi 2019), Neural A* (Yonetani 2021). On standard 2D benchmarks, learned heuristics cut expansions 5–20× with no loss of completeness because A* falls back to its default $h$ when the learned value is non-admissible.
• Motion-planning networks (MPNet) (Qureshi et al 2019, T-RO). A neural network ingests the obstacle representation and the current ${\mathbf{q}}_t$ and emits the next ${\mathbf{q}}_{t+1}$ directly, with a beam-search fallback when collision is detected. Runs at ~1 ms per step; outperforms RRT* by 30–200× in wall time on training-distribution scenes.
• Diffusion / score-based planners (Janner 2022, Carvalho 2023). Treat the trajectory as a sample from a diffusion model conditioned on (start, goal, obstacles); generate by reverse-diffusion. Naturally handles multi-modal solutions. Limited by sample-time (~100 ms per trajectory on a 2024 GPU); active research area.

A robust 2026 production stack uses neural heuristics as a refinement over a classical planner — the geometric backbone (A*, BIT*) guarantees completeness and the learned component accelerates the common case.

3.6 Multi-robot path planning

Coupled (joint configuration space, exponential in robot count): infeasible beyond 3–4 robots.

Decoupled / Prioritised (Erdmann, Lozano-Pérez 1987): plan robot 1, then robot 2 treating robot 1’s trajectory as a moving obstacle, and so on. Fast but incomplete.

CBS — Conflict-Based Search (Sharon, Stern, Felner, Sturtevant 2015 AIJ). Two-level search: low-level plans each agent independently with A*; high-level adds constraints when conflicts arise and re-plans. Complete and optimal for grid MAPF. Wins ICAPS MAPF competitions; deployed at Amazon Robotics and Geek+ for warehouse fleets.

ECBS (bounded-suboptimal CBS), M* (Wagner 2015) and PBS / PP (priority-based variants) trade optimality for scale; modern MAPF solvers handle 1000+ agents on warehouse floors.

4. Practical math + worked examples

Example A — A* on a 50 × 50 occupancy grid

Setup: 50 × 50 grid, cell size 10 cm, start (5, 5), goal (45, 45), five rectangular obstacles totalling ~15 % of cells. 8-connected with diagonal cost $\sqrt{2}$.

Heuristic: octile $h=\max (|\Delta x|,|\Delta y|)+(\sqrt{2}-1)\min (|\Delta x|,|\Delta y|)$. Admissible and consistent → optimal A*.

Open set: binary heap keyed on $f=g+h$, ~50 000 push/pop ops total. Closed set: 2D boolean array.

Result on a typical instance (Python+numpy, 2024 laptop):

Algorithm	Nodes expanded	Wall time	Path length (cells × 10 cm)
Dijkstra	1547	38 ms	57.0
A* (Octile)	287	4.1 ms	57.0
Theta*	312	5.0 ms	53.6 (any-angle)
JPS	41	0.6 ms	57.0

JPS wins on uniform grids; Theta* wins on path quality. In ROS 2 Nav2, SmacPlanner2D with lattice_filepath set to an 8-connected lattice is the standard production choice.

Example B — RRT-Connect for a 6-DoF arm pick-and-place

Setup: a 6-DoF arm (UR5e DH from [[Robotics/kinematics-dh]]) in a tabletop cell with a fixture, a bin, and a target part. Plan from home pose to a grasp pose 80 cm in front of the base.

Parameters: step size $ϵ=0.05$ rad in joint space, goal-bias 5 %, max iterations 5000, FCL collision check against URDF meshes.

Typical OMPL RRTConnect performance (Intel i7, single-threaded):

Stage	Time	Notes
Tree extension (first feasible path)	35 ms	~600 iterations, ~3000 collision checks
Path simplification (shortcut, 20 attempts)	18 ms	Path length reduced from 11.4 rad → 6.8 rad
B-spline smoothing	4 ms	C² output
Total	57 ms	Sub-second budget easily met

Then handed to time-optimal parameterisation (TOPP-RA / Ruckig in [[Robotics/trajectory-generation]]) → 1.2 s execution at 60 °/s average velocity.

Goal-bias tuning: bias < 1 % → tree wanders, slow. Bias > 30 % → premature convergence; stuck on local obstacles. Bias 5–10 % is the broad sweet spot; OMPL default is 5 %.

Example B.2 — RRT* convergence vs samples (6-DoF arm)

Same scene as Example B; instead of stopping at first feasible path, run RRT* and log the best-cost path every 100 samples.

Samples	Best cost (rad)	Wall time	Notes
600	11.4	35 ms	First feasible (= RRT-Connect result)
1 500	8.7	95 ms	Rewiring dominant
5 000	7.2	380 ms	Approaching optimum
20 000	6.9	1.7 s	Within 5 % of optimum
100 000	6.84	9.2 s	$1\%$ residual; diminishing returns

The empirical convergence rate is $c(n)-c^{\ast }=\Theta (n^{-1/d})$ where $d$ is the C-space dimension; for $d=6$ this is a slow $n^{-1/6}$ decay, which is why Informed RRT* and BIT* outperform plain RRT* by 10–100× in wall time to the same cost.

Example C — Hybrid-A* for car parking

Setup: 2 m × 4.5 m car (Ackermann, min turning radius 5 m), parking lot with 6 m × 12 m drivable region and a 2 m × 5 m parking slot perpendicular to the lane. Start: heading north in the lane. Goal: parked, heading east.

State: $(x,y,\theta )$ discretised at 0.2 m × 0.2 m × 5°. Motion primitives: 6 — three steering angles (left max, straight, right max) × two directions (forward, reverse), each integrated for 0.7 m.

Heuristic: max of (a) Dijkstra in 2D ignoring heading and non-holonomy, precomputed once at scene-load; (b) Reeds-Shepp path length from current pose to goal, computed analytically per node.

Reeds-Shepp shortcut attempt every 10 expansions; if collision-free, terminate.

Typical performance (Autoware’s freespace_planner, AMD Ryzen 7 single-thread):

Metric	Value
Nodes expanded	380
Wall time	220 ms
Reeds-Shepp shortcut attempts	38
Path length (forward + reverse)	11.8 m
Reverse manoeuvres	2

If you replace either heuristic with a constant 0, the planner still works but expands 8–15× more nodes. Both heuristics together (max) are the production default.

5. Design heuristics

• Inflate the costmap by half the robot’s circumscribed radius, not the full radius. Half captures most of the safety margin while leaving narrow passages traversable; full inflation closes corridors that are demonstrably navigable.
• 8-connected grid + octile heuristic is the standard 2D default. 4-connected gives noticeably jagged paths and is slower per unit path-length. 16-connected gains little over 8 for the doubled branching factor; use Theta* or JPS+ instead for smoother paths.
• Grid resolution for mobile robots: 5 cm for indoor (cluttered office, warehouse aisles), 10 cm for outdoor (open campus), 20–50 cm for long-range (city-scale). Refine adaptively near the robot using a rolling-window costmap (Nav2 default).
• For arms, never plan in workspace — plan in joint space. Workspace planning forces an IK at every collision check, multiplies cost 10×, and singularity-jumps cause trajectory discontinuities. Compute the workspace path from $\mathbf{q}(t)$ via FK on output.
• Bias and step size for RRT are the two most important hyperparameters. Goal bias 5–10 %; step size $ϵ\approx 0.05$ rad per joint or 5–10 % of typical obstacle scale. Larger step → faster exploration but more collision-rejected edges; smaller → more thorough but slow.
• Always shortcut + smooth after RRT. A raw RRT path is ~30–70 % longer than the optimum. Shortcut: repeatedly pick two random points on the path and try the straight line; if collision-free, replace. 20–50 attempts converges. Then fit a B-spline or quintic for $C^2$.
• PRM only pays off above ~100 queries per roadmap. Below that, a fresh RRT-Connect per query is cheaper than building + maintaining the roadmap. PRM* with $\sim 5000$ nodes covers a 7-DoF cell in 2–10 s and lasts as long as the scene is static.
• CHOMP / TrajOpt as post-processor, not primary planner. Run a fast sampling-based planner first to escape the local optimum’s basin, then optimise. Pure trajectory-opt from a straight-line initial guess fails on cluttered scenes 30–50 % of the time.
• Constraint-manifold planning (CBiRRT): never sample uniformly in $\mathcal{C}$ and project — most samples land off the manifold and the projection iterations dominate runtime. Instead, project the tangent extension step from ${\mathbf{q}}_{\text{near}}$ each time. 10–100× faster than reject-and-project.
• Replanning frequency for mobile robots: global planner at 1 Hz, local planner at 10–20 Hz. Faster global replans cause oscillation as transient sensor noise shifts the costmap; slower local replans cause collisions with moving obstacles.
• Use multiple goal poses for end-effector tasks. The grasp may not require a unique 6-DoF pose — a cylindrical part can be grasped at any orientation about its axis. Submit a goal set (e.g., 8 evenly-spaced rotations) and let the planner pick the closest reachable one. Cuts planning failure rate 5–10×.
• Symmetry-breaking in MAPF: enforce a deterministic tie-break (e.g., agent ID) to avoid CBS branching on equivalent conflicts.
• Costmap layered architecture (Nav2 / ROS): keep static, obstacle, inflation, and voxel layers separate. Each can be toggled, retuned, or substituted independently. A single monolithic costmap conflates failures and makes debugging painful.
• Inflate by Minkowski sum, not isotropic blur when the robot is non-circular (forklifts, elongated AGVs). Standard inflation assumes a disk footprint; for elongated robots compute the obstacle inflation per orientation bin (4–8 bins typical).
• Plan in $SE(2)$ for tractor-trailer and articulated vehicles with a hitch-angle joint; tree-style RRT handles the kinematic linkage but Hybrid-A* needs to discretise the hitch angle as an additional state dimension.
• Bidirectional search (RRT-Connect, ME-A*) usually wins on long traverses with goals deep in narrow regions; the goal-side tree pulls toward the start-side tree and meets at a topologically “easy” intermediate.
• Hierarchical planning for long-range navigation: coarse global plan on a topological map (rooms, lanes, building floors) + fine local plan on the costmap. The two layers communicate via sub-goals; long-range A* on a few thousand topo nodes finishes in microseconds while local Hybrid-A* handles the geometry.
• Time budget allocation in MoveIt: split planner timeout between OMPL planning and simplification — default 5 s OMPL + 1 s simplification works well; for hard scenes raise OMPL to 30 s before declaring failure.

6. Components & sourcing — software libraries

Library	Domain	Algorithms exposed	Licence
OMPL 1.6 (Şucan, Moll, Kavraki)	Sampling-based, generic	RRT, RRT*, RRT-Connect, Informed RRT*, BIT*, FMT*, PRM, PRM*, EST, KPIECE, SST	BSD-3
MoveIt 2 (PickNik / OSRF)	Manipulator high-level	OMPL backend, CHOMP, STOMP, PILZ industrial, TrajOpt (via Tesseract); MoveIt Pro adds commercial features	BSD-3
Nav2 (Steve Macenski et al)	ROS 2 mobile robot	NavFn (Dijkstra), SmacPlanner2D (A*/JPS), SmacPlannerHybrid (Hybrid-A*), SmacPlannerLattice	Apache-2
FCL (Pan, Manocha 2012)	Collision detection	GJK, EPA, CCD on convex+mesh, bounding-volume hierarchies	BSD-3
Bullet	Physics + collision	Used in PyBullet, Drake, Isaac Sim	Zlib
HPP-FCL (LAAS-CNRS Pinocchio team)	Faster FCL fork	GJK improvements, Pinocchio integration	BSD-2
Drake (TRI)	Symbolic + planning	TrajOpt, CHOMP-like, MIQP/SOS planners	BSD-3
TEB Local Planner	ROS 2 local replanning	Time-elastic band; non-holonomic	BSD-3
Aikido / Pearl (CMU PRL)	Constraint manifolds	CBiRRT2, projected RRT*, world-pose planners	BSD-3
PythonRobotics (Sakai)	Educational	All textbook algorithms in pure Python; ~20k stars	MIT
Autoware Universe	Autonomous-vehicle	freespace_planner (Hybrid-A*), lane-driving planners	Apache-2
Apollo (Baidu)	Autonomous-vehicle	EM planner, lattice planner	Apache-2
MAPF benchmark (Sturtevant et al)	Multi-agent	CBS, ECBS, EECBS, PBS, M*, PIBT reference implementations	MIT
MoveBoss / TOPP-RA	Trajectory-opt	Convex parameterisation; feeds path from this layer	MIT

For a typical mid-2020s production stack, Nav2 + OMPL + MoveIt 2 + FCL covers ~90 % of robot-arm + mobile-base needs in a single ROS 2 distribution. Industrial vendor stacks (KUKA, ABB, FANUC, Yaskawa) ship proprietary planners that follow the same algorithmic taxonomy; KUKA Sunrise.OS uses a closed-form PtP plus jerk-limited blending, with sampling-based fallback for cluttered scenes.

Robot-class quick-pick

A condensed decision table for “which planner do I reach for first”:

Robot class	Primary planner	Replanning	Smoothing	Library entry-point
2D differential-drive mobile (TurtleBot, AMRs)	A* / Theta* on 2D costmap	D* Lite at 1 Hz	DWA / TEB local	nav2_smac_planner
Ackermann car / parking	Hybrid-A* + Reeds-Shepp shortcut	Replan when goal moves > 0.5 m	Optimisation-based smoother (e.g., LQR or polynomial fit)	nav2_smac_planner (motion_model: ackermann), Autoware freespace_planner
Highway-driving AV	Lattice planner over lane samples	EM planner / iterative deepening; 10 Hz	Quintic / B-spline lateral	Apollo lattice_planner, Autoware mission_planner
Quadrotor / multirotor	RRT* or BIT* in ${\mathbb{R}}^3$	Receding-horizon at 10 Hz	Minimum-snap polynomial ([[Robotics/trajectory-generation]])	Fast-Planner (HKUST), Ego-Planner, mav_trajectory_generation
6-DoF arm pick-and-place	OMPL RRTConnect	One plan per pick; cache + parameterise	TOPP-RA + Ruckig	MoveIt 2 + ompl_interface
7-DoF redundant arm with task constraint	CBiRRT or OMPL RCRRT	Per task instance	TOPP-RA on manifold	Aikido, OMPL constrained-state-space
Humanoid full-body	PRM with task-space sampling + footstep planner	Long-cycle	Optimisation per phase	TRAC-IK + custom; ETH ANYmal / Atlas custom stacks
Dual-arm bimanual	Coupled BIT* (lazy collision)	Per task	TrajOpt with closure constraint	MoveIt + Tesseract trajopt_ifopt
Multi-AGV warehouse	CBS / ECBS over space-time grid	Continuous windowed replan	Constant-velocity primitives	libMultiRobotPlanning::CBS

7. Reference data

Algorithm comparison

Algorithm	Completeness	Optimality	Complexity (per node / per step)	DoF range	Single/multi query
Dijkstra	Complete	Optimal	$O(\log V)$ heap	2–3	Single
A*	Complete	Optimal (admissible $h$)	$O(\log V)$ heap	2–4	Single
Theta*	Resolution-complete	Near-optimal	$O(\log V)+$ LOS check	2–3	Single
JPS	Complete	Optimal	$O(\log V)+$ jump scan	2 (uniform grid)	Single
D* Lite	Complete	Optimal	$O(\log V)$ amortised	2–4	Single (incremental)
RRT	Prob. complete	Not optimal	$O(\log n)$ NN + 1 CC	2–20	Single
RRT-Connect	Prob. complete	Not optimal	$O(\log n)$ NN	2–20	Single
RRT*	Prob. complete	Asymptotically optimal	$O(k_n\log n)$	2–12	Single
Informed RRT*	Prob. complete	Asymptotically optimal	Same as RRT*	2–12	Single
BIT*	Prob. complete	Asymptotically optimal	$O(\log n)$ heap	2–14	Single
FMT*	Prob. complete	Asymptotically optimal	$O(n\log n)$ batch	2–10	Single (batch)
PRM / PRM*	Prob. complete	(Asympt.) optimal	$O(nk)$ build, $O(\log V)$ query	2–8	Multi
Hybrid-A*	Resolution-complete	Near-optimal	$O(\log V)+$ RS	$SE(2)$ + non-holo	Single
CHOMP / STOMP	Local	Local	$O(N\cdot$ SDF lookup$)$ per iter	2–30	Local refinement
TrajOpt	Local	Local KKT	$O(N\cdot$ SQP$)$	2–30	Local refinement
CBS (MAPF)	Complete	Optimal	Exponential worst-case	2D grid	Multi-agent

CC = collision check, NN = nearest neighbour, LOS = line of sight, RS = Reeds-Shepp closed form, $k_n$ = neighbour radius shrinking with $n$.

OMPL planners — default parameters (rev 1.6, 2025)

Planner	Class	Default range	Default goal_bias	Notes
RRT	Single-query	0.0 (auto)	0.05	Original LaValle 1998
RRTConnect	Single-query	0.0 (auto)	n/a (no bias; bidirectional)	Default OMPL recommendation for arms
RRTstar	Asymptotically optimal	0.0 (auto)	0.05	rewireFactor = 1.1
LBTRRT	Lower-bound tree	0.0 (auto)	0.05	$ϵ=0.4$
BITstar	Asymp. optimal heuristic	n/a	n/a	samplesPerBatch = 100
FMT	Batch optimal	n/a	n/a	numSamples = 1000
PRM	Multi-query	0.0	n/a	maxNearestNeighbors = 10
LazyPRM	Multi-query lazy	0.0	n/a
KPIECE1	Discretisation-guided	0.0	0.05	goodCellScoreFactor 0.9
EST	Expansive-space	0.0	0.05
SST	Kinodynamic sparse	0.0	0.05	selectionRadius 0.2

range = 0 makes OMPL set $ϵ$ to ~5 % of state-space extent. goal_bias is the probability of sampling the goal directly.

Library function map

Need	Library / function
Plan 2D path on costmap	nav2_smac_planner::SmacPlanner2D::createPlan
Plan car path	nav2_smac_planner::SmacPlannerHybrid::createPlan
Plan arm joint path	moveit::planning_interface::MoveGroupInterface::plan (delegates to OMPL)
Collision check pair of poses	fcl::collide(o1, o2, request, result)
Distance field for CHOMP	moveit::distance_field::PropagationDistanceField
Time-optimal parameterisation	moveit::trajectory_processing::TimeOptimalTrajectoryGeneration::computeTimeStamps (or TOPP-RA Python)
Multi-robot MAPF	libMultiRobotPlanning::CBS<...>::search

8. Failure modes & debugging

• No path found, but visually one exists. Symptom: planner returns failure within timeout. Causes: (a) goal pose in ${\mathcal{C}}_O$ — typical when the IK solution clips a workspace obstacle; check goal validity before planning. (b) Start in ${\mathcal{C}}_O$ — the robot already touches an obstacle in the URDF; reduce link-mesh inflation in the URDF. (c) Costmap stale — robot’s local view contradicts the global map; force a costmap clear. Fix: in MoveIt, enable start_state_validity_check and inspect the validity_request response.
• Path returned, then trajectory execution collides. Symptom: simulated path is clean; real-robot execution hits the obstacle. Causes: (i) URDF mesh underestimates real geometry — link collision mesh missing the gripper; add fingertip collision; (ii) octomap voxel resolution too coarse for the thin obstacle; switch to 1 cm voxels in the cluttered area; (iii) the joint-space path is collision-free at sample points but tunnels between them; enable CCD or finer interpolation; (iv) the planner cleared its costmap during execution and stopped seeing a static obstacle. Fix: replay the path in simulation with 5× tighter step; run continuous collision detection.
• Plan time blows up on a “simple” goal. Symptom: a pose change of 5 cm takes 800 ms to plan instead of the usual 50 ms. Causes: the start pose moved adjacent to an obstacle, putting the planner in a narrow passage; default RRT-Connect struggles. Fix: use OBPRM-style Gaussian sampling near obstacles, or switch to BIT* which copes better in narrow passages.
• Path passes through a tiny gap that the real robot can’t fit. Symptom: planner satisfied, trajectory executes, gripper scrapes. Cause: robot mesh inflated less than the gripper extent; or workspace obstacles not inflated by half the robot circumscribed radius. Fix: re-inflate costmap or robot model; for arms, set padding parameter on the planning scene.
• Costmap stale / lag. Symptom: planner outputs paths that ignore a wall the camera just saw. Causes: TF tree latency; sensor topic dropped frames; pointcloud-to-costmap pipeline backed up. Fix: check ros2 topic hz on the costmap; verify tf_buffer_duration > sensor latency; reduce pointcloud rate if compute-bound.
• Local planner thrashing. Symptom: mobile robot oscillates left/right at a corridor entrance. Cause: local planner re-decides every 100 ms based on slightly different costmap snapshots. Fix: raise hysteresis in the goal-selection logic, smooth the costmap with a low-pass over 200–500 ms, raise the DWA / TEB path_distance_bias to make path-following stickier.
• High-DoF planner painfully slow. Symptom: 14-DoF dual-arm plans take >10 s. Cause: collision check dominates; uniform sampling wastes samples on infeasible regions. Fix: task-space sampling (sample EE pose, IK to joint space); enable lazy collision (BIT* / LazyPRM); pre-compute a self-collision SDF for the robot; use GPU collision (NVIDIA cuRobo, 2023).
• RRT non-determinism breaks regression tests. Symptom: CI suite flakes; same scene gives different plans each run. Cause: random sampler unseeded. Fix: pin ompl::RNG::setSeed(42) at start of test; allow multi-shot averaging if statistical determinism is required.
• Constraint manifold violations during execution. Symptom: CBiRRT path returned but the cup tilts 8° in the middle. Cause: linear interpolation between projected waypoints drifts off the manifold. Fix: re-project at each interpolated sample (cost = extra IK or pseudo-inverse step per control cycle); or use a denser CBiRRT.
• Replanning oscillation in D* Lite. Symptom: every costmap update triggers a path that differs slightly from the previous, causing the controller to chase a moving goal. Fix: add hysteresis — only commit a new path if it improves cost by > $\delta$ and differs from the old by more than a homotopy threshold.
• Tolerance interpretation bug. Symptom: planner reports success at tolerance 0.01, but the EE stops 8 mm away. Cause: tolerance interpreted in joint-space radians, not workspace metres. Fix: read the planner’s tolerance docs carefully; in MoveIt, distinguish goal_joint_tolerance (radians) from goal_position_tolerance (metres) and goal_orientation_tolerance (radians of rotation).
• Multi-robot deadlock under prioritised planning. Symptom: agent 3 has no valid path because agents 1 and 2 occupy its goal region. Cause: prioritised planning is incomplete by construction. Fix: switch to CBS / ECBS for completeness; or implement a “back off and retry” with reshuffled priorities.
• GPU memory blowing up under cuRobo / curobo / IsaacLab planners. Symptom: planner OOM on a “small” 7-DoF arm scene. Cause: each parallel rollout instantiates the full collision scene; batch size × scene-triangle count × float-per-triangle dominates. Fix: cap batch size (cuRobo defaults to 1024; drop to 256 for large scenes); pre-decimate the scene mesh; use convex-hull collision proxies.
• Octree resolution mismatch between perception and planner. Symptom: a 5 cm wall is recorded as 10 cm in the octomap, blocking a valid passage. Cause: octree leaf-size > camera resolution at the relevant distance. Fix: shrink octomap resolution to ≤ half the smallest passable feature; tune octomap_server resolution parameter.
• Mismatched coordinate frames between planner and controller. Symptom: planner outputs joint angles that the controller rejects because they exceed the joint-limit field. Cause: URDF and SRDF disagree on joint limits, or the planner uses signed joint angles where the controller expects 0–2π. Fix: read both files; pick the tighter limits; check set_joint_value_limits in the planning scene.
• Dynamic obstacles invisible to the global planner. Symptom: global plan crosses through a person who just walked into the corridor. Cause: global planner runs at 1 Hz on a static costmap; the local planner is the only layer that sees dynamic obstacles. Fix: maintain a separate “social” costmap with predicted human trajectories (DRL or constant-velocity prediction); raise local-planner authority over the global plan in the trajectory-tracking layer.

9. Case studies

9.1 DARPA Urban Challenge 2007 — Stanford “Junior” and CMU “Boss”

The 2007 DARPA Urban Challenge required autonomous cars to drive through a simulated urban environment with traffic and four-way stops. Both Stanford’s Junior (second place) and CMU’s Boss (first place) used path-planning stacks layered as: (a) route planner at the road-network level (Dijkstra on a road graph); (b) lane planner producing reference trajectories along lanes; (c) lateral / unstructured planner for parking lots, U-turns, and obstacle avoidance.

Stanford’s contribution was Hybrid-A* (Dolgov, Thrun, Montemerlo, Diebel 2010 IJRR), used in the unstructured planner: continuous state $(x,y,\theta )$, discrete bin lookup, dual heuristic (workspace Dijkstra + Reeds-Shepp), periodic Reeds-Shepp shortcut attempts. The algorithm replanned at 5 Hz on a desktop-class processor and handled parking, three-point turns, and obstacle-avoidance manoeuvres without explicit special-case logic.

CMU’s Boss used a state-lattice planner (Pivtoraiko, Knepper, Kelly 2009 JFR) with a few hundred pre-computed motion primitives and an Anytime D* (Likhachev, Stentz 2005) backend. Both planners’ source is available — Hybrid-A* lives in Autoware’s freespace_planner and Nav2’s SmacPlannerHybrid; the lattice planner lives in Apollo’s lattice_planner.

9.2 Boston Dynamics Spot autonomy SDK

The Spot quadruped (commercial since 2020) ships an autonomy stack with three layers visible to integrators: GraphNav (a topological-metric SLAM-built map of “waypoints” connected by traversable “edges”), Autowalk (record-and-replay), and a local planner that handles short-range obstacle avoidance + body-pose adjustments.

The local planner uses an RRT-style sampling planner in the body’s $SE(2)$ × footstep-plan space, with an obstacle representation derived from the on-board RGB-D cameras and 3D voxel grid. Planning runs at 10 Hz on the on-board CPU; replans every cycle to handle dynamic obstacles. Footstep planning is layered on top: given the body trajectory, the foot-placement planner picks footholds that satisfy stability + reachability constraints, similar to ANYmal’s RaiSim-trained policy.

The global planner is Dijkstra on the GraphNav waypoint graph (essentially A* with the Euclidean heuristic between waypoints), with the local planner handling the metric edge traversal. Public reference: Boston Dynamics Spot SDK documentation, bosdyn.client.graph_nav.

9.3 Amazon Robotics — multi-agent warehouse floor

Amazon’s fulfilment centres (formerly Kiva Systems, acquired 2012) run fleets of 50–4000 mobile drive units on a single warehouse floor. The control architecture is a centralised MAPF planner with on-line replanning: each robot’s path through the warehouse grid is a sequence of (cell, time) tuples, and the planner ensures no two robots occupy the same cell at the same time.

The published-academic equivalent is CBS / ECBS (Sharon, Stern, Felner, Sturtevant 2015 — Conflict-Based Search) with bounded sub-optimality $ϵ=1.5$ for scale. The high-level search branches on detected conflicts; the low-level is A* on a 2D grid with time as an explicit dimension (so the state is (x, y, t)), giving “space-time A*“. Wait actions are encoded as zero-cost edges $(x,y,t)\to (x,y,t+1)$.

Practical scaling tricks deployed in published warehouse stacks: (a) windowed planning — only plan the next 30–60 s of each robot’s trajectory and replan continuously; (b) safe-interval path planning (SIPP) (Phillips, Likhachev 2011) — collapse all time-points where a cell is free into intervals, reducing the search dimension; (c) rolling horizon — every $\Delta t$ seconds, accept the next $k$ committed actions and replan the rest. Public references: Wurman, D’Andrea, Mountz “Coordinating hundreds of cooperative, autonomous vehicles in warehouses” AI Magazine 2008; Geek+, Quicktron, AutoStore use similar architectures.

9.4 Universal Robots URCap + MoveIt 2 integration

The UR e-series cobots (UR3e, UR5e, UR10e, UR16e, UR20, UR30) all expose joint trajectory streaming via the External Control URCap (open source, Universal Robots A/S). MoveIt 2 integrates through ur_robot_driver (ROS-Industrial maintained); the path-planning pipeline is:

1. OMPL RRTConnect plans in 6D joint space (~50 ms typical for tabletop cells); collision check via FCL against the URDF + planning scene meshes.
2. Path simplification — shortcut with 20 attempts, then B-spline smoothing → C² output.
3. Time-optimal parameterisation — TimeOptimalTrajectoryGeneration (Pham 2017) gives the time-optimal $s(t)$ respecting per-joint $v_{\max },a_{\max }$.
4. Ruckig ([[Robotics/trajectory-generation]] §9.2) replans online to enforce jerk limits and handle preemption.
5. Stream to UR controller at 500 Hz via the RTDE interface; the UR closed-source PID + feedforward executes.

Typical cycle: pick from bin → transit → place on conveyor → return = 4 motions × 60 ms planning = 240 ms planning overhead per cycle, which fits comfortably in the 1.2-s mechanical cycle time. For higher-rate applications (sub-second pick-and-place), the planner is invoked once at startup and the trajectory cached; only the start and end joint values are re-parameterised per cycle (a 5 ms operation). Public reference: ur_robot_driver README and ROS-Industrial documentation.

10. Cross-references

• [[Robotics/trajectory-generation]] — the consumer of this note’s output: turns the path into $\mathbf{q}(t),\dot{\mathbf{q}}(t),\ddot{\mathbf{q}}(t)$ for the controller.
• [[Robotics/kinematics-dh]] — defines configuration space, the manipulator Jacobian for task-space planning, and the FK used in every collision check.
• [[Robotics/dynamics-rigid-body]] — planned sibling; torque limits along the path are consumed by TOPP-RA and kinodynamic planners.
• [[Robotics/pid-control]] — planned sibling; the controller that executes the trajectory derived from the planned path.
• [[Robotics/slam]] — planned sibling; produces the costmap / occupancy grid / 3D voxel grid the planner queries.
• [[Robotics/manipulator-design]] — planned sibling; joint limits and self-collision pairs that constrain the planner.
• [[Robotics/mobile-base-wheeled]] — planned sibling; Ackermann / differential-drive constraints handled by Hybrid-A* and lattice planners.
• [[Robotics/multirotor-design]] — planned sibling; differential flatness justifies polynomial-spline planning in ${\mathbb{R}}^3$ + yaw.
• [[Robotics/computer-vision-robotics]] — planned sibling; produces the obstacle geometry consumed by the planner.
• [[Engineering/state-space-methods]] — Lie-group state, controllability tied to non-holonomic constraints.
• [[Engineering/mpc-control]] — model-predictive control as an alternative kinodynamic planner; receding-horizon trajectory generation that consumes the global path as a warm start.
• [[Languages/Tier3/robotics-control]] — nav_msgs/Path, moveit_msgs/MotionPlanRequest, geometry_msgs/PoseStamped — the wire formats path planners produce.
• [[Languages/Tier3/ros2-robotics-config]] — planned Tier 3 reference for Nav2 / MoveIt 2 YAML parameter files (planner_server, controller_server, behavior_server).

11. Citations

1. Hart, P.E., Nilsson, N.J. & Raphael, B. “A Formal Basis for the Heuristic Determination of Minimum Cost Paths.” IEEE Transactions on Systems Science and Cybernetics, SSC-4(2):100–107, 1968. The canonical A* paper.
2. LaValle, S.M. “Rapidly-Exploring Random Trees: A New Tool for Path Planning.” Technical Report CS-TR-98-11, Iowa State University, 1998. The original RRT paper.
3. LaValle, S.M. & Kuffner, J.J. “Randomized Kinodynamic Planning.” International Journal of Robotics Research, 20(5):378–400, 2001.
4. Kavraki, L.E., Svestka, P., Latombe, J.-C. & Overmars, M.H. “Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces.” IEEE Transactions on Robotics and Automation, 12(4):566–580, 1996. The PRM paper.
5. Karaman, S. & Frazzoli, E. “Sampling-based algorithms for optimal motion planning.” International Journal of Robotics Research, 30(7):846–894, 2011. RRT* and PRM* asymptotic optimality proofs.
6. Kuffner, J.J. & LaValle, S.M. “RRT-Connect: An Efficient Approach to Single-Query Path Planning.” IEEE ICRA 2000, 995–1001.
7. Gammell, J.D., Srinivasa, S.S. & Barfoot, T.D. “Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic.” IROS 2014, 2997–3004.
8. Gammell, J.D., Srinivasa, S.S. & Barfoot, T.D. “Batch Informed Trees (BIT*): Sampling-based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric Graphs.” ICRA 2015, 3067–3074.
9. Janson, L., Schmerling, E., Clark, A. & Pavone, M. “Fast Marching Tree: a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions.” International Journal of Robotics Research, 34(7):883–921, 2015. FMT*.
10. Dolgov, D., Thrun, S., Montemerlo, M. & Diebel, J. “Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments.” International Journal of Robotics Research, 29(5):485–501, 2010. Hybrid-A* on Stanford’s Junior.
11. Pivtoraiko, M., Knepper, R.A. & Kelly, A. “Differentially constrained mobile robot motion planning in state lattices.” Journal of Field Robotics, 26(3):308–333, 2009.
12. Stentz, A. “The Focussed D* Algorithm for Real-Time Replanning.” IJCAI 1995, 1652–1659.
13. Koenig, S. & Likhachev, M. “D* Lite.” AAAI 2002, 476–483.
14. Likhachev, M., Gordon, G. & Thrun, S. “ARA*: Anytime A* with Provable Bounds on Sub-optimality.” NeurIPS 2003.
15. Daniel, K., Nash, A., Koenig, S. & Felner, A. “Theta*: Any-Angle Path Planning on Grids.” Journal of Artificial Intelligence Research, 39:533–579, 2010.
16. Harabor, D. & Grastien, A. “Online Graph Pruning for Pathfinding on Grid Maps.” AAAI 2011. Jump Point Search.
17. Ratliff, N., Zucker, M., Bagnell, J.A. & Srinivasa, S. “CHOMP: Gradient Optimization Techniques for Efficient Motion Planning.” ICRA 2009, 489–494.
18. Kalakrishnan, M., Chitta, S., Theodorou, E., Pastor, P. & Schaal, S. “STOMP: Stochastic Trajectory Optimization for Motion Planning.” ICRA 2011, 4569–4574.
19. Schulman, J., Ho, J., Lee, A., Awwal, I., Bradlow, H., Pan, J., Patil, S., Goldberg, K. & Abbeel, P. “Motion Planning with Sequential Convex Optimization and Convex Collision Checking.” International Journal of Robotics Research, 33(9):1251–1270, 2014. TrajOpt.
20. Mukadam, M., Dong, J., Yan, X., Dellaert, F. & Boots, B. “Continuous-time Gaussian Process Motion Planning via Probabilistic Inference.” International Journal of Robotics Research, 37(11):1319–1340, 2018. GPMP2.
21. Berenson, D., Srinivasa, S.S., Ferguson, D., Collet, A. & Kuffner, J.J. “Manipulation Planning on Constraint Manifolds.” ICRA 2009, 625–632. CBiRRT.
22. Sharon, G., Stern, R., Felner, A. & Sturtevant, N.R. “Conflict-Based Search for Optimal Multi-Agent Pathfinding.” Artificial Intelligence Journal, 219:40–66, 2015.
23. Şucan, I.A., Moll, M. & Kavraki, L.E. “The Open Motion Planning Library.” IEEE Robotics & Automation Magazine, 19(4):72–82, 2012.
24. Pan, J., Chitta, S. & Manocha, D. “FCL: A General Purpose Library for Collision and Proximity Queries.” ICRA 2012, 3859–3866.
25. Reeds, J.A. & Shepp, L.A. “Optimal paths for a car that goes both forwards and backwards.” Pacific Journal of Mathematics, 145(2):367–393, 1990.
26. LaValle, S.M. Planning Algorithms. Cambridge University Press, 2006. Free online at lavalle.pl/planning/. The canonical textbook for this layer.
27. Lynch, K.M. & Park, F.C. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017. Chapter 10 on motion planning; free PDF at hades.mech.northwestern.edu.
28. Choset, H., Lynch, K.M., Hutchinson, S., Kantor, G.A., Burgard, W., Kavraki, L.E. & Thrun, S. Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press, 2005. ISBN 978-0-262-03327-5.
29. MoveIt 2 documentation. PickNik Robotics / OSRF, https://moveit.picknik.ai/. Rev tracks ROS 2 Lyrical (2026).
30. Nav2 documentation. Steve Macenski et al, https://docs.nav2.org/. Rev 1.3 (2026); SmacPlanner, SmacPlannerHybrid reference.
31. OMPL — The Open Motion Planning Library. Şucan, Moll, Kavraki, https://ompl.kavrakilab.org/. Rev 1.6 (2025).
32. Autoware Universe documentation. Autoware Foundation, https://autowarefoundation.github.io/autoware-documentation/. freespace_planner Hybrid-A* reference.
33. Apollo Auto. Baidu, https://github.com/ApolloAuto/apollo. EM planner, lattice planner reference (rev 9.0, 2025).
34. Pham, H. & Pham, Q.-C. “A new approach to time-optimal path parameterization based on reachability analysis.” IEEE Transactions on Robotics, 34(3):645–659, 2018. TOPP-RA — the bridge to [[Robotics/trajectory-generation]].