Tree Planners: EST, RRT, and Friends

Single-query planning as tree growth — EST pushes from lonely nodes, RRT pulls toward empty space and its pull is a Voronoi bias; RRT-Connect, SBL, Choset's SRT that collapses to PRM or RRT with three parameters, and KPIECE.

The problem: the roadmap is stale

Build Chapter 11's roadmap for Reach on the Workbench. Then slide the block half a metre toward the base.

Every edge of the roadmap was collision-checked against a world that no longer exists. The red ones now pass through the block's new C-obstacle, and the purple path the roadmap would still confidently return runs straight through them. The roadmap does not know which edges died. To find out, it must re-check all of them — which costs about what it cost to build — and a roadmap that must be rebuilt after every change has lost the one thing it was for.

The tree on the right never learned the old world. Rooted at the start, it grows toward the goal on the world as it is now, spends its collision checks only on the part of $\Qfree$ this query passes through, and stops the moment a node reaches the goal. For one query it is cheaper; for one query in a world that moves it is the only sensible thing to do.

That is the single-query planner, and the chapter is about how a tree decides where to grow.

Building intuition: push from the lonely node, or pull toward empty space

Two instincts. EST pushes: pick a node that has few neighbors — it is on the frontier — and sample a new configuration near it. RRT pulls: pick a random target anywhere in $\Q$, find the tree node nearest to it, and step from that node toward the target. Both ask the tree where it has not been; they answer in different geometry.

Leave the Voronoi overlay on and watch a dozen iterations. The shaded cells are the Voronoi regions of the tree's nodes — every point of the square is colored by the node nearest to it, and the shade is the cell's area. The random target lands somewhere; the node that owns that somewhere is the one pulled. Big cells get pulled often, small ones rarely, and a cell is big exactly when its node faces unexplored space. Nothing random decides that; the tree does. The randomness only decides which large cell goes next.

Now the misconception the widget exists to kill: "RRT explores randomly." It does not. It explores where the Voronoi cells are large, which is a deterministic function of the current tree, and the proof is one line in the Foundation section. Three more things to try. Slide $\eta$ down and the tree becomes dense and short-edged, every step small, many iterations to cross the square. Slide it up and Trapped flashes at the amber band — a long step from a node near the C-obstacle lands inside it. Set the goal bias to one and the planner stops being a tree planner at all: every iteration pulls the goal-nearest node toward the goal, straight into the first wall, and the "stuck" badge lights. That is Chapter 7's gradient descent wearing a tree's clothes.

The mathematics

Definitions

The Voronoi-bias lemma

Step size and metric: two knobs with no theory behind their defaults

Two parameters sit in Algorithm 11 with nothing in the proofs to set them, and Choset's §7.2.2 is honest about both.

Step size $\eta$. Small $\eta$ makes every extension short and almost always free: the tree is dense, the frontier advances slowly, and iterations are spent densifying. Large $\eta$ crosses the space in a few steps and is Trapped whenever a node near an obstacle owns a large cell on the far side of it — which, by the Voronoi lemma, is often. The widget's iterations-to-solution curve has a minimum in between, and its location depends on the clearance of the problem, not on anything the algorithm knows. A common practical rule sets $\eta$ near the local planner's check step times a small integer, so that an extension is a few collision checks; the book's defaults are $0.6$ m for Rusty and $0.4$ rad for Reach, chosen by looking at the widget.

The metric. Nearest-neighbor under a different dist is a different planner, because the cells are different. On $T^2$ the circular metric is forced by the topology. On $\SEtwo$ there is no canonical choice, and the exchange rate between a metre and a radian decides whether the tree explores positions or headings first. Chapter 5 introduced the weight as a constructor argument for exactly this reason; Exercise 1 follows it to the limit. Cheng and LaValle's observation [C ref. 103] is the practical one: a metric that approximates the cost-to-go under the robot's own motion makes the Voronoi cells meaningful, and a straight-line metric for a car does not — which is the Hitch row of the lab.

Goal bias $p$ is the third knob, and it does have a theory, in Derivation 4: any $p < 1$ keeps the planner complete, and $p$ near $0.05$ is what the experiments favor.

