Sampling-Based Motion Planning

Chapter 1: Sampling Quality — Dispersion & Discrepancy

The planner probes C-space by sampling configurations. How well you cover C-space determines how quickly you find paths and how reliably you detect narrow passages. Two algorithms that both produce “uniform random” samples can have wildly different coverage quality on any finite budget of samples. This chapter is about making that difference precise — and about deterministic sequences that do better than random.

The van der Corput sequence: better than random

Here is LaValle’s cleanest demonstration (Figure 5.2). Suppose you want to place 8 sample points in [0, 1]. The naive approach: evenly spaced at 0, 1/8, 2/8, ..., 7/8. But you need to commit to n = 8 upfront. What if you don’t know n in advance? If you lay them down one at a time in sorted order — 0, 1/16, 1/8, 3/16, ... — after placing 4 points you’ve only covered [0, 3/16]. The entire right half of [0, 1] is unexplored.

The van der Corput sequence fixes this with a trick: reverse the binary digits of the index. The index 1 in binary is “1”, reversed and placed after the decimal point gives “0.1” in binary = 1/2. Index 2 = “10”, reversed = “0.01” = 1/4. Index 3 = “11”, reversed = “0.11” = 3/4. Index 4 = “100”, reversed = “0.001” = 1/8. The sequence: 0, 1/2, 1/4, 3/4, 1/8, 5/8, 3/8, 7/8, ...

For higher dimensions, the Halton sequence uses van der Corput independently on each axis, with successive prime bases: base 2 for axis 0, base 3 for axis 1, base 5 for axis 2, and so on. Because different primes produce decorrelated sequences, the resulting 2D (or kD) points are well-distributed across all dimensions simultaneously.

python
def van_der_corput(n: int, base: int = 2) -> float:
    """n-th term of the van der Corput sequence in given base.

    Worked examples (base 2):
      n=1  ->  0.5    (binary:  1  -> 0.1)
      n=2  ->  0.25   (binary: 10  -> 0.01)
      n=3  ->  0.75   (binary: 11  -> 0.11)
      n=4  ->  0.125  (binary: 100 -> 0.001)
      n=5  ->  0.625  (binary: 101 -> 0.101)
    """
    q, r = 0, 1
    while n:
        n, remainder = divmod(n, base)
        r *= base
        q += remainder / r
    return q

def halton_sequence(n: int, dim: int = 2) -> list:
    """Generate n Halton points in [0,1]^dim.

    Uses van der Corput with prime base per dimension.
    Output: list of n tuples (each of length dim).
    """
    primes = [2, 3, 5, 7, 11, 13, 17, 19, 23, 29]
    return [
        tuple(van_der_corput(i, primes[d]) for d in range(dim))
        for i in range(1, n + 1)
    ]

# Concrete comparison: 20 random vs 20 Halton points in [0,1]^2
import numpy as np
random_pts  = np.random.rand(20, 2)
halton_pts  = np.array(halton_sequence(20, 2))

# Largest empty circle radius (dispersion) for each:
# random_pts  -> typically ~0.25 (varies widely each run)
# halton_pts  -> consistently ~0.12 (stays stable)

Dispersion: the size of the largest gap

To make coverage quality precise, LaValle defines dispersion (equation 5.19):

In words: for every point x in the space, find the closest sample p; take the maximum of these distances over all x. Dispersion equals the radius of the largest empty ball — the biggest uncovered hole in your sample set. Lower dispersion means smaller largest gap, which means better coverage.

The theoretically optimal deterministic strategy is the Sukharev grid: place k^1/n samples per axis in an n-dimensional space, at cell centers. This achieves dispersion ½ · k^−1/n. The catch: to achieve dispersion ε in n dimensions you need (1/2ε)ⁿ points. In 10 dimensions with ε = 0.01, that’s 50¹⁰ ≈ 10¹⁶ points. This is the curse of dimensionality hitting dispersion directly — grid-based planners are dead on arrival in high-dimensional C-spaces.

Random sampling doesn’t achieve deterministically small dispersion — there’s always a chance a large gap remains. But random sequences are dense with probability 1: any ball of positive radius eventually gets a sample inside. The price is irregular coverage: some regions get many samples, others get few.

Sampling uniformly on SO(3): the Hopf fibration trick

For 3D rigid bodies, configurations include orientation, which lives on SO(3) — the 3-sphere S³ in quaternion representation with antipodal points identified. The naive approach: pick random Euler angles φ, θ, ψ uniformly from [0, 2π]. This is wrong. The Euler angle parameterization is not volume-preserving on SO(3); regions near the poles (θ near 0 or π) get oversampled.

