Graph Search for Planners

Best-first search is one algorithm with one knob — breadth-first, Dijkstra, greedy and A* — with the optimality proof, the counterexample that breaks it, D* and D* Lite for repairing a search instead of redoing it, and universal plans as the reader's first value function.

The problem: a goal is not a plan

Put Rusty in room A of the Apartment and the goal in room B, and write the first controller anyone writes: drive toward the goal.

It wedges into the first wall. The goal is $2.2$ m away through the plaster, the doorway is two metres behind and to the left, and nothing in the controller can know that, because the controller only looks at where it is and where it wants to be. Something must look ahead — must consider positions Rusty is not in, order them by how promising they are, and remember which one led to which. That something is a graph search, and the rest of the chapter is what the words "promising" and "remember" mean precisely.

Building intuition: one search, one knob

Rasterize the Apartment's south rooms and corridor into a grid of half-metre cells, connect each cell to its eight neighbors, and search it four ways at once.

The four lanes run the same loop. Pop the most promising node, mark it done, look at its neighbors, push the ones not yet done. The only thing that differs is what "most promising" means:

• Breadth-first ranks by link length, the number of edges from the start. It floods outward in rings, and the first time it reaches the goal it has found the path with the fewest edges — which, on a grid where a diagonal step costs $1.4$ and an axial one costs $1$, is not the cheapest path.
• Dijkstra ranks by $g$, the cost of the cheapest known path from the start. It floods in cost rings, and the first time it pops the goal it has the cheapest path — at the price of expanding every cell cheaper than the goal.
• Greedy ranks by $h$, a guess at the cost from each node to the goal. It darts toward the goal and often arrives in a handful of expansions with a path that is visibly bad.
• A* ranks by $f = g + \varepsilon h$. With $\varepsilon = 1$ it expands only the cells it must and matches Dijkstra's cost to the last digit.

Now slide $\varepsilon$. Below one, the A* lane's cost does not move; only its expansion count does, climbing toward Dijkstra's as $\varepsilon \to 0$ and the heuristic switches off. Above one the expansion count keeps falling — and the path cost drifts above Dijkstra's. The badge marks the first $\varepsilon$ at which that happens. This is the misconception the widget exists to kill: a better heuristic always means a better path. A stronger heuristic means fewer expansions only while it stays optimistic; past that, it buys speed with path quality, and the chapter's derivations say how much of each.

The mathematics

Graphs, grids, and two kinds of length

The weights on a grid are the chapter's first honesty item. Choset's §H.2.4 charges $1$ for an axial step and $1.4$ for a diagonal — his approximation of $\sqrt 2$, chosen so that hand arithmetic comes out in tenths. This is not the Euclidean metric: two cells sideways and one up costs $2.4$, not $\sqrt 5 = 2.24$. It is a perfectly good metric on the graph, and the design of this chapter keeps it, because it is what makes the examples traceable. Code that wants $\sqrt 2$ changes one constant.

The link length $\ell(\pi)$ of a path is its number of edges, weights ignored. Breadth-first search minimizes link length; Dijkstra minimizes weighted cost; the two coincide only when all weights are equal. On the $1/1.4$ grid they disagree in a concrete way: four axial steps cost $4.0$ and three diagonals cost $4.2$, so a path with more links can be cheaper. This is why the Search Theater's breadth-first lane reports a different cost from Dijkstra's, and why the Rust port relaxes a node by depth in breadth-first mode and by cost everywhere else.

Expansion, open and closed, and what a heuristic promises

Expanding a node $n$ (Choset §H.2.1) moves it from the open set $O$ to the closed set $C$ and, for every $x \in \mathrm{Star}(n) \setminus C$, either inserts $x$ into $O$ with $g(x) = g(n) + c(n, x)$ and $b(x) = n$, or — if $x$ is already open and the new route is cheaper — updates $g(x)$ and $b(x)$. The closed set is what stops a search from walking in circles; the open set is the frontier between what is known and what is not. In the colors every figure on this page uses:

The cost-to-come $g$ is measured from the green start; the heuristic $h$ is an estimate toward the red goal; the blue explored set is the price of the search; the purple back-pointer path is its product. Point at a term and the matching layer in the Search Theater comes forward.

Consistency is not in Choset; it is the standard strengthening that makes the no-reopening theorem below true, and the octile heuristic — $h = 1.4\min(\Delta x, \Delta y) + |\Delta x - \Delta y|$, the exact cost-to-go on an empty $1/1.4$ grid — has it. Exercise 1 asks for the proof, and for the slightly embarrassing fact that the same formula is not admissible if diagonals cost $\sqrt 2$.

Where optimism fails: the four-node counterexample

The hand trace: five pops on a $4 \times 3$ grid

Here is the micro-example the tests pin, in Choset's §H.2.4 style. An 8-connected $4 \times 3$ grid, cells $(\mathrm{col}, \mathrm{row})$ with $\mathrm{col} \in \{1, \dots, 4\}$ and $\mathrm{row} \in \{1, 2, 3\}$; obstacle cells $(2, 1)$ and $(2, 2)$ are deleted from the graph; $\qstart = (1, 1)$, $\qgoal = (4, 1)$; axial step $1$, diagonal $1.4$; octile heuristic. Ties in $f$ break toward smaller $h$, then lexicographically.

Back pointers give $(1,1) \to (1,2) \to (2,3) \to (3,2) \to (4,1)$ at cost $1 + 1.4 + 1.4 + 1.4 = \mathbf{5.2}$ in five expansions. Dijkstra on the same grid, $h \equiv 0$, expands all ten free cells — every other free cell has $g < 5.2$, so all of them are popped before the goal — and returns the same cost. Greedy, $f = h$, happens to find the same path here in five expansions, with no guarantee that it would anywhere else. All three facts are asserted by astar_trace_matches_text and the TypeScript check that mirrors it.

Repairing a search: D*, and universal plans

Complexity and completeness

Running time is measured asymptotically (Choset Def. G.1.1): $f \in O(g)$ if $f(n) \le c\,g(n)$ for all large $n$; $\Omega$ is the lower bound, $\Theta$ both, $o$ and $\omega$ the strict versions. A* with a binary heap runs in $O((V + E)\log V)$ in the worst case, the same as Dijkstra, and the heuristic buys a smaller constant — fewer expansions — not a better exponent.

The algorithm

Choset's Algorithm 24 is the master template; everything else in this chapter is a special case, a repair of it, or a run of it in reverse.

Two implementation notes that the proofs depend on. The open set is a lazy binary heap: improving a node pushes a fresh entry and the stale one is discarded when it surfaces, so line 8 never needs a decrease-key. And if a node already in $C$ receives a cheaper $g$ — which Derivation 4 says cannot happen under a consistent heuristic, and which does happen with $\varepsilon > 1$ — it is reopened: removed from $C$ and pushed again. Correctness over speed, and a counter the widget shows.

Replanning, watched

With Choset's gate world the numbers are the ones in his figures. The initial backward Dijkstra labels the start $(2,1)$ with $h = 7.0$ — five diagonals — and $(4,1)$ with $h = 6.2$, and Rusty sets off along $(2,1)\,(3,2)\,(4,3)\,(5,4)\,(6,5)\,(7,6)$. The gate closes; at $(3,2)$ Rusty is adjacent and notices. D* re-inserts the gate and its neighbors, pops $(3,2)$ as a RAISE state — its $h$ has jumped above Choset's obstacle cost of $10000$ while its key stays at the old $7.0$ — and the LOWER neighbor $(4,1)$ rescues it at $6.2 + 1.4 = 7.6$. The repair pops 15 states, five of them raised, and Rusty drives $(3,2)\,(4,1)\,(5,2)\,(6,3)\,(7,4)\,(7,5)\,(7,6)$ at cost $7.6$. Redoing the backward Dijkstra from scratch on the changed map would pop 32; a from-scratch A* from $(3,2)$ needs only 7 expansions on this tiny grid, which is Exercise 4's point — repair is not always cheaper than redo, and the raised/lowered classification says when it is.