Single-query PRM and Lazy PRM, for the record

Choset points out that PRM itself can be run single-query: insert $\qstart$ and $\qgoal$ as the first two nodes, grow, and stop when they share a component — which is exactly how Chapter 11's Roadmap Rain works. Bohlin and Kavraki's Lazy PRM goes further and creates a roadmap whose nodes and edges have not been checked at all, searches it, and checks only along candidate paths, coarse-to-fine, recording per edge the resolution reached so that a later path does not repeat work. It was shown experimentally to find a free path well before the roadmap was fully checked. The tree planners of this chapter are the other answer to the same question, and SBL borrows Lazy PRM's idea wholesale.

Probabilistic completeness, honestly

Choset's text says RRT "has been shown to be probabilistically complete under certain assumptions [271]" and leaves it there. The 2005 book contains no argument, and the chapter should say why the argument is harder than it looks.

EST covers an expansive space

Goal bias: why $p = 1$ is a potential-field planner

SRT specializes to PRM, RRT and EST

The algorithms

Line 7 matters more than it looks: the trees swap only after a successful first extension, so the tree that just grew hands the initiative to the other, and neither starves (Exercise 2).

Four planners, one seed, in lockstep. Choset's Algorithm 13 extends both trees by $\eta$; RRT-Connect extends one and lets the other Connect — keep stepping toward the new node until it is reached or trapped; the greediest setting lets both Connect. The misconception to watch die is "greedier is always faster." In the open rooms a Connect run crosses metres in one attempt and the greedy planners merge early. Near the doorways a greedy run is a long sequence of steps that ends Trapped against the wall, and every step of it was paid for in collision checks — the table's third column. Which setting wins depends on the clutter, and the chapter has no theory for the crossover (Exercise 4 measures it).

The fourth lane is SBL — Sánchez and Latombe's Single-query, Bi-directional, Lazy planner: a bidirectional EST with the grid-cell $\pi_T$ of §7.2.3, whose edges are not checked when created. Only when a new node lands within reach of the other tree does SBL check the candidate path joining the two roots — bridge first, then the longest unchecked edges — and if an edge fails it cuts the tree there and hands the severed piece to the other side, so no sample is wasted. The dashed edges on the canvas have never been collision-checked. The check counter shows what that saves: the check SBL: lazy edges records more than a thousand edges created and a few dozen ever verified.

Turn the dial. At one node per tree, one close pair and no merges, the picture is Chapter 11's roadmap: dots joined by straight edges. At zero roadmap trees it is this chapter's bidirectional RRT. Between, Choset's figure 7.16 — blobs of tree joined by solid close-pair edges and dashed merges. The misconception, "PRM and RRT are different algorithms," does not survive the slider.

KPIECE: EST's grid, generalized

The grid-cell $\pi_T$ of SBL bins tree nodes by their chart coordinates and picks a cell before a node. Şucan and Kavraki's KPIECE (2008) makes the bin a projection — any low-dimensional function of the configuration, joint angles for Reach, position for Hitch — and chooses the cell by an importance that rewards cells recently discovered, productive when expanded, rarely selected, isolated from neighbors and sparsely covered:

with a cell's score starting at $1 + \log(\text{iteration created})$ and decaying by a factor after every expansion — more after a failed one. Exterior cells (missing a neighbor) are chosen with high probability; the expansion itself is EST's push, a sample in the $\eta$-ball of a motion in the cell joined by $\Dist$. It is the planner OMPL reaches for by default, it is labelled modern here because it postdates Choset, and the implementation is single-level where the paper uses several.

The three extensions, by hand

The example the implementation is tested against. A point robot in $\Q = [0, 10]^2$ with the obstacle $\QO = [3, 4] \times [0, 3]$, root $q_0 = (1, 1)$, $\eta = 1$, Euclidean dist, straight-line extend with subdivision at $0.25$. The targets are fixed and replayed through Chapter 11's Scripted sampler — as targets, not nodes, because Choset's RANDOM_CONFIG draws from all of $\Q$ and the third target sits on the obstacle's edge on purpose: $q^{(1)}_{rand} = (5, 4)$, $q^{(2)}_{rand} = (2, 6)$, $q^{(3)}_{rand} = (4, 1.5)$.

