Optimal Sampling-Based Planning

Why RRT's path never improves and what fixing it costs — RRT*, PRM* and the radius r_n = γ(log n/n)^{1/d} with its 2011 and 2020 constants, Informed RRT*'s ellipse, FMT*'s lazy dynamic programming, BIT*'s batches, and the anytime contract for a robot that must move before the planner is done.

The problem: the path does not get better

Let Reach execute the path Chapter 12's RRT found on the Workbench.

Three needless reversals of the fingertip, a loop that goes the long way round the block. Give the planner ten thousand more iterations and the path does not change: the tree gets denser everywhere, and the path to the goal is the same jagged chain of edges it was when it first arrived. On the right, the same random samples, in the same order, with one flag flipped: the tree is allowed to reconsider which node each node hangs from. The cost drops within the first few hundred iterations after the first solution and keeps dropping, and the arm goes the short way.

Feasibility and quality are separate contracts. Part III has so far delivered only the first.

Building intuition: a tree allowed to change its mind

The ordinary RRT adds a node by attaching it to the nearest tree node, and that attachment is final. RRT* adds the same node — the Voronoi bias of Chapter 12 is untouched — and then does two things in a ball around it. Choose-parent: among the tree nodes in the ball, attach to the one through which the new node's cost-to-come is smallest, not the nearest. Rewire: for every other node in the ball, if going through the new node would be cheaper than its current cost, make the new node its parent and propagate the saving down its subtree.

Left and right consume the same seeded stream of random targets, so they have the same nodes; only the edges differ. Watch the two cost curves. The left one is flat — it changes only on the rare occasion that a new node happens to enter the goal region more cheaply than the old one, and it never approaches the dashed lattice floor. The right one descends, and every drop is a flash of rewired edges somewhere in the tree. The misconception this widget exists to kill is "more samples make RRT's path better." They do not. They make RRT*'s path better, and the whole chapter is about what the asterisk costs.

Two more things to try. Slide $\gamma / \gamma^*$ below one. The right curve stalls — not immediately, but it stops approaching the floor — because the ball now shrinks faster than samples arrive to fill it, and the theorem's hypothesis has failed. Slide it to three and the descent is faster per iteration and slower per second: every insertion now considers dozens of neighbors, and the histogram of $\lvert Q_{near} \rvert$ tells you how many. The constant $\gamma^*$ is where those two pressures balance, and it is a computed number, not a tuning knob.

The mathematics

Definitions

RRT is almost surely suboptimal

PRM*, RRG and RRT* are asymptotically optimal

The 2020 correction

The $(\epsilon, \alpha, \beta)$ lens, and the $k$-nearest variant

Two remarks tie the theorem back to Chapter 11. First, Step 6 needs consecutive balls to see each other through the local planner, which is $\epsilon$-goodness in miniature: the shadowing balls are mutually visible because the tube around $\sigma^*$ is free. A space that is not $\epsilon$-good — a corridor thinner than every $r_n$ for the $n$ you can afford — defeats the asymptotic argument in practice exactly as it defeated Theorem 7.4.2's constants. Second, the $k$-nearest rule is the same theorem in a different coordinate. In a region of density $\rho$ the $k_n$ nearest neighbors occupy a ball of radius $(k_n / \rho \zeta_d)^{1/d} \propto (\log n / n)^{1/d}$ — the radius rule again, with the density made implicit — and the constant $k^* = e(1 + 1/d)$ comes out of the same union bound with a Chernoff bound on a Poisson count in place of the emptiness probability. The $k$-nearest rule adapts to local density, which is why some implementations prefer it; the radius rule is the one whose constant is tied to $\mu(\Qfree)$ and therefore to the world. The check k-nearest RRT* runs both on seed 13 and finds their costs within one percent of each other.

The constants, for $n = 1000$ in the unit square

$(\log n / n)^{1/2} = (6.9078 / 1000)^{1/2} = 0.08311$. With $d = 2$, $\mu(\Qfree) = 1$, $\zeta_2 = \pi$:

A ball of radius $0.08$ in a unit square after a thousand samples: about twenty neighbors to consider per insertion, each a steer call. That is the price of the asterisk, and the check r_1000 in the unit square pins every number in the table.

Informed sampling is exactly where improvement is possible

FMT* and BIT*: search the graph you never build

The algorithm

