Probabilistic Roadmaps

Stop describing the free space and ask it questions — probabilistic roadmaps with samplers, kd-trees on the torus, the straight-line local planner and its two check schedules, narrow passages, and probabilistic completeness stated with constants you can compute.

The problem: a slot nobody can describe

Put two bars on the Workbench, leaving a half-metre gap between them, and ask Reach to bring its forearm from the home pose to a pose inside the gap.

Think about what Part II would do with this. Chapter 8's visibility graph wants obstacle vertices in configuration space, and the C-obstacle of the bars on the torus has no vertices — it is a curved amber region we can only sample, as the right-hand panel does. Chapter 10's exact decomposition of the torus would run to thousands of cells for one slot, and every new object on the bench would rebuild all of them. Neither can start until someone has written $\Qfree$ down.

The planner in the figure never did. It drew six hundred configurations at random, kept the ones the Chapter 2 checker called free, joined each to its six nearest neighbors with a straight line in joint space when the checker approved every point along it, and asked Chapter 6's Dijkstra for a path. The only thing it knows about the world is the answer to one yes/no question, asked a few tens of thousands of times.

That is the whole idea. The rest of the chapter is about making it precise — what the planner promises, why the promise is weaker than Part II's, and exactly how much weaker.

Building intuition: coarse rain, fine rain

A probabilistic roadmap samples at two resolutions. Coarse rain: configurations fall at random, and the ones that land in $\Qfree$ become nodes. Fine rain: for each node and each of its $k$ nearest neighbors, a local planner proposes a short path — here, a straight line — and walks it in small steps, asking the checker at each one. If every step is free, the edge is added. Everything the chapter does is a choice about where one of the two rains falls.

Watch the component counter, not the dots. At first every node is its own component; then components swallow one another in bursts; then the count sits at two or three for a long time while the rain fills in regions that were already connected. The frame where $\qstart$ and $\qgoal$ first share a component is marked. Now re-roll the seed. The dots are all different. The curve is the same shape, and the marker lands in the same neighborhood of $n$. A PRM is a random graph, but whether it connects is not luck. The randomness lives in the constants — how many samples it takes — and the constants are the subject of the Foundation section.

Two more things to notice. The planner is metered in two currencies: calls to the local planner $\Dist$ (how many edges were tried) and collision checks (how many points were asked about). The first is the coarse rain's cost, the second the fine rain's, and every comparison in Part III is in the second unit, because that is the unit Choset's quotation above is about. And the torus tab runs the same code on Reach's $T^2$ as the Apartment tab runs on Rusty's $\mathbb{R}^2$ — the planner asks the manifold for distances and interpolants and never does arithmetic on coordinates, which is what Chapter 5 was for.

The mathematics

Two phases and one predicate

Three decisions hide inside that definition, and §7.1.2 of Choset is a list of them: what dist means, so that "nearest" is defined; what $\Dist$ is, so that an edge can be proposed; and how $\Dist$ decides, so that an edge can be accepted. The chapter takes them in order after saying what the planner promises.

What "probabilistically complete" means

Part II's planners were complete: a path exists if and only if they find one, in finite time. Chapter 6's grid search was resolution complete: complete relative to a discretization, so that a path thinner than a cell can be missed. This chapter's planners make a third kind of promise.

Read the quantifiers carefully, because every honest sentence about PRM depends on them. The statement is about a limit. At any finite $n$ the planner can report "no path" when one exists, and nothing in the definition says how often. The content of §7.4 is that, under hypotheses on the space, the limit is approached exponentially fast — and the content of this chapter's honesty items is that the exponent's constants can be hopeless.

Metrics, embeddings, and a kd-tree that knows about seams

"Nearest" needs a metric, and the metric is the first place a planner can quietly go wrong. On $\mathbb{R}^2$ the Euclidean distance is the obvious choice. On Reach's torus the right choice is Chapter 5's $d_{T^2}$, the $\ell^2$ norm of per-joint circular distances, so that $\theta = 3.1$ and $\theta = -3.1$ are $0.08$ apart, not $6.2$. On Hitch's $\SEtwo$ the chapter uses the weighted metric $w_t \lVert X' - X'' \rVert + w_r \lvert \theta' - \theta'' \rvert_{S^1}$ with an exchange rate between metres and radians that is a parameter, not a fact — Choset's §7.1.2 is explicit that no natural metric exists on $\SEtwo$, and the book keeps it visible.