A plan that is a field

A path is a plan for one start. Run the backward Dijkstra to exhaustion and every free cell of the Apartment carries a cost-to-go and an arrow; kick Rusty anywhere and the arrows bring it home. The equal-cost contours are wave fronts — Chapter 7's wave-front planner is this picture with unit costs — and the panel under the canvas evaluates $h(X) = \min_Y [c(X, Y) + h(Y)]$ at the cell under the cursor, term by term, with the minimizing $Y$ being the arrow. The chapter's check asserts the equation holds at every labelled cell to $10^{-15}$ and that following the arrows from sixty random cells reaches the goal at exactly Dijkstra's cost for that start.

Implementation in Rust

The search crate is introduced here and imported by Chapters 7, 8, 10, 16, 18, 21 and 22. Its interface is the smallest thing best-first search needs.

/// Anything best-first search can walk. Costs are non-negative edge weights
/// (Choset App. H.1); `Node` must be hashable so open/closed sets are maps.
pub trait SearchSpace {
    type Node: Copy + Eq + std::hash::Hash;
    /// Star(n) with edge costs c(n, x) — Choset's notation, made an iterator.
    fn neighbors(&self, n: Self::Node) -> impl Iterator<Item = (Self::Node, f64)> + '_;
}

/// 8-connected lattice over a rasterized Q_free (Chapter 4's `cspace::Raster`). `diag` is
/// 1.4 for Choset's grid metric (figure H.16) or SQRT_2 for the exact one. Obstacle cells
/// are deleted from the graph: a neighbor that is occupied is simply not yielded.
pub struct Grid8<'a> { pub raster: &'a cspace::Raster, pub diag: f64 }

impl SearchSpace for Grid8<'_> {
    type Node = (i32, i32);
    fn neighbors(&self, (i, j): (i32, i32)) -> impl Iterator<Item = ((i32, i32), f64)> + '_ {
        const STEPS: [(i32, i32); 8] = [(1,0), (-1,0), (0,1), (0,-1), (1,1), (1,-1), (-1,1), (-1,-1)];
        STEPS.iter().filter_map(move |&(di, dj)| {
            let n = (i + di, j + dj);
            if !self.raster.is_free(n) { return None; }
            let diag = di != 0 && dj != 0;
            Some((n, if diag { self.diag } else { 1.0 } * self.raster.cell_size()))
        })
    }
}

The engine is one function. Priority is the knob; Trace records the pop order so the examples can print Choset-style tables and the widgets can paint the frontier.

use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap, HashSet};

/// The one knob: how a node's priority is formed from g, h and depth.
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum Priority { LinkLength, CostToCome, HeuristicOnly, Sum { eps: f64 } }

impl Priority {
    fn f(self, g: f64, h: f64, depth: usize) -> f64 {
        match self {
            Priority::LinkLength => depth as f64,
            Priority::CostToCome => g,
            Priority::HeuristicOnly => h,
            Priority::Sum { eps } => g + eps * h,
        }
    }
}

/// One row of the trace table and the result of a search.
pub struct Pop<N> { pub node: N, pub g: f64, pub h: f64, pub f: f64 }
pub struct Trace<N> { pub pops: Vec<Pop<N>>, pub path: Vec<N>, pub cost: f64, pub reopened: usize }

/// Heap entries ordered by (f, h, seq): ties toward smaller h, then insertion order.
/// `Reverse` semantics are written out so the comparison is readable.
struct Entry<N> { f: f64, h: f64, seq: u64, g: f64, node: N }
impl<N> PartialEq for Entry<N> { fn eq(&self, o: &Self) -> bool { self.cmp(o) == Ordering::Equal } }
impl<N> Eq for Entry<N> {}
impl<N> PartialOrd for Entry<N> { fn partial_cmp(&self, o: &Self) -> Option<Ordering> { Some(self.cmp(o)) } }
impl<N> Ord for Entry<N> {
    fn cmp(&self, o: &Self) -> Ordering {
        // BinaryHeap is a max-heap: reverse every component so the smallest f pops first.
        o.f.total_cmp(&self.f).then(o.h.total_cmp(&self.h)).then(o.seq.cmp(&self.seq))
    }
}