Line 3 is sorted so that the first free candidate is the answer; line 2's $\eta$ cap is what keeps the steer calls short. With rewire = false and $Q_{near} = \{q_{near}\}$ the same code is Chapter 12's RRT, which is how the widgets feed one seed to both.

The five-node rewire, by hand

Euclidean plane, no obstacles, a tree of four nodes before one RRT* iteration: the root $A = (0, 0)$ with $\mathrm{Cost} = 0$; $B = (3, 0)$ under $A$, $\mathrm{Cost} = 3$; $C = (3, 3)$ under $B$, $\mathrm{Cost} = 6$; $D = (0, 3)$ under $A$, $\mathrm{Cost} = 3$. The new sample is $E = (1.5, 1.5)$ and the neighborhood radius for this step is $r = 2.5$.

Near set. $\mathrm{dist}(E, \cdot) = 1.5\sqrt2 = 2.1213$ to each of $A, B, C, D$: all four are in $Q_{near}$.

Choose-parent. Via $A$: $0 + 2.1213 = 2.1213$. Via $B$ or $D$: $3 + 2.1213 = 5.1213$. Via $C$: $6 + 2.1213 = 8.1213$. Parent $A$; $\mathrm{Cost}(E) = 2.1213$.

Rewire. The cost through $E$ to any neighbor is $2.1213 + 2.1213 = 4.2426 = 3\sqrt2$. $B$: $4.2426 > 3$, keep. $D$: keep. $C$: $4.2426 < 6$ — rewire $\mathrm{par}(C) := E$ and $\mathrm{Cost}(C)$ drops from $6$ to $4.2426$, a $29.3$ percent saving; $C$ has no children, so the propagation is a no-op. Plain RRT would have attached $E$ under its nearest node — a tie here, broken toward $A$ — and left $C$ at $6$ forever.

Implementation in Rust

sampling::optimal wraps Chapter 12's tree; it does not fork it. Nothing from Chapters 11 or 12 is redefined, and the radius functions are plain, so that a widget can violate the bound on purpose.

/// r_n = γ (log n / n)^{1/d}. γ is a parameter, not hidden: the widget must be able to break it.
pub fn r_n(n: usize, d: usize, gamma: f64) -> f64 {
    gamma * ((n as f64).ln() / n as f64).powf(1.0 / d as f64)
}
/// γ* = 2 (1 + 1/d)^{1/d} (μ_free / ζ_d)^{1/d} — Karaman & Frazzoli 2011, Theorem 38.
pub fn gamma_star_kf2011(d: usize, mu_free: f64) -> f64 {
    2.0 * (1.0 + 1.0 / d as f64).powf(1.0 / d as f64) * (mu_free / unit_ball_volume(d)).powf(1.0 / d as f64)
}
/// γ* = (2 (1 + 1/d))^{1/d} (μ_free / ζ_d)^{1/d} — Solovey, Janson, Schmerling, Frazzoli & Pavone 2020.
pub fn gamma_star_solovey2020(d: usize, mu_free: f64) -> f64 {
    (2.0 * (1.0 + 1.0 / d as f64)).powf(1.0 / d as f64) * (mu_free / unit_ball_volume(d)).powf(1.0 / d as f64)
}
/// k_n = ⌈e (1 + 1/d) log n⌉.
pub fn k_n(n: usize, d: usize) -> usize { (std::f64::consts::E * (1.0 + 1.0 / d as f64) * (n as f64).ln()).ceil() as usize }

pub enum Neighborhood { Radius { gamma: f64, eta: f64 }, KNearest { k_star: f64 } }

/// Ch. 12's tree plus cost-to-come and child lists. Wrapping, not forking: `inner` is the
/// same arena the Ch. 12 widgets draw, so RRT and RRT* can be fed identical samples.
pub struct CostTree<M: Chart> { pub inner: Tree<M>, pub cost: Vec<f64>, children: Vec<SmallVec<[NodeId; 4]>> }

