Potential Functions

Attractive and repulsive potentials and gradient descent; distance from a range scan and from a grid; brushfire and wave-front as breadth-first search; the local-minimum disease of additive potentials and the navigation functions that cure it; and lifting workspace forces to Reach's joint torques through the transposed Jacobian.

The problem: roll downhill, and ask who built the hill

Put a bead at the left edge of a table and a goal at the right. Define a scalar field $U$ that is small at the goal and large near obstacles, and let the bead move with velocity $-\nabla U$. With a single disc between them, the bead swerves around the disc and arrives. Replace the disc by a horseshoe whose mouth faces the bead, and it rolls into the mouth and stops.

Nothing is broken. The badge under the canvas reports what the mathematics says: the gradient has vanished at a point that is not the goal, and both eigenvalues of the Hessian there are positive. The bead sits at the bottom of a genuine bowl. The goal's pull and the base's push cancel exactly on the axis of the horseshoe, and the two arms push back toward the axis whenever the bead drifts off it, so the point is stable in every direction. Choset's figure 4.10 draws this and his figure 4.11 makes the sharper point: switch the widget to two convex discs and the bead still stops, between them. Concavity has nothing to do with it.

Now switch on wave-front mode. The heightfield changes to the breadth-first cost-to-goal of Chapter 6, computed on a tenth-of-a-metre raster of the same obstacles, and the bead escapes along a grid-shortest path. That field has one minimum, provably, because it is distance to the goal. The rest of the chapter is the difference between those two heightfields.

Building intuition

Why a sum has extra valleys

The additive potential is $U = U_{att} + U_{rep}$: a bowl centred at the goal plus a spike on each obstacle. Far from any obstacle only the bowl matters and the bead heads for the goal. Near an obstacle the spike takes over and pushes the bead off. In between, the two gradients can cancel, and the only question is whether the cancellation point is stable. Between two obstacles symmetric about the line to the goal it is: the repulsive push is purely axial on the axis by symmetry, it grows without bound toward the gap while the attractive pull stays bounded, so somewhere on the axis the two cancel; and off the axis both spikes push back toward it. Derivation 3 writes this out. The Potential Well's $Q^\star$ slider changes where the cancellation happens but cannot make it go away; the $\zeta$ and $\eta$ sliders in the disclosure move it along the axis. Only the obstacle layout can remove a minimum, and the planner does not control the layout.

Fronts on a grid

The grid cure is older than the word "potential". Label every obstacle pixel $1$, its free neighbours $2$, their unlabelled neighbours $3$, and so on: a fire spreading outward from every obstacle at once. When it is done, each pixel's label is one more than its link length to the nearest obstacle. That is brushfire, Choset §4.3.2, and it is a multi-source breadth-first search.

Two things to watch. First, the shape of a front: under 4-point connectivity the fronts are diamonds, because the label counts Manhattan steps; under 8-point they are squares, because the label counts chessboard steps. Neither is a circle. Brushfire computes a grid metric, and the hover readout compares it with the exact Euclidean distance transform ported from the sister book: the two agree only along the axes. Second, the solid pixels where two fronts meet. On the micro-grid that is column 4, equidistant from both obstacles; each of its three pixels was reached by both fronts and keeps two back pointers. Chapter 8 will call that column the grid generalized Voronoi diagram. Seed the same search from the goal pixel instead, labelled $2$, and you have the wave-front planner of §4.5: a pixel's label is $2 +$ its grid distance to the goal, and "step to any neighbour labelled one less" is a shortest grid path.

One minimum, smoothly

Grids are exponential in dimension. Is the single minimum available without one? Rimon and Koditschek's answer is yes, on a sphere world — a disc with disjoint disc obstacles — and on anything diffeomorphic to one.

The field is $\varphi = d^2 / [d^{2\kappa} + \beta]^{1/\kappa}$, where $d$ is distance to the goal and $\beta$ is a product that vanishes on every obstacle boundary. At $\kappa = 2$ the Sculptor finds three minima; at $\kappa = 3$ two; from $\kappa = 7$ exactly one. Slide $\kappa$ and watch the spurious minima slide toward their obstacles and turn into saddles. This is the point the widget exists to make: a potential with no spurious minima exists, and it is smooth. The price is the family of worlds it works on and a sharpening exponent whose threshold the theorem only promises exists. Star mode shows the trick that widens the family: a map $h_\lambda$ that squashes each star-shaped obstacle onto a disc, with $\varphi \circ h_\lambda$ inheriting the single minimum because critical points correspond one-to-one under a diffeomorphism.