/// BFS, Dijkstra, greedy, and (weighted) A* are this function with four `Priority` values.
/// Returns `None` when the open set empties — the completeness half of Derivation 2.
pub fn best_first<S: SearchSpace>(
    space: &S, start: S::Node, goal: S::Node,
    h: impl Fn(S::Node) -> f64, prio: Priority,
) -> Option<Trace<S::Node>> {
    let mut g: HashMap<S::Node, f64> = HashMap::from([(start, 0.0)]);
    let mut depth: HashMap<S::Node, usize> = HashMap::from([(start, 0)]);
    let mut parent: HashMap<S::Node, S::Node> = HashMap::new();
    let mut closed: HashSet<S::Node> = HashSet::new();
    let mut open = BinaryHeap::new();
    let (mut seq, mut pops, mut reopened) = (0u64, Vec::new(), 0usize);
    open.push(Entry { f: prio.f(0.0, h(start), 0), h: h(start), seq, g: 0.0, node: start });

    while let Some(Entry { node, g: gn, .. }) = open.pop() {
        // Lazy deletion: a stale entry (better g pushed later, or already closed) costs nothing.
        if closed.contains(&node) || gn > g[&node] + 1e-12 { continue; }
        closed.insert(node);
        let d = depth[&node];
        pops.push(Pop { node, g: gn, h: h(node), f: prio.f(gn, h(node), d) });
        if node == goal {
            let mut path = vec![goal];
            while let Some(&p) = parent.get(path.last().unwrap()) { path.push(p); }
            path.reverse();
            return Some(Trace { pops, path, cost: gn, reopened });
        }
        for (x, c) in space.neighbors(node) {
            let tentative = gn + c;
            // Breadth-first relaxes on link length (first discovery wins); the rest on cost.
            let better = match prio {
                Priority::LinkLength => d + 1 < *depth.get(&x).unwrap_or(&usize::MAX),
                _ => tentative < *g.get(&x).unwrap_or(&f64::INFINITY) - 1e-12,
            };
            if !better { continue; }
            if closed.remove(&x) { reopened += 1; }       // only an inconsistent h gets here
            g.insert(x, tentative); depth.insert(x, d + 1); parent.insert(x, node);
            seq += 1;
            open.push(Entry { f: prio.f(tentative, h(x), d + 1), h: h(x), seq, g: tentative, node: x });
        }
    }
    None
}

pub fn astar<S: SearchSpace>(s: &S, a: S::Node, b: S::Node, h: impl Fn(S::Node) -> f64) -> Option<Trace<S::Node>> {
    best_first(s, a, b, h, Priority::Sum { eps: 1.0 })
}
pub fn dijkstra<S: SearchSpace>(s: &S, a: S::Node, b: S::Node) -> Option<Trace<S::Node>> {
    best_first(s, a, b, |_| 0.0, Priority::CostToCome)
}

D* Lite is the default replanner. The whole algorithm is the key, the two update cases, and the modifier $k_m$; everything else is bookkeeping.

/// Incremental replanner (Koenig & Likhachev 2002/2005): g/rhs tables plus the key
/// modifier k_m, so the robot's own motion never forces a heap rebuild.
pub struct DStarLite<S: SearchSpace> {
    space: S,
    g: HashMap<S::Node, f64>,
    rhs: HashMap<S::Node, f64>,
    queue: KeyedHeap<S::Node, [f64; 2]>,   // lazy; stale entries detected by key mismatch
    start: S::Node, last: S::Node, goal: S::Node,
    km: f64,
    h: fn(S::Node, S::Node) -> f64,
}