Extension 1. The only node is $q_0$, at distance $5$ from $(5, 4)$. The direction is $(4, 3) / 5 = (0.8, 0.6)$, so $q_{new} = (1.8, 1.6)$. The edge never enters $x \ge 3$: free. Advanced.

Extension 2. Distances to $(2, 6)$: $\sqrt{26} = 5.099$ from $q_0$, $\sqrt{0.04 + 19.36} = 4.4045$ from $(1.8, 1.6)$. So $q_{near} = (1.8, 1.6)$; the direction is $(0.2, 4.4) / 4.4045$ and $q_{new} = (1.8454, 2.5990)$. Advanced.

Extension 3. Distances to $(4, 1.5)$: $3.0414$ from $q_0$, $2.2023$ from $(1.8, 1.6)$, $2.4187$ from $(1.8454, 2.5990)$. The nearest is $(1.8, 1.6)$ again — it becomes the branch point. Direction $(2.2, -0.1) / 2.2023$, $q_{new} = (2.7990, 1.5546)$, with $x < 3$, so node and edge are free. Advanced.

Final tree: four nodes; parents $(1.8, 1.6) \mapsto q_0$, $(1.8454, 2.5990) \mapsto (1.8, 1.6)$, $(2.7990, 1.5546) \mapsto (1.8, 1.6)$; total edge length $3.000$ — three steps of exactly $\eta$.

The same third extension with $\eta = 1.5$. From $(1.8, 1.6)$ a step of $1.5$ toward $(4, 1.5)$ lands at $(3.2985, 1.5319)$, inside $\QO$. The local planner checks the far endpoint first and rejects it: Trapped, tree unchanged at three nodes. Nothing in the tree records the attempt except the counter — which is why the widget's iterations-versus-nodes gap is the trapped count.

Implementation in Rust

Everything from Chapter 11 is imported; nothing is re-introduced. The chapter adds one tree type, one trait with a default method, and the planners.

use manifold::Manifold;                                                   // Ch. 5
use crate::{Chart, FreeSpace, Sampler, Steer, Uniform, nn::KdTree};        // Ch. 11
use rand::rngs::SmallRng;

pub type NodeId = usize;
pub struct Node<M: Manifold> { pub q: M::Point, pub parent: Option<NodeId>, pub depth: u32 }

/// One tree type for every planner in Part III. Ch. 13 wraps it with costs; it does not fork it,
/// so RRT and RRT* can be fed identical samples side by side.
pub struct Tree<M: Chart> { pub nodes: Vec<Node<M>>, nn: KdTree<M>, pub space: M }

impl<M: Chart> Tree<M> {
    pub fn rooted(space: M, q0: M::Point) -> Self {
        let mut nn = KdTree::new(space.clone());
        nn.insert(0, q0.clone());
        Self { nodes: vec![Node { q: q0, parent: None, depth: 0 }], nn, space }
    }
    /// O(log n) expected — the kd-tree with periodic axes from Chapter 11.
    pub fn nearest(&self, q: &M::Point) -> (NodeId, f64) { self.nn.k_nearest(q, 1)[0] }
    pub fn add(&mut self, q: M::Point, parent: NodeId) -> NodeId {
        let id = self.nodes.len();
        self.nn.insert(id, q.clone());
        self.nodes.push(Node { q, parent: Some(parent), depth: self.nodes[parent].depth + 1 });
        id
    }
    /// Root … v, by following parents. Every node has one: that is what "tree" buys.
    pub fn path_to_root(&self, mut v: NodeId) -> Vec<M::Point> {
        let mut out = vec![self.nodes[v].q.clone()];
        while let Some(p) = self.nodes[v].parent { out.push(self.nodes[p].q.clone()); v = p; }
        out.reverse();
        out
    }
}

The chapter's one addition to Chapter 11's Steer is a trait with a default method, blanket- implemented for every steer — so Chapter 21's Dubins steer gets an RRT for free.