impl<M: Chart> CostTree<M> {
    pub fn near(&self, q: &M::Point, r: f64) -> Vec<(NodeId, f64)> { self.inner.nn.within(q, r) }   // Ch. 11 KdTree::within
    pub fn add(&mut self, q: M::Point, parent: NodeId, edge: f64) -> NodeId {
        let id = self.inner.add(q, parent);
        self.cost.push(self.cost[parent] + edge);
        self.children.push(SmallVec::new());
        self.children[parent].push(id);
        id
    }
    /// Change v's parent and push the cost change through its subtree (iterative DFS).
    pub fn reparent(&mut self, v: NodeId, new_parent: NodeId, edge: f64) -> usize {
        let old = self.inner.nodes[v].parent.unwrap();
        self.children[old].retain(|&c| c != v);
        self.inner.nodes[v].parent = Some(new_parent);
        self.children[new_parent].push(v);
        let delta = self.cost[new_parent] + edge - self.cost[v];
        let (mut stack, mut touched) = (vec![v], 0);
        while let Some(u) = stack.pop() { self.cost[u] += delta; touched += 1; stack.extend(self.children[u].iter().copied()); }
        touched - 1
    }
}

impl<M: Chart, S: Extend<M>, Smp: Sampler<M>> RrtStar<M, S, Smp> {
    /// (parent, edge cost) minimizing Cost(v) + c(v, q_new) over `near` with Δ succeeding.
    /// Sorted by that sum, so the first candidate whose segment is free is the answer.
    fn choose_parent(&self, free: &dyn FreeSpace<M>, q_new: &M::Point, near: &[(NodeId, f64)]) -> Option<(NodeId, f64)> {
        let mut cands: Vec<_> = near.iter().map(|&(v, d)| (self.tree.cost[v] + d, v, d)).collect();
        cands.sort_by(|a, b| a.0.partial_cmp(&b.0).unwrap());
        cands.into_iter().find(|&(_, v, _)| self.steer.steer(&self.space, free, &self.tree.q(v), q_new).is_some())
             .map(|(_, v, d)| (v, d))
    }

    /// For each v in `near`: if Cost(new) + c(new, v) < Cost(v) and Δ(new, v), reparent.
    fn rewire(&mut self, free: &dyn FreeSpace<M>, new: NodeId, near: &[(NodeId, f64)]) -> usize {
        let c_new = self.tree.cost[new];
        let mut count = 0;
        for &(v, d) in near {
            if v == self.tree.parent(new) { continue; }
            if c_new + d < self.tree.cost[v] && self.steer.steer(&self.space, free, &self.tree.q(new), &self.tree.q(v)).is_some() {
                self.propagated += self.tree.reparent(v, new, d);
                count += 1;
            }
        }
        count
    }

    /// One iteration: sample → nearest → extend → near set → choose_parent → rewire → goal test.
    pub fn iterate(&mut self, free: &dyn FreeSpace<M>) -> Option<f64> {
        let q_rand = self.sample(free)?;
        let (near_id, _) = self.tree.inner.nearest(&q_rand);
        let q_new = match self.steer.extend(&self.space, free, &self.tree.q(near_id), &q_rand, self.eta) {
            ExtendStatus::Trapped => return None, ExtendStatus::Reached(q) | ExtendStatus::Advanced(q) => q };
        let n = self.tree.size() + 1;
        let mut near = match self.nbhd {
            Neighborhood::Radius { gamma, eta } => self.tree.near(&q_new, r_n(n, M::D, gamma).min(eta)),
            Neighborhood::KNearest { k_star } => self.tree.inner.nn.k_nearest(&q_new, k_n(n, M::D)),
        };
        if !near.iter().any(|&(v, _)| v == near_id) { near.push((near_id, self.space.dist(&self.tree.q(near_id), &q_new))); }
        // The nearest node's segment is already known free: it is always a valid parent.
        let (parent, edge) = self.choose_parent(free, &q_new, &near).unwrap_or((near_id, self.space.dist(&self.tree.q(near_id), &q_new)));
        let id = self.tree.add(q_new.clone(), parent, edge);
        self.rewires += self.rewire(free, id, &near);
        if self.space.dist(&q_new, &self.goal) <= self.goal_radius && self.steer.steer(&self.space, free, &q_new, &self.goal).is_some() { self.goal_nodes.push(id); }
        self.refresh_best()                                 // Some(cost) only if the incumbent dropped
    }
}

PRM* is Chapter 11's Prm with the connection rule tied to $n$, and the anytime contract is a trait with three methods.