A body is not a point

Reach's configuration space is a torus; there is no "goal point" in the workspace to put a bowl at. Choset's §4.7 writes the potential in the workspace, at a few control points on the arm, and lifts each workspace force to a joint torque with the transposed Jacobian of that point.

Two red control points on the end effector pull it to the goal pose; slate points on the links push away from the block, the post and the shelf. The bars under the canvases are the lifted torques $J_j^\top f_j$, one row per control point, and their sum — the gradient the arm descends. Watch the default run twice. Without $r_{float}$ link 1 drives straight through the post between its two repulsive points, which are far enough from the post that their push is weak; with $r_{float}$ — a floating control point at whichever link point is nearest any obstacle — the same start reaches the goal. Repelling the vertices of a body does not protect its edges (Choset's figure 4.20); $r_{float}$ reduces the risk and does not eliminate it, and the widget's alternate mode lets you find starts where even that is not enough.

The mathematics

Potentials, critical points, and the Hessian

In the colours every figure on this page uses, the descent is

The orange bead is $c(t)$, the red goal sits at the bottom of the bowl, and every slate obstacle contributes one spike through its distance $d_i$. Point at a term and the matching layer in the Potential Well comes forward.

The local-minimum disease

Brushfire and wave-front are breadth-first search

Navigation functions

Lifting forces to configuration space

The algorithm

Choset's Algorithm 4 is the whole method; brushfire and wave-front are Chapter 6's breadth-first search with a different seed set; navigation functions are a formula; and lifting is a sum.

Implementation in Rust

The potential crate is introduced here; brushfire is the owned grid distance transform that Chapters 8 and 10 import, and Potential is the interface Chapter 19's CHOMP reads a distance field through. Configurations are SVector<f64, N> so one Attractive serves a point in the plane and a control point on Reach.

use nalgebra::SVector;

/// A differentiable scalar field over an N-dimensional configuration space. The gradient is
/// read as a *velocity* for first-order robots (Choset Ch. 4's convention through §4.6) and
/// as a *force* once it is lifted through Jᵀ (§4.7).
pub trait Potential<const N: usize> {
    fn value(&self, q: &SVector<f64, N>) -> f64;
    fn gradient(&self, q: &SVector<f64, N>) -> SVector<f64, N>;
}

/// Choset eqs. (4.2)–(4.3): quadratic within `d_goal_star`, conic beyond. The two agree in value
/// and gradient at the seam (Derivation 1), so descent never sees it; `f64::INFINITY` gives the
/// pure quadratic of eq. (4.1).
pub struct Attractive<const N: usize> { pub goal: SVector<f64, N>, pub zeta: f64, pub d_goal_star: f64 }

impl<const N: usize> Potential<N> for Attractive<N> {
    fn value(&self, q: &SVector<f64, N>) -> f64 {
        let d = (q - self.goal).norm();
        if d <= self.d_goal_star { 0.5 * self.zeta * d * d }
        else { self.d_goal_star * self.zeta * d - 0.5 * self.zeta * self.d_goal_star * self.d_goal_star }
    }
    fn gradient(&self, q: &SVector<f64, N>) -> SVector<f64, N> {
        let diff = q - self.goal;
        let d = diff.norm();
        if d <= self.d_goal_star { self.zeta * diff }               // eq. (4.1): ζ(q − q_goal)
        else { (self.d_goal_star * self.zeta / d) * diff }          // eq. (4.3): magnitude ζ d*
    }
}

/// One repulsive term per obstacle (Choset §4.1, after eq. 4.7), so the gradient does not jump
/// when the nearest obstacle changes. `dist` is the Chapter 4 query: d_i(q) and its closest point.
pub struct Repulsive<'w, const N: usize> { pub dist: &'w dyn cspace::DistanceQuery<N>, pub eta: f64, pub q_star: f64 }