/// The three-valued answer RRT-Connect's greedy loop needs (Choset Algs. 11–12).
pub enum ExtendStatus<P> { Reached(P), Advanced(P), Trapped }

/// Ch. 12's addition to Ch. 11's `Steer`: move at most `eta` toward `toward` along the
/// manifold's interpolant, then run the local planner on that short piece.
pub trait Extend<M: Manifold>: Steer<M> {
    fn extend(&self, space: &M, free: &dyn FreeSpace<M>, from: &M::Point, toward: &M::Point, eta: f64)
        -> ExtendStatus<M::Point>
    {
        let d = space.dist(from, toward);
        if d <= eta {                                   // close enough: try to reach exactly
            return match self.steer(space, free, from, toward) {
                Some(_) => ExtendStatus::Reached(toward.clone()),
                None => ExtendStatus::Trapped,
            };
        }
        // Why a fixed stride: the node that owns q_rand's Voronoi cell moves η toward it.
        // That is the whole Voronoi bias; a variable stride would be a different planner.
        let q_new = space.interpolate(from, toward, eta / d);
        match self.steer(space, free, from, &q_new) {
            Some(_) => ExtendStatus::Advanced(q_new),
            None => ExtendStatus::Trapped,
        }
    }
}
impl<M: Manifold, S: Steer<M>> Extend<M> for S {}       // every Steer, including Ch. 21's Dubins

pub struct GoalBias { pub p: f64 }                       // default 0.05 (Choset §7.2.2)

pub struct Rrt<M: Chart, S: Extend<M>, Smp: Sampler<M>> {
    pub tree: Tree<M>, pub steer: S, pub sampler: Smp, pub eta: f64, pub bias: GoalBias,
    pub rng: SmallRng,
    pub trapped: u64,                                    // the counter the widgets display
    pub goal_node: Option<NodeId>,
}

impl<M: Chart, S: Extend<M>, Smp: Sampler<M>> Rrt<M, S, Smp> {
    /// Choset Alg. 11 driven by the sampler; returns the status so callers can count Trapped.
    pub fn extend_once(&mut self, free: &dyn FreeSpace<M>, goal: &M::Point) -> ExtendStatus<M::Point> {
        let q_rand = if self.rng.random::<f64>() < self.bias.p { goal.clone() }
                     else { match self.sampler.sample(&self.tree.space, free, &mut self.rng) {
                         Some(q) => q, None => return ExtendStatus::Trapped } };
        let (near, _) = self.tree.nearest(&q_rand);
        let st = self.steer.extend(&self.tree.space, free, &self.tree.nodes[near].q, &q_rand, self.eta);
        match &st {
            ExtendStatus::Trapped => self.trapped += 1,
            ExtendStatus::Reached(q) | ExtendStatus::Advanced(q) => {
                let id = self.tree.add(q.clone(), near);
                // Goal test: within η of the goal, and the last short segment is free.
                if self.goal_node.is_none() && self.tree.space.dist(q, goal) <= self.eta
                    && self.steer.steer(&self.tree.space, free, q, goal).is_some() { self.goal_node = Some(id); }
            }
        }
        st
    }

    /// Choset Alg. 12: repeat extend toward q until Reached or Trapped.
    pub fn connect(&mut self, free: &dyn FreeSpace<M>, q: &M::Point) -> bool {
        loop {
            let (near, _) = self.tree.nearest(q);
            match self.steer.extend(&self.tree.space, free, &self.tree.nodes[near].q, q, self.eta) {
                ExtendStatus::Advanced(qn) => { self.tree.add(qn, near); }
                ExtendStatus::Reached(qn) => { self.tree.add(qn, near); return true; }
                ExtendStatus::Trapped => { self.trapped += 1; return false; }
            }
        }
    }

    /// Choset Alg. 10 plus the goal test, single tree.
    pub fn plan(&mut self, free: &dyn FreeSpace<M>, goal: &M::Point, n: usize) -> Option<Vec<M::Point>> {
        for _ in 0..n { if self.goal_node.is_some() { break; } self.extend_once(free, goal); }
        self.goal_node.map(|v| { let mut p = self.tree.path_to_root(v); p.push(goal.clone()); p })
    }
}