/// Ch. 11's PRM with the connection rule tied to n: a ball of radius r_n, or the k_n nearest.
/// Each new node connects under the *current* rule (the incremental form); the expected number
/// of edges is O(n log n), which is what makes the roadmap both connected and affordable.
pub struct PrmStar<M: Chart, S: Sampler<M>> { pub prm: Prm<M, S, KdTree<M>>, nbhd: Neighborhood }

impl<M: Chart, S: Sampler<M>> PrmStar<M, S> {
    pub fn add_sample(&mut self, free: &dyn FreeSpace<M>) -> Option<usize> {
        let n = self.prm.roadmap.node_count() + 1;
        self.prm.connect = match self.nbhd {
            Neighborhood::Radius { gamma, .. } => Connect::Radius { r: r_n(n, M::D, gamma) },
            Neighborhood::KNearest { .. } => Connect::KNearest { k: k_n(n, M::D) },
        };
        self.prm.add_sample(free)
    }
}

pub struct Solution<P> { pub path: Vec<P>, pub cost: f64 }

/// "Deadline in, best-so-far out." One unit of work per `step`, `Some(cost)` when the incumbent
/// improved; `best()` is always available. Chapter 23's planning stack consumes exactly this.
pub trait Anytime<P> {
    fn step(&mut self) -> Option<f64>;
    fn best(&self) -> Option<Solution<P>>;
    fn run_until(&mut self, deadline: std::time::Instant) -> Option<Solution<P>> {
        while std::time::Instant::now() < deadline { self.step(); }
        self.best()
    }
}

FMT* is a batch and a heap; BIT* is the same heap with a heuristic and a second queue.

impl<M: Chart, S: Steer<M>> Anytime<M::Point> for Fmt<M, S> {
    /// Expand one frontier node — lazy dynamic programming, one collision check per accepted edge.
    fn step(&mut self) -> Option<f64> {
        let z = self.open.pop()?;                                    // least cost-to-come
        for (x, _) in self.nn.within(&self.samples[z], self.r) {
            if self.state[x] != Unvisited { continue; }
            // Locally optimal parent among OPEN nodes in x's ball, obstacles ignored …
            let Some((y, c)) = self.nn.within(&self.samples[x], self.r).into_iter()
                .filter(|&(y, _)| self.state[y] == Open)
                .map(|(y, d)| (y, self.cost[y] + d))
                .min_by(|a, b| a.1.partial_cmp(&b.1).unwrap()) else { continue };
            // … then check only that edge. Wrong exactly when the best parent is blocked.
            if self.steer.steer(&self.space, &*self.free, &self.samples[y], &self.samples[x]).is_some() {
                self.cost[x] = c; self.parent[x] = Some(y); self.state[x] = Open; self.open.push(x, c);
            }
        }
        self.state[z] = Closed;
        (z == self.goal_idx).then(|| { self.done = true; self.cost[z] })
    }
    fn best(&self) -> Option<Solution<M::Point>> { self.done.then(|| self.path_to(self.goal_idx)) }
}

impl<M: Chart, S: Steer<M>> Anytime<M::Point> for Bit<M, S> {
    /// One unit of work: pop the best edge (expanding vertices while a vertex could beat it), or
    /// open a new batch when both queues are empty. Keys are Ch. 6's f = g + h on the implicit RGG.
    fn step(&mut self) -> Option<f64> {
        if self.q_v.is_empty() && self.q_e.is_empty() { self.new_batch(); return None; }   // prune, sample, re-key
        while self.q_v.peek_key() <= self.q_e.peek_key() { let v = self.q_v.pop().unwrap(); self.expand(v); }
        let (v, x) = self.q_e.pop()?;
        let c = self.space.dist(&self.pts[v], &self.pts[x]);
        if self.g[v] + c + self.h_hat(x) >= self.c_best { self.q_v.clear(); self.q_e.clear(); return None; }
        if self.g[v] + c >= self.g[x] { return None; }
        self.edge_checks += 1;
        self.steer.steer(&self.space, &*self.free, &self.pts[v], &self.pts[x])?;    // lazy: checked on pop
        self.attach(x, v, c);                                                       // or rewire + propagate
        (self.in_tree[GOAL] && self.g[GOAL] < self.c_best).then(|| { self.c_best = self.g[GOAL]; self.c_best })
    }
    fn best(&self) -> Option<Solution<M::Point>> { self.c_best.is_finite().then(|| self.path_to(GOAL)) }
}