impl<S: SearchSpace> DStarLite<S> {
    fn key(&self, s: S::Node) -> [f64; 2] {
        let m = self.g[&s].min(self.rhs[&s]);
        [m + (self.h)(self.start, s) + self.km, m]
    }

    fn update_vertex(&mut self, u: S::Node) {
        if u != self.goal {
            let best = self.space.neighbors(u).map(|(v, c)| c + self.g[&v]).fold(f64::INFINITY, f64::min);
            self.rhs.insert(u, best);
        }
        self.queue.remove(u);
        if self.g[&u] != self.rhs[&u] { let k = self.key(u); self.queue.insert(u, k); }
    }

    pub fn compute_shortest_path(&mut self) -> usize {
        let mut pops = 0;
        // Process ties with the start's key as well: a stale vertex left at an equal key
        // can win a tie in the greedy descent and misreport the path's cost.
        while self.queue.top_key().is_some_and(|k| k <= self.key(self.start)) || self.rhs[&self.start] != self.g[&self.start] {
            let Some((u, k_old)) = self.queue.pop() else { break };
            pops += 1;
            let k_new = self.key(u);
            if k_old < k_new { self.queue.insert(u, k_new); }
            else if self.g[&u] > self.rhs[&u] {
                self.g.insert(u, self.rhs[&u]);                      // overconsistent: commit
                for (p, _) in self.space.neighbors(u) { self.update_vertex(p); }
            } else {
                self.g.insert(u, f64::INFINITY);                     // underconsistent: re-derive
                for (p, _) in self.space.neighbors(u) { self.update_vertex(p); }
                self.update_vertex(u);
            }
        }
        pops
    }

    /// Report observed cost changes; returns how many vertices' values moved (w6.2's statistic).
    pub fn update_edges(&mut self, changed: &[(S::Node, S::Node)]) -> usize {
        self.km += (self.h)(self.last, self.start);
        self.last = self.start;
        let mut touched = HashSet::new();
        for &(u, v) in changed { for s in [u, v] { self.update_vertex(s); touched.insert(s); } }
        self.compute_shortest_path();
        touched.len()
    }

    pub fn next_step(&mut self) -> Option<S::Node> {
        if self.g[&self.start].is_infinite() { return None; }
        let (s, _) = self.space.neighbors(self.start)
            .map(|(v, c)| (v, c + self.g[&v]))
            .min_by(|a, b| a.1.total_cmp(&b.1))?;
        self.start = s;
        Some(s)
    }
}

The universal plan is Dijkstra with the early exit removed and the direction reversed.

/// Choset §H.4: Dijkstra run backward from the goal yields cost-to-go and a successor
/// for *every* node — a universal plan. The sister book's Chapter 21 generalizes `min` to E[·].
pub struct UniversalPlan<N> { pub cost_to_go: HashMap<N, f64>, pub next: HashMap<N, N> }

pub fn universal_plan<S: SearchSpace>(space: &S, goal: S::Node) -> UniversalPlan<S::Node> {
    let mut cost_to_go = HashMap::from([(goal, 0.0)]);
    let mut next = HashMap::from([(goal, goal)]);
    let mut closed = HashSet::new();
    let mut heap = BinaryHeap::new();
    heap.push(MinEntry { key: 0.0, node: goal });
    while let Some(MinEntry { key, node: x }) = heap.pop() {
        if !closed.insert(x) || key > cost_to_go[&x] + 1e-12 { continue; }
        for (y, c) in space.neighbors(x) {              // symmetric edges: neighbors = predecessors
            let cand = cost_to_go[&x] + c;
            if cand < *cost_to_go.get(&y).unwrap_or(&f64::INFINITY) - 1e-12 {
                cost_to_go.insert(y, cand);
                next.insert(y, x);
                heap.push(MinEntry { key: cand, node: y });
            }
        }
    }
    UniversalPlan { cost_to_go, next }
}