impl<const N: usize> Potential<N> for Repulsive<'_, N> {
    fn value(&self, q: &SVector<f64, N>) -> f64 {
        self.dist.distances(q).iter().filter(|o| o.d <= self.q_star)
            .map(|o| 0.5 * self.eta * (1.0 / o.d - 1.0 / self.q_star).powi(2))      // eq. (4.4)
            .sum()
    }
    fn gradient(&self, q: &SVector<f64, N>) -> SVector<f64, N> {
        let mut g = SVector::zeros();
        for o in self.dist.distances(q).iter().filter(|o| o.d <= self.q_star) {
            // eq. (4.5): η (1/Q* − 1/d) (1/d²) ∇d, with ∇d = (q − c)/d (Derivation 2).
            g += self.eta * (1.0 / self.q_star - 1.0 / o.d) / (o.d * o.d) * o.grad;
        }
        g
    }
}

/// U = Σ terms — the whole method and its local minima.
pub struct Additive<const N: usize>(pub Vec<Box<dyn Potential<N>>>);

Gradient descent returns the iterates and the reason it stopped, with the Hessian's eigenvalues when it stopped at a critical point that is not the goal — the stuck badge of the Potential Well.

use nalgebra::{SMatrix, SVector};

pub enum DescentOutcome { Goal, LocalMinimum { hessian_eigs: Vec<f64> }, StepLimit, Collision, Stalled }

/// Choset Algorithm 4 with the sign the text intends: q(i+1) = q(i) − α(i) ∇U(q(i)).
/// `alpha` is a schedule ("often made on an ad hoc or empirical basis", Choset); `eps` the
/// forgiving termination ‖∇U‖ < ε; `max_step` caps a repulsive spike's fling.
pub fn gradient_descent<const N: usize>(
    u: &dyn Potential<N>, q_start: SVector<f64, N>, alpha: impl Fn(usize) -> f64,
    eps: f64, max_steps: usize, goal: Option<(SVector<f64, N>, f64)>, max_step: f64,
) -> (Vec<SVector<f64, N>>, DescentOutcome) {
    let mut path = vec![q_start];
    for i in 0..max_steps {
        let q = *path.last().unwrap();
        if let Some((g, tol)) = goal { if (q - g).norm() <= tol { return (path, DescentOutcome::Goal); } }
        let grad = u.gradient(&q);
        if grad.norm() < eps {
            // Not at the goal, gradient gone: classify the critical point by its Hessian.
            let h = hessian(u, &q, 1e-4);
            let eigs = h.symmetric_eigenvalues().iter().copied().collect();
            return (path, DescentOutcome::LocalMinimum { hessian_eigs: eigs });
        }
        let mut step = -alpha(i) * grad;
        if step.norm() > max_step { step *= max_step / step.norm(); }
        path.push(q + step);
    }
    (path, DescentOutcome::StepLimit)
}

/// ∇²U by central differences of the gradient, symmetrized — Choset reads the type of a
/// critical point off its signature, and Chapter 8 reads the Morse index off the same matrix.
pub fn hessian<const N: usize>(u: &dyn Potential<N>, q: &SVector<f64, N>, h: f64) -> SMatrix<f64, N, N> {
    let mut m = SMatrix::<f64, N, N>::zeros();
    for j in 0..N {
        let (mut qp, mut qm) = (*q, *q);
        qp[j] += h; qm[j] -= h;
        m.set_column(j, &((u.gradient(&qp) - u.gradient(&qm)) / (2.0 * h)));
    }
    0.5 * (m + m.transpose())
}

Brushfire is the owned artifact. It is Chapter 6's breadth-first search with every obstacle pixel as a source, and it keeps the second back pointer that Chapter 8 reads as the grid GVD.

use std::collections::VecDeque;

#[derive(Clone, Copy)] pub enum Conn { Four, Eight }

/// Choset's labels: obstacle 1, free pixels 2, 3, …; `back2` is set where a second front from a
/// *different* obstacle arrived — §5.2.5's "two different back pointers", the grid GVD.
pub struct DistGrid { pub label: Vec<u32>, pub back: Vec<Option<usize>>, pub back2: Vec<Option<usize>>, pub source: Vec<i32>, pub w: usize, pub h: usize }