The informed sampler is a Chapter 11 Sampler, so Informed RRT* is RRT* with a different sampler and no planner code changed.

pub struct InformedSampler<M: Chart> {
    start: M::Point, goal: M::Point, c_min: f64, pub c_best: f64,
    center: DVector<f64>, axis: DVector<f64>, fallback: Uniform,
}

impl<M: Chart> Sampler<M> for InformedSampler<M> {
    fn sample(&mut self, space: &M, free: &dyn FreeSpace<M>, rng: &mut SmallRng) -> Option<M::Point> {
        if !self.c_best.is_finite() { return self.fallback.sample(space, free, rng); }   // no incumbent yet
        let a = self.c_best / 2.0;
        let b = (self.c_best.powi(2) - self.c_min.powi(2)).max(0.0).sqrt() / 2.0;
        // Unit ball → scale → rotate (Householder: e₁ ↦ axis) → translate → reject outside 𝒬.
        let u = unit_ball(rng);
        let s = SVector::from_fn(|i, _| if i == 0 { a * u[i] } else { b * u[i] });
        let v = (SVector::ith(0, 1.0) - self.axis).normalize();
        let r = s - 2.0 * v.dot(&s) * v;
        let c = r + self.center;
        let q = space.from_coords(&c)?;                       // None if outside the chart's box
        free.is_free(&q).then_some(q)
    }
}

impl<M: Chart> InformedSampler<M> {
    pub fn update(&mut self, c_best: f64) { self.c_best = c_best; }
    /// μ(X_f̂ ∩ 𝒬)/μ(𝒬) — w13.2's readout; in the plane the ellipse is clipped against the box by quadrature.
    pub fn measure_ratio(&self, space: &M) -> f64 { ellipse_box_area(self, space.bounds()) / space.measure() }
}

The worked example is an executable whose output is the test's expectation.

fn main() {
    let plane = R2::unit_box(20.0).centered();                                    // no obstacles
    let mut star = RrtStar::new(plane, [0.0, 0.0], StraightLine::subdivision(0.25), Uniform,
                                Neighborhood::Radius { gamma: 1.0, eta: 10.0 }, SmallRng::seed_from_u64(0));
    let b = star.tree.add([3.0, 0.0], 0, 3.0);
    let c = star.tree.add([3.0, 3.0], b, 3.0);
    let _d = star.tree.add([0.0, 3.0], 0, 3.0);
    let e = [1.5, 1.5];
    let near = star.tree.near(&e, 2.5);
    for &(v, d) in &near { println!("{}: dist {d:.4}, via {:.4}", NAMES[v], star.tree.cost[v] + d); }
    let (parent, edge) = star.choose_parent(&Free, &e, &near).unwrap();
    let id = star.tree.add(e, parent, edge);
    println!("parent {}, Cost(E) = {:.4}", NAMES[parent], star.tree.cost[id]);
    let before = star.tree.cost[c];
    star.rewire(&Free, id, &near);
    println!("C: {before:.4} → {:.4}, parent now {}", star.tree.cost[c], NAMES[star.tree.parent(c)]);
    for (name, g) in [("2011", gamma_star_kf2011(2, 1.0)), ("2020", gamma_star_solovey2020(2, 1.0))] {
        println!("γ*_{name} = {g:.4}, r_1000 = {:.4}", r_n(1000, 2, g));
    }
    println!("k_1000 = {}", k_n(1000, 2));
}

A: dist 2.1213, via 2.1213
B: dist 2.1213, via 5.1213
C: dist 2.1213, via 8.1213
D: dist 2.1213, via 5.1213
parent A, Cost(E) = 2.1213
B keep, D keep, C rewire
C: 6.0000 → 4.2426, parent now E
γ*_2011 = 1.3820, r_1000 = 0.1149
γ*_2020 = 0.9772, r_1000 = 0.0812
k_1000 = 29

#[test]
fn reproduces_five_node_rewire() {
    let r = five_node_rewire();
    assert_eq!(r.near.len(), 4);
    assert!((r.cost_e - 2.1213).abs() < 1e-4);
    assert_eq!(r.parent_c, r.e);                  // rewired under E
    assert!((r.cost_c - 4.2426).abs() < 1e-4 && (r.cost_b - 3.0).abs() < 1e-9);
}