/// The deterministic Bellman residual: max |h(X) − min_Y [c(X,Y) + h(Y)]|, zero for a correct plan.
pub fn bellman_residual<S: SearchSpace>(space: &S, plan: &UniversalPlan<S::Node>) -> f64 {
    plan.cost_to_go.iter().filter(|(x, _)| plan.next[x] != **x).map(|(&x, &hx)| {
        let best = space.neighbors(x).filter_map(|(y, c)| plan.cost_to_go.get(&y).map(|hy| c + hy)).fold(f64::INFINITY, f64::min);
        (hx - best).abs()
    }).fold(0.0, f64::max)
}

The worked example, and its printed output

fn main() {
    let raster = cspace::Raster::from_ascii(&["....", ".#..", ".#.."]);   // first row is the top
    let grid = Grid8 { raster: &raster, diag: 1.4 };
    let (start, goal) = ((0, 0), (3, 0));                                    // (col,row) = (1,1), (4,1)
    let h = |(i, j): (i32, i32)| { let (dx, dy) = ((3 - i).abs() as f64, (0 - j).abs() as f64); 1.4 * dx.min(dy) + (dx - dy).abs() };
    let t = astar(&grid, start, goal, h).expect("a path exists");
    println!("pop  node   g    h    f");
    for (k, p) in t.pops.iter().enumerate() {
        println!("{:>3}  ({},{}) {:.1}  {:.1}  {:.1}", k + 1, p.node.0 + 1, p.node.1 + 1, p.g, p.h, p.f);
    }
    println!("path: {}", t.path.iter().map(|(i, j)| format!("({},{})", i + 1, j + 1)).collect::<Vec<_>>().join(" "));
    println!("cost = {:.1}", t.cost);
    let d = dijkstra(&grid, start, goal).unwrap();
    println!("dijkstra: expansions = {}, cost = {:.1}", d.pops.len(), d.cost);
}

pop  node   g    h    f
  1  (1,1) 0.0  3.0  3.0
  2  (1,2) 1.0  3.4  4.4
  3  (2,3) 2.4  2.8  5.2
  4  (3,2) 3.8  1.4  5.2
  5  (4,1) 5.2  0.0  5.2
path: (1,1) (1,2) (2,3) (3,2) (4,1)
cost = 5.2
dijkstra: expansions = 10, cost = 5.2

A* returned cost 8 via A; optimal is 5 via B
initial path: (2,1) (3,2) (4,3) (5,4) (6,5) (7,6)  cost 7.0
gate (4,3) closed at (3,2): repaired k(3,2) = h(3,2) = 7.6 via (4,1)  —  15 states popped (5 raised) vs 32 for a fresh backward Dijkstra

astar_trace_matches_text asserts the pop order, the $g/h/f$ triples to $10^{-9}$, the path, and both expansion counts. astar_equals_dijkstra_on_random_grids runs two hundred seeded SmallRng grids and asserts equal costs, zero reopenings, and equality with pathfinding::prelude::astar (the crate is a test dependency only; no search crate is in the library path). nonoptimistic_loses asserts $8$ and $5$. dstar_gate_repairs_to_7_6 asserts the initial path, $h(4,1) = 6.2$, and the repaired $k(3,2) = 7.6$ with back pointer $(4,1)$; the work counts are regression-locked, as the design doc asks, because they depend on the choice to re-insert both endpoints of a changed arc. dstar_lite_equals_fresh_astar flips random cells on forty grids as the robot walks and asserts, after every flip and every step, that D* Lite's cost-to-goal equals a fresh A*'s and that its greedy descent has that cost. universal_plan_is_bellman_fixed_point asserts the residual is below $10^{-9}$ on the Apartment and that following the policy from sixty random cells reaches the goal at Dijkstra's cost. The TypeScript port in web/lib/search/ runs the same eleven checks, and every widget on this page is that port.

Putting it together: a door closes