pub fn brushfire(occ: &cspace::Raster, conn: Conn) -> DistGrid {
    let (w, h) = (occ.width(), occ.height());
    let comp = obstacle_components(occ);                       // a front's source is an obstacle, not a pixel
    let mut g = DistGrid { label: vec![0; w * h], back: vec![None; w * h], back2: vec![None; w * h], source: vec![-1; w * h], w, h };
    let mut queue = VecDeque::new();
    for k in 0..w * h {
        if occ.is_occupied(k) { g.label[k] = 1; continue; }
        // Step 1 of the text: free pixels neighbouring an obstacle are labelled 2.
        if let Some(o) = occ.lattice_neighbors(k, conn).find(|&n| occ.is_occupied(n)) {
            g.label[k] = 2; g.back[k] = Some(o); g.source[k] = comp[o]; queue.push_back(k);
        }
    }
    while let Some(c) = queue.pop_front() {
        let (lc, sc) = (g.label[c], g.source[c]);
        for x in occ.free_neighbors(c, conn) {               // Star(c) on Chapter 6's grid graph
            if g.label[x] == 0 {
                g.label[x] = lc + 1; g.back[x] = Some(c); g.source[x] = sc; queue.push_back(x);
            } else if g.source[x] != sc && g.source[x] >= 0 && g.label[x] >= lc {
                // Two fronts meet. Even gap: x was labelled lc + 1 by the other front and now gets
                // its second back pointer. Odd gap: both pixels carry lc and the collision falls on
                // their shared edge — both are flagged so the ridge has no holes.
                if g.label[x] == lc + 1 && g.back2[x].is_none() { g.back2[x] = Some(c); }
                g.mark_ridge(x); if g.label[x] == lc { g.mark_ridge(c); }
            }
        }
    }
    g
}

impl DistGrid {
    /// Pixels two fronts from different obstacles reached — Chapter 8's grid GVD.
    pub fn ridge_cells(&self) -> Vec<usize> { (0..self.w * self.h).filter(|&k| self.is_ridge(k)).collect() }
    /// "Point to a lowest neighbor" (Choset §4.3.2); ties to the first in scan order.
    pub fn grid_gradient(&self, cell: usize, conn: Conn) -> Option<usize> { /* lowest-label neighbour */ todo_lowest(self, cell, conn) }
}

/// Wave-front (§4.5): the same propagation from the goal pixel alone, seeded with 2.
pub fn wavefront(occ: &cspace::Raster, goal: usize, conn: Conn) -> DistGrid { propagate(occ, &[(goal, 2, 0)], conn) }

The navigation function is eq. (4.13) with its analytic gradient, and the lifted potential for Reach is a sum of Jᵀ f terms.

/// Eq. (4.13) on a sphere world; `Potential<2>` via the quotient rule.
pub struct NavigationFunction<'w> { pub world: &'w SphereWorld, pub goal: Point2<f64>, pub kappa: u32 }

impl Potential<2> for NavigationFunction<'_> {
    fn value(&self, q: &Vector2<f64>) -> f64 {
        let a = (q - self.goal.coords).norm_squared();                 // γ_1 = d²
        let b = a.powi(self.kappa as i32) + self.world.beta(q);        // d^{2κ} + β
        if b <= 0.0 { 1.0 } else { a * b.powf(-1.0 / self.kappa as f64) }
    }
    fn gradient(&self, q: &Vector2<f64>) -> Vector2<f64> {
        let k = self.kappa as f64;
        let diff = q - self.goal.coords;
        let a = diff.norm_squared();
        let (beta, grad_beta) = self.world.beta_and_gradient(q);
        let b = a.powf(k) + beta;
        let bm = b.powf(-1.0 / k);
        let da = 2.0 * diff;
        let db = k * a.powf(k - 1.0) * da + grad_beta;
        da * bm - (a / k) * (bm / b) * db                              // quotient rule
    }
}

/// §4.7.2–4.7.3: control points on Reach, each with a workspace potential; `gradient` is
/// u(q) = Σ_j J_j(q)ᵀ ∇U_j(r_j(q)), summed in C-space (eq. 4.26) — never in the workspace.
pub struct LiftedPotential<'r> { pub arm: &'r robots::Reach, pub scene: &'r collide::Scene, pub points: Vec<ControlPoint>, pub goal: Tn<2>, pub floating: bool }