#[test]
fn radius_constants() {
    assert!((r_n(1000, 2, gamma_star_kf2011(2, 1.0)) - 0.1149).abs() < 5e-5);
    assert!((r_n(1000, 2, gamma_star_solovey2020(2, 1.0)) - 0.0812).abs() < 5e-5);
    assert_eq!(k_n(1000, 2), 29);
}

/// Seed 13 on the Apartment, 6 000 iterations: RRT* non-increasing and within 5% of the lattice
/// floor; RRT on the same samples never rewires and ends well above it.
#[test]
fn rrt_star_cost_converges_under_fixed_seed() {
    let (plain, star, floor) = seed_13_scoreboard(6000);
    assert!(star.cost_history.windows(2).all(|w| w[1].cost <= w[0].cost + 1e-9));
    assert!(star.best_cost < floor * 1.05);
    assert!(plain.best_cost > star.best_cost * 1.2);
}

The TypeScript port's checks replay all of it: the five-node rewire and the radii exactly, and the seed-13 scoreboard — RRT at $13.680$ m, RRT* at $10.885$ m against a lattice floor of $11.508$ m after six thousand iterations, the same node set in both trees, and every returned path re-checked at five millimetres.

Putting it together: the anytime contract

The lab is the Batch Bench. Four asymptotically optimal planners, one seed, one world, and one deadline each: stop at the deadline, execute what you have, keep planning, and switch to the better path when it arrives. That loop is the one Chapter 23's planning stack runs for Reach, and it is what trait Anytime is for — one unit of work per step, Some(cost) when the incumbent improved, best() always available.

Read the curves at three deadlines. At a hundred milliseconds, RRT* and Informed RRT* have usually found a first path and begun to improve it; BIT* has processed its first batch and often holds the best path of the four; FMT* has nothing — it is still drawing its batch or marching through it. At half a second the curves have crossed at least once. At two seconds FMT* has returned its single answer, often the best, and will never improve it; the incremental planners are still descending. The misconception the lab kills is "asymptotically optimal means better at every budget." The theorems are all about $n \to \infty$; at a deadline the winner is decided by constants, and the constants depend on the world.

Three honesty items the chapter keeps. Every theorem here is asymptotic — at finite $n$ the cost is a random variable, and the widgets show its spread across seeds. The hypotheses are necessary: robust optimality, dist as the cost, and the $\gamma$ bound, which Rewire Watch lets you violate. And nothing in this chapter makes Hitch drivable: the straight-line steer is still wrong for a car, and the cost it optimizes is the length of a path the car cannot follow. Kinodynamic RRT* replaces the steer with a two-point boundary-value solver in Chapter 21.

One last contrast before Part III closes. Asymptotic optimality is global and slow: the planner converges to the best path in any homotopy class, eventually. Chapter 19's trajectory optimization is the complementary route — local and fast, descending a cost from an initial guess and never leaving its homotopy class. The two meet in practice as a pipeline: an RRT* path to pick the class, an optimizer to make it smooth.

Exercises

1. Foundation exerciseFDifficulty 3 of 3Where (1 + 1/d) comes from

   Carry out Step 4 of the optimality derivation with the constants kept. Tile a path of length $L$ with balls of radius $r_n / 4$ whose centers are $r_n / 4$ apart; count them; write the probability that a given ball is empty after $n$ uniform samples in $\Qfree$; apply the union bound; and find the smallest exponent of $\gamma$ for which $\sum_n \Pr[\text{some ball empty at } n]$ converges. Identify the step at which $(1 + 1/d)$ enters.

   Stuck? Get a hint

   With d = 2 and μ(Q_free)/ζ_d = 1/π, what is the threshold on γ² from this tiling (balls of radius r_n/4)?

   Check Reveal
2. Foundation exerciseFDifficulty 2 of 3The informed set on the torus

   Show that on $T^2$ the planar ellipse formula undercounts $\mu(X_{\hat f})$ once $c_{best}$ exceeds the torus circumference $2\pi$: the geodesic from a focus to a point may go through the seam, so points the planar formula excludes satisfy $\hat g + \hat h \le c_{best}$. Derive the correct sampler — a union of ellipses over the seam images of the foci, or rejection on the unwrapped cover — and say which the implementation uses and why.

3. Conceptual exerciseCDifficulty 1 of 3Predict the half-radius, then verify
   Predict first

   In Rewire Watch on the Apartment at γ/γ* = 1 and seed 13, RRT* ends within a few percent of the lattice floor by 6 000 iterations. Before touching the slider: what does γ/γ* = 0.5 do to its cost curve?

   Nothing — the ball is still large enough for choose-parent and rewire to work

   A slower descent that stalls above the floor

   A flat curve from the start, identical to RRT
4. Conceptual exerciseCDifficulty 2 of 3Where BIT* and RRT* cross

   In Batch Bench on the Workbench, find the deadline at which BIT*'s and RRT*'s costs are equal on a fixed seed, then vary the batch size and find the one that moves the crossing earliest. Relate it to how many batches fit before the deadline: a batch too small is searched before it has enough samples to improve the incumbent, a batch too large is still being searched when the clock stops.

5. Practical exercisePDifficulty 2 of 3The k-nearest neighborhood

   Implement Neighborhood::KNearest with $k_n = \lceil k^* \log n \rceil$, $k^* = e(1 + 1/d)$, and a test that the radius and $k$-nearest variants reach costs within 3 percent of each other after 6 000 iterations under seed 13 on the Apartment. The TypeScript check k-nearest RRT* asks for 10 percent; tighten it and report how many seeds survive.

6. Practical exercisePDifficulty 3 of 3Shortcutting cannot win the homotopy class

   Run Chapter 11's shortcut_greedy on RRT's path and plot, over 100 seeds, its cost against RRT*'s at equal wall-clock time on the Apartment. Then build the two-homotopy-class map where it cannot win: a single obstacle between start and goal, with the shorter way round narrower than the longer. Show that RRT commits to the wide side on a fixed fraction of seeds and that no amount of shortcutting moves it to the narrow one, while RRT*'s incumbent eventually does.

References

1. Karaman, S. and Frazzoli, E. (2011) Sampling-based Algorithms for Optimal Motion Planning. International Journal of Robotics Research 30(7), 846–894.doi:10.1177/0278364911406761 (opens in a new tab)

   RRT's almost-sure suboptimality, PRM*, RRG and RRT*, and the radius r_n = γ(log n/n)^{1/d} with the 2011 constant this chapter calls γ*₂₀₁₁.

2. Solovey, K., Janson, L., Schmerling, E., Frazzoli, E., and Pavone, M. (2020) Revisiting the Asymptotic Optimality of RRT*. IEEE International Conference on Robotics and Automation.link to Revisiting the Asymptotic Optimality of RRT* (opens in a new tab)

   The gap in the 2011 RRT* proof and its repair, with the smaller constant γ*₂₀₂₀ the implementation defaults to.

3. Gammell, J. D., Srinivasa, S. S., and Barfoot, T. D. (2014) Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic. IEEE/RSJ International Conference on Intelligent Robots and Systems.doi:10.1109/IROS.2014.6942976 (opens in a new tab)

   The informed set, its direct sampling by scaling and rotating the unit ball, and the measure ratio w13.2 reads out.

4. Gammell, J. D., Barfoot, T. D., and Srinivasa, S. S. (2020) Batch Informed Trees (BIT*): Informed Asymptotically Optimal Anytime Search. International Journal of Robotics Research 39(5).link to Batch Informed Trees (BIT*): Informed Asymptotically Optimal Anytime Search (opens in a new tab)

   BIT* in its journal form: batches on an implicit random geometric graph, the edge queue keyed ĝ + ĉ + ĥ, pruning and tree reuse. The ICRA 2015 paper introduced it.

5. Janson, L., Schmerling, E., Clark, A., and Pavone, M. (2015) Fast Marching Tree: a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions. International Journal of Robotics Research 34(7), 883–921.doi:10.1177/0278364915577958 (opens in a new tab)

   FMT*: lazy dynamic programming on one batch, its radius constant, and the O(n log n) collision-check bound.

6. 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)

   The scaffolding: Theorem 7.4.1's ball tiling and the (ε, α, β) constants reappear inside the optimality proofs; §7.1.2's shortcutting is the pre-2011 answer to path quality, and the epigraph's 'large dense roadmap' is the intuition PRM* turns into a theorem.

7. Ş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 RRT*, PRM*, Informed RRT*, FMT* and BIT*, against which this chapter's constants and defaults were compared.