Throwing Darts at the Configuration Space: Sampling-Based Planning & RRT

Chapter 1: Don't Map the Space — Probe It

The grid's fatal flaw is that it insists on visiting every cell, free or occupied, near the path or far from it. But to find a route you don't need a complete map — you need a sparse set of stepping stones that happen to connect start to goal through free space. Sampling-based planning builds exactly that: a thin scaffold of free configurations, ignoring the vast empty regions a grid would dutifully enumerate.

The recipe is startlingly simple. It rests on three primitives, each cheap and each independent of dimension:

• Sample. Draw a random configuration x_rand uniformly from the C-space. In 2D that's a random (x, y); in 6D it's six random angles. Drawing a random point costs the same whether you have 2 dimensions or 600.
• Collision-check. Ask a black-box module: is this configuration free? The collision checker takes a configuration and returns True/False. Crucially, it works in the workspace (the physical 2D/3D world where the obstacles live), so it never has to reason about the high-dimensional C-space directly.
• Connect. Try to join two free configurations with a local path — usually a straight line — and check that the line, too, is collision-free. If it is, the two samples are now neighbors in a graph.

Why does random sampling cover the space at all? Because uniform random points, given enough of them, eventually land in every region that has nonzero volume. A narrow corridor is harder to hit (it has small volume), but a wide-open room fills in fast. We never enumerate the free space; we sample it densely enough to stitch together a path.

Play with the sampler below. Each click of sample throws down a batch of random points; green ones are free (the collision checker accepted them), red ones landed in an obstacle and get discarded. Watch how quickly the free region gets populated — and how the obstacle interiors stay empty because every sample there is rejected.

Here is the collision check in code — the "black box" the whole field is built around. For point-vs-axis-aligned-rectangle obstacles it is a few comparisons:

python
import numpy as np

# Each obstacle is a box: (xmin, ymin, xmax, ymax).
obstacles = [(2,2,5,7), (6,1,8,5)]

def is_free(x):
    # x is a configuration, here a 2-vector (px, py).
    px, py = x
    for (xmn, ymn, xmx, ymx) in obstacles:
        if xmn <= px <= xmx and ymn <= py <= ymx:
            return False          # inside a box -> in collision
    return True                   # touched nothing -> free

def sample_free(bounds):
    # keep drawing until we get a free configuration
    while True:
        x = np.random.uniform(bounds[0], bounds[1])  # uniform in the box
        if is_free(x):
            return x

That is_free function is the only thing that ever touches the obstacle geometry. Everything else — PRM, RRT, RRT* — treats it as a sealed oracle. Swap in a 3D mesh-collision library or a full arm-vs-environment check and the planners above it don't change a line. That black-box abstraction is exactly why one sampling algorithm serves wheeled robots, drones, and arms alike.

Chapter 2: Probabilistic Roadmaps — Build Once, Query Many

The first great sampling-based planner is the Probabilistic Roadmap (PRM) [Kavraki et al., 1996]. Its philosophy: pay a big up-front cost to build a reusable map of the free space — a roadmap — then answer many start–goal queries cheaply against that one map. PRM is therefore a multi-query planner: build the roadmap once, reuse it as long as the environment doesn't change.

A PRM produces a graph G = (V, E) embedded in the free space. The vertices V are collision-free configurations; the edges E are collision-free straight-line connections between nearby vertices. Once built, any planning query becomes a graph search — A* or Dijkstra — over this roadmap.

The algorithm, line by line

Stanford's Lecture 6 gives the preprocessing step. Every symbol is defined as it appears:

• Step 1 — Sample. Draw n random configurations x₁, …, x_n from the C-space. Run each through the collision checker; discard any that land in an obstacle. The survivors become the vertices V.
• Step 2 — Connect. Pick a connection radius r — a distance threshold. For every pair of vertices x, x′ with ‖x − x′‖ ≤ r (close enough), test whether the straight line between them is collision-free. If it is, add an edge. The notation ‖x − x′‖ means the Euclidean distance between the two configurations.
• Query. Given a start s and goal t, connect each to the nearest roadmap vertices, then run a graph search to find a route along the edges.