impl Potential<2> for LiftedPotential<'_> {
    fn value(&self, q: &Vector2<f64>) -> f64 { self.terms(q).iter().map(|t| t.value).sum() }
    fn gradient(&self, q: &Vector2<f64>) -> Vector2<f64> {
        let mut u = Vector2::zeros();
        for t in self.terms(q) {
            let j = cspace::control_point_jacobian(self.arm, q, t.link, t.t);   // Chapter 4's jacobian, shortened
            u += j.transpose() * t.force;                                          // u = Jᵀ f
        }
        u
    }
}

The worked example, and its printed output

fn main() {
    // Part B: one evaluation and one step, as in the text.
    let goal = Vector2::new(4.0, 3.0);
    let att = Attractive { goal, zeta: 1.0, d_goal_star: 10.0 };
    let disc = cspace::DiscObstacle { center: Point2::new(0.0, -2.0), radius: 1.0 };
    let rep = Repulsive { dist: &disc, eta: 1.0, q_star: 2.0 };
    let q = Vector2::zeros();
    println!("U_att = {}  grad = {:?}", att.value(&q), att.gradient(&q));
    println!("U_rep = {}  grad = {:?}", rep.value(&q), rep.gradient(&q));
    let u = Additive(vec![Box::new(att), Box::new(rep)]);
    println!("U = {}  grad = {:?}", u.value(&q), u.gradient(&q));
    println!("q1 = {:?}", q - 0.1 * u.gradient(&q));

    // Part A: the 7 × 3 micro-grid.
    let occ = cspace::Raster::from_ascii(&[".......", "#.....#", "......."]);  // first row is the top
    let d = brushfire(&occ, Conn::Four);
    for row in d.rows_top_first() { println!("{}", row.iter().map(u32::to_string).collect::<Vec<_>>().join(" ")); }
    println!("ridge: {}", d.ridge_cells().iter().map(|&k| d.col_row(k)).map(|(c, r)| format!("({c},{r})")).collect::<Vec<_>>().join(" "));

    // The κ sweep on the five-disc world.
    for kappa in [2, 3, 4, 6, 7, 8, 10] {
        let c = critical_points(&FIVE_DISC_WORLD, FIVE_DISC_GOAL, kappa);
        println!("kappa={kappa}: minima={} saddles={}", c.minima, c.saddles);
    }
}

U_att = 12.5  grad = [-4.0, -3.0]
U_rep = 0.125  grad = [0.0, -0.5]
U = 12.625  grad = [-4.0, -3.5]
q1 = [0.4, 0.35]
2 3 4 5 4 3 2
1 2 3 4 3 2 1
2 3 4 5 4 3 2
ridge: (4,1) (4,2) (4,3)
kappa=2: minima=3 saddles=7
kappa=3: minima=2 saddles=6
kappa=4: minima=2 saddles=6
kappa=6: minima=2 saddles=6
kappa=7: minima=1 saddles=5
kappa=8: minima=1 saddles=5
kappa=10: minima=1 saddles=5

Read Part B against the text. $U_{att} = \tfrac12 \cdot 25 = 12.5$ with gradient $q - \qgoal = (-4, -3)$. The disc's closest point is $c = (0, -1)$, so $d_1 = 1$ and $\nabla d_1 = (0, 1)$; with $\eta = 1$ and $Q^\star = 2$, $U_{rep} = \tfrac12 (1 - \tfrac12)^2 = 0.125$ and $\nabla U_{rep} = (\tfrac12 - 1) \cdot 1 \cdot (0, 1) = (0, -0.5)$. The total is $U = 12.625$, $\nabla U = (-4, -3.5)$, and one step with $\alpha = 0.1$ lands at $q(1) = (0.4, 0.35)$ — away from the obstacle and toward the goal. descent_step_matches_text pins these to $10^{-12}$; brushfire_7x3_labels asserts all 21 labels and the ridge set; brushfire_equals_bfs_depth_plus_one checks on seeded grids that brushfire equals search::bfs multi-source depth $+ 1$ and that the wave-front descent has the Dijkstra cost under unit weights; kappa_sweep pins exactly one minimum from $\kappa = 7$; reach_field lifts a unit force at Reach's tip and asserts $u_\theta = (r \times f)_z$. The TypeScript port in web/lib/potential/ runs the same thirteen checks, and every widget on this page is that port.

