Section 30.3: Sampling-based planning: RRT, RRT*, PRM

Sampling-based planning is useful when the planner can explain the accepted path and the safety reason each rejected path failed.

A Local Planner With Commitment Issues

Problem First

Lay a grid over the 7 joint angles of a robot arm at a coarse 10 cells per axis, and you have already conjured ten million cells. Most of them sit inside a wall or cannot be reached. A manipulator, drone, or car-like robot lives in a continuous configuration space: the space of all possible robot poses, where each axis is one degree of freedom such as a joint angle or a base coordinate. Discretizing everything there becomes expensive or misleading. As Figure 30.3.1 illustrates, a sampling-based planner earns its keep only once its visual idea ties to a concrete state variable, an uncertainty model, and the next robot action.

Sampling-based planners trade exhaustive enumeration for random coverage. RRT grows a tree toward random samples, RRTstar rewires toward asymptotic optimality, and PRM builds a reusable roadmap. Collision checking is the core cost center. A planner that finds a path through configuration space but cannot confirm that path is collision-free has not solved the problem: it has only deferred the collision to execution time.

On a physical robot, an undetected collision is not a search failure: it is a broken joint, a damaged sensor, or an injured person. Every candidate edge in the tree must be verified before the robot can act on it, so the number of collision queries, not the tree structure, dominates planning time. A 7-DOF (Degrees of Freedom) arm issuing 10,000 samples per planning cycle executes tens of millions of geometry queries per second at full replanning rate.

Mechanically, a collision check sweeps the robot's geometry along the proposed edge in configuration space and tests for overlap with obstacle meshes at discrete intervals. Each test transforms the robot's link volumes into world coordinates, then runs a convex-hull or signed-distance query against every obstacle. Step size governs both safety and speed: large steps can jump through thin obstacles, while small steps multiply the query count. Libraries such as FCL and its GPU successor coalFCL (coal is a maintained fork of FCL that offloads the same bounding-volume queries to the GPU for higher query throughput) speed this up. They maintain bounding-volume hierarchies that reject non-overlapping pairs in one comparison rather than testing all geometry pairs.

RRT versus RRT*: first feasible against near-optimal

Every variant below reuses the same steer-and-check primitive that the Formal Model section states precisely later in this section; for now, read "steer toward the sample" as "take a small, collision-checked step from the nearest existing node in the direction of the random sample."

Figure 30.3.2 below contrasts the two behaviors side by side. The difference between RRT and RRT* matters in practice. Plain RRT stops as soon as any path reaches the goal. The result is the first feasible path found, not a good one. A robot arm navigating around a shelf may sweep 270 degrees around an obstacle when a 90-degree rotation was available. RRT* adds a rewiring step. After placing a new node, it checks whether nearby tree nodes would reach lower cost through the new node, and updates parent pointers accordingly. This mechanism is called asymptotic optimality through local rewiring. The difference is typically substantial, though the exact gap depends on the benchmark and implementation: on a 7-DOF arm benchmark (sampling-planner comparisons circa 2016-2020), plain RRT settles at path costs 40-60% above optimal. RRT* converges within 5-15% of optimal on the same 10,000-sample budget, with the exact margin depending on environment clutter and corridor width. Given enough samples, RRT* produces paths that converge toward the true optimum. PRM takes a third approach: it spends planning budget offline to build a dense roadmap of the configuration space, then answers repeated queries by searching that roadmap with Dijkstra or a similar graph planner. Concretely, building the roadmap means sampling many random configurations once, connecting each to its nearby feasible neighbors with the same collision-checked steering step used by RRT, and caching the resulting graph; every subsequent start-goal query then reuses that cached graph and only needs a fast graph search, not a fresh sampling pass. PRM is the right choice when the same workspace is queried thousands of times, as in a fixed industrial cell.

Formal Model

Those three behaviors, growing a tree, rewiring it, and reusing a roadmap, all reduce to the same single primitive: take a step from a known node toward a random sample and keep it only if it stays in free space.

Most navigation methods can be read as constrained search or optimization:

$$ q_{\mathrm{new}}=\mathrm{steer}(q_{\mathrm{near}},q_{\mathrm{rand}},\eta),\quad q_{\mathrm{new}}\in C_{\mathrm{free}} $$

The objective names feasible connection to the goal; constraints are collision, steering feasibility, clearance, dynamic limits, and planning-time budget.

Worked Diagnostic

The steering line in that loop is where most planner bugs hide, so before trusting a library it pays to watch a single step execute on paper.

Code Fragment 1 isolates sampling-planner invariants: valid sample, nearest neighbor, collision-free edge, and extracted path. The point is to catch geometry bugs before using OMPL.

# One RRT steering step in a 2D configuration space.
# The step-size limit prevents the tree from jumping through obstacles.
import numpy as np

q_near = np.array([1.0, 1.0])
q_rand = np.array([4.0, 5.0])
eta = 1.5
direction = q_rand - q_near
q_new = q_near + eta * direction / np.linalg.norm(direction)
print(np.round(q_new, 2))

Expected output interpretation. The new node lands partway toward the random sample, not at it, because the steering radius caps each expansion to a feasible local move. A real planner collision-checks this intermediate state, so the coordinates of `q_new`, not the sampled target, are what matter.

Tool Workflow

Once the steering and collision invariants from that diagnostic are understood, there is no reason to keep hand-writing them: the same primitives are already hardened in production libraries.

Keep the tiny RRT or PRM implementation as a regression test for sampling, nearest neighbor, collision checks, and path extraction. Use OMPL or MoveIt 2 for production planning.