/// Choset Alg. 13 with a greediness switch per side; RRT-Connect = (Extend, Connect).
pub enum Greed { Extend, Connect }
pub struct RrtConnect<M: Chart, S: Extend<M>> { pub a: Rrt<M, S, Uniform>, pub b: Rrt<M, S, Uniform>, pub greed: (Greed, Greed) }
impl<M: Chart, S: Extend<M>> RrtConnect<M, S> {
    pub fn merge(&mut self, free: &dyn FreeSpace<M>, attempts: usize) -> Option<Vec<M::Point>> {
        let (mut t1, mut t2) = (&mut self.a, &mut self.b);
        for _ in 0..attempts {
            let Some(q_rand) = t1.sampler.sample(&t1.tree.space, free, &mut t1.rng) else { continue };
            if let Some(q_new) = t1.grow(free, &q_rand, &self.greed.0) {
                if t2.grow_to(free, &q_new, &self.greed.1) { return Some(join(&self.a, &self.b, &q_new)); }
            }
            std::mem::swap(&mut t1, &mut t2);            // Alg. 13 line 9: neither tree starves
        }
        None
    }
}

EST and SBL share the push and differ in bookkeeping: EST keeps a density weight per node, SBL keeps a grid of cells and a lazy flag per edge.

pub enum Density { Naive { radius: f64 }, GridCells { cell: f64 } }

pub struct Est<M: Chart, S: Steer<M>> {
    pub tree: Tree<M>, pub steer: S, pub radius: f64, pub density: Density,
    weight: Vec<f64>,                       // w_T per node under Density::Naive
    cells: HashMap<Vec<i64>, Vec<NodeId>>,  // occupied cells under Density::GridCells
    pub rng: SmallRng,
}

impl<M: Chart, S: Steer<M>> Est<M, S> {
    /// π_T: the node to push from. Sparse neighborhoods — the frontier — are favored.
    fn select(&mut self) -> NodeId {
        match self.density {
            Density::Naive { .. } => {
                // Why 1/w_T and not uniform: uniform over V pushes the dense interior most often.
                let dist = WeightedIndex::new(self.weight.iter().map(|w| 1.0 / w)).unwrap();
                dist.sample(&mut self.rng)
            }
            Density::GridCells { .. } => {
                // §7.2.3: a random non-empty cell, then a random node in it — O(1), same bias.
                let cell = self.cells.values().choose(&mut self.rng).unwrap();
                *cell.choose(&mut self.rng).unwrap()
            }
        }
    }

    /// Choset Alg. 9 from a chosen node: a free sample in its ball, joined by Δ.
    pub fn extend_once(&mut self, free: &dyn FreeSpace<M>) -> Option<NodeId> {
        let from = self.select();
        let q_new = sample_ball(&self.tree.space, &self.tree.nodes[from].q, self.radius, &mut self.rng);
        if !free.is_free(&q_new) { return None; }
        self.steer.steer(&self.tree.space, free, &self.tree.nodes[from].q, &q_new)?;
        let id = self.tree.add(q_new, from);
        self.register(id);                  // bump w_T of the neighbors, or file the cell
        Some(id)
    }
}

struct LazyNode<P> { q: P, parent: Option<NodeId>, tree: Side, checked: bool }

impl<M: Chart, S: Steer<M>> Sbl<M, S> {
    /// One iteration: expand the tree whose turn it is (grid-cell π_T), then try to bridge.
    pub fn step(&mut self, free: &dyn FreeSpace<M>) -> Option<Vec<M::Point>> {
        let side = self.turn(); let from = self.select(side);
        let q_new = self.sample_near(from, free)?;            // nodes ARE checked on creation
        let id = self.push(LazyNode { q: q_new.clone(), parent: Some(from), tree: side, checked: false });
        let (other, d) = self.nearest_in(side.other(), &q_new)?;
        if d > self.radius { return None; }
        self.try_bridge(free, id, other)                        // the only place edges get checked
    }