Putting it together: Rusty in a doorway, Reach at the bench

The integration lab runs Rusty in the Apartment under a sensor-based potential: the repulsive term is fed by the local minima of a 72-ray scan of radius 3 m (Algorithm SENSOR-BASED DISTANCE), the attractive term by the goal in the corridor, and the descent is Algorithm 4 at $\alpha = 0.02$. From the middle of room A Rusty heads for the door, and in the alcove beside the jamb — where the goal pulls through the wall and the two wall faces push toward the centre of the alcove — it stops, with the stuck badge reading a positive-definite Hessian. The same raster the Chapter 6 planner used, run through wavefront from the goal cell at 4-point connectivity, gives a field with one minimum, and the grid descent walks Rusty out of the alcove and through the door — hugging the jamb at one cell's clearance, which is Choset's "dangerously close to obstacles" and the reason Chapter 8's Voronoi roadmap exists. The Potential Well's wave-front toggle is this lab on a table; the Apartment version is Exercise 4's.

Reach's half of the lab is the Arm in a Field widget run to the end: the same start descended twice, with the first collision logged. Without $r_{float}$ link 1 meets the post after a dozen steps between its two fixed repulsive points; with it the arm reaches the goal pose in about five hundred. Move the goal into the block's shadow and you will find starts where it stalls in a local minimum of the lifted potential instead — the additive disease, now on the torus — and starts where even $r_{float}$ fails, because a floating point repels the nearest link point and nothing else.

Three pointers close the chapter. Brushfire's double-back-pointer pixels are the grid generalized Voronoi diagram, and Chapter 8 proves the exact version is a roadmap, with the Morse vocabulary this chapter's "nondegenerate critical point" was borrowing from. Chapter 19's CHOMP descends a potential whose obstacle term is a distance field exactly like the brushfire one, over a whole trajectory at once, and inherits every local minimum on this page. And Part III's planners are the field's eventual answer to incompleteness: when the valley you roll into is not the goal's, sample.

Exercises

1. Foundation exerciseFDifficulty 2 of 3Convexity of d_i and the seams of U_rep

   Prove that $d_i(q) = \min_{c \in \QO_i} \|q - c\|$ is a convex function of $q$ when $\QO_i$ is convex (Choset Ch. 5, Problem 19, brought forward). Conclude that with the per-obstacle sum $U_{rep} = \sum_i U_{rep_i}$, the only places $\nabla U_{rep}$ can fail to be continuous are the spheres $d_i = Q^\star_i$ — and show that in fact it is continuous there too, so the seams that remain are those of Derivation 2's Step 4: where a nonconvex obstacle has two closest points.

   Stuck? Get a hint
2. Foundation exerciseFDifficulty 2 of 3Wave-front paths are L₁-shortest

   Show that the path produced by wave-front descent under 4-point connectivity is shortest in the $L_1$ (Manhattan) metric among all grid paths from the start to the goal (Choset Ch. 4, Problem 2). Then give a $3 \times 3$ example where the 8-point descent path — shortest in link length — is not Euclidean-shortest among 8-connected paths, and say by how much.

   On an empty 3 × 3 grid from the bottom-left to the top-right cell under 8-point connectivity, the wave-front path has two links. What is the Euclidean length of a two-link path that goes diagonal, diagonal, divided by the length of one that goes east, east, then north, north? (Give the ratio as a decimal.)

   Check Reveal
3. Conceptual exerciseCDifficulty 1 of 3Predict the κ threshold, then move an obstacle
   Predict first

   In the Navigation Function Sculptor, read the critical-point count at κ = 6: two minima, six saddles. Without moving the slider: what is the smallest κ at which the count will read one minimum?

   κ = 7

   κ = 10, as in Choset's figure 4.15

   It never reads one — some layout always keeps a spurious minimum
