A robot must reach a goal without driving through walls. We turn that messy geometric problem into a graph, then find the cheapest path through it — optimally, and fast. This is the Motion Planning module of the autonomy stack.
You hand a delivery robot a simple instruction: "go to the kitchen." It knows where it is. It knows where the kitchen is. The obvious move is to point itself at the goal and drive in a straight line. So it does — and two seconds later it is grinding its wheels against a sofa it cannot push through.
This is the whole problem of motion planning: the shortest path is rarely a straight line, because the world is full of obstacles. Stanford's definition is worth memorizing word for word: motion planning is computing a sequence of actions that drives a robot from an initial condition to a terminal condition while avoiding obstacles, respecting motion constraints, and possibly optimizing a cost function.
Three demands hide in that sentence. Avoid obstacles — don't hit the sofa. Respect motion constraints — a car can't slide sideways. Optimize a cost — don't take the scenic route when a short one exists. A naive "aim and go" controller satisfies none of them.
Could we just try every possible path? In principle yes — but the number of paths through a continuous 2D world is infinite, and even after we chop the world into a grid, the count explodes. A modest 100×100 grid has 10,000 cells; the number of distinct routes between two corners is astronomically larger than the number of atoms in the observable universe. Brute force is hopeless. We need an algorithm that finds the best path while looking at only a tiny fraction of the possibilities.
Before we build that algorithm, let's feel the failure. Below is a robot in a small maze. Press Straight line and watch it charge the goal and stall against a wall. Then press Smart plan to see what a planner produces — a route that detours around the obstacles and actually arrives.
The robot (teal) must reach the green goal. Straight line aims directly at the goal and stops the moment it hits a wall — it never arrives. Smart plan computes a collision-free route first, then follows it. Same robot, same goal; only the thinking differs.
The straight-line robot fails not because its motors are weak but because it never reasoned. The planned robot looks at the whole map, discovers a corridor, and commits to a route before moving. That reasoning is the job of the entire rest of this lesson.
Here is the road ahead. First we reframe the geometry so the robot becomes a single point (Chapter 1). Then we chop the world into a graph (Chapter 2). Then we learn to search that graph — first blindly (Chapter 3), then with a hint about where the goal is (Chapters 4–5), arriving at A*, the algorithm that finds the optimal path while expanding remarkably few nodes.
The robot in Chapter 0 is a disk with a real radius. Checking whether any part of that disk overlaps any part of an obstacle, for every position the robot might take, is fiddly. Stanford's key insight — the one that makes planning tractable — is to change the space we plan in.
Recall from Lesson 1 that the robot's configuration is the smallest set of numbers that completely pins down where it is and how it's posed. For a disk robot rolling on a floor, that's just its center: q = (x, y). The set of all possible configurations is the configuration space, written 𝒞 (script C). For our disk, every point in the plane is a possible configuration, so 𝒞 = ℝ2.
But here's the catch. If the robot is a point, where did its body go? It didn't vanish — we pushed it into the obstacles. Wherever the robot's center can sit without its body touching an obstacle is free; everywhere else is forbidden. Stanford writes this precisely. Let R(q) be the set of world points the robot's body occupies when its configuration is q, and let O be the obstacle region. The robot is in collision exactly when its body overlaps an obstacle:
So the free space — the configurations that are safe — is everything that doesn't collide:
Read that aloud: "the free space is the set of all configurations q for which the robot's body, placed at q, has no overlap with the obstacles." Everything not in 𝒞free is the forbidden space 𝒞obs. The planning problem becomes a clean one-liner: find a continuous path τ: [0,1] → 𝒞free with τ(0) = qinit and τ(1) = qgoal. A path is just a curve through the free space from start to finish.
For a disk robot of radius r, the rule for turning world obstacles into C-space obstacles is beautifully simple: inflate every obstacle outward by r, then treat the robot as a point. Why does that work? The robot's center collides with an obstacle exactly when the center comes within distance r of the obstacle's surface — because that's when the rim of the disk touches. So the set of forbidden center positions is the obstacle "puffed up" by r in every direction. (Mathematically this is a Minkowski sum of the obstacle with the robot's disk; the name isn't important — the picture is.)
Say a square obstacle occupies the world region x ∈ [4, 6], y ∈ [4, 6] (in metres) and the robot is a disk of radius r = 0.5 m. The inflated C-space obstacle is the square grown by 0.5 m on each side, plus rounded corners of radius 0.5 m. Its bounding box is now x ∈ [3.5, 6.5], y ∈ [3.5, 6.5].
Check a configuration. Can the robot's center sit at q = (3.7, 5.0)? In the original world the center at (3.7, 5.0) is 0.3 m to the left of the obstacle's left edge (x = 4). The disk of radius 0.5 sticks out to x = 3.7 + 0.5 = 4.2, which is inside the obstacle — collision. In C-space, (3.7, 5.0) has x = 3.7 < 3.5? No, 3.7 > 3.5, so it's inside the inflated box — forbidden. Both descriptions agree, which is the whole point: planning over the inflated obstacles with a point robot gives exactly the same collision verdict as the fat robot in the real world.
Drag the disk robot below around its world. Its C-space twin — a single point at the robot's center — moves in the right panel, where the obstacles are the inflated versions. When the body touches an obstacle on the left, the point lands inside a forbidden region on the right. Toggle the robot radius and watch the inflated obstacles grow and shrink.
Left: the real world — a disk robot (teal) you can drag, with solid obstacles. Right: configuration space — the robot is a single point, obstacles are inflated by the robot's radius. Red glow = collision. Use the slider to change the robot radius and watch the C-space obstacles breathe.
Notice what happened to the gap between the two obstacles. In the world it looks wide enough for the robot to slip through; in C-space, with a large radius, the inflated obstacles merge and the gap closes. The robot can no longer fit — and the C-space picture tells you that before you ever try the move. That is the gift of configuration space: it bakes the robot's size into the map, so planning can ignore the body entirely.
We've shrunk the robot to a point moving through a continuous free space. But "continuous" still means infinitely many positions, and a computer can't search infinitely many anythings. Stanford's grid-based approach makes one decisive move: discretize. Chop the continuous C-space into a finite grid of cells, declare each cell either free or forbidden, and only allow the point to live at cell centers and step between adjacent cells.
That single move converts an impossible continuous problem into a finite one. Each free cell becomes a node (also called a vertex); each pair of adjacent free cells gets an edge. The collection of nodes and edges is a graph, written G = (V, E) — V the set of vertices, E the set of edges. Planning a robot path is now exactly the classic computer-science problem of finding a path through a graph.
This is a real design choice, not a detail. The two standard answers:
4-connectivity. A cell connects only to its up, down, left, and right neighbours — the four cells sharing an edge. The robot moves in cardinal directions only, like a rook taking one step. Every move has the same length: one cell-width.
8-connectivity. A cell also connects to its four diagonal neighbours — the cells sharing only a corner. The robot can move like a king. Diagonal moves are longer: by Pythagoras, a diagonal step across a unit cell has length √2 ≈ 1.414, not 1.
The connectivity choice changes the answers. Under 4-connectivity, going from one corner of a square room to the opposite corner forces a staircase of horizontal and vertical steps — the famous "Manhattan" path. Under 8-connectivity, the robot can cut the corner diagonally and travel a shorter, more natural-looking route. We'll use 8-connectivity in most of this lesson because it produces straighter, more realistic paths; but the search algorithms work identically for either.
Each edge carries a cost — how expensive it is to traverse. For geometric path-length planning, the cost of an edge is simply the Euclidean distance between the two cell centers it connects. If cells are unit-width:
Getting these costs right matters enormously. If you sloppily charge every move — cardinal or diagonal — a cost of 1, the planner thinks a zigzag of diagonals is as cheap as a straight run, and it will return ugly, physically-wrong "shortest" paths. The cost is the definition of what "best" means. Change the cost function and you change the optimal path: charge extra for turns and you get smooth paths; charge extra near obstacles and you get cautious, wall-avoiding paths.
Below, paint obstacles into a grid by clicking, toggle between 4- and 8-connectivity, and watch the edges light up from whatever cell you hover. Notice how 8-connectivity gives a cell up to eight outgoing edges, and how each diagonal edge is labelled √2 while cardinals are 1.
Click a cell to toggle it between free and obstacle. Hover (or tap) any free cell to see its outgoing edges and their costs. Switch connectivity to compare how many neighbours each cell gets. This is the graph A* will later search.
One subtlety the homework flags: in this lesson we collision-check the cells (nodes), not the edges between them. That's cheaper, and because our obstacles are grid-aligned, any path between free cells stays collision-free. In the wild, with off-grid or thin obstacles, you'd also check that each edge doesn't clip an obstacle, or simply inflate obstacles a little extra to leave margin — which is exactly the inflation trick from Chapter 1.
We have a graph. How do we find the cheapest path through it? Stanford frames every search algorithm in this lesson as a single family — label-correcting algorithms — that differ only in which node they explore next. Get the skeleton first; the named algorithms are then just one knob turned three ways.
Every algorithm keeps three pieces of bookkeeping:
The loop is dead simple. Pull a node q off the frontier. For each neighbour q′, compute the candidate cost of reaching it through q:
(The tilde just means "candidate" — a value we're proposing, not yet committed.) If that candidate beats the best cost we'd previously recorded for q′, we've found a better route: lower g(q′), set q as q′'s parent, and add q′ to the frontier. Repeat until the frontier empties. The name "label-correcting" comes from this correcting of the cost labels as better paths surface.
Breadth-first search (BFS) treats the frontier as a plain queue: first node in is the first node out. It explores all nodes one step from the start, then all nodes two steps away, then three — an expanding ripple, like a drop in a pond. BFS finds the path with the fewest edges. On a graph where every edge costs the same (e.g. 4-connectivity, all moves cost 1), fewest-edges is cheapest, so BFS is optimal. But the moment edges have different costs — like our √2 diagonals — "fewest edges" and "cheapest" diverge, and BFS can return a path that uses few but expensive edges. BFS is the right tool only when all moves cost the same.
Dijkstra's algorithm (Stanford calls it best-first search) fixes that by always pulling the frontier node with the smallest cost-of-arrival g:
It greedily commits to the cheapest-so-far node, which guarantees that once you pull a node off the frontier its cost is final — it can never be reached more cheaply later (as long as all edge costs are non-negative, which distances always are). Dijkstra finds the genuinely cheapest path under any non-negative cost function. Its weakness: it explores uniformly outward in all directions, paying no attention to where the goal is. It will happily flood half the map away from the goal before stumbling onto it.
Let's run Dijkstra fully by hand. Five nodes — S (start), A, B, C, G (goal) — with these edge costs:
Initialize: g(S)=0, everything else ∞. Frontier = {S}. We track, each step, which node we pop (smallest g on the frontier), and how it updates its neighbours.
| Step | Pop (min g) | Updates from it | g after step | Frontier after |
|---|---|---|---|---|
| 1 | S (g=0) | A: 0+2=2 · B: 0+5=5 | S0 A2 B5 C∞ G∞ | {A:2, B:5} |
| 2 | A (g=2) | B: 2+1=3 (beats 5!) · C: 2+7=9 | S0 A2 B3 C9 G∞ | {B:3, C:9} |
| 3 | B (g=3) | C: 3+3=6 (beats 9!) · G: 3+6=9 | S0 A2 B3 C6 G9 | {C:6, G:9} |
| 4 | C (g=6) | G: 6+2=8 (beats 9!) | S0 A2 B3 C6 G8 | {G:8} |
| 5 | G (g=8) | goal popped — done | g(G)=8 | {} |
Trace the corrections. At step 1, B's cost is set to 5 (the direct S–B edge). But at step 2, going S→A→B costs only 2+1 = 3, which beats 5 — so B's label is corrected from 5 to 3, and B's parent flips from S to A. That correction then cascades: a cheaper B makes a cheaper C (6 instead of 9), which makes a cheaper G. The final answer: g(G) = 8, via the path S→A→B→C→G. Follow the parents backward from G: G←C←B←A←S. Reverse it and you have the optimal route.
Notice Dijkstra popped every single node — S, A, B, C, G — before finishing. It had no idea G was the goal until it popped it. That blindness is exactly what A* will fix.
python def dijkstra(graph, start, goal): # graph[u] = list of (v, cost) neighbour pairs g = {u: float('inf') for u in graph} # cost-of-arrival, all infinite g[start] = 0.0 parent = {start: None} frontier = [start] # the "alive" / open set while frontier: # GetNext: pop the frontier node with smallest g (Dijkstra's rule) q = min(frontier, key=lambda n: g[n]) frontier.remove(q) if q == goal: break for (qp, c) in graph[q]: cand = g[q] + c # g~(q') candidate cost if cand < g[qp]: # found a cheaper route -> correct the label g[qp] = cand parent[qp] = q if qp not in frontier: frontier.append(qp) # reconstruct path by walking parents backward from goal path, n = [], goal while n is not None: path.append(n); n = parent.get(n) return path[::-1], g[goal]
That linear-scan min(frontier, ...) is O(V) per pop, making the whole thing O(V²) — fine for teaching, slow for big graphs. The idiomatic version replaces the frontier with a binary heap (priority queue) so each pop is O(log V):
python import heapq def dijkstra(graph, start, goal): g = {start: 0.0}; parent = {start: None} pq = [(0.0, start)] # (priority, node); heap keeps min on top while pq: cost, q = heapq.heappop(pq) # O(log V) pop of cheapest node if q == goal: break if cost > g.get(q, float('inf')): continue # stale heap entry, skip for (qp, c) in graph[q]: cand = cost + c if cand < g.get(qp, float('inf')): g[qp] = cand; parent[qp] = q heapq.heappush(pq, (cand, qp)) path, n = [], goal while n is not None: path.append(n); n = parent.get(n) return path[::-1], g.get(goal)
Watch Dijkstra flood below. Press Run and see the explored region (the closed cells) spread outward in roughly circular rings from the start — equal-cost contours — with no bias toward the goal at all. Count how many cells it touches.
Dijkstra expands cells in order of cost-of-arrival — rings of equal distance from the start (teal). It pays no attention to the goal (green), so it floods symmetrically until the goal happens to be reached. The counter shows how many cells were expanded.
Dijkstra's flaw is that it ignores the goal. Imagine searching for a friend's house with no map, exploring every street equidistant from your start before trying any in your friend's direction. Silly — you know roughly which way the house is, so you'd head that way. A heuristic is exactly that "rough sense of which way the goal lies," encoded as a number.
Formally, Stanford introduces the cost-to-go: the true cheapest cost to get from a node q all the way to the goal. We almost never know it (knowing it would mean we'd already solved the problem). So we use an estimate — the heuristic h(q) — a cheap-to-compute guess of how far q is from the goal.
You can't use just any guess. For the search to stay optimal — to keep returning genuinely cheapest paths — the heuristic must satisfy one rule:
where h*(q) is the true cost-to-go. In words: the heuristic must never overestimate the remaining cost — it's always optimistic, a lower bound, a positive underestimate. A heuristic with this property is called admissible.
Why does optimism keep us honest? Intuition: as long as h never claims the goal is farther than it really is, the search can never be fooled into prematurely abandoning a route that is actually the best. If h were allowed to overestimate — to pessimistically inflate the remaining distance of the truly-best route — the search might dismiss that route in favour of a worse one that merely looked closer. Optimism is the safety property. (We'll prove the optimality claim cleanly in Chapter 6.)
For grid planning, two heuristics dominate. Put the goal at grid cell (xg, yg) and the current cell at (x, y). Let dx = |x − xg| and dy = |y − yg|.
Manhattan (L1) distance.
The number of cells you'd cross moving only in cardinal directions — like a taxi on Manhattan's grid streets. Admissible only for 4-connected grids, where you truly can't move diagonally.
Euclidean (L2) distance.
The straight-line "as-the-crow-flies" distance. Always admissible (no path can be shorter than a straight line). Slightly weaker for 4-connected grids, but the natural choice for 8-connectivity.
Worked numbers. Suppose the current cell is (1, 1) and the goal is (4, 5), with unit cells. Then dx = |1−4| = 3 and dy = |1−5| = 4.
On an 8-connected grid, the true shortest distance from (1,1) to (4,5) uses 3 diagonal steps (covering dx=3 and 3 of the dy) then 1 cardinal step: cost = 3√2 + 1 ≈ 5.24. Check admissibility: Euclidean's 5 ≤ 5.24 ✓ (underestimate, safe). Manhattan's 7 > 5.24 ✗ — Manhattan overestimates for an 8-connected grid, so it is not admissible there and could break optimality. This is the single most common heuristic bug: using Manhattan distance on a grid that allows diagonal moves.
There's a sibling property worth knowing. A heuristic is consistent (or monotone) if, for every edge from q to neighbour q′:
This is a triangle inequality: the estimated remaining cost from q can't exceed the cost of one step plus the estimate from where that step lands you. Consistency implies admissibility (it's stronger), and it gives a bonus: with a consistent heuristic, once A* pops a node, its cost is final — the node never needs re-expanding. Both Euclidean and (on 4-connected grids) Manhattan are consistent. Consistency is what lets the efficient "closed set" version of A* in the next chapter skip already-expanded nodes safely.
Play with the heuristic field below. Each cell is tinted by its heuristic value — bright near the goal, dark far away. Switch between Manhattan and Euclidean and watch the contours change shape: Manhattan's iso-lines are diamonds, Euclidean's are circles. The heuristic is the "downhill toward the goal" landscape that A* will sled down.
Every cell is shaded by its heuristic distance to the goal (green) — brighter = closer. Toggle Manhattan vs. Euclidean and watch the iso-contours morph from diamonds to circles. Click any cell to read its exact h-value. This field is the "hint" that points A* at the goal.
Now we marry the two ideas. Dijkstra orders the frontier by cost-behind us, g(q). The heuristic estimates cost-ahead, h(q). A* orders the frontier by their sum:
Read it as: f = (cheapest cost known to reach q) + (estimated cost from q to the goal) = an estimate of the total cost of the best path through q. A* always expands the frontier node with the smallest f. That's the whole change from Dijkstra: swap the priority from g to g + h. With h(q) = 0 for all q, A* is Dijkstra — the heuristic is the only difference.
A* maintains an open set O (the frontier, ordered by f) and a closed set C (nodes already fully expanded, so we never redo them). Here is the loop in words, matching HW2's Algorithm 1:
This is the heart of the lesson. We'll run A* completely by hand — every open-list state, every g, h, f, every parent pointer — on a small grid you can hold in your head.
The grid is 4×4, columns 0–3, rows 0–3. We use 4-connectivity (cardinal moves only, each cost 1) so the arithmetic stays clean, and the Manhattan heuristic (admissible for 4-connectivity). Start is S = (0,0) (bottom-left), goal is G = (3,3) (top-right). Two cells are obstacles: (1,1) and (2,1) — a little wall.
S=(0,0), G=(3,3), obstacles at (1,1) and (2,1). Each free cell is labelled with its Manhattan heuristic h = |x−3| + |y−3|. This is the board we run A* on by hand below.
First, the heuristic for each cell, h(x,y) = |x−3| + |y−3|. For example h(0,0) = 3+3 = 6; h(3,3) = 0; h(0,3) = 3; h(3,0) = 3.
Now the run. Each row is one iteration: we pop the lowest-f open node, push it to closed, and relax its free neighbours. When two open nodes tie on f, we break the tie by preferring the larger g (closer to the goal — a standard tie-break we justify in Chapter 6). I list each open node as cell(g,h,f).
| Iter | Pop (min f) | Neighbours relaxed | Open set after — cell(g,h,f) | Closed |
|---|---|---|---|---|
| 0 | — (init) | start (0,0): g=0, f=0+6=6 | (0,0)(0,6,6) | ∅ |
| 1 | (0,0) f=6 | (1,0): g=1,h=5,f=6 · (0,1): g=1,h=5,f=6 | (1,0)(1,5,6), (0,1)(1,5,6) | (0,0) |
| 2 | (1,0) f=6 | (2,0): g=2,h=4,f=6 · (1,1) is obstacle | (0,1)(1,5,6), (2,0)(2,4,6) | (0,0),(1,0) |
| 3 | (2,0) f=6 | (3,0): g=3,h=3,f=6 · (2,1) is obstacle | (0,1)(1,5,6), (3,0)(3,3,6) | + (2,0) |
| 4 | (3,0) f=6 | (3,1): g=4,h=2,f=6 | (0,1)(1,5,6), (3,1)(4,2,6) | + (3,0) |
| 5 | (3,1) f=6 | (3,2): g=5,h=1,f=6 | (0,1)(1,5,6), (3,2)(5,1,6) | + (3,1) |
| 6 | (3,2) f=6 | (3,3)=G: g=6,h=0,f=6 · (2,2): g=6,h=2,f=8 | (0,1)(1,5,6), (3,3)(6,0,6), (2,2)(6,2,8) | + (3,2) |
| 7 | (3,3) f=6 | goal popped — done | — | + (3,3) |
The path, reconstructed by following parents from G backward: (3,3)←(3,2)←(3,1)←(3,0)←(2,0)←(1,0)←(0,0). Reverse it: (0,0)→(1,0)→(2,0)→(3,0)→(3,1)→(3,2)→(3,3) — cost 6, a clean L-shape sweeping under the little wall.
Now savour the efficiency. Every node A* popped had f = 6 — the exact cost of the optimal path. A* marched straight along the optimal route, never expanding (0,1) at all (it sat in the open set the whole time with f=6 but was never the uniquely-lowest pick under our tie-break, and the goal was reached first). It expanded 7 nodes on a 14-free-cell grid. Dijkstra, on the same grid, would have expanded essentially all of them before reaching G — because every cell within cost 6 of the start gets popped. The heuristic earned its keep.
python import heapq def heuristic(a, b): # Euclidean (admissible for 8-connectivity); use Manhattan for 4-conn return ((a[0]-b[0])**2 + (a[1]-b[1])**2) ** 0.5 def astar(graph, start, goal): g = {start: 0.0}; parent = {start: None} open_set = [(heuristic(start, goal), start)] # priority = f = g + h closed = set() while open_set: f_cur, q = heapq.heappop(open_set) # GetNext: lowest f (the ONLY change vs Dijkstra) if q == goal: break if q in closed: continue closed.add(q) for (qp, c) in graph[q]: if qp in closed: continue cand = g[q] + c # g~ tentative cost-to-arrive if cand < g.get(qp, float('inf')): g[qp] = cand; parent[qp] = q f = cand + heuristic(qp, goal) # f = g + h heapq.heappush(open_set, (f, qp)) path, n = [], goal while n is not None: path.append(n); n = parent.get(n) return path[::-1] if path[-1] == start else None
The library version is the same logic, just leaning on heapq for the open set and a dict for g/parents — which is exactly what production planners (and Stanford's P1_astar.py) do. The four functions HW2 asks you to fill in — is_free, distance, get_neighbors, solve — map one-to-one onto: the collision check, the edge cost, the graph's neighbour rule, and this very loop.
A planner you can't trust is worse than no planner — it'll confidently drive the robot into a wall or up a wasteful detour. So we owe ourselves two guarantees, and A* delivers both. Stanford states them as theorems; let's understand why they hold, not just that they do.
An algorithm is complete if, whenever a solution exists, it finds one in finite time; and if none exists, it reports failure. A* is complete on a finite graph. The argument is short: A* never re-expands a closed node, and there are finitely many nodes, so it can only run for finitely many iterations — it must terminate. If a path from start to goal exists, the goal is reachable, so it will eventually enter the open set and be popped (its f is finite while unreachable nodes stay at ∞). If no path exists, the open set drains without ever reaching the goal, and A* correctly returns failure.
An algorithm is optimal if the path it returns has the lowest possible cost. A* is optimal when its heuristic is admissible (and, for the closed-set version, consistent). Here's the clean argument.
Suppose A* is about to pop the goal G, and call the path it found P with cost g(G). Because G is being popped, its f(G) = g(G) + h(G) = g(G) + 0 = g(G) is the smallest f on the open set. Now suppose, for contradiction, there were a cheaper path P* with cost C* < g(G). Somewhere on P* there must sit a node n that's currently in the open set (the first unexpanded node along P*). For that node:
The middle step uses admissibility: h(n) ≤ h*(n), the true cost-to-go. And g(n) + h*(n) is exactly the cost of P* (cost to reach n, plus true cost from n to G). So f(n) ≤ C* < g(G) = f(G). But then n has a strictly smaller f than G, so A* would have popped n before G — contradicting that G was popped first. Therefore no cheaper path exists: the path A* returned is optimal. Optimism (admissibility) is the load-bearing assumption.
Optimality is the guarantee; efficiency is the reward. The better (closer to h*) the heuristic, the fewer nodes A* expands. Three regimes:
| Heuristic | What A* becomes | Nodes expanded | Path quality |
|---|---|---|---|
| h = 0 | Dijkstra (blind) | most — floods all directions | optimal |
| 0 < h ≤ h* (admissible) | A* | fewer — biased toward goal | optimal |
| h = h* (perfect) | A* with an oracle | fewest — only optimal-path nodes | optimal |
On our 4×4 hand-run, Manhattan happened to be perfect, so A* expanded only the 7 nodes on the optimal route. Real heuristics live in the middle: better than blind, worse than oracle. A* expands a "teardrop" of nodes bulging toward the goal — you'll see this vividly in the showcase.
When several open nodes share the same f, which to pop? It doesn't affect optimality (any pick still returns a cheapest path) but it dramatically affects how many nodes you touch. The standard trick: among equal-f nodes, prefer the one with larger g (equivalently, smaller h) — the node deeper along its path, closer to the goal. This pushes the search to finish a promising route rather than fan out across many equal-f alternatives. (That's the tie-break we used in the hand-run, which is why A* stayed glued to the optimal path.) A common implementation: store (f, h) as the priority, so ties on f resolve by smaller h.
A* is optimal, but optimality costs node expansions, and on a huge map even A* can be too slow for a robot that must replan ten times a second. Engineers routinely accept a slightly longer path in exchange for a much faster search. The dial that controls this trade is a single weight on the heuristic.
Multiply the heuristic by a weight ε ≥ 1:
With ε = 1 this is ordinary A*. Crank ε up and you over-weight the "head toward the goal" term relative to the "honest cost so far" term — the search becomes greedier, drives harder at the goal, and expands fewer nodes. The catch: you may inflate an admissible h into an inadmissible ε·h, losing the optimality guarantee. But not by an unbounded amount — there's a clean promise:
Push the dial all the way: ignore g entirely and order the frontier by h alone.
This is "always step toward whatever looks closest to the goal." It's blisteringly fast and expands very few nodes — but it has thrown away all memory of cost-so-far, so it cheerfully takes long, silly detours and is not optimal (and can even get stuck behind concave obstacles, taking far longer routes). Greedy is the ε → ∞ limit of weighted A*.
Picture the same open map, start lower-left, goal upper-right, one obstacle between them. Roughly what each method does:
| Method | Priority | Nodes expanded (illustrative) | Path cost | Optimal? |
|---|---|---|---|---|
| Dijkstra | g | ~450 (floods everywhere) | 28.3 (best) | yes |
| A* (ε=1) | g + h | ~120 (teardrop to goal) | 28.3 (best) | yes |
| Weighted A* (ε=1.5) | g + 1.5h | ~55 (narrow funnel) | 29.1 (≤ 1.5×best) | bounded |
| Greedy best-first | h | ~35 (beeline) | 34.0 (detours) | no |
The numbers are illustrative, but the shape is the universal truth: as you weight h more, expansions fall and path cost rises. Dijkstra and A* sit at the optimal end (A* far cheaper to compute); greedy sits at the fast-but-sloppy end; weighted A* lets you pick any point in between with a guaranteed worst case.
Below, run all four methods on the same map and watch the expanded region shrink — and the path quality degrade — as you move from Dijkstra toward greedy. The expansion counter and path cost update live so you can read the trade-off directly.
Same map, four search rules. Pick a method and press Run. Dim cells = expanded (touched), warm = frontier when it finished, green = the path found. Watch expanded drop and path cost rise as you weight the heuristic more heavily.
Everything comes together here. Paint your own obstacles, place the start and goal wherever you like, choose a search rule, and press Run — then watch the frontier expand cell by cell. Open-set cells glow warm, fully-expanded (closed) cells fade dim, and the final path lights up green. The expansion counter is the scoreboard: the same path, found by touching fewer cells, is the whole game of informed search.
Click/drag to paint or erase obstacles. Use the mode buttons to instead place the start (teal) or goal (green). Pick a heuristic and weight, then Run. Warm = open frontier, dim = closed/expanded, green = path. The counter shows cells expanded — lower is a smarter search. Build a U-wall to watch the flood pour around it.
Try this sequence to see the chapters. (1) Set the weight to 1 and heuristic to Dijkstra: a fat symmetric flood, many expansions. (2) Switch to Euclidean, weight 1: the same optimal path, but a teardrop biased toward the goal — far fewer expansions. (3) Crank the weight to 3: a narrow funnel firing at the goal, fewest expansions, but now build a wall in its way and watch the path get a little longer than optimal — the bounded-suboptimality trade from Chapter 7. (4) Wall the goal off completely and run: the flood fills the reachable region and reports no path — A*'s completeness, returning failure cleanly.
The expansion counter is the punchline of the entire lesson. All four settings return a valid path (when one exists); they differ only in how hard they had to think. Dijkstra is the honest grind; A* is the same answer with a compass; weighted A* and greedy trade a little path quality for a lot of speed. Informed search is the art of finding the best path while looking at as few possibilities as you can get away with.
The 4-connected grid was a teaching device. Real robots demand more, and a few practical refinements separate a textbook A* from one that drives an actual car. None of them change the algorithm — they change the graph A* searches.
A 4- or 8-connected grid lets the robot teleport sideways instant-turns — fine for a disk, impossible for a car that can't pivot in place. The fix: build the graph from motion primitives, short, dynamically-feasible motion snippets the robot can actually execute (a gentle left arc, a straight segment, a tight right). Lay these primitives down on a grid of states that now includes heading — a state lattice — and A*'s edges become real maneuvers, not magic sideways jumps. A* doesn't change at all; only the neighbour function (get_neighbors) and edge cost (distance) do. The path A* returns is then automatically drivable.
is_free, distance, get_neighbors, and solve. The search (solve) is fixed and reusable; the robot-specific physics live entirely in the other three. Swap a disk for a car by rewriting get_neighbors to emit motion primitives and distance to charge for them — the optimal search loop never changes. That clean interface is exactly the autonomy-stack "typed arrows" idea from Lesson 1.A robot's map is rarely complete or static. A pedestrian steps out; a door that was open is now shut; the robot discovers an obstacle its sensors couldn't see from the start. The naive response — rerun A* from scratch every time anything changes — works but wastes enormous effort recomputing a plan that mostly didn't change.
Incremental replanners fix this. D*-Lite (and its ancestor D*) is the standard: it runs A*-like search backward from the goal and, when an edge cost changes, surgically repairs only the affected part of the search tree instead of starting over. The Mars rovers used D* family planners. You don't need its internals today — just know that "A*, but reuses previous work when the map changes a little" is a named, solved problem, and it's what runs on robots that replan continuously.
Grid A* is a champion in low dimensions, but Stanford is blunt about its ceiling: grid size is exponential in the number of degrees of freedom. A planar disk is 2D (x, y) — a few thousand cells, trivial. A car with heading is 3D. A 7-joint robot arm is 7-dimensional: a modest 100-step grid would have 1007 = 1014 cells. No computer can build, let alone search, that grid. This is the curse of dimensionality, and it's a wall, not a speed bump.
A nice bridge: HW2's A* and RRT solve the same 2D planning problem. A* discretizes deterministically with a grid and uses the heuristic for guidance; RRT samples the continuous space randomly and grows a tree toward those samples (with goal-biasing as a poor-man's heuristic). Both produce a graph, both reconstruct a path through it — they just build the graph differently. When the dimension is low and you want optimality, reach for A*; when it's high or the geometry is nasty, reach for RRT.
You can now take "go to the kitchen," turn the cluttered world into a graph, and find the provably-cheapest collision-free route through it — expanding far fewer nodes than a blind search. That is the core of the Motion Planning module of the autonomy stack from Lesson 1 (the warm band): map + goal → collision-free trajectory.
| Algorithm | Frontier ordered by | Optimal? | Expansions | Use when |
|---|---|---|---|---|
| BFS | insertion order (FIFO) | only if all edge costs equal | many | unweighted graphs, fewest-edges path |
| Dijkstra | g (cost-of-arrival) | yes (non-neg costs) | most — floods outward | no goal hint available; multiple goals |
| A* | f = g + h | yes (admissible h) | fewer — biased to goal | single goal + a good heuristic |
| Weighted A* | f = g + εh, ε>1 | bounded: ≤ ε·C* | fewest while still bounded | need speed, can accept ≤ε× longer |
| Greedy best-first | h alone | no | very few | wide-open maps, speed over quality |
On the autonomy stack, A* is the Planning layer: it consumes the map (from Localization & SLAM, Lessons 11–14) and a goal, and produces a collision-free trajectory for the Control layer (Lessons 2, 6) to track. HW2 ties this whole arc together: Problem 1 is the A* you just learned; Problem 2 is RRT; Problem 3 smooths the A* path into a trackable trajectory and hands it to the Problem-Set-1 controllers.