    /// Check the candidate path lazily: the bridge first, then unchecked edges longest-first.
    /// The first failure cuts that edge and re-roots the severed piece onto the other tree
    /// through the (verified) bridge — no sample is wasted.
    fn try_bridge(&mut self, free: &dyn FreeSpace<M>, a: NodeId, b: NodeId) -> Option<Vec<M::Point>> {
        self.edge_checks += 1;
        self.steer.steer(&self.tree.space, free, &self.q(a), &self.q(b))?;
        for e in self.unchecked_edges_on(a, b).sorted_by_length_desc() {
            self.edge_checks += 1;
            if self.steer.steer(&self.tree.space, free, &self.q(e.parent), &self.q(e.child)).is_some() {
                self.nodes[e.child].checked = true;
            } else {
                self.cut_and_transfer(e.child, a, b);
                return None;
            }
        }
        Some(self.join(a, b))
    }
}

The worked example is an executable whose output the test asserts, and the TypeScript port is pinned to the same four numbers.

fn main() {
    let world = BoxWorld::new([0.0, 10.0], [0.0, 10.0], &[[3.0, 4.0, 0.0, 3.0]]);
    let targets = vec![[5.0, 4.0], [2.0, 6.0], [4.0, 1.5]];
    for eta in [1.0, 1.5] {
        let mut rrt = Rrt::new(R2::unit_box(10.0), [1.0, 1.0],
            StraightLine { check: EdgeCheck::Subdivision { step: 0.25 } },
            Scripted::targets(targets.clone()),           // targets need not be free: RANDOM_CONFIG draws from Q
            eta, GoalBias { p: 0.0 }, SmallRng::seed_from_u64(12));
        println!("η = {eta}");
        for i in 1..=3 {
            let st = rrt.extend_once(&world, &[9.0, 9.0]);
            println!("  extension {i}: {st:?}");
        }
        println!("  parents: {:?}, total length {:.3}", rrt.tree.parent_map(), rrt.tree.total_length());
    }
}

η = 1
  extension 1: q_near = (1, 1), dist 5.0000, q_new = (1.8000, 1.6000), Advanced
  extension 2: q_near = (1.8, 1.6), dist 4.4045, q_new = (1.8454, 2.5990), Advanced
  extension 3: q_near = (1.8, 1.6), dist 2.2023, q_new = (2.7990, 1.5546), Advanced
  parents: {1: 0, 2: 1, 3: 1}, total length 3.000
η = 1.5
  extension 1: q_new = (2.2000, 1.9000), Advanced
  extension 2: q_new = (2.1335, 3.3985), Advanced
  extension 3: q_near = (2.2, 1.9), q_new = (3.6837, 1.5704) in QO, Trapped
  parents: {1: 0, 2: 1}, total length 3.000

The test pins the $\eta = 1$ trace to $10^{-4}$ and the statuses exactly; a second test pins the first twenty nodes of a SmallRng::seed_from_u64(12) run so the TypeScript port can be compared node by node; a third checks the SRT degeneration against Chapter 11's six-sample edge set. The TypeScript checks three extensions (η = 1) and third extension with η = 1.5 … Trapped replay the same trace — the second one computing the blocked configuration $(3.2985, 1.5319)$ from $(1.8, 1.6)$, which is where the port's third extension starts because its first two were the $\eta = 1$ ones.

#[test]
fn reproduces_three_extensions() {
    let (nodes, statuses, length) = trace(1.0);
    assert!(statuses.iter().all(|s| matches!(s, ExtendStatus::Advanced(_))));
    for (got, want) in nodes.iter().zip([[1.8, 1.6], [1.8454, 2.5990], [2.7990, 1.5546]]) {
        assert!((got[0] - want[0]).abs() < 1e-4 && (got[1] - want[1]).abs() < 1e-4);
    }
    assert!((length - 3.0).abs() < 1e-9);
    let (_, statuses15, _) = trace(1.5);
    assert!(matches!(statuses15[2], ExtendStatus::Trapped));
}