Choset's alternative is the embedding: pick $p$ points on the robot, map a configuration to the $p \cdot \dim \W$ vector of their workspace positions, and use Euclidean distance there. It is a metric on the robot's actual geometry, it is what Kavraki's original planner used, and it costs a forward kinematics evaluation per distance. The book's Manifold::dist is the default; emb is an exercise.

Given a metric, $N_q$ asks for the $k$ nearest of up to a thousand nodes, a thousand times. Choset recommends the kd-tree of de Berg, van Kreveld and Overmars: $O(d\,n \log n)$ to build, $O(n^{1 - 1/d} + m)$ per range query returning $m$ points. A textbook kd-tree splits on coordinates and prunes a subtree when the query's distance to the subtree's box exceeds the current best. On a torus that bound is wrong: a box touching the seam at $\theta = \pi$ is adjacent to a query at $\theta = -3.1$, and a flat bound would prune the very subtree the answer lives in.

The fix is one line. The distance from a query coordinate $c$ to a box edge $s$ along a periodic axis of period $T$ is $\min(\lvert c - s \rvert,\; T - \lvert c - s \rvert)$, and the bound on the box is the $\ell^2$ sum of per-axis gaps, each scaled by the metric's weight on that axis. The bound never exceeds the true distance, so pruning never discards a real neighbor — the check kd-tree (periodic axes) equals brute force on ℝ² and T² compares two thousand torus points and two hundred queries, seam-crossing neighbors included, against a linear scan.

The local planner and its two schedules