The integration lab is the Apartment scenario of the D* Replanner. Rasterize the Apartment at half-metre cells for disc-Rusty with the Chapter 4 inflation of $0.25$ m, plan from room A to the bedroom with astar under Choset's $1/1.4$ metric, and let Rusty drive. At the chosen step the cell on the plan about halfway along — in the Apartment that is almost always a doorway — becomes an obstacle, a door closing while Rusty is en route. When Rusty is adjacent it notices, and D* Lite repairs: it updates the cells around the door, propagates until the band of keys below Rusty's own is consistent again, and hands back a path through a different door. The blue footprint is the repair; the readout compares its size with the expansions a from-scratch A* needs from the same cell. In the open corridor the two are comparable, because the repair has to re-derive the whole region whose cost-to-go flowed through the closed door; deep inside a room, where a closed door changes only a few cells' routes, the repair is a tenth of the redo. Exercise 4 asks you to find the other kind of case.

Three pointers forward close the chapter. Chapter 7's brushfire and wave-front planners are breadth-first search from a different seed set — all obstacle cells, or the goal — and its navigation functions are universal plans with a smoothness requirement. Chapter 8 runs A* on the visibility graph and the generalized Voronoi diagram, and Chapter 10 on a cell decomposition's adjacency graph; none of them re-implement anything on this page. And the sister book's Chapter 21 takes the deterministic Bellman equation of the universal plan, replaces the minimum over successors by an expectation over outcomes, and arrives at value iteration — the same table, now a value function.

Exercises

1. Foundation exerciseFDifficulty 2 of 3Octile is consistent — for the right diagonal

   Prove that $h(n) = 1.4\min(\Delta x, \Delta y) + |\Delta x - \Delta y|$ is consistent on the 8-connected grid with Choset's $1/1.4$ metric: show $h(n) \le c(n, x) + h(x)$ for each of the eight moves and $h(\qgoal) = 0$. Then give a two-cell example showing that the same formula is not admissible if diagonals cost $\sqrt 2$ instead of $1.4$.

   Stuck? Get a hint
2. Foundation exerciseFDifficulty 2 of 3Obstacles as 10000-cost nodes

   Choset's grid A* (§H.2.4) keeps obstacle pixels in the graph as nodes that cost $10000$ to enter. Show that on any grid with fewer than $10000 / 1.4$ cells this returns the same path as deleting them. Then exhibit a grid where the two differ — and say which answer a robot should prefer.

3. Conceptual exerciseCDifficulty 1 of 3Predict the weighted-A* bound, then verify
   Predict first

   In the Search Theater set ε = 2. Before pressing Run: can the A* lane's path cost exceed Dijkstra's by more than a factor of 2 on any layout?

   Yes — once the heuristic is inadmissible, anything goes

   No — the cost is at most 2 × Dijkstra's, and usually much closer

   No — the cost never changes at all, only the expansion count does
4. Conceptual exerciseCDifficulty 2 of 3When repair costs more than redo

   In the D* Replanner, find a gate position — on Choset's grid or in the Apartment — for which D* Lite touches more cells than a from-scratch A* expands. Explain, using the raised/lowered classification, why this is rare: which cells must a repair touch, which cells must a redo touch, and when is the first set the larger?

   On Choset's gate grid, how many states does the D* repair pop after the gate closes with Rusty at (3,2)?

   Check Reveal
5. Practical exercisePDifficulty 2 of 3A true FIFO

   Implement Priority::LinkLength as a true first-in-first-out queue — not a heap with constant keys — and add a property test that breadth-first search and Dijkstra agree on every unit-weight grid over 200 seeded trials: same link length, same cost. Then switch the weights to $1/1.4$ and write the test that shows where they disagree, with the four-axial-versus-three-diagonal example as its smallest witness.

6. Practical exercisePDifficulty 3 of 3ARA*

   Implement anytime repairing A* in src/ara.rs with the schedule $\varepsilon = 3, 2, 1.5, 1$, reusing the open set between iterations and keeping the inconsistent list that the algorithm uses to seed the next round. Add a fifth lane to the Search Theater that shows each iterate's path, and assert in a test that the final cost equals Dijkstra's. The paper is Likhachev, Gordon and Thrun's; the design is three pages, and Derivation 4 is the part of this chapter you will need.