#[test]
fn srt_degenerates_to_prm() {
    let srt = Srt::prm_like(R2::unit_box(10.0), k = 2, Scripted::new(SIX_SAMPLES.to_vec(), true));
    srt.add_trees(6); srt.connect_all();
    assert_eq!(srt.edge_labels(), ["12", "23", "36", "45", "56"]);
}

Putting it together: five planners, three worlds

The integration lab runs RRT, EST, RRT-Connect, SBL and KPIECE on the Apartment (Rusty as a disc, $\mathbb{R}^2$), the Workbench (Reach, $T^2$) and the Lot (Hitch, $\SEtwo$), twenty seeds each, and reports medians of iterations, collision checks and path length. The bars are the data the tree_bench example writes; hover for the numbers.

Four things to read off it.

Single-query planners beat the roadmap at their own game. On the Workbench, RRT-Connect merges in a handful of attempts and a few hundred collision checks — less than a hundredth of what the Chapter 11 roadmap spent to learn the whole torus. That is the economics the hook promised: pay for the query you have.

The lazy planner buys checks with path quality. SBL's collision-check bill is the smallest of the five on the Apartment, and its path is the longest: it verified only the edges on one bridging path and never looked at the rest. Shortcutting repairs some of that, as w12.2's last column shows.

Push is more expensive than pull in these worlds. EST spends an order of magnitude more checks than RRT on the Apartment — its ball samples near frontier nodes fail often in the rooms' corners, and each failure is a check — while its coverage guarantee is no better. Where EST and KPIECE earn their keep is in spaces with no useful metric for the pull, which is Chapter 14's forward-propagation preview.

The Hitch row is wrong, on purpose. Every planner finds a path for Hitch in the Lot on most seeds with a straight-line steer in $\SEtwo$ — one that interpolates heading as freely as position, so the car slides sideways and spins in place along it. It is drawn in orange with the sentence it deserves: a steer must respect the robot. Hitch's kinematics exist in Chapter 2 and this chapter deliberately does not use them. Chapter 14 previews the alternative — replace extend with a simulator, so that EST's push returns as forward propagation — and Chapter 21 does it properly with a Dubins Steer that drops into this chapter's Rrt through the blanket Extend.

And one thing none of the planners can answer. Look at any of the paths: jagged, a random accident of sample order, and feasible. How good is it — and does more sampling make it better? For RRT the answer, proved in 2011, is no, almost surely. That is Chapter 13.

Exercises

1. Foundation exerciseFDifficulty 2 of 3Voronoi bias under a weighted metric

   Prove the Voronoi-bias lemma for the weighted $\SEtwo$ metric $w_t \lVert X' - X'' \rVert + w_r \lvert \theta' - \theta'' \rvert_{S^1}$, and describe how the Voronoi cells deform as $w_r \to 0$. What does RRT do to Hitch's heading in that limit, and why does the resulting tree look two-dimensional?

   Stuck? Get a hint
2. Foundation exerciseFDifficulty 2 of 3Why Algorithm 13 swaps

   Choset's MERGE swaps $T_1$ and $T_2$ only after a successful first extension. Show by example that without the swap one tree can starve — receive no new nodes for arbitrarily many iterations — and state the invariant the swap maintains about the two trees' expected node counts. Then explain why RRT-Connect's Connect on the second tree makes the swap more important, not less.

3. Conceptual exerciseCDifficulty 1 of 3Predict the step-size trade-off, then verify
   Predict first

   In Tree Grower at η = 0.15 rad the first solution takes many hundred iterations with a modest fraction Trapped. Before moving the slider: at η = 0.6 rad, what happens to the iteration count and to the Trapped fraction?

   Both fall — bigger steps cover the square faster and fail no more often

   Iterations fall, the Trapped fraction rises

   Both rise — long steps keep missing the goal