4. Conceptual exerciseCDifficulty 2 of 3A world of only convex discs, and the price of the escape

   In the Potential Well choose two convex discs, drag the goal and the discs until the bead gets stuck, and record the Hessian eigenvalues the badge prints. Then enable wave-front mode and estimate how close the escape path passes to the discs — Choset's "dangerously close to obstacles" in §4.5. Explain why the wave-front path must graze: among all grid paths of the same link length to the goal it has no reason to prefer clearance, and the one found first by "step to any neighbour labelled one less" is whichever the scan order produced.

   Stuck? Get a hint
5. Practical exercisePDifficulty 2 of 3Brushfire as Dijkstra, within one cell of the EDT

   Implement an 8-point brushfire with diagonal cost $1.4$ (Chapter 6's grid metric) as a Dijkstra rather than a BFS: labels become real-valued distances and the queue becomes a heap. Property-test it on 200 seeded grids against the exact Euclidean distance transform ported from the sister book (mapping/edt): assert that every cell's label is within one cell of the EDT, and record the worst gap. Then repeat with diagonal cost $\sqrt 2$ and report whether the worst gap improves — and why it cannot reach zero (the octile metric is not the Euclidean one).

6. Practical exercisePDifficulty 3 of 3Wave-front on the torus

   Implement Choset Ch. 4 Problem 5: a wave-front planner for Reach on the $T^2$ raster of Chapter 4, with the goal seeded at the goal configuration and adjacency wrapping across the seam at $\pm\pi$ in both axes. Animate the arm on the Workbench along the descent path, and assert in a test that the path's link length equals the Chapter 6 breadth-first distance on the same torus graph — then compare the number of cells the wave-front labels with the number A* expands for the same query, and say which planner you would run twice.

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 4 is this chapter's source: the additive potential and its honesty items, Algorithm 4, sensor-based and brushfire distance, the local-minimum figures 4.10–4.11, the wave-front planner, Definition 4.6.1 and the sphere- and star-world constructions, and the virtual-work lifting of §4.7 with Example 4.7.1 and figure 4.20.

2. Khatib, O. (1986) Real-Time Obstacle Avoidance for Manipulators and Mobile Robots. International Journal of Robotics Research 5(1), 90–98.doi:10.1177/027836498600500106 (opens in a new tab)

   The artificial potential field for manipulators: attractive goal, repulsive obstacles, and forces at points on the links lifted through the Jacobian — §4.7 in its original form, written for control rather than planning.

3. Rimon, E. and Koditschek, D. E. (1992) Exact Robot Navigation Using Artificial Potential Functions. IEEE Transactions on Robotics and Automation 8(5), 501–518.doi:10.1109/70.163777 (opens in a new tab)

   Navigation functions: Definition 4.6.1, the sphere-world formula (4.13) with its existence-in-κ theorem, and the star-to-sphere diffeomorphism h_λ with its switches — Derivations 5 and 6.

4. Barraquand, J. and Latombe, J.-C. (1991) Robot Motion Planning: A Distributed Representation Approach. International Journal of Robotics Research 10(6), 628–649.doi:10.1177/027836499101000604 (opens in a new tab)

   The Randomized Path Planner: gradient descent on a grid potential with random walks to escape local minima — Choset's reference [37], the honest 1991 fix and the ancestor of Part III's sampling.

5. Latombe, J.-C. (1991) Robot Motion Planning. Kluwer Academic Publishers.link to Robot Motion Planning (opens in a new tab)

   Chapter 7 is the classical textbook treatment of potential-field methods, including the wave-front (numerical navigation function) planner and the local-minimum analysis this chapter condenses.

6. Ratliff, N., Zucker, M., Bagnell, J. A., and Srinivasa, S. (2009) CHOMP: Gradient Optimization Techniques for Efficient Motion Planning. IEEE International Conference on Robotics and Automation (ICRA), 489–494.doi:10.1109/ROBOT.2009.5152817 (opens in a new tab)

   Forward pointer only: the obstacle term of CHOMP is a distance field like brushfire's, descended over a whole trajectory, and it inherits this chapter's local minima. Chapter 19 derives it.

7. Thrun, S., Burgard, W., and Fox, D. (2005) Probabilistic Robotics. MIT Press.link to Probabilistic Robotics (opens in a new tab)

   The sister volume's map representations chapter ports the exact Euclidean distance transform this chapter compares brushfire against; the two are different algorithms computing different metrics.