Worked example: edge validation by hand

Let's connect two sampled vertices. Suppose the sampler produced A = (1, 1) and B = (4, 5), and we picked r = 6. First, are they within radius?

5 ≤ 6, so they're candidates — we proceed to check the segment. Edge validation is done by densely sampling the line and collision-checking each point. Walk from A to B in, say, 5 steps; at fraction t the point is A + t(B − A):

Every sampled point is free, so the edge A–B is added. If even one intermediate point had been inside an obstacle, we'd reject the edge — the line "passes through a wall." (Denser sampling catches thinner obstacles; the trade-off is more collision checks per edge.)

Build a roadmap yourself below. Press sample to scatter free vertices, connect to wire up neighbors within the radius, and query to find a path from start (orange) to goal (green) by searching the roadmap.

PRM in code

The whole preprocessing step is a couple of loops — sample-and-filter, then connect-within-radius:

python
import numpy as np

def build_prm(n, r, bounds):
    # Step 1: sample n free vertices
    V = []
    while len(V) < n:
        x = np.random.uniform(bounds[0], bounds[1])
        if is_free(x):
            V.append(x)
    V = np.array(V)

    # Step 2: connect pairs within radius r whose segment is free
    E = []
    for i in range(n):
        for j in range(i+1, n):           # each unordered pair once
            if np.linalg.norm(V[i]-V[j]) <= r and segment_free(V[i], V[j]):
                E.append((i, j))
    return V, E

def segment_free(a, b, steps=20):
    # densely sample the line a->b and collision-check every point
    for t in np.linspace(0, 1, steps):
        if not is_free(a + t*(b-a)):
            return False
    return True

After build_prm, a query is just: add s and t as temporary vertices, connect them to nearby roadmap nodes, and run A* over (V, E) — the very algorithm from Lesson 3, now on a sparse graph instead of a dense grid.

Chapter 3: Rapidly-exploring Random Trees — Grow a Tree From the Start

For a single query in a possibly-changing world, building a whole roadmap is wasteful. Rapidly-exploring Random Trees (RRT) [LaValle, 1998] takes the opposite tack: grow a single tree T, rooted at the start configuration, that reaches incrementally toward unexplored regions until it touches the goal. A tree is a graph with exactly one path between any two vertices — so once the goal is in the tree, the path back to the start is unique and instantly read off.

Each iteration adds (at most) one node by four steps. Define each symbol the first time it appears:

• 1. Sample. Draw a random configuration x_rand from the C-space (uniformly, for now).
• 2. Nearest. Find the tree vertex x_near closest to x_rand — the nearest-neighbor over all current tree nodes.
• 3. Steer. Take a single step of length η (the step size, also written ε) from x_near toward x_rand, producing a new candidate x_new. We don't jump all the way to x_rand — we inch toward it, so the tree grows in controlled increments.
• 4. Collision-check & add. If the segment x_near → x_new is collision-free, add x_new as a vertex and the edge (x_near, x_new). Otherwise discard and try a fresh sample.

Worked example: one extension, with actual numbers

Let's run a single RRT iteration by hand. Suppose the tree currently has three nodes:

Step 1 — Sample. The random sampler returns x_rand = (5.0, 4.0).

Step 2 — Nearest. Compute the Euclidean distance from x_rand to each tree node:

The smallest is d(x₂) ≈ 3.64, so x_near = x₂ = (1.5, 3.0).

Step 3 — Steer with η = 1.0. First the unit direction from x_near toward x_rand. The displacement is (5−1.5, 4−3) = (3.5, 1.0) with length √(3.5²+1²) = √13.25 ≈ 3.64. The unit vector is:

Step distance η = 1.0 along that direction, added to x_near:

Step 4 — Collision-check & add. Test the segment from (1.5, 3.0) to (2.462, 3.275). Suppose is_free returns True all along it. Then we add the vertex x_new = (2.462, 3.275) and the edge (x₂, x_new). The tree now has four nodes, and it just reached one step closer to the bottom-right region the sample came from. One iteration, done.