4. Conceptual exerciseCDifficulty 2 of 3Where greedy loses

   In Bidirectional Connect, compare Connect/Connect with Extend/Extend on median collision checks over at least twenty seeds. Then raise the merge budget and look for the regime in which the greedy planner's advantage disappears or reverses. Relate the crossover to the expected length of a Connect run before it ends Trapped — a geometric random variable whose parameter is the probability that one step of $\eta$ is blocked.

   If each step of a Connect run is blocked independently with probability 0.3, what is the expected number of steps (and collision-check rows) in a run that ends Trapped?

   Check Reveal
5. Practical exercisePDifficulty 2 of 3KPIECE with a projection

   Implement KPIECE's cell-importance selection over a projection of the configuration — the end-effector position for Reach, rather than the joint angles — in kpiece.rs. Keep the importance formula of this chapter (score, selections, neighbors, coverage), initialize a cell's score from the iteration of its discovery, and decay it after every expansion. Add it to tree_bench and report where it beats RRT and where it does not, on all three worlds.

6. Practical exercisePDifficulty 3 of 3Resolution-aware lazy edges

   Make Sbl's lazy edge store resolution-aware, as Choset's description of lazy evaluation suggests: record the coarsest resolution at which an edge has been checked so far, and when an edge recurs on a candidate path, resume from that resolution instead of restarting. Then move the goal slightly, keep both trees, and measure over fifty seeds how many collision checks reuse saves against planning from scratch.

References

1. Choset, H., Lynch, K. M., Hutchinson, S., Kantor, G., Burgard, W., Kavraki, L. E., and Thrun, S. (2005) Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press.link to Principles of Robot Motion: Theory, Algorithms, and Implementations (opens in a new tab)

   §7.2 and §7.3 are this chapter's source: Algorithms 8–14, the two-family framing, the step-size and metric-sensitivity discussion, SBL (ref. 367, Sánchez and Latombe), and SRT (refs. 14, 43). The epigraph is from §7.2.

2. LaValle, S. M. and Kuffner, J. J. (2001) Randomized Kinodynamic Planning. International Journal of Robotics Research 20(5), 378–400.doi:10.1177/02783640122067453 (opens in a new tab)

   RRTs, with the Voronoi-bias intuition and the kinodynamic setting this chapter defers to Chapters 14 and 21.

3. Kuffner, J. J. and LaValle, S. M. (2000) RRT-Connect: An Efficient Approach to Single-Query Path Planning. IEEE International Conference on Robotics and Automation, 995–1001.doi:10.1109/ROBOT.2000.844730 (opens in a new tab)

   The (Extend, Connect) setting of w12.2's dial, and the greedy heuristic the chapter labels classical.

4. Hsu, D., Latombe, J.-C., and Motwani, R. (1999) Path Planning in Expansive Configuration Spaces. International Journal of Computational Geometry and Applications 9(4–5), 495–512.doi:10.1142/S0218195999000285 (opens in a new tab)

   EST and its coverage analysis — the push, the 1/w_T weighting, and the linking-sequence argument Derivation 3 follows.

5. Kleinbort, M., Solovey, K., Littlefield, Z., Bekris, K. E., and Halperin, D. (2019) Probabilistic Completeness of RRT for Geometric and Kinodynamic Planning with Forward Propagation. IEEE Robotics and Automation Letters 4(2).link to Probabilistic Completeness of RRT for Geometric and Kinodynamic Planning with Forward Propagation (opens in a new tab)

   The rigorous completeness proof this chapter cites in place of the 2005 textbook's sentence — including the delicate step that the nearest node need not be the one in the current ball.

6. Şucan, I. A., Moll, M., and Kavraki, L. E. (2012) The Open Motion Planning Library. IEEE Robotics and Automation Magazine 19(4), 72–82.doi:10.1109/MRA.2012.2205651 (opens in a new tab)

   Reference implementations of EST, RRT, RRT-Connect, SBL and KPIECE; KPIECE itself is Şucan and Kavraki's WAFR 2008 paper, Kinodynamic Motion Planning by Interior-Exterior Cell Exploration, whose importance formula this chapter's implementation follows.

7. LaValle, S. M. (2006) Planning Algorithms. Cambridge University Press.link to Planning Algorithms (opens in a new tab)

   §5.5 is the fullest textbook treatment of RRTs, bidirectional variants and their failure modes.