$\Dist(q, q')$ is the fine rain. The straight-line planner follows the manifold's own geodesic — interpolate, which on $T^2$ goes the short way through the seam — and samples it at a resolution step far finer than the node sampling. Two orders of asking are in Choset's figure 7.6.

• Incremental: $q_1, q_2, \dots$ in order from one end; stop at the first collision.
• Subdivision: the midpoint first, then the quarter points, then the eighths — breadth-first over intervals. A blocked edge is usually blocked somewhere in its middle, and the midpoint finds that first; a free edge of length $\ell$ is declared free after $\lceil \ell / \text{step} \rceil$ checks either way.

Hover any edge in Roadmap Rain to see the numbered checks of the active schedule. Subdivision tends to win [C refs. 162, 367] because an edge that is going to fail fails sooner. Both share a flaw the prose must keep: a finite step can miss a collision thinner than the step. An obstacle $0.2$ m wide between two checks $0.25$ m apart is invisible to both schedules. Only a distance-based schedule — march by the checker's distance to the nearest obstacle, so that consecutive checks are guaranteed to overlap, as Chapter 2's edgeFree does — cannot miss, at the price of a distance query instead of a boolean one. The book's StraightLine is the fixed-step version because that is what Choset analyzes; the chapter says so every time it matters.

After a path is found, shortcutting (figure 7.7) tries to connect $\qstart$ straight to $\qgoal$, then to the configuration before it, and so on, replacing jagged pieces by geodesics between their own points. It never lengthens a path and never changes its homotopy class. Chapter 13 will show that this is why it is postprocessing and not optimization.

The sampler zoo

Uniform sampling is the baseline, and Theorem 7.4.1 is about it. Everything else in §7.1.3 is a bias toward where uniform sampling is poor: near obstacles, and inside narrow passages.

• Gaussian [C ref. 59, Boor, Overmars and van der Stappen]: draw $q_1$ uniformly, $q_2$ a Gaussian step of scale $\sigma$ from it, and keep the free one only if the other is in collision.
• Bridge test [C ref. 193, Hsu, Jiang, Reif and Sun]: draw two configurations a random length apart; if both are in collision and their midpoint is free, keep the midpoint.
• OBPRM [C ref. 18, Amato et al.]: from a configuration in collision, march in a random direction until free, then bisect back toward the surface.
• Medial-axis retraction [C ref. 427, Wilmarth, Amato and Stiller]: push a free sample up the clearance function until a second obstacle becomes equidistant — onto Chapter 8's generalized Voronoi diagram.
• Quasirandom sequences: Halton points replace the random generator with a deterministic low-discrepancy sequence; the planner becomes resolution complete rather than probabilistically complete, and two runs at the same $n$ give the same roadmap.

Where do the biased samplers put their mass? The question has a clean answer.

Connection strategies

The $k$-nearest rule is the default. Three refinements are in §7.1.3–7.1.4.

• Sparse roadmaps: connect a new node only to the nearest node of each other component plus a few redundant neighbors. The result is nearly a forest; it has the connectivity and few of the edges.
• Connection sampling: after the first pass, pick nodes with probability inversely related to their degree, sample around them, and connect as usual. It never creates a component on its own and tends to join the ones that remain.
• Lazy evaluation [C ref. 53, Bohlin and Kavraki]: add every candidate edge unchecked, search, and verify only the edges on the returned path, coarse-to-fine; delete a blocked edge and search again. Collision checks are spent only where a path wanted to go.

Each is a Connect variant in the implementation, and Exercise 5 asks for the lazy one's accounting.

Theorem 7.4.1 — the ball tiling

The first analysis assumes nothing about the space except that a solution exists with some room around it.

Now the numbers, because the shape of a bound is cheap and its constants are the point. Take $d = 2$, a free space occupying $\mu(\Qfree) = 0.9$ of the unit square, a path of length $L = 1.2$ with clearance $\rho = 0.1$. Then $\sigma = \pi / (4 \cdot 0.9) = 0.8727$, $\sigma\rho^2 = 0.008727$, and $m = \lceil 24 \rceil = 24$ balls. The bound is $24\, e^{-0.008727\, n}$:

Exponential in shape; nine hundred samples for a two-dimensional problem a child could solve by eye. Theorem 7.4.1 is a proof that PRM works, not a budget, and the gap between the two is where the sampler zoo earns its keep. The check Theorem 7.4.1 constants pins all five numbers.

Theorem 7.4.2 — expansive spaces

The second analysis drops the known path and characterizes the space. Three definitions, each about how much of $\Qfree$ a point can see.

Choset's figure 7.19 gives the intuition: two squares of side $W$ joined by a corridor of width $w$ and length $W$. A point in mid-corridor sees a sliver of each room, a point in a room sees most of that room and almost nothing beyond the corridor, so $\epsilon \approx \alpha \approx \beta \approx w / W$. Exercise 2 asks for the computation; the widget below measures what the estimate predicts.

Put in the corridor's numbers. At $\epsilon = \alpha = \beta = 0.1$ and $\delta = 0.05$ the theorem asks for $n = 7775$, that is $2n + 2 = 15{,}552$ samples. The narrow-passage widget connects a corridor with $w / W = 0.125$ on every seed with a hundred and fifty. Right shape, hopeless constants — the theorem's value is not the number but the dependence: $1 / \epsilon\alpha$ in front of a logarithm, so halving the corridor roughly quadruples the samples the bound demands, and the widget's uniform curve falls off in just that way as $w$ shrinks.

Theorem 7.4.3 — abstract path tiling

The third analysis is the most general and, once seen, the simplest. It does not care what $\Dist$ is or how samples are drawn.

The algorithm

Two orders are both in Choset and both in the implementation. build(n) is Algorithm 6 literally: all nodes first, then every node's $k$ nearest among all nodes — the order the worked example below is traced under. grow(n) is the incremental form of §7.1.1's "adding to the roadmap": each new node connects to its $k$ nearest among the nodes already present, which is what lets Roadmap Rain scrub the build by time, and what makes the planner single-query when $\qstart$ and $\qgoal$ are the first two nodes (§7.2).

The implementation prefers, among $\qgoal$'s candidates, those already in $\qstart$'s component — a cheap way to avoid attaching the two endpoints to two components that will never meet. And it can report FAILURE falsely: the roadmap may simply not have seen the passage yet. That is the honest reading of "probabilistically complete", and the planner's answer to it is to grow and ask again.

The six-sample roadmap, by hand

The example the implementation is tested against is small enough to trace. A point robot lives in $\W = [0, 10]^2$ with one wall $\WO = [4.5, 5.5] \times [0, 7]$, so $\QO = \WO$ and the gap is above $y = 7$. Six free samples arrive in order: $s_1 = (1, 2)$, $s_2 = (3, 5)$, $s_3 = (2, 8)$, $s_4 = (7, 2)$, $s_5 = (8, 6)$, $s_6 = (6, 9)$. The metric is Euclidean, $\Dist$ is the straight line with subdivision at step $0.25$. The distances that matter:

$k = 1$. Each sample's single nearest neighbor: $s_1 \to s_2$, $s_2 \to s_3$, $s_3 \to s_2$, $s_4 \to s_5$, $s_5 \to s_6$, $s_6 \to s_5$. The candidate edges are $\{s_1 s_2, s_2 s_3, s_4 s_5, s_5 s_6\}$, every one of them lies on one side of the wall, and $\Dist$ accepts all four. The roadmap has two components, $\{s_1, s_2, s_3\}$ and $\{s_4, s_5, s_6\}$. It has not seen the gap.

$k = 2$. The second neighbors add $s_1 s_4$, $s_2 s_4$ and $s_3 s_6$ (the others repeat). The segment $s_1 s_4$ runs along $y = 2$ straight through the wall: NIL. The segment $s_2 s_4$ crosses the wall's $x$-band at $y \in [3.125, 3.875]$, well below the gap: NIL. The segment $s_3 s_6$ crosses the band at $y \in [8.625, 8.875]$, above $y = 7$: free. Five edges, $\{s_1 s_2, s_2 s_3, s_3 s_6, s_5 s_6, s_4 s_5\}$, one component, and the roadmap path from $s_1$ to $s_4$ goes the long way round the top:

Two rejections and one extra edge are the whole difference between a roadmap that has found the gap and one that has not — which is the $k$-nearest gap of Theorem 7.4.1 in miniature.

Implementation in Rust

The sampling crate is introduced here and imported by Chapters 12, 13, 14, 21 and 22. Its design rule is the chapter's thesis: the planner may ask the world one question, and it may ask the configuration space three. Everything is generic over Chapter 5's Manifold, which is not redefined.

use manifold::Manifold;                 // Ch. 5: type Point; dim, dist, interpolate, sample
use rand::rngs::SmallRng;

/// What this chapter asks of the world — and nothing else. Blanket-implemented for any
/// (robot, world) pair with a Ch. 2 `Collision` impl, so planners never see parry2d.
pub trait FreeSpace<M: Manifold> {
    fn is_free(&self, q: &M::Point) -> bool;
    /// Collision-check counter: the book's cost unit, shown separately from Δ calls.
    fn checks(&self) -> u64;
}

/// Node sampler. `None` = attempt rejected — the Gaussian, bridge and OBPRM samplers reject
/// most attempts *by design*, so callers loop on attempts and count them.
pub trait Sampler<M: Manifold> {
    fn sample(&mut self, space: &M, free: &dyn FreeSpace<M>, rng: &mut SmallRng) -> Option<M::Point>;
}
pub struct Uniform;
pub struct Gaussian { pub sigma: f64 }
pub struct Bridge { pub length: f64 }
pub struct Obprm { pub step: f64 }
pub struct MedialAxis { pub step: f64 }
pub struct Hybrid<M: Manifold> { pub parts: Vec<(Box<dyn Sampler<M>>, f64)> }
/// Replays a fixed list — the device that makes the six-sample roadmap independent of any
/// RNG's bit stream. Ch. 12's hand-traced RRT replays its targets through it too.
pub struct Scripted<P> { list: Vec<P>, i: usize, require_free: bool }

/// Coordinates for kd-tree splitting; `period(axis)` is Some(2π) on S¹ factors.
/// The metric stays Manifold::dist — the chart only decides how to split and prune.
/// `DVector`, not `SVector<f64, { Self::D }>`: stable Rust cannot size an array with an
/// associated const inside generic code (that needs the unstable `generic_const_exprs`).
pub trait Chart: Manifold {
    const D: usize;
    fn coords(&self, q: &Self::Point) -> nalgebra::DVector<f64>;
    fn period(&self, axis: usize) -> Option<f64>;
}
pub trait NearestNeighbors<M: Manifold> {
    fn insert(&mut self, idx: usize, q: M::Point);
    fn k_nearest(&self, q: &M::Point, k: usize) -> Vec<(usize, f64)>;   // sorted by dist
    fn within(&self, q: &M::Point, r: f64) -> Vec<(usize, f64)>;        // Ch. 13's Q_near
}

pub enum EdgeCheck { Incremental { step: f64 }, Subdivision { step: f64 } }   // figure 7.6
pub struct LocalPath<M: Manifold> { pub waypoints: Vec<M::Point>, pub length: f64 }

/// Δ(q, q'): the local planner. `StraightLine` is deterministic and symmetric; Ch. 21's
/// Dubins and Reeds–Shepp impls are asymmetric and the roadmap becomes directed (§7.1.1).
pub trait Steer<M: Manifold> {
    fn steer(&self, space: &M, free: &dyn FreeSpace<M>, from: &M::Point, to: &M::Point)
        -> Option<LocalPath<M>>;
    fn symmetric(&self) -> bool { true }
}
pub struct StraightLine { pub check: EdgeCheck }

The kd-tree is hand-rolled, not because a crate could not do flat $\mathbb{R}^d$ — kiddo is the candidate cross-check in the tests — but because $T^2$ and $\SEtwo$ need periodic axes, and the pruning bound is the one place the periodicity must be exact.

/// A kd-tree that splits on chart coordinates and measures with the manifold's metric.
/// Insertion is incremental (depth-cycled split axis): every planner in Part III grows
/// its point set one node at a time.
pub struct KdTree<M: Chart> {
    root: Option<Box<Node<M>>>,
    weights: DVector<f64>,               // per-axis scale so the bound never exceeds dist
    space: M,
}
struct Node<M: Chart> { idx: usize, c: DVector<f64>, axis: usize,
                        left: Option<Box<Node<M>>>, right: Option<Box<Node<M>>> }

impl<M: Chart> KdTree<M> {
    pub fn insert(&mut self, idx: usize, q: M::Point) {
        let c = self.space.coords(&q);
        let mut depth = 0;
        let mut slot = &mut self.root;
        while let Some(node) = slot {
            let a = node.axis;
            slot = if c[a] < node.c[a] { &mut node.left } else { &mut node.right };
            depth += 1;
        }
        *slot = Some(Box::new(Node { idx, c, axis: depth % M::D, left: None, right: None }));
    }

    /// √Σ (wᵢ·gapᵢ)²: the distance from a query to a box, with the circular gap on a
    /// periodic axis — min(|c − s|, T − |c − s|) — so a subtree touching the seam is
    /// never pruned away from a query on the other side of it.
    fn lower_bound(&self, qc: &DVector<f64>, lo: &[f64], hi: &[f64]) -> f64 {
        let mut s = 0.0;
        for i in 0..M::D {
            let c = qc[i];
            let gap = if c < lo[i] || c > hi[i] {
                match self.space.period(i) {
                    Some(t) => {
                        let (dl, dh) = ((c - lo[i]).abs(), (c - hi[i]).abs());
                        dl.min(t - dl).min(dh).min(t - dh)
                    }
                    None => if c < lo[i] { lo[i] - c } else { c - hi[i] },
                }
            } else { 0.0 };
            s += (gap * self.weights[i]).powi(2);
        }
        s.sqrt()
    }
    // k_nearest / within: depth-first, nearer child first, far child only if
    // lower_bound(...) <= current k-th best (or r). See `kd_knn`, `kd_range`.
}

The planner itself is short, because every hard decision was pushed into a trait.

use petgraph::{graph::UnGraph, unionfind::UnionFind};

pub enum Connect { KNearest { k: usize }, Radius { r: f64 }, Sparse { k: usize, redundant: usize }, Lazy { k: usize } }

pub struct Roadmap<M: Manifold> {
    pub graph: UnGraph<M::Point, f64>,   // edge weight = local path length
    pub components: UnionFind<usize>,    // O(1) "how many components?" after every edge
}

pub struct Prm<M: Chart, S: Sampler<M>, N: NearestNeighbors<M>> {
    pub space: M, pub sampler: S, pub nn: N, pub steer: Box<dyn Steer<M>>, pub connect: Connect,
    pub roadmap: Roadmap<M>,
    pub rng: SmallRng,                   // seeded by the caller; the widget shows the seed
    pub delta_calls: u64,                // the coarse rain's cost
}

impl<M: Chart, S: Sampler<M>, N: NearestNeighbors<M>> Prm<M, S, N> {
    /// Choset Algorithm 6. Resumable: a larger `n` extends the roadmap (§7.1.1 "Adding").
    pub fn build(&mut self, free: &dyn FreeSpace<M>, n: usize) {
        let first_new = self.roadmap.graph.node_count();
        while self.roadmap.graph.node_count() < n {
            if let Some(q) = self.sampler.sample(&self.space, free, &mut self.rng) {
                let i = self.roadmap.graph.add_node(q.clone()).index();
                self.nn.insert(i, q);
            }
        }
        for i in 0..self.roadmap.graph.node_count() {
            let q = &self.roadmap.graph[i.into()];
            for (j, d) in self.candidates(i, q) {
                // Why skip old–old pairs: they were tried when the old nodes were connected.
                if i < first_new && j < first_new { continue; }
                if self.roadmap.graph.find_edge(i.into(), j.into()).is_some() { continue; }
                self.delta_calls += 1;
                if let Some(lp) = self.steer.steer(&self.space, free, q, &self.roadmap.graph[j.into()]) {
                    self.roadmap.graph.add_edge(i.into(), j.into(), lp.length);
                    self.roadmap.components.union(i, j);
                }
            }
        }
    }

    /// Choset Algorithm 7: attach both endpoints, then Ch. 6 Dijkstra on the roadmap.
    pub fn query(&mut self, free: &dyn FreeSpace<M>, q_start: &M::Point, q_goal: &M::Point, k: usize)
        -> Option<Vec<M::Point>>
    {
        let a = self.attach(free, q_start, k, None)?;
        // Prefer goal candidates already in the start's component: cheap, and it avoids
        // attaching the two endpoints to components that will never meet.
        let comp = self.roadmap.components.find(a);
        let b = self.attach(free, q_goal, k, Some(comp))?;
        let path = search::dijkstra(&self.roadmap.graph, a.into(), b.into())?;
        Some(std::iter::once(q_start.clone())
            .chain(path.iter().map(|&i| self.roadmap.graph[i].clone()))
            .chain(std::iter::once(q_goal.clone()))
            .collect())
    }
}

The worked example is an executable, and its output is what the test asserts.

use cspace::R2;
use sampling::{analysis::{prm_failure_bound, samples_for_failure}, *};

fn main() {
    let world = BoxWorld::new([0.0, 10.0], [0.0, 10.0], &[[4.5, 5.5, 0.0, 7.0]]);   // the wall
    let samples = vec![[1., 2.], [3., 5.], [2., 8.], [7., 2.], [8., 6.], [6., 9.]];
    for k in [1usize, 2] {
        let mut prm = Prm::new(R2::unit_box(10.0), Scripted::new(samples.clone(), true),
                               KdTree::new(), Box::new(StraightLine { check: EdgeCheck::Subdivision { step: 0.25 } }),
                               Connect::KNearest { k }, SmallRng::seed_from_u64(1));
        prm.build(&world, 6);
        println!("k = {k}: edges {:?}, {} component(s)", prm.roadmap.edge_labels(), prm.roadmap.component_count());
        if let Some(len) = prm.roadmap.path_length(0, 3) { println!("  path s1 → s4: {len:.3}"); }
    }
    for n in [500, 1000] { println!("Pr[FAILURE] ≤ {:.4} at n = {n}", prm_failure_bound(2, 0.9, 1.2, 0.1, n)); }
    println!("smallest n with bound ≤ 1%: {}", samples_for_failure(2, 0.9, 1.2, 0.1, 0.01));
}

k = 1: edges ["12", "23", "45", "56"], 2 component(s)
k = 2: Δ(s1, s4) = NIL (crosses the wall at y = 2); Δ(s2, s4) = NIL (crosses at y ∈ [3.125, 3.875])
k = 2: edges ["12", "23", "36", "45", "56"], 1 component(s)
  path s1 → s4: 18.620
Pr[FAILURE] ≤ 0.3060 at n = 500
Pr[FAILURE] ≤ 0.0039 at n = 1000
smallest n with bound ≤ 1%: 892

#[test]
fn reproduces_six_sample_roadmap() {
    let (edges1, comps1) = six_sample(1);
    assert_eq!(edges1, ["12", "23", "45", "56"]);
    assert_eq!(comps1, 2);
    let (edges2, comps2, len) = six_sample_with_path(2);
    assert_eq!(edges2, ["12", "23", "36", "45", "56"]);
    assert_eq!(comps2, 1);
    assert!((len - 18.620).abs() < 1e-3);
}

/// 2 000 points on T², 200 queries, k = 8: the periodic kd-tree agrees with a linear scan,
/// including the neighbors that live across the seam.
#[test]
fn kd_tree_matches_linear_on_torus() {
    let mut rng = SmallRng::seed_from_u64(5);
    let (mut kd, mut lin) = (KdTree::new(T2), Linear::new(T2));
    for i in 0..2000 { let q = T2.sample(&mut rng); kd.insert(i, q); lin.insert(i, q); }
    for _ in 0..200 {
        let q = T2.sample(&mut rng);
        assert_eq!(kd.k_nearest(&q, 8), lin.k_nearest(&q, 8));
    }
}

The TypeScript port that runs the widgets, web/lib/sampling/{prm,sampler,kdtree,steer}.ts, is pinned to the same roadmap: the check six-sample roadmap, k = 2 asserts the edge set $\{12, 23, 36, 45, 56\}$, one component and the path length $18.620$ to $10^{-3}$, and the constants check asserts $\sigma = 0.8727$, $m = 24$, $0.306$, $0.0039$ and $892$.

Putting it together: one planner, two robots

The integration lab is the point of the generic design. The same Prm — the same file, the same code path, no planner line changed — runs on Rusty as a disc in the Apartment, where the space is $\mathbb{R}^2$ with Euclidean distance, and on Reach on the Workbench, where the space is $T^2$ and the straight line between two configurations may pass through the seam. The only two things that differ are the Manifold and the FreeSpace, and both come from earlier chapters. Roadmap Rain's two tabs are that lab; the checks PRM path is collision-free by an independent dense check and the same Prm on T² (Reach, Workbench) re-verify every returned path at five millimetres, independently of the planner's own checks, on both robots.

Three lessons to take from the lab before the chapter closes.

The cost unit is the collision check. Build a six-hundred-node roadmap with $k = 6$ and watch the two counters: a few thousand calls to $\Dist$, and an order of magnitude more collision checks, because every edge is a row of them. When Chapter 12 claims that a tree planner is cheaper for one query, or Chapter 13 that lazy collision checking saves most of FMT*'s budget, those claims are in this unit, and they are only meaningful because this chapter counted it separately.

The roadmap is the expensive thing; the query is cheap. Once built, a query costs two attachments and a Dijkstra — milliseconds. That is the economics of a multi-query planner: pay once, ask often. It is the right economics for a robot that lives in one room and is asked for a thousand paths.

It is the wrong economics for a robot that is asked for one path, now. Most robots have one query — from where they are to where they want to be — and then the world changes. Move one obstacle on the Workbench and the roadmap is stale: every edge through the moved object's new C-obstacle is wrong, and nothing in the roadmap knows which. A planner that builds only the part of $\Qfree$ the current query needs, and stops the instant start and goal connect, would spend its budget on the right problem. That planner is a tree, and it is Chapter 12.

Exercises

1. Foundation exerciseFDifficulty 2 of 3The k-nearest gap in Theorem 7.4.1

   Step 2 of the ball-tiling proof assumes every pair of samples within $\rho$ of each other is tried by $\Dist$. Exhibit a configuration of samples, in the plane, where every ball $B_{\rho/2}(x_i)$ of the tiling contains a sample and yet the $k = 1$ roadmap is disconnected. Then state the smallest change to the connection rule that restores the proof, and say what it costs in calls to $\Dist$ per node as $n$ grows.

   Stuck? Get a hint
2. Foundation exerciseFDifficulty 2 of 3Figure 7.19 by measure

   Two squares of side $W$ are joined by a corridor of width $w$ and length $W$. Compute $\mu(\mathrm{reach}(x))$ for a point $x$ at mid-corridor to leading order in $w / W$, and for a point in the middle of a room. Deduce the $\epsilon$ of Definition 7.4.2, then argue — by taking $S$ to be one room plus half the corridor — that $\alpha$ and $\beta$ are also of order $w / W$. Where does the straight-line local planner enter?

   For w / W = 0.1, what is ε to one significant figure (as a fraction of μ(Q_free) ≈ 2W²)?

   Check Reveal
3. Conceptual exerciseCDifficulty 1 of 3Predict the narrow passage, then verify
   Predict first

   In Narrow Passage at n = 150 the uniform sampler connects every one of 24 seeds at w = 0.50 m (w/W = 0.125). Before touching the slider: what fraction of seeds connect at w = 0.15 m (w/W = 0.0375)?

   Still nearly all — the corridor is only three times narrower

   Roughly half

   Essentially none
4. Conceptual exerciseCDifficulty 2 of 3Interleaved components on the torus

   In Roadmap Rain's torus tab, find a seed and a sample count $n$ at which two roadmap components interleave on the unwrapped square — their nodes alternate along a strip — separated only by the amber band of the block's C-obstacle. Which connection strategy would join them soonest: more neighbors $k$, connection sampling around low-degree nodes, or the bridge test? Explain using where each puts its next sample.

5. Practical exercisePDifficulty 2 of 3Lazy PRM

   Implement Connect::Lazy (Bohlin and Kavraki, Choset ref. 53): build the roadmap with no edge checks at all, search, then check the nodes and then the edges along the candidate path coarse-to-fine — the midpoints of every edge first, then the quarter points — deleting the first blocked edge and searching again until a path survives. Record, per edge, the resolution at which it was last checked so that a later path does not repeat work. Report collision checks against KNearest on the Workbench over 50 seeds, at the same $n$ and $k$, and explain the ratio.

6. Practical exercisePDifficulty 3 of 3Halton points and dispersion

   Add a Halton sampler (bases 2 and 3 mapped onto the torus) and an estimator of dispersion $\delta(P, \rho)$ by probing with a fine grid. Show that two builds at the same $n$ produce identical roadmaps, that dispersion falls like $n^{-1/2}$ on $T^2$, and that the planner is now resolution complete rather than probabilistically complete. Then build the adversarial layout Choset promises exists: a free space where the Halton roadmap at some $n$ misses a passage that uniform sampling finds on most seeds.

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)

   Chapter 7, §7.1 and §7.4, is this chapter's source: Algorithms 6, 7 and 15, the sampler zoo of §7.1.3, and the three completeness analyses with the definitions quoted here. The bridge test (ref. 193), the Gaussian sampler (ref. 59), OBPRM (ref. 18), MAPRM (ref. 427), Lazy PRM (refs. 53–54) and the kd-tree (ref. 124) are cited through its bibliography.