Step through it live. sample drops a random target and highlights the nearest node and the η-step; auto-grow repeats until the tree spans the space. Notice the tree's reach: it preferentially pushes into the empty regions.

Why does the tree rush into empty space? The Voronoi bias

This is RRT's secret and the reason for its name. Imagine drawing the Voronoi diagram of the tree's vertices — the partition of space where each region (a Voronoi cell) contains all points closer to that one vertex than to any other. Vertices on the frontier of the tree, bordering big empty regions, own large Voronoi cells; vertices buried inside the tree own tiny ones.

Now: a uniformly random sample is most likely to land in a large Voronoi cell — there's simply more area there. And whichever cell it lands in, that cell's frontier vertex becomes x_near and gets extended. So the tree is biased to grow from its frontier into the unexplored void — automatically, with no special machinery. The exploration is a free side-effect of "sample uniformly, extend the nearest node."

python
def rrt_extend(T, bounds, eta):
    x_rand = np.random.uniform(bounds[0], bounds[1])   # 1. sample
    i_near = nearest(T.nodes, x_rand)                  # 2. nearest tree vertex
    x_near = T.nodes[i_near]
    x_new  = steer(x_near, x_rand, eta)               # 3. step eta toward x_rand
    if segment_free(x_near, x_new):                   # 4. collision-check & add
        T.add_node(x_new, parent=i_near)
    return T

def nearest(nodes, q):
    d = np.linalg.norm(nodes - q, axis=1)         # distance to every node
    return int(np.argmin(d))                       # index of the closest

def steer(a, b, eta):
    v = b - a
    dist = np.linalg.norm(v)
    if dist <= eta:
        return b                                  # close enough: land on b
    return a + (v/dist)*eta                       # else one unit step toward b

That steer function is exactly the hand arithmetic above: normalize the displacement to a unit vector, scale by η, add to x_near. The whole RRT is this 4-line loop repeated until the goal is reached.

Chapter 4: Goal Biasing — A Gentle Pull Toward the Target

Pure RRT explores magnificently but aimlessly — it fills the space evenly, with no preference for the goal. Left alone it might wander for thousands of iterations before a sample happens to fall near the target. We can speed this up enormously with one tiny change: goal biasing.

The trick: with a small probability p on each iteration, instead of sampling randomly, set x_rand = x_goal. The rest of the iteration is unchanged — find the nearest tree node, step η toward the goal, collision-check. So p×100% of the time the tree is actively reaching for the goal; the other (1−p)×100% it keeps exploring so it doesn't get trapped.

Stanford's HW2 pseudocode (Algorithm 2) writes it exactly. Each iteration:

Worked example: how often do we aim at the goal?

With p = 0.05 and the coin flips z = [0.71, 0.03, 0.88, 0.42, 0.01] over five iterations:

Two of five iterations aimed at the goal here — higher than the 5% long-run average, just sampling noise over a short run. On average about 1 in 20 iterations pulls toward the goal; the other 19 explore.

When do we stop?

Two termination conditions, from the same pseudocode:

• Success. When x_new reaches the goal (or lands within a small goal radius and the segment to the goal is free), add the goal and return the path by walking parent-pointers from the goal back to the root.
• Failure. If we hit the iteration limit K without reaching the goal, return Failure. (Remember from Chapter 0: sampling planners can't prove no path exists — "failure" just means "not found within K tries.")

Drag the goal-bias slider below and watch the tree's personality change. At p = 0 it's a pure explorer, filling space evenly. Crank p up and the tree visibly leans toward the goal — faster, but watch it occasionally get hung up on an obstacle when the bias is too aggressive.

python
def rrt_plan(x_init, x_goal, bounds, eta, p, K):
    T = Tree(x_init)
    for k in range(K):
        # goal biasing: with prob p, aim straight at the goal
        if np.random.rand() < p:
            x_rand = x_goal
        else:
            x_rand = np.random.uniform(bounds[0], bounds[1])

        i_near = nearest(T.nodes, x_rand)
        x_new  = steer(T.nodes[i_near], x_rand, eta)

        if segment_free(T.nodes[i_near], x_new):
            j = T.add_node(x_new, parent=i_near)
            if np.linalg.norm(x_new - x_goal) < eta:    # reached!
                return T.path_to(j)                       # walk parents back to root
    return None                                       # Failure within K iterations

Chapter 5: What RRT Promises — and What It Doesn't

RRT finds paths fast. But what kind of guarantee comes with that speed? This is where we must be careful, because sampling planners trade away the iron-clad completeness of a grid for a softer, probabilistic promise.

Probabilistic completeness

A planner is complete if it always finds a path when one exists, and reports failure when none does. RRT is not complete in that strong sense. Instead it is probabilistically complete:

Compare the three guarantee levels you now know:

RRT says nothing about optimality

Here is the sharper limitation. Even when RRT does find a path, that path is usually far from optimal — jagged, meandering, much longer than the straight-line shortest route. Karaman and Frazzoli (2011) proved it formally: RRT can return arbitrarily-bad paths with non-negligible probability. The classic picture is a planner that, given a long way around and a short way through, prefers the long red detour.

Why? Three reasons baked into the algorithm:

• Random sampling. The path threads through whatever random nodes happened to get added — no node was placed to make the path short, only to explore.
• Fixed parents. Once a node joins the tree, its parent is set forever. Even if a much better (shorter) connection appears later, vanilla RRT never reconsiders — it "traps itself," in Lecture 6's words.
• The η-step staircase. Edges are short straight segments at the angles dictated by random samples, so the path zig-zags rather than running straight.

See the jaggedness for yourself. The widget grows an RRT to a goal and draws the raw path it found; the dotted grey line is the straight-line ideal. The gap between them is RRT's suboptimality — and it persists no matter how long you grow the tree.

So we have a planner that is fast and probabilistically complete but produces ugly paths. Two ways out, which fill the rest of the lesson: fix the path afterward (smoothing, Chapter 7) or fix the tree as it grows so it converges to the optimum (RRT*, next).

Chapter 6: RRT* — Rewiring Toward the Optimum

RRT's suboptimality came from one place: once a node's parent is set, it's never reconsidered. RRT* [Karaman & Frazzoli, 2011] fixes exactly that. It adds two local-optimization steps to every iteration so that the tree's connections keep improving as samples accumulate — making RRT* asymptotically optimal: the path cost converges to the true optimum as n → ∞.

The new ingredient is a cost-to-come stored at every node. Write it c(x): the total path length from the root to x along the tree. It satisfies c(x) = c(parent) + ‖x − parent‖ — each node's cost is its parent's cost plus the edge length. RRT* uses c to make smarter choices.

RRT* runs the same sample → nearest → steer → collision-check as RRT, then inserts two extra steps before moving on:

• Choose-parent (within a radius). Instead of automatically parenting x_new to x_near, look at all tree nodes within a radius r of x_new. Among those whose connecting segment is collision-free, pick the parent x_p that minimizes the cost-to-come: c(x_p) + ‖x_new − x_p‖. So the new node attaches via the cheapest route, not just the nearest one.
• Rewire the neighbors. Now check the reverse: for each neighbor x_nb within r, would routing it through x_new be cheaper than its current parent? That is, is c(x_new) + ‖x_nb − x_new‖ < c(x_nb)? If so (and the segment is free), re-parent x_nb to x_new. The tree literally rewires itself to use the better connection.

Worked example: choose-parent by hand

A new node x_new = (4.0, 4.0) has appeared. Within radius r = 2.5 sit three existing nodes, each with a known cost-to-come:

Plain RRT would parent x_new to the nearest node, x_near (distance 1.00), giving cost-to-come 6.0 + 1.0 = 7.0. But RRT* checks all three: x_a is slightly farther (1.5) yet has a much lower own-cost (4.5), so routing through it gives 4.5 + 1.5 = 6.0 — cheaper. RRT* parents x_new to x_a. The new node's cost-to-come is 6.0, a full unit better than RRT's greedy choice.

The shrinking radius

The connection radius isn't fixed — it shrinks as the tree grows, like r_n = γ(log n / n)^1/d, where n is the node count, d the dimension, and γ a constant depending on d (Karaman & Frazzoli 2011). The intuition: when the tree is sparse (small n) you need a big radius to find any neighbors to rewire; as nodes pack in densely, a smaller radius still captures plenty of neighbors, and shrinking it keeps the per-iteration cost bounded. The log n / n form is precisely the rate that guarantees enough rewiring for optimality without the neighbor-checks ballooning.

Watch RRT and RRT* race on the same problem. The left tree is plain RRT — jagged, frozen edges. The right is RRT* — you can see the rewiring straighten its edges over time, and its path (orange) hugs the straight-line ideal far more tightly. Both reach the goal; only RRT* keeps improving.

python
def rrt_star_insert(T, x_new, r):
    near_ids = T.within_radius(x_new, r)            # neighbors within r

    # --- choose-parent: pick the lowest-cost collision-free parent ---
    best, best_cost = None, np.inf
    for i in near_ids:
        if segment_free(T.nodes[i], x_new):
            c = T.cost[i] + np.linalg.norm(x_new - T.nodes[i])
            if c < best_cost:
                best, best_cost = i, c
    j = T.add_node(x_new, parent=best, cost=best_cost)

    # --- rewire: re-parent neighbors that x_new now reaches more cheaply ---
    for i in near_ids:
        thru = best_cost + np.linalg.norm(T.nodes[i] - x_new)
        if thru < T.cost[i] and segment_free(x_new, T.nodes[i]):
            T.set_parent(i, j); T.cost[i] = thru     # rewired!
    return T

Chapter 7: Path Smoothing & Kinodynamic RRT

Sometimes you don't need the global optimum — you just need to clean up RRT's jagged output cheaply. And sometimes the path itself isn't enough: a real wheeled robot can't follow sharp corners. This chapter covers both fixes: shortcutting (post-processing the path) and kinodynamic planning (building dynamically-feasible edges in the first place).

Shortcutting: snip the corners

The simplest path cleanup is the Shortcut algorithm (HW2 Algorithm 3). The idea is one line: if you can connect a node's parent directly to its child with a collision-free straight line, delete the node in between. Repeat until no more deletions are possible.

Worked example: one shortcut

Say RRT returned the path A=(0,0) → B=(2,3) → C=(4,0) — a peak that detours up to B. Total length:

Shortcut asks: is the segment A→C (skipping B) collision-free? If yes, delete B. The new straight path A→C has length:

We cut the path from 7.22 to 4.00 — a 45% reduction — by removing one needless corner. That's the whole algorithm.

Try it: grow an RRT, then press shortcut and watch corners vanish as the path tightens against the obstacles.

Kinodynamic RRT: edges the robot can actually drive

Geometric RRT connects nodes with straight lines. But recall Lesson 2: a unicycle / differential-drive robot cannot move sideways — the no-slip constraint forbids it. A geometric path with a 90° corner demands the robot stop, spin in place, and restart. That's why HW2 notes geometric paths "are not ideal candidates to track… sharp corners would require stopping and turning at most nodes."

Kinodynamic motion planning bakes the robot's motion constraints into the tree. The state isn't just position — it includes orientation and possibly velocity — and the dynamics are governed by ẋ = f(x, u), read "the rate of change of the state equals some function of the current state x and the control input u." Two ways to grow constraint-respecting edges (Lecture 6):

• Forward-propagation (control sampling). Instead of steering with a straight line, sample a random control u and a random duration t, then integrate the dynamics ẋ = f(x, u) forward from x_near for time t. The resulting state x_new is reachable by construction, so every edge is dynamically feasible. Downside: it can't aim precisely at a chosen state.
• Steering-based. Use a steering function that solves the two-point boundary-value problem — "find a feasible trajectory from x to x′." For the Dubins car (fixed forward speed, bounded turning radius) and the Reeds-Shepp car (same, but can also reverse) there are analytical shortest-path solutions, so the steering function is fast. HW2 uses exactly the Dubins car for its kinodynamic RRT.

Chapter 8: The RRT Playground — Grow a Tree to the Goal

Everything comes together here. This is a full RRT / RRT* planner in a 2D obstacle field. You set the start, goal, and obstacles; press Grow; and watch the tree expand node-by-node toward the goal, then report the path it found. Toggle RRT vs. RRT* to see rewiring straighten the path, and tune the step size η and goal-bias p live.

Things to try, each demonstrating a chapter:

• Watch the Voronoi bias (Ch 3). Grow with p = 0 and see the tree rush evenly into every open pocket before any sample lands near the goal.
• Tune goal bias (Ch 4). Raise p to ~0.2 and the tree leans hard toward the goal — faster, until you crowd it with obstacles and it stalls trying to charge through them.
• See RRT's jaggedness (Ch 5). In RRT mode, the path cost stays well above the straight-line ideal no matter how long you grow.
• See RRT* rewire (Ch 6). Flip to RRT* and keep growing — the path cost drops over time toward the ideal as the tree re-parents nodes.
• Break it. Build a wall of obstacles fully around the goal: the tree fills the reachable region and the path never appears — sampling planners can't prove it's blocked, they just keep searching (Ch 5's no-completeness caveat, live).

Notice what's not here: a grid. There is no discretization anywhere in the showcase — the tree probes a continuous space directly. That is the entire point of sampling-based planning, and why the same five-line loop scales from this 2D toy to a robot arm in a cluttered kitchen.

Chapter 9: A* or RRT*? — When to Search, When to Sample

You now own two whole families of planners: search-based (Lesson 3's grid A*) and sampling-based (this lesson's PRM / RRT / RRT*). They are not rivals so much as tools for different jobs. Picking the wrong one makes a problem hard that the other makes easy — so let's settle when each wins.

Concretely:

• A* shines for a delivery robot routing across a 2D floorplan: low-D, you want the shortest route, and you can prove a corridor is truly blocked. This is exactly Lesson 3's setting and HW2 Problem 1.
• RRT* shines for a 7-DOF arm reaching into a cluttered shelf: the C-space is far too big to grid, you want a good-enough path now and a better one if you have spare milliseconds, and obstacle geometry is checked only where you sample.
• PRM shines for a fixed-layout warehouse where the same map serves hundreds of pick-and-place queries: amortize one expensive roadmap build over many cheap queries.
• FMT* (mentioned in Lecture 6) blends both: it runs dynamic programming over sampled nodes, lazily skipping colliding connections — fast like RRT, asymptotically optimal like PRM*, with the collision-check count of the two approaches going to zero relative to PRM*.

And don't forget the start of the lesson: deterministic sampling (low-dispersion sequences) can match the asymptotic optimality of random sampling while being reproducible and certifiable — valuable for safety-critical robots where "the planner did something different this run" is unacceptable.

Chapter 10: Connections & Cheat-Sheet

You can now plan motions in spaces too big to grid. On the autonomy stack, this lesson lives in the Motion Planning module — it consumes a map + goal and produces a collision-free path. Here is where it sits among its neighbors.

Cheat-sheet

Where to go next on Engineermaxxing

• Lesson 3 — Configuration Space & A* — the search-based planner this lesson scales beyond; revisit the C-space and the optimal low-dimensional baseline.
• Lesson 5 — Trajectory Optimization — turning a planned path into a smooth, dynamically-feasible, time-parameterized trajectory the controller can actually track.
• Lesson 2 — Rigid-Body Transforms & Kinematics — the SE(2) / no-slip model behind Dubins-car steering and kinodynamic edges.
• Markov Decision Processes — planning under uncertainty, when the next state isn't deterministic.
• Robotics Simulation — the collision-checking and kinematics machinery the planner calls as black boxes.