The correct method (LaValle, equation 5.15) uses three uniform variables u₁, u₂, u₃ ∈ [0, 1] through the Hopf fibration decomposition:

This quaternion q = (q₁, q₂, q₃, q₄) is uniformly distributed on S³, giving a uniform distribution over all 3D orientations. The math comes from the Hopf fibration structure: S³ fibers over S² with S¹ fibers. Variable u₁ samples the base S² (the √ terms handle the non-uniform Jacobian), while u₂ and u₃ sample the fiber S¹.

Chapter 2: Collision Detection — The Black Box

The collision detector is the most expensive component of a sampling-based planner. In practice, 90%+ of planning time is spent here. Understanding how it works — and how to query it efficiently — determines whether your planner runs in 0.1 seconds or 10 minutes on the same problem.

The question is always the same: is A(q) ∩ O = ∅? Is the robot body at configuration q, which I’ll write as A(q), disjoint from the obstacle set O? Yes means q ∈ C_free. No means q ∈ C_obs. The planner never needs anything beyond this binary answer — but the collision detector can optionally return the distance to the nearest obstacle, which the planner can exploit to skip redundant checks.

Broad phase: culling the obvious non-collisions

A robot with many links and a scene with many obstacles requires checking many pairs. A naive O(n · m) check (n robot parts times m obstacles) is too slow. The broad phase eliminates pairs that clearly can’t intersect, leaving a small number of candidate pairs for exact narrow-phase checking.

The most common technique: Axis-Aligned Bounding Boxes (AABBs). Every object is wrapped in the tightest axis-aligned box. If two AABBs don’t overlap on ALL three coordinate axes simultaneously, the objects inside them can’t collide — no exact check needed. AABB overlap testing takes O(1) per pair. Combined with a Bounding Volume Hierarchy (BVH) — a tree of nested AABBs — most of the O(n · m) pairs are eliminated in O((n + m) log(n + m)) time.

Narrow phase: exact checking for candidate pairs

After the broad phase, a few pairs remain as potential collisions. The narrow phase does exact geometry: do these two specific shapes actually intersect?

For convex polyhedra, the gold-standard algorithm is GJK (Gilbert-Johnson-Keerthi). GJK finds the closest points between two convex shapes by iteratively improving a simplex in the Minkowski difference space. Each iteration is O(v) in the number of vertices. Crucially: GJK returns the distance between the shapes, not just a yes/no. This distance information is directly exploitable by the planner.

For robotics applications, Lin-Canny is often faster in practice. It tracks the “closest feature pair” between two convex polyhedra — which pair of features (vertex-vertex, vertex-edge, edge-edge, edge-face) is geometrically closest. When the robot moves a small amount, the closest feature pair usually doesn’t change. The next check starts with a warm cache and terminates in nearly constant time. This incremental coherence is crucial for planners checking many nearby configurations along a path.

Checking a path segment (Section 5.3.4)

The planner doesn’t just check isolated configurations — it checks whether a path from q₁ to q₂ is entirely collision-free. The path has uncountably many points; you can’t check them all. The standard strategy: subdivide the segment and check at a finite number of sample points.

The naive approach checks at evenly spaced intervals τ(t) = (1−t)q₁ + tq₂ for t = 0, Δ, 2Δ, ... This works but is wasteful: it checks both endpoints before the middle. If there’s a collision at t = 0.5, the naive checker wastes time on t = 0, Δ, 2Δ, ... before reaching it.

The smarter strategy: check samples in van der Corput order. Check t = 1/2 first, then 1/4 and 3/4, then 1/8, 3/8, 5/8, 7/8, and so on. This is binary search over the segment. Collisions anywhere on the segment are found faster because the strategy immediately explores the middle of every unexplored sub-interval.

Stop when the dispersion of the checked set falls below Δq — the resolution at which you’re guaranteed not to miss an obstacle. If any checked point is in C_obs, immediately return “not collision-free.” If all points down to resolution Δq pass, the segment is declared safe.

Using GJK’s distance output: if A(q) is d units from the nearest obstacle, the robot can translate up to d without collision in any direction. If the workspace displacement corresponding to the next step Δq is ≤ d, you can skip the check entirely and advance by d/Δq steps at once. This distance-based stepping can reduce collision checks by 10x in open regions of C-space.