2. Kavraki, L. E., Švestka, P., Latombe, J.-C., and Overmars, M. H. (1996) Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces. IEEE Transactions on Robotics and Automation 12(4), 566–580.doi:10.1109/70.508439 (opens in a new tab)

   The planner. Two phases, coarse and fine sampling, the k-nearest connection rule and the first experiments on many-degree-of-freedom arms.

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

   (ε, α, β)-expansiveness and Theorem 7.4.2; the paper's linking sequences are the proof device Derivation 2 follows.

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

   Where the k-nearest gap of Theorem 7.4.1 is closed: the r_n and k_n connection rules of PRM*, which Chapter 13 derives.

5. de Berg, M., Cheong, O., van Kreveld, M., and Overmars, M. (2008) Computational Geometry: Algorithms and Applications. Springer, 3rd edition.doi:10.1007/978-3-540-77974-2 (opens in a new tab)

   Chapter 5 is the kd-tree Choset recommends: O(n log n) to build, O(n^{1−1/d} + m) per range query. The periodic pruning bound is this book's addition.

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

   Chapter 5 treats sampling-based planning with the fullest discussion of metrics on SE(2) and SE(3), dispersion and discrepancy, and Sukharev grids.

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)

   The reference implementations of PRM and of every sampler in this chapter, with the state-space abstraction this book's Manifold trait mirrors.