python
def van_der_corput_order(depth: int) -> list:
    """t-values to check along a path segment, in van der Corput order.

    depth=1: [0.5]                    (just the midpoint)
    depth=2: [0.5, 0.25, 0.75]        (midpoint, then quarter-points)
    depth=3: [0.5, 0.25, 0.75, 0.125, 0.625, 0.375, 0.875]
    """
    result = []
    for k in range(1, depth + 1):
        step = 1.0 / (2 ** k)
        for j in range(1, 2 ** k, 2):   # only odd multiples of step
            result.append(j * step)
    return result

def check_path_segment(q1, q2, collision_fn, depth=8) -> bool:
    """Check straight-line path q1 -> q2 for collisions.

    Uses van der Corput ordering: checks the MIDDLE of each
    unexplored interval first, finding collisions faster.

    Returns True if path is clear; False at first collision found.
    collision_fn(q) -> bool: True means q is in C_obs.
    """
    for t in van_der_corput_order(depth):
        q = q1 + t * (q2 - q1)        # linear interpolation
        if collision_fn(q):
            return False             # collision found, stop immediately
    return True                       # all checks passed

# At depth=8: 2^8 - 1 = 255 t-values checked, dispersion = 1/512 along path
# For a step_size = 0.05m path, 255 checks guarantees no gap > 0.05/512 = 0.1mm

Chapter 3: The VSM + LPM Framework

Here is something surprising: RRT, PRM, SBL, KPIECE, EST — all the major sampling-based planners you’ll encounter in the robotics literature — are instances of the same two-line template. LaValle (Section 5.4.1) distills them into a framework built from two interchangeable components. Once you see this structure, reading new planner papers becomes much faster: you just need to identify the VSM and LPM.

The general incremental planner template

The framework maintains a graph G = (V, E) of collision-free configurations. Vertices V are configurations verified to be in C_free. Edges E are verified collision-free paths between adjacent vertices. The planner builds this graph iteratively until G connects q_I to q_G:

The Vertex Selection Method (VSM) decides which vertex to expand next. RRT’s VSM: pick the vertex nearest to a random sample α(i). PRM’s VSM: pick any vertex (the PRM builds the graph in a preprocessing phase, ignoring the goal). KPIECE’s VSM: pick a vertex in an underexplored cell of a discrete workspace projection. EST’s VSM: pick a vertex randomly weighted by the inverse of its local density.

The Local Planning Method (LPM) decides how to extend from q_cur to a new configuration. RRT’s LPM: shoot toward the random sample, stopping at the C_obs boundary. PRM’s LPM: connect to a nearby pre-sampled configuration with a straight-line path. Kinodynamic planners’ LPM: simulate the robot’s dynamics forward in time with a random control input.

The bug trap problem

Why can’t we just run grid search on C-space? The bug trap (Figure 5.13 in LaValle) illustrates why. Imagine a C-space where C_obs forms a deep bowl — a concavity with a narrow exit. The start is deep inside the bowl; the goal is outside. Grid-based BFS expands uniformly outward from the start, flooding the bowl’s interior before it can escape. In a 10-dimensional C-space, the bowl may contain an exponential number of grid cells.

The fundamental problem: uniform expansion allocates resources proportional to volume, not to progress. The bowl’s interior is a geometric dead end, but BFS can’t know that without first exploring it. The narrow exit — the critical feature — receives almost no attention until the entire bowl is exhausted.

Unidirectional, bidirectional, multidirectional

The simplest case: one tree from q_I reaches toward q_G. Disadvantage: if q_G is deep inside a narrow passage, the tree must find that passage from one side only — like threading a needle from one end.

Bidirectional search grows two trees simultaneously: T_a from q_I and T_b from q_G. When the trees are close, an LPM connection attempt is made. For a narrow passage, T_a approaches from one side while T_b approaches from the other. They need only meet somewhere in the passage — each tree needs to enter only halfway, not traverse the full length.

Multidirectional search places multiple trees at random configurations throughout C-space. Trees that grow close to each other are connected. This helps in C-spaces with many disconnected narrow passages, at the cost of managing which trees are worth extending.

Chapter 4: The RRT Algorithm — LaValle’s Invention

In 1998, Steven LaValle was a graduate student at Iowa State working on motion planning for systems with differential constraints. He needed an algorithm that could efficiently explore high-dimensional spaces without any prior knowledge of C_obs, and that worked for robots with dynamics (not just geometric constraints). What he invented, the Rapidly-exploring Random Tree (RRT), became one of the most-cited algorithms in all of robotics. This chapter explains exactly why it works.

The RDT without obstacles (Figure 5.16)

Start with the simplest possible version: an RDT (Rapidly-exploring Dense Tree) growing in free space with no obstacles. We just want the tree to quickly cover the space. The algorithm:

algorithm
SIMPLE_RDT(q_init, alpha, k):
  G.init(q_init)                       # single-vertex tree at start
  for i = 1 to k:
    q_new  = alpha(i)                  # sample a configuration
    q_near = nearest(G.vertices, q_new) # find closest existing vertex
    G.add_vertex(q_new)
    G.add_edge(q_near, q_new)           # connect nearest to new sample
  return G

Where α is a sequence of configurations — random (making it an RRT) or a deterministic low-discrepancy sequence (making it an RDT). The key line is nearest(G.vertices, q_new): instead of adding q_new as a free-floating point, we always attach it to the closest existing vertex. This one choice makes the tree rapidly explore.

The RRT with obstacles (Figure 5.21)

Adding obstacles requires one modification: instead of adding the sample directly, extend toward it from the nearest vertex, stopping at the C_obs boundary. The function stopping_config(q_near, q_rand) steps in direction (q_rand − q_near) at increment Δq until either a collision is detected or q_rand is reached. It returns the last collision-free configuration along this ray.

algorithm
RDT(q_init, alpha, k, stopping_config):
  G.init(q_init)
  for i = 1 to k:
    q_near = nearest(G.vertices, alpha(i))
    q_s    = stopping_config(q_near, alpha(i))  # extend until C_obs boundary
    if q_s != q_near:                             # made progress
      G.add_vertex(q_s)
      G.add_edge(q_near, q_s)                    # edge is collision-free by construction
  return G

The edge (q_near, q_s) is guaranteed collision-free because stopping_config checked every step along it. No separate path-segment check is needed. This is a key efficiency: the tree’s extension and its collision check are one unified operation.

The Voronoi bias: why RRTs explore rapidly

Consider the Voronoi diagram of the tree’s current vertices: the partition of C-space where each region contains all points closer to one vertex than to any other. Vertices that sit deep inside the already-explored region have small Voronoi cells — most nearby territory is already claimed by their neighbors. Vertices on the frontier have large Voronoi cells extending into unexplored space.

When we draw a uniform random sample α(i), the probability it lands in any vertex’s Voronoi cell is proportional to that cell’s volume. Large cells attract more samples. Therefore: frontier vertices attract more samples. Therefore: nearest(G, α(i)) more frequently returns a frontier vertex. Therefore: extensions happen preferentially in unexplored directions.

The result: RRT trees have fractal-like structure. After 45 iterations, tendrils reach into every corner of the space. After 2345 iterations, coverage is remarkably uniform (Figure 5.19 in LaValle). Grid-based BFS, by contrast, grows as a uniform sphere — it must fully explore every configuration within radius r before reaching any configuration at radius r + ε.

Full Python implementation

python
import numpy as np
from typing import Optional, Callable, List

class RRT:
    """Rapidly-exploring Random Tree (LaValle 1998).

    Works in any dimension. Only requires a collision checker
    (point query: is this configuration in C_free?) and a sampler.
    No knowledge of C_obs geometry is needed.
    """

    def __init__(self,
                 q_init: np.ndarray,
                 q_goal: np.ndarray,
                 collision_free: Callable[[np.ndarray], bool],
                 bounds: np.ndarray,        # shape (dim, 2): lo/hi per axis
                 step_size: float = 0.05,
                 goal_bias: float = 0.05):
        self.q_init       = q_init
        self.q_goal       = q_goal
        self.collision_free = collision_free
        self.bounds       = bounds
        self.step_size    = step_size
        self.goal_bias    = goal_bias
        self.dim          = len(q_init)
        self.vertices     = [q_init]      # list[np.ndarray]
        self.parent       = {0: None}    # vertex_index -> parent_index

    def _sample(self) -> np.ndarray:
        """Uniform random sample with small probability of returning goal."""
        if np.random.rand() < self.goal_bias:
            return self.q_goal
        lo, hi = self.bounds[:, 0], self.bounds[:, 1]
        return lo + np.random.rand(self.dim) * (hi - lo)

    def _nearest(self, q: np.ndarray) -> int:
        """Index of vertex nearest to q (Euclidean, O(n) naive scan)."""
        dists = [np.linalg.norm(v - q) for v in self.vertices]
        return int(np.argmin(dists))

    def _steer(self, q_near: np.ndarray, q_rand: np.ndarray) -> np.ndarray:
        """stopping_config: advance from q_near toward q_rand,
        stopping at C_obs boundary. Returns last collision-free config."""
        direction = q_rand - q_near
        dist = np.linalg.norm(direction)
        if dist < 1e-9: return q_near
        unit = direction / dist
        q_s  = q_near
        steps = max(int(dist / self.step_size), 1)
        for _ in range(steps):
            q_next = q_s + self.step_size * unit
            if not self.collision_free(q_next):
                break              # hit C_obs, stop before this point
            q_s = q_next
            if np.linalg.norm(q_s - q_near) >= dist:
                break              # reached q_rand, done
        return q_s

    def plan(self, max_iter: int = 5000) -> Optional[List[np.ndarray]]:
        """Run RRT. Returns path (list of configs) or None if failed."""
        for _ in range(max_iter):
            q_rand    = self._sample()
            idx_near  = self._nearest(q_rand)
            q_s       = self._steer(self.vertices[idx_near], q_rand)

            if np.linalg.norm(q_s - self.vertices[idx_near]) < 1e-9:
                continue           # didn't move (immediate obstacle), skip

            idx_new = len(self.vertices)
            self.vertices.append(q_s)
            self.parent[idx_new] = idx_near

            if np.linalg.norm(q_s - self.q_goal) < self.step_size:
                return self._extract_path(idx_new)

        return None               # max iterations exceeded

    def _extract_path(self, goal_idx: int) -> List[np.ndarray]:
        path, idx = [], goal_idx
        while idx is not None:
            path.append(self.vertices[idx])
            idx = self.parent[idx]
        return path[::-1]          # reverse: start -> goal

# Example: 2D point robot with a circular obstacle
def clear_of_circle(q: np.ndarray) -> bool:
    """True if q is outside circle of radius 0.25 centered at (0.5, 0.5)."""
    return np.linalg.norm(q - np.array([0.5, 0.5])) > 0.25

rrt = RRT(
    q_init=np.array([0.1, 0.1]),
    q_goal=np.array([0.9, 0.9]),
    collision_free=clear_of_circle,
    bounds=np.array([[0.0, 1.0], [0.0, 1.0]]),
    step_size=0.05, goal_bias=0.1
)
path = rrt.plan()
print(f"Found {len(path)} waypoints in {len(rrt.vertices)} tree vertices")
# Typical: "Found 43 waypoints in 287 tree vertices" (varies per run)

Chapter 5: RRT Variants — Connect, Star, Bidirectional

The basic RRT is already surprisingly effective, but three extensions — bidirectional growth, greedy connection, and rewiring — dramatically improve performance for different problem types. Understanding which variant to choose is as important as understanding the base algorithm.

RRT-Connect: bidirectional greedy extension (LaValle & Kuffner 2001)

Grow two trees simultaneously: T_a rooted at q_I and T_b rooted at q_G. Each iteration extends T_a toward a random sample α(i), then immediately tries to greedily connect T_b to the new vertex. “Greedy connect” means: extend T_b toward the new vertex in a loop, step after step, until either the trees connect or we hit C_obs. If they connect: solution found.

algorithm
RDT_BALANCED_BIDIRECTIONAL(q_I, q_G, alpha, K):
  T_a.init(q_I);   T_b.init(q_G)
  for i = 1 to K:
    q_n  = nearest(T_a, alpha(i))
    q_s  = stopping_config(q_n, alpha(i))
    if q_s != q_n:
      T_a.add(q_s, parent=q_n)
      q_n2 = nearest(T_b, q_s)            # T_b tries to reach q_s
      q_s2 = stopping_config(q_n2, q_s)   # greedy: loop until stuck or connected
      if q_s2 != q_n2:
        T_b.add(q_s2, parent=q_n2)
        if q_s2 == q_s:                    # trees met exactly: SOLUTION
          return extract_path(T_a, T_b, q_s)
    if |T_b| > |T_a|:  SWAP(T_a, T_b)    # always extend the smaller tree
  return FAILURE

The balancing step — SWAP(T_a, T_b) when T_b grows larger than T_a — prevents one tree from dominating. If T_a is stuck in a narrow passage and grows slowly, the algorithm automatically switches to extending T_b instead, approaching the passage from the outside where there is more room to maneuver.

RRT*: asymptotic optimality (Karaman & Frazzoli 2011)

A standard RRT finds some path, not necessarily a short one. Path quality is determined by the random sample order and step size — essentially arbitrary. RRT* adds two operations after each vertex insertion to guarantee that as iterations → ∞, the path cost converges to the global optimum.

Step 1 — Choose best parent. When adding q_new, find all existing vertices within a ball of radius r(n) = γ (log n / n)^1/d (where n = current vertex count, d = dimension). Instead of connecting to the nearest vertex, connect to whichever neighbor minimizes the total cost from q_I to q_new. This is local Dijkstra: pick q_best = argmin_{q ∈ near} [cost(root → q) + dist(q, q_new)], verifying each candidate edge is collision-free.

Step 2 — Rewire. After adding q_new, check whether any nearby vertex q_near can be reached more cheaply through q_new. If cost(root → q_new) + dist(q_new, q_near) < cost(root → q_near), and the edge (q_new, q_near) is collision-free, then redirect q_near’s parent to q_new. Propagate cost updates to all descendants.

python
def rrt_star_extend(tree, q_rand, collision_free, gamma, d):
    """One RRT* iteration: steer, choose best parent, rewire."""
    # Standard RRT steer
    q_near = tree.nearest(q_rand)
    q_new  = tree.steer(q_near, q_rand)
    if not collision_free(q_new): return

    # RRT*: find ALL neighbors within shrinking ball
    n = len(tree.vertices)
    r = gamma * (np.log(n) / n) ** (1.0 / d)   # radius shrinks as tree grows
    near = tree.neighbors_within(q_new, r)

    # Choose best parent (minimum cost path from root)
    best_parent = q_near
    best_cost   = tree.cost(q_near) + tree.dist(q_near, q_new)
    for v in near:
        c = tree.cost(v) + tree.dist(v, q_new)
        if c < best_cost and collision_free_segment(tree[v], q_new):
            best_cost, best_parent = c, v
    new_idx = tree.add(q_new, parent=best_parent, cost=best_cost)

    # Rewire: can any neighbor reach root cheaper through q_new?
    for v in near:
        c_through = best_cost + tree.dist(new_idx, v)
        if c_through < tree.cost(v) and \
           collision_free_segment(q_new, tree[v]):
            tree.rewire(v, new_parent=new_idx, new_cost=c_through)
            # propagate cost improvements to all descendants of v

Informed RRT*: focusing the search

Once RRT* finds an initial solution of cost c_best, any strictly shorter path must pass through the prolate hyperspheroid (elongated ellipsoid):

This is the set of all points q that could be on a path no longer than c_best. Points outside E cannot possibly improve the solution. Informed RRT* (Gammell et al., 2014) restricts all future samples to this ellipsoid once a solution exists. The ellipsoid is easy to sample: transform a uniform unit-ball sample by scaling along the focus axis by c_best/2 and transverse axes by √(c_best² − d_foci²)/2, where d_foci = dist(q_I, q_G). As the path improves, the ellipsoid shrinks, further concentrating sampling.

Chapter 6: The PRM — Probabilistic Roadmaps

RRT grows a single tree for one query. What if you need to plan a thousand different paths through the same kitchen, or a robot arm that picks objects from a fixed workspace all day? Running RRT a thousand times is wasteful. Probabilistic Roadmaps (PRM), introduced by Kavraki et al. (1996) and described in LaValle Section 5.6, take a different approach: build a rich graph of C_free once, then answer any query quickly by connecting to that graph.

This is the multi-query philosophy. The work is front-loaded into a preprocessing phase; queries become cheap lookups.

The BUILD_ROADMAP algorithm

The preprocessing phase (Algorithm 5.25 in the book) loops until N collision-free vertices are added:

python
def build_roadmap(N, sample, collision_free, connect, neighborhood):
    """PRM preprocessing phase."""
    G = Graph()
    while len(G.vertices) < N:
        q = sample()                          # random config in C
        if not collision_free(q): continue   # reject if in C_obs
        G.add_vertex(q)
        for v in neighborhood(q, G):          # nearby vertices
            if G.same_component(q, v): continue   # already connected
            if connect(q, v):              # collision-free local path
                G.add_edge(q, v)
    return G

def query_roadmap(G, q_I, q_G, connect, search):
    """PRM query phase: attach start/goal, find path."""
    for v in neighborhood(q_I, G):
        if connect(q_I, v): G.add_edge(q_I, v)
    for v in neighborhood(q_G, G):
        if connect(q_G, v): G.add_edge(q_G, v)
    return search(G, q_I, q_G)            # Dijkstra / A*

The same_component check is crucial for efficiency. Using union-find (disjoint set union), each check is near-O(1). It prevents wasting time trying to connect vertices already in the same connected component. As the roadmap grows, the whole C_free becomes one component and new edges stop being needed.

Neighborhood strategies

How big should the neighborhood be? LaValle describes four strategies, each with different tradeoffs:

The Component K strategy is particularly smart: it ensures each new vertex connects to multiple different components, aggressively merging the roadmap. The visibility strategy leads to the Visibility Roadmap (Section 5.6.2).

Visibility roadmaps: drastically fewer vertices

The basic PRM can accumulate thousands of vertices in open space where a handful would suffice. The visibility roadmap maintains only two types of vertices:

• Guards: a new sample is added as a guard only if it cannot see any existing guard. Guards explore new territory.
• Connectors: a new sample is added as a connector if it can see vertices from at least two different components. Connectors bridge components.

Any sample that can see exactly one guard is discarded — it adds no new information. The result is a minimal roadmap: guards occupy distinct regions of C_free, connectors bridge between them. For well-connected spaces this is dramatically smaller than a basic PRM.

Chapter 7: Narrow Passages — The Achilles Heel

Every sampling-based planner — RRT, PRM, all of them — shares a fundamental weakness. Picture a room connected to another room by a doorway just barely wide enough for the robot. Randomly throwing darts at C-space, how often do you hit a doorway that’s 1% as wide as the whole space? About 1% of the time. A doorway 0.01% as wide? About 1-in-10,000 samples. In high dimensions, this gets catastrophically worse.

LaValle formalizes this with ε-goodness (eq. 5.41). For any configuration q ∈ C_free, define V(q) as the set of all configurations visible from q (connected by a collision-free straight line). The ε-goodness of C_free is:

This measures the smallest fraction of C_free visible from any single point. A narrow passage has small ε: the configuration at the passage entrance can see very little of C_free. PRM’s convergence rate scales with ε — small ε means exponentially more samples needed.

Heuristics that help (Section 5.6.3)

Gaussian sampling: Generate a pair (q₁, q₂) where q₂ is drawn from a Gaussian centered on q₁. If one is in C_free and the other is in C_obs, keep the one in C_free. Discard if both are free (not near a boundary) or both are in C_obs. This concentrates samples near the C_obs surface.

Bridge test (Figure 5.29b): Sample q₁ and q₂ from a Gaussian pair. If BOTH are in C_obs but their midpoint q_mid = (q₁+q₂)/2 is in C_free, then q_mid is likely inside a narrow passage. Keep q_mid. This specifically targets the passage interior.

Medial axis sampling: The medial axis is the skeleton of C_free — the set of points equidistant from at least two obstacle boundaries. Sampling on the medial axis produces configurations maximally clear of obstacles AND near the critical bottlenecks where passages are defined.

Boundary sampling: Binary search between a C_free point and a C_obs point to find a configuration on the boundary, then perturb along the boundary surface. Efficiently fills narrow corridors with samples right at the walls.

Chapter 8: Showcase — RRT vs PRM Side by Side

Everything converges here. The same obstacle environment, two different planners, running in real time. Watch how their philosophies play out: RRT sprints to a solution, PRM invests heavily upfront then answers each new query instantly. This is the definitive comparison that Chapter 6 promised.

The honest comparison table

Chapter 9: Path Smoothing & Practical Details

You found a path. Now look at it — it zigzags, doubles back, takes unnecessary detours. Sampling-based planners find feasible paths, not good ones. The fix is simple and effective: iterative shortcutting.

Iterative shortcutting (eq. 5.39)

Pick two random waypoints on the path. Try a straight-line shortcut between them. If the shortcut is collision-free, replace the intermediate waypoints with the direct connection. Repeat. Each successful shortcut eliminates zigzags and reduces path length. After enough iterations, the path converges toward the shortest path in its homotopy class.

python
def smooth_path(path, collision_free, iters=200):
    """Iterative shortcutting (LaValle eq. 5.39)."""
    for _ in range(iters):
        if len(path) < 3: break
        i = random.randint(0, len(path) - 2)
        j = random.randint(i + 1, len(path) - 1)
        if collision_free_segment(path[i], path[j]):
            path = path[:i+1] + path[j:]   # remove intermediate waypoints
    return path

Important limitation: shortcutting cannot change the homotopy class. If RRT went around the left side of an obstacle, shortcutting gives the shortest left-side path — it will never discover that going right was shorter. For homotopy-class exploration, you need multi-query planners or topology-aware methods.

Distance metrics matter

The nearest-neighbor query in RRT depends entirely on your distance metric. Use the wrong metric and the Voronoi bias misfires.

• Rⁿ: Euclidean. Fine for translating robots.
• S¹ (circle, one joint angle): arc length, not absolute difference. Angles 350° and 10° are 20° apart, not 340°.
• SE(2) (2D position + heading): weighted sum of translation and rotation distances. The weight must reflect which matters more for your robot.
• SE(3) (6-DOF): use a quaternion metric for rotation, not Euler angles (which have singularities).

Practical nearest-neighbor

Naive nearest-neighbor: for every new q_rand, check all n vertices in the tree. O(n) per query. For n=10,000 vertices: fine. For n=1,000,000: slow. Better structures:

• Kd-tree: O(log n) average for low dimensions (d ≤ 20). The standard for 2D–6D problems. Used in OMPL by default.
• Approximate NN (FLANN): For d > 20 dimensions, exact NN is nearly as slow as linear search. FLANN uses randomized kd-trees or LSH for fast approximate neighbors.
• Cover tree: Provably O(log n) in any metric space, including SE(3) with proper metrics.

Chapter 10: Completeness — What Can Go Wrong

RRT is probabilistically complete. That sounds reassuring. But what exactly does it mean, and what does it NOT guarantee?

The dense sequence argument

RRT’s probabilistic completeness (Theorem 5.6) relies on the random sequence being dense in C_free with probability 1. Formally: for every open set U ⊂ C_free and every N, infinitely many of the random samples fall in U. This is guaranteed by the law of large numbers for any continuous distribution over C.

But “dense with probability 1” is a statement about infinite sequences. In practice you run N iterations and stop. Resolution completeness is actually more useful for implementation: use a deterministic dense sequence (Halton, van der Corput) with fine enough resolution, and you will find any solution larger than δ clearance, deterministically.

What probabilistic completeness does NOT guarantee

• Optimality: The first RRT solution is almost certainly suboptimal. Only RRT* and variants provide asymptotic optimality.
• Polynomial time: Convergence rate depends on ε-goodness. Tiny ε (narrow passages) means exponentially many samples. No polynomial-time guarantee.
• Infeasibility certificate: When RRT fails, it cannot tell you whether no path exists or the path is just hard to find. Complete planners can certify infeasibility; probabilistic ones cannot.

Chapter 11: Connections

Chapter 5 sits at the center of modern motion planning. Here’s where everything connects.

Key equations from this chapter

Algorithm family tree

What connects to what in LaValle

• Chapter 4 (Configuration Space): The prerequisite. C-space, C_obs, and why the explicit computation is intractable. Everything in Ch 5 lives in C-space.
• Chapter 6 (Combinatorial Planning): Exact complete methods. Cell decomposition, visibility graphs, Voronoi roadmaps. Computationally harder but complete — can certify infeasibility.
• Chapter 7 (Extensions): Planning under uncertainty, belief-space planning, lazy edge evaluation.
• Chapter 14 (Differential Constraints): When your robot has dynamics, replace Euclidean steering with numerical integration. RRT extends naturally to kinodynamic planning.

Implementation: OMPL

python (OMPL-style pseudocode)
# Define C-space
space = RealVectorStateSpace(dim=2)
space.setBounds(-1, 1)
si = SpaceInformation(space)
si.setStateValidityChecker(your_collision_checker)

# Define problem
start = State(space); start[0] = -0.5; start[1] = -0.5
goal  = State(space); goal[0]  =  0.5; goal[1]  =  0.5
pdef = ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)

# Choose algorithm and solve
planner = RRTConnect(si)   # or RRT(), RRTstar(), PRM()
planner.setProblemDefinition(pdef)
planner.setup()
solved = planner.solve(5.0)   # 5s timeout
if solved:
    path = pdef.getSolutionPath()
    path.interpolate(100)      # smooth to 100 waypoints

The book is free

Steven LaValle’s Planning Algorithms (Cambridge University Press, 2006) is available free at lavalle.pl/planning/. Chapter 5 is the core of this lesson. Chapters 4, 6, and 14 are the natural next reads.

“The goal of planning is to find a feasible course of action, to provide high-level specifications of behavior that enable a system to act autonomously.”
— Steven M. LaValle, Planning Algorithms, 2006