Path Planning
How a robot finds a way from A to B: grids and configuration space, breadth-first search, Dijkstra and A* with heuristics, random trees and obstacle avoidance — with a planner you can watch step by step.
What is path planning?
Path planning is finding a route for a robot from a start to a goal that avoids obstacles and, usually, is as short or as cheap as possible. A warehouse robot plans a route to the next shelf, a vacuum cleaner to the next unswept room, a robot arm a motion that does not hit the fixture next to it.
A planner works on a model of the world: for a mobile robot usually the occupancy grid that its localization system keeps (see Localization and Mapping). Its result is a path: a list of positions. Turning that into a trajectory, positions with times and speeds the motors can follow, is the job of motion control (see Motion Control and PID).
Real systems split the work in two:
- A global planner plans the whole route on the map, from here to the goal. It may take a little time and is rerun now and then.
- A local planner looks only a few meters ahead, many times per second, and picks the next speed and turn that follow the global path while avoiding what the sensors see right now: a person, a cart, a box that is not on the map.
Nav2, the navigation framework of ROS 2, is built this way, with a global planner and a local controller (see ROS 2). Most of this page is about the global planner; the last section is about the local one.
Maps as graphs
Search algorithms work on a graph: a set of points (vertices) joined by connections (edges), each with a cost. A grid map becomes a graph in the obvious way: every free cell is a vertex, and every move to a neighboring free cell is an edge (see weighted graphs).
On a 4-connected grid the robot moves only along the rows and columns, each step costing 1. On an 8-connected grid it may also move diagonally, at a cost of √2 per step (see the Pythagorean theorem). 8-connected paths are shorter and look more natural; both versions still zigzag a little, which is why planners often smooth the result afterward.
Grids are not the only choice. A roadmap is a sparse graph of chosen points, such as junctions of warehouse aisles or the corners of obstacles, joined by straight free lines. It is much smaller than a grid, and fine when the routes are fixed anyway.
Configuration space
A robot is not a point: a round robot 0.4 m across cannot pass through a gap of 0.3 m, even if the grid shows free cells there. The trick is to plan in the configuration space (C-space): the space of all configurations the robot can take, in which every obstacle is grown by the robot's own size. For a round robot that means growing every obstacle by its radius. In that space the robot is a single point at its center, and every algorithm below can treat it as one.
import math
# A 2 m x 0.8 m strip of floor in 0.1 m cells, with a shelf and a pillar.
MAP = ["....................",
"....................",
"....####............",
"....####.......#....",
"...............#....",
"....................",
"....................",
"...................."]
CELL = 0.1 # meters per cell
ROBOT_RADIUS = 0.2 # a round robot, 0.4 m across
def inflate(rows, radius):
"""Mark every cell within `radius` of an obstacle: configuration space."""
reach = math.ceil(radius / CELL)
out = [list(row) for row in rows]
for r, row in enumerate(rows):
for c, ch in enumerate(row):
if ch != "#":
continue
for dr in range(-reach, reach + 1):
for dc in range(-reach, reach + 1):
rr, cc = r + dr, c + dc
inside = 0 <= rr < len(rows) and 0 <= cc < len(row)
if inside and math.hypot(dr, dc) * CELL <= radius and out[rr][cc] == ".":
out[rr][cc] = "+"
return ["".join(row) for row in out]
for plain, grown in zip(MAP, inflate(MAP, ROBOT_RADIUS)):
print(plain, " ", grown).................... ....++++............ .................... ...++++++......+.... ....####............ ..++####++....+++... ....####.......#.... ..++####++...++#++.. ...............#.... ...++++++....++#++.. .................... ....++++......+++... .................... ...............+.... .................... ....................
The 0.7 m gap between shelf and pillar shrinks to three free cells in C-space: the robot's center can pass there, so the robot can. Real navigation stacks inflate a little more than the radius, and mark a soft zone around it that is allowed but expensive, so paths keep a comfortable distance from walls instead of scraping along them.
For a robot arm, the configuration is a list of joint angles, so its C-space has one dimension per joint: six for a typical industrial arm (see Kinematics). Obstacles in that space have complicated shapes that nobody draws; the planner only asks, point by point, "would the arm collide in this configuration?". That is where the sampling planners further down come in.
Breadth-first search
The simplest way to find a route on a grid where every step costs the same is breadth-first search (BFS): explore the start's neighbors first, then their neighbors, and so on, in rings of growing distance, until the goal turns up. The cells waiting to be explored sit in a queue: first in, first out. Each cell remembers which cell it was reached from, so the route can be read backward from the goal. Because the rings grow one step at a time, the first route to reach the goal has the fewest possible steps.
The example searches a 5 × 5 grid with walls (#), from
S in the top-left corner to G in the
bottom-right one, moving 4-connected:
from collections import deque
GRID = ["S....",
".###.",
"...#.",
".#...",
"...#G"]
def find(ch):
for r, row in enumerate(GRID):
if ch in row:
return (r, row.index(ch))
def neighbors(cell):
r, c = cell
for dr, dc in ((1, 0), (-1, 0), (0, 1), (0, -1)): # 4-connected
nr, nc = r + dr, c + dc
if 0 <= nr < 5 and 0 <= nc < 5 and GRID[nr][nc] != "#":
yield (nr, nc)
def bfs(start, goal):
came_from = {start: None}
queue = deque([start])
expanded = 0
while queue:
cell = queue.popleft() # first in, first out: ring by ring
expanded += 1
if cell == goal:
break
for nxt in neighbors(cell):
if nxt not in came_from: # not seen before
came_from[nxt] = cell
queue.append(nxt)
path = []
while cell is not None:
path.append(cell)
cell = came_from[cell]
return path[::-1], expanded
path, expanded = bfs(find("S"), find("G"))
print(f"{len(path) - 1} steps, {expanded} cells expanded")
for r, row in enumerate(GRID):
print("".join("*" if (r, c) in path and ch == "." else ch for c, ch in enumerate(row)))8 steps, 19 cells expanded S.... *###. ***#. .#*** ...#G
Eight steps is the minimum here, but not the only route of that length: going along the top row and down the right side is also eight steps. Which one a search returns depends on the order in which it tries the neighbors. BFS has one limit: it counts steps, not costs. As soon as moves cost different amounts (diagonals, rough floor, a busy aisle to avoid), the fewest steps is no longer the cheapest route.
Dijkstra's algorithm
Dijkstra's algorithm, published by Edsger W. Dijkstra in 1959 ("A note on two problems in connexion with graphs", Numerische Mathematik), handles costs. Instead of a plain queue it keeps a priority queue ordered by g(n), the cheapest known cost from the start to cell n, and always expands the cell with the lowest g. When a cell is expanded, its cost is final. The math page works through it by hand (Dijkstra by hand); the same algorithm also picks routes in computer networks.
Dijkstra explores in rings too, only now the rings are of equal cost, not equal step count. It searches in every direction equally, toward the goal and away from it, because it has no idea where the goal is. On a big map that is wasted work, and A* fixes exactly that.
A* search
A* search (pronounced "A star") is Dijkstra's algorithm with a sense of direction. Peter Hart, Nils Nilsson and Bertram Raphael published it in 1968 ("A Formal Basis for the Heuristic Determination of Minimum Cost Paths", IEEE Transactions on Systems Science and Cybernetics); it came out of the Shakey robot project (see What Is Robotics?). For every cell it adds two numbers:
g(n) is the known cost from the start to n, as in Dijkstra; h(n) is an estimate of the cost still to go from n to the goal. A* always expands the open cell with the lowest f: the cell that lies on the cheapest-looking complete route. Cells in the direction of the goal look cheaper and are explored first.
Two special cases show what the heuristic does. With h = 0, A* is exactly Dijkstra's algorithm. With f = h alone, ignoring the cost so far, it becomes greedy best-first search: very fast toward the goal, but easily fooled into a long route.
The example runs A* with
Python's
heapq as the priority queue, on the same grid as BFS.
Entries are (f, g, cell) tuples, so ties are broken in
a fixed order and the output is repeatable:
import heapq
GRID = ["S....",
".###.",
"...#.",
".#...",
"...#G"]
START, GOAL = (0, 0), (4, 4)
def neighbors(cell):
r, c = cell
for dr, dc in ((1, 0), (-1, 0), (0, 1), (0, -1)):
nr, nc = r + dr, c + dc
if 0 <= nr < 5 and 0 <= nc < 5 and GRID[nr][nc] != "#":
yield (nr, nc)
def manhattan(cell):
return abs(cell[0] - GOAL[0]) + abs(cell[1] - GOAL[1])
def astar(h):
open_heap = [(h(START), 0, START)] # (f, g, cell): ties go to the lower g
g = {START: 0}
came_from = {START: None}
expanded = 0
while open_heap:
f, cost, cell = heapq.heappop(open_heap)
if cost > g[cell]:
continue # an outdated entry: skip it
expanded += 1
if cell == GOAL:
path = []
while cell is not None:
path.append(cell)
cell = came_from[cell]
return path[::-1], cost, expanded
for nxt in neighbors(cell):
new_cost = cost + 1
if new_cost < g.get(nxt, float("inf")):
g[nxt] = new_cost
came_from[nxt] = cell
heapq.heappush(open_heap, (new_cost + h(nxt), new_cost, nxt))
return None, None, expanded
path, cost, expanded = astar(manhattan)
print(f"A* (Manhattan): cost {cost}, {expanded} expanded")
print("path:", path)
path, cost, expanded = astar(lambda cell: 0)
print(f"Dijkstra (h = 0): cost {cost}, {expanded} expanded")A* (Manhattan): cost 8, 19 expanded path: [(0, 0), (0, 1), (0, 2), (0, 3), (0, 4), (1, 4), (2, 4), (3, 4), (4, 4)] Dijkstra (h = 0): cost 8, 19 expanded
A* found the other eight-step route, along the top. It expanded 19 cells, exactly as many as Dijkstra, and that is every free cell of this grid: in a small maze every corridor might be the right one, and no estimate can rule any of them out. The heuristic pays off on open maps with an obstacle or two, which is where the simulation below shows a much larger difference.
Heuristics and admissibility
A heuristic function is a rule of thumb that estimates, quickly, the cost from a cell to the goal. On grids the usual ones use the column and row differences \Delta x and \Delta y to the goal:
The important property is that the estimate is admissible: it never overestimates the real remaining cost. Hart, Nilsson and Raphael proved that A* with an admissible heuristic always returns a cheapest route. The reason in one sentence: a cell on the cheapest route always looks at least as good as it really is, so A* cannot settle for a worse route while that cell is still waiting.
- On a 4-connected grid, Manhattan distance is admissible and the best choice: you cannot get there in fewer straight steps than the column and row differences.
- On an 8-connected grid, Manhattan is not admissible: one diagonal step costs √2 ≈ 1.41, but Manhattan counts it as 2. Euclidean distance (the straight-line distance) is admissible on any grid. The octile distance, \max(|\Delta x|,|\Delta y|) + (\sqrt{2} - 1)\min(|\Delta x|,|\Delta y|), is the exact cost on an empty 8-connected grid, and therefore the tightest admissible choice there.
- h = 0 is always admissible, and makes A* into Dijkstra: correct, but with no sense of direction.
The closer an admissible heuristic comes to the real cost, the fewer cells A* expands. An overestimating heuristic often finds a route faster still, but may miss the cheapest one. The example checks that on 200 random maps, comparing Manhattan with Euclidean on an 8-connected grid:
import heapq
import math
import random
# Is Manhattan distance safe on an 8-connected grid? Compare it with the
# Euclidean heuristic (admissible) on random maps with walls on 28% of cells.
COLS, ROWS = 24, 16
START, GOAL = (2, 8), (21, 8)
def neighbors(walls, cell):
x, y = cell
free = lambda cx, cy: 0 <= cx < COLS and 0 <= cy < ROWS and (cx, cy) not in walls
for dx, dy in [(1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (1, -1), (-1, 1), (-1, -1)]:
diagonal = dx != 0 and dy != 0
if free(x + dx, y + dy) and (not diagonal or (free(x + dx, y) and free(x, y + dy))):
yield (x + dx, y + dy), math.sqrt(2) if diagonal else 1
def path_cost(walls, h):
"""A* returning the cost of the path it finds (None if there is no path)."""
g, closed, count = {START: 0}, set(), 0
heap = [(h(START), 0, START)]
while heap:
_, _, cell = heapq.heappop(heap)
if cell in closed:
continue
if cell == GOAL:
return g[cell]
closed.add(cell)
for nxt, cost in neighbors(walls, cell):
if nxt not in closed and g[cell] + cost < g.get(nxt, math.inf):
g[nxt] = g[cell] + cost
count += 1 # a counter breaks ties: stable order
heapq.heappush(heap, (g[nxt] + h(nxt), count, nxt))
return None
euclidean = lambda c: math.hypot(c[0] - GOAL[0], c[1] - GOAL[1])
manhattan = lambda c: abs(c[0] - GOAL[0]) + abs(c[1] - GOAL[1])
random.seed(2026)
maps = longer = 0
worst = 0.0
while maps < 200:
walls = {(x, y) for x in range(COLS) for y in range(ROWS)
if random.random() < 0.28 and (x, y) not in (START, GOAL)}
best = path_cost(walls, euclidean)
if best is None:
continue # walled in: try another map
maps += 1
found = path_cost(walls, manhattan)
if found > best + 1e-9:
longer += 1
worst = max(worst, found - best)
print(f"{maps} maps with a path: Manhattan found a longer path on {longer}, "
f"at most {worst:.2f} longer")200 maps with a path: Manhattan found a longer path on 23, at most 2.59 longer
On most maps the overestimate does no harm, which is exactly what makes it a trap: a planner can look fine in testing and still return a longer route on one map in nine. When a route must be the shortest, use an admissible heuristic; when "short enough, found fast" will do, some planners deliberately inflate the heuristic and accept a known bound on the extra cost.
A* on a bigger map
Here is a Python version of the simulation's ROBOT MATH: the same 24 × 16 demo map (a cup that opens toward the start), the same costs, no corner cutting, and the same tie rules (lowest f, then lowest h, then the oldest open cell). It counts the expanded cells for each heuristic and move set; the counter in the simulation's readout should show the same numbers:
import math
# A Python version of the simulation's ROBOT MATH: the same 24 x 16 demo map
# (a cup that opens toward the start), the same costs and the same tie rules.
COLS, ROWS = 24, 16
walls = set()
for y in range(3, 13):
walls.add((14, y)) # the bottom of the cup
for x in range(9, 15):
walls.add((x, 3)) # its two sides
walls.add((x, 12))
START, GOAL = (2, 8), (21, 8)
HEURISTICS = {
"zero": lambda dx, dy: 0, # A* with h = 0 is Dijkstra's algorithm
"manhattan": lambda dx, dy: dx + dy,
"euclidean": lambda dx, dy: math.hypot(dx, dy),
}
def neighbors(cell, diagonal):
x, y = cell
free = lambda cx, cy: 0 <= cx < COLS and 0 <= cy < ROWS and (cx, cy) not in walls
for dy in (-1, 0, 1):
for dx in (-1, 0, 1):
straight = dx == 0 or dy == 0
if (dx, dy) == (0, 0) or (not straight and not diagonal):
continue
if not free(x + dx, y + dy):
continue
if not straight and not (free(x + dx, y) and free(x, y + dy)):
continue # no cutting corners
yield (x + dx, y + dy), 1 if straight else math.sqrt(2)
def search(name, diagonal):
h = lambda c: HEURISTICS[name](abs(c[0] - GOAL[0]), abs(c[1] - GOAL[1]))
g, parent, closed, open_list = {START: 0}, {START: None}, set(), [START]
while open_list:
# The open cell with the lowest f = g + h; ties: lowest h, then the oldest.
best = 0
for k in range(1, len(open_list)):
a, b = open_list[k], open_list[best]
fa, fb = g[a] + h(a), g[b] + h(b)
if fa < fb - 1e-9 or (abs(fa - fb) <= 1e-9 and h(a) < h(b)):
best = k
current = open_list.pop(best)
closed.add(current)
if current == GOAL:
steps = 0
while parent[current]:
current, steps = parent[current], steps + 1
return len(closed), steps, g[GOAL]
for nxt, cost in neighbors(current, diagonal):
if nxt in closed:
continue
if g[current] + cost < g.get(nxt, math.inf) - 1e-9:
if nxt not in g:
open_list.append(nxt)
g[nxt], parent[nxt] = g[current] + cost, current
return len(closed), None, None
for diagonal in (False, True):
for name in ("zero", "euclidean", "manhattan"):
expanded, steps, cost = search(name, diagonal)
print(f"{8 if diagonal else 4}-connected, {name:9}: expanded {expanded:3}, "
f"path {steps} steps, cost {cost:.2f}")4-connected, zero : expanded 353, path 29 steps, cost 29.00 4-connected, euclidean: expanded 252, path 29 steps, cost 29.00 4-connected, manhattan: expanded 135, path 29 steps, cost 29.00 8-connected, zero : expanded 339, path 19 steps, cost 23.14 8-connected, euclidean: expanded 134, path 19 steps, cost 23.14 8-connected, manhattan: expanded 124, path 19 steps, cost 23.14
Within each move set, every heuristic finds a route of the same cost, but the work differs by a factor of about two and a half: Dijkstra searches 339 cells on the 8-connected map, A* with Euclidean distance 134. On 4-connected moves Manhattan is admissible and tighter than Euclidean, so it wins clearly. On 8-connected moves Manhattan expands the fewest cells and happens to find the cheapest route on this map, but it is not admissible there, as the random maps above showed. The open list here is a plain list scanned for its minimum, as in the simulation: easy to follow, and fast enough for 384 cells. A binary heap, as in the earlier examples, is the right choice for big maps (see Big O notation).
Try it: watch A* search
The simulation below runs A* on a 24 × 16 grid, one expansion at a time, and then lets a small robot drive the path it found. It is a small model of a planner on a grid, not a real robot: the grid is already the robot's configuration space, and the robot is a point.
How to use it
Press Play first. Keys only reach the simulation after you click in the frame once or move to the drawing with Tab. The frame follows your browser's light or dark mode, not this site's theme switch.
| Mouse or touch | Keyboard (Tab to the grid) |
|---|---|
| Press on a free cell and drag to draw walls; press on a wall and drag to erase | Arrow keys move a cursor (an accent square); Space toggles a wall |
| Drag the start (filled dot) or the goal (ring with a cross); neither can go onto a wall | S sets the start, G the goal |
| Buttons Step, Run/Pause and Reset | Enter takes one step, R runs or pauses |
| Buttons Clear walls and Random walls (28% of the cells); selects Heuristic and Moves; slider Speed (1–120 steps per second) | The selects and the slider work with the keyboard as usual |
The colors: walls in ink, the closed set (cells already expanded) light gray, the open set (found, not yet expanded) a light accent tint, the cell being expanded a stronger tint, and the final path tinted with an accent line through it. The readout counts the expanded and open cells, the path's steps and its cost, and says whether the chosen heuristic is admissible. Any edit or setting change clears the search; Run after a finished search starts again.
<main class="sim">
<div class="stage">
<canvas
id="view"
tabindex="0"
aria-label="Grid map for path planning. Draw walls with the mouse, or use the arrow keys and Space. S sets the start, G the goal, Enter takes one step, R runs."
></canvas>
<p id="status" class="status" aria-live="polite"></p>
</div>
<div class="panel">
<div class="buttons">
<button id="step" type="button">Step</button>
<button id="run" type="button">Run</button>
<button id="reset" type="button">Reset</button>
</div>
<div class="group">
<label for="heuristic">Heuristic</label>
<select id="heuristic">
<option value="manhattan">Manhattan</option>
<option value="euclidean" selected>Euclidean</option>
<option value="zero">Zero (Dijkstra)</option>
</select>
<label for="moves">Moves</label>
<select id="moves">
<option value="4">4-connected</option>
<option value="8" selected>8-connected</option>
</select>
<label for="speed"
>Speed <output id="speed-out">20 steps/s</output></label
>
<input type="range" id="speed" min="1" max="120" step="1" value="20" />
</div>
<div class="buttons">
<button id="clear" type="button">Clear walls</button>
<button id="random" type="button">Random walls</button>
</div>
<pre id="readout" class="readout" aria-live="off"></pre>
<p class="help">
Mouse: draw or erase walls, drag the start (dot) and the goal (ring). Keys
on the grid: arrows move the cursor, Space toggles a wall, S start, G
goal, Enter step, R run.
</p>
</div>
</main>/* All colors in one place. script.js reads --paper, --ink, --grid,
--soft and --accent from here, so the drawing follows dark mode too. */
:root {
--paper: #ffffff;
--ink: #111111;
--muted: #666666;
--grid: #e4e4e4;
--soft: #b8b8b8;
--accent: #677449;
--font: system-ui, -apple-system, "Segoe UI", sans-serif;
--mono: ui-monospace, "SFMono-Regular", Menlo, Consolas, monospace;
color-scheme: light;
}
@media (prefers-color-scheme: dark) {
:root {
--paper: #161616;
--ink: #ededed;
--muted: #a3a3a3;
--grid: #2a2a2a;
--soft: #5a5a5a;
--accent: #a6b082;
color-scheme: dark;
}
}
html,
body {
height: 100%;
margin: 0;
background: var(--paper);
color: var(--ink);
font: 13px/1.4 var(--font);
}
/* Drawing on the left, controls on the right; stacked on a phone. */
.sim {
display: grid;
grid-template-columns: minmax(0, 1fr) 15rem;
height: 100%;
}
.stage {
position: relative;
min-height: 0;
}
.buttons {
display: flex;
flex-wrap: wrap;
gap: 6px;
}
.buttons button {
flex: 1;
}
#view {
display: block;
min-height: 0;
width: 100%;
height: 100%;
cursor: crosshair;
touch-action: none; /* a finger draws walls, not scrolls the page */
}
#view:focus-visible {
outline: 1px solid var(--ink);
outline-offset: -4px;
}
.status {
position: absolute;
left: 12px;
top: 8px;
margin: 0;
font: 12px/1.4 var(--mono);
color: var(--accent);
pointer-events: none;
}
.panel {
display: flex;
flex-direction: column;
gap: 10px;
padding: 12px 14px;
overflow-y: auto;
border-left: 1px solid var(--ink);
}
.group {
display: grid;
gap: 4px;
}
.group label:not(.check) {
display: flex;
justify-content: space-between;
}
output {
font-family: var(--mono);
}
input[type="range"] {
width: 100%;
margin: 0 0 6px;
accent-color: var(--accent);
}
select {
padding: 4px 6px;
font: inherit;
color: var(--ink);
background: var(--paper);
border: 1px solid var(--ink);
border-radius: 0;
}
.group select {
margin-bottom: 6px;
}
.help {
margin: 0;
font-size: 12px;
color: var(--muted);
}
button {
padding: 5px 10px;
font: inherit;
color: var(--ink);
background: transparent;
border: 1px solid var(--ink);
border-radius: 0;
cursor: pointer;
}
button:hover {
background: var(--grid);
}
button:focus-visible,
select:focus-visible,
input:focus-visible {
outline: 1px solid var(--accent);
outline-offset: 2px;
}
.readout {
margin: 0;
padding: 8px;
font: 12px/1.5 var(--mono);
white-space: pre;
border: 1px solid var(--grid);
}
.note {
margin: 0;
padding: 24px;
color: var(--muted);
}
@media (max-width: 560px) {
.sim {
grid-template-columns: 1fr;
grid-template-rows: minmax(300px, 70vh) auto;
height: auto;
}
.panel {
border-left: 0;
border-top: 1px solid var(--ink);
}
}import * as THREE from "three";
// ---- ROBOT MATH (pure functions: no Three.js, no DOM) ----
// A* on a grid. Cells are numbered row by row: i = y * cols + x, with
// (0, 0) at the top left. walls[i] is 1 for a wall, 0 for free floor.
// A straight move costs 1, a diagonal move √2.
const COLS = 24;
const ROWS = 16;
const DRIVE_SPEED = 4; // cells per second for the little robot
const MAX_STEP = 0.05; // longest time step we accept, in seconds
const HEURISTICS = {
manhattan: (dx, dy) => dx + dy,
euclidean: (dx, dy) => Math.hypot(dx, dy),
zero: () => 0, // A* with h = 0 is Dijkstra's algorithm
};
const NAMES = { manhattan: "Manhattan", euclidean: "Euclidean", zero: "Zero" };
/** The estimate from cell i to the goal, by name. */
function heuristic(name, i, goal, cols = COLS) {
const dx = Math.abs((i % cols) - (goal % cols));
const dy = Math.abs(Math.floor(i / cols) - Math.floor(goal / cols));
return HEURISTICS[name](dx, dy);
}
/** Admissible = never overestimates. Manhattan does on diagonal moves. */
function isAdmissible(name, diagonal) {
return !(name === "manhattan" && diagonal);
}
/** Free neighbors of cell i with the cost to step there. Diagonal steps
* may not cut a corner: both cells beside the step must be free too. */
function neighbors(walls, cols, rows, i, diagonal) {
const x = i % cols;
const y = Math.floor(i / cols);
const free = (cx, cy) =>
cx >= 0 && cy >= 0 && cx < cols && cy < rows && !walls[cy * cols + cx];
const result = [];
for (let dy = -1; dy <= 1; dy++) {
for (let dx = -1; dx <= 1; dx++) {
const straight = dx === 0 || dy === 0;
if ((dx === 0 && dy === 0) || (!straight && !diagonal)) continue;
if (!free(x + dx, y + dy)) continue;
if (!straight && !(free(x + dx, y) && free(x, y + dy))) continue;
result.push({ i: i + dy * cols + dx, cost: straight ? 1 : Math.SQRT2 });
}
}
return result;
}
/** A new search; stepSearch() then expands one cell at a time. */
function createSearch(walls, cols, rows, start, goal, options) {
const n = cols * rows;
const search = {
walls,
cols,
rows,
start,
goal,
heuristic: options.heuristic,
diagonal: options.diagonal,
g: new Float64Array(n).fill(Infinity), // best known cost from the start
parent: new Int32Array(n).fill(-1),
closed: new Uint8Array(n), // 1 = expanded, final
open: [start], // cells found but not expanded yet
expanded: 0,
status: "searching", // then "found" or "no path"
path: null,
current: -1,
};
search.g[start] = 0;
return search;
}
/** f = g + h: the cost so far plus the estimate of what is left. */
function fScore(search, i) {
const h = heuristic(search.heuristic, i, search.goal, search.cols);
return { f: search.g[i] + h, h };
}
/** Expands the open cell with the lowest f (ties: lowest h, then the
* oldest). Changes the search object; returns it. */
function stepSearch(search) {
if (search.status !== "searching") return search;
if (search.open.length === 0) {
search.status = "no path";
search.current = -1;
return search;
}
let best = 0;
let bestScore = fScore(search, search.open[0]);
for (let k = 1; k < search.open.length; k++) {
const score = fScore(search, search.open[k]);
const lower = score.f < bestScore.f - 1e-9;
const tie =
Math.abs(score.f - bestScore.f) <= 1e-9 && score.h < bestScore.h;
if (lower || tie) {
best = k;
bestScore = score;
}
}
const current = search.open.splice(best, 1)[0];
search.current = current;
search.closed[current] = 1;
search.expanded++;
if (current === search.goal) {
search.status = "found";
search.path = tracePath(search.parent, current);
return search;
}
const { walls, cols, rows, diagonal } = search;
for (const next of neighbors(walls, cols, rows, current, diagonal)) {
if (search.closed[next.i]) continue;
const g = search.g[current] + next.cost;
if (g < search.g[next.i] - 1e-9) {
if (search.g[next.i] === Infinity) search.open.push(next.i);
search.g[next.i] = g;
search.parent[next.i] = current;
}
}
return search;
}
/** Steps until the search ends; returns it. */
function runSearch(search, limit = 1e6) {
for (let k = 0; k < limit && search.status === "searching"; k++) {
stepSearch(search);
}
return search;
}
/** Follows the parents back from cell i to the start. */
function tracePath(parent, i) {
const path = [i];
while (parent[path[0]] !== -1) path.unshift(parent[path[0]]);
return path;
}
/** The cost of a path: 1 per straight step, √2 per diagonal step. */
function pathCost(path, cols = COLS) {
let cost = 0;
for (let k = 1; k < path.length; k++) {
const dx = Math.abs((path[k] % cols) - (path[k - 1] % cols));
const dy = Math.abs(
Math.floor(path[k] / cols) - Math.floor(path[k - 1] / cols),
);
cost += dx && dy ? Math.SQRT2 : 1;
}
return cost;
}
/** Random walls (a share density of the cells), never on start or goal. */
function randomWalls(cols, rows, density, random, keep = []) {
const walls = new Uint8Array(cols * rows);
for (let i = 0; i < walls.length; i++) {
walls[i] = random() < density && !keep.includes(i) ? 1 : 0;
}
return walls;
}
/** The demo map: a cup that opens toward the start, a classic trap. */
function demoWalls(cols = COLS, rows = ROWS) {
const walls = new Uint8Array(cols * rows);
for (let y = 3; y <= 12; y++) walls[y * cols + 14] = 1;
for (let x = 9; x <= 14; x++) walls[3 * cols + x] = walls[12 * cols + x] = 1;
return walls;
}
/** World position of a cell's middle: x to the right, y up, row 0 on top. */
function cellCenter(i, cols = COLS) {
return { x: (i % cols) + 0.5, y: -(Math.floor(i / cols) + 0.5) };
}
/** The cell under a world point, or -1 outside the grid. */
function cellAt(wx, wy, cols = COLS, rows = ROWS) {
const x = Math.floor(wx);
const y = Math.floor(-wy);
return x >= 0 && y >= 0 && x < cols && y < rows ? y * cols + x : -1;
}
/** An orthographic view that shows the whole grid plus a margin, with
* one extra row of room on top for the status line. */
function fitView(width, height, cols = COLS, rows = ROWS, margin = 0.6) {
const aspect = width / Math.max(height, 1);
let w = cols / 2 + margin; // half the width and height to show
let h = (rows + 1) / 2 + margin;
if (w / h > aspect) h = w / aspect;
else w = h * aspect;
const cx = cols / 2;
const cy = -(rows - 1) / 2; // the middle, half a row up
return { left: cx - w, right: cx + w, top: cy + h, bottom: cy - h };
}
/** A pixel on the canvas (0,0 = top left) to a point in the world. */
function screenToWorld(px, py, width, height, view) {
return {
x: view.left + (px / width) * (view.right - view.left),
y: view.top - (py / height) * (view.top - view.bottom),
};
}
/** Where a robot is after driving distance d along the points. */
function pointAlong(points, d) {
for (let k = 1; k < points.length; k++) {
const a = points[k - 1];
const b = points[k];
const length = Math.hypot(b.x - a.x, b.y - a.y);
const heading = Math.atan2(b.y - a.y, b.x - a.x);
if (d <= length || k === points.length - 1) {
const t = Math.min(1, Math.max(0, d / length));
const done = k === points.length - 1 && d >= length;
const p = { x: a.x + (b.x - a.x) * t, y: a.y + (b.y - a.y) * t };
return { ...p, heading, done };
}
d -= length;
}
return { ...points[0], heading: 0, done: true };
}
/** What a cell shows: start, goal, wall, path, current, closed, open, free. */
function cellKind(i, search, walls, start, goal) {
if (i === start) return "start";
if (i === goal) return "goal";
if (walls[i]) return "wall";
if (!search) return "free";
if (search.path && search.path.includes(i)) return "path";
if (i === search.current) return "current";
if (search.closed[i]) return "closed";
if (search.g[i] !== Infinity) return "open";
return "free";
}
/** The text of the readout panel. */
function readoutText(search, options) {
const ok = isAdmissible(options.heuristic, options.diagonal);
const lines = [
`heuristic ${NAMES[options.heuristic]}`,
`admissible ${ok ? "yes" : "no"}`,
`moves ${options.diagonal ? 8 : 4}-connected`,
`expanded ${search ? search.expanded : 0}`,
`open ${search ? search.open.length : 0}`,
];
const path = search && search.path;
const none = search && search.status === "no path";
lines.push(
`path ${path ? path.length - 1 + " steps" : none ? "none" : "…"}`,
);
if (path) lines.push(`cost ${pathCost(path, search.cols).toFixed(2)}`);
return lines.join("\n");
}
// ---- END OF ROBOT MATH ----
// ---- Page elements ----
const $ = (id) => document.getElementById(id);
const canvas = $("view");
const statusLine = $("status");
// ---- State ----
let walls = demoWalls();
let start = 8 * COLS + 2;
let goal = 8 * COLS + 21;
let search = null; // null until the first Step or Run
let running = false;
let saved = 0; // seconds toward the next step while running
let cursor = start; // keyboard cursor
let showCursor = false;
let driven = 0; // how far the robot has driven along the path
let route = null; // the path as world points, for the robot
let routeLength = 0;
let view = fitView(1, 1);
let dirty = true; // cell colors need repainting
function options() {
return {
heuristic: $("heuristic").value,
diagonal: $("moves").value === "8",
};
}
// ---- Renderer, scene and camera ----
let renderer;
try {
renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
} catch (error) {
statusLine.textContent = "WebGL is not available, so nothing can be drawn.";
throw error;
}
renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2));
const scene = new THREE.Scene();
const camera = new THREE.OrthographicCamera(-1, 1, 1, -1, -10, 10);
const mat = {
grid: new THREE.LineBasicMaterial(),
ink: new THREE.LineBasicMaterial(),
inkFill: new THREE.MeshBasicMaterial(),
paper: new THREE.MeshBasicMaterial(),
accent: new THREE.LineBasicMaterial(),
cells: new THREE.MeshBasicMaterial(),
};
const tint = {}; // one THREE.Color per cell kind
function readColors() {
const style = getComputedStyle(document.documentElement);
const pick = (name, fallback) =>
style.getPropertyValue(name).trim() || fallback;
return {
paper: pick("--paper", "#ffffff"),
ink: pick("--ink", "#111111"),
grid: pick("--grid", "#e4e4e4"),
soft: pick("--soft", "#b8b8b8"),
accent: pick("--accent", "#b5441f"),
};
}
function applyColors() {
const c = readColors();
const color = (value) => new THREE.Color(value);
const mix = (share) => color(c.paper).lerp(color(c.accent), share);
scene.background = color(c.paper);
mat.grid.color.set(c.grid);
mat.ink.color.set(c.ink);
mat.inkFill.color.set(c.ink);
mat.paper.color.set(c.paper);
mat.accent.color.set(c.accent);
Object.assign(tint, {
free: color(c.paper),
start: color(c.paper),
goal: color(c.paper),
wall: color(c.ink),
closed: color(c.grid),
open: mix(0.22),
current: mix(0.6),
path: mix(0.35),
});
dirty = true;
}
/** Puts an object in the scene with a name, at height z (drawing order). */
function add(object, name, z) {
object.name = name;
object.position.z = z;
scene.add(object);
return object;
}
function lineFrom(points, material, closed = false) {
const geometry = new THREE.BufferGeometry();
const xyz = [];
for (let i = 0; i < points.length; i += 2)
xyz.push(points[i], points[i + 1], 0);
geometry.setAttribute("position", new THREE.Float32BufferAttribute(xyz, 3));
return closed
? new THREE.LineLoop(geometry, material)
: new THREE.Line(geometry, material);
}
const circle = (r, n = 32) => {
const p = [];
for (let a = 0; a < 2 * Math.PI - 1e-9; a += (2 * Math.PI) / n) {
p.push(r * Math.cos(a), r * Math.sin(a));
}
return p;
};
// ---- The cells: one InstancedMesh, one color per cell ----
const square = new THREE.PlaneGeometry(1, 1);
const cells = new THREE.InstancedMesh(square, mat.cells, COLS * ROWS);
add(cells, "cells", 0);
const place = new THREE.Matrix4();
for (let i = 0; i < COLS * ROWS; i++) {
const c = cellCenter(i);
cells.setMatrixAt(i, place.makeTranslation(c.x, c.y, 0));
cells.setColorAt(i, new THREE.Color(1, 1, 1));
}
// Hairlines between the cells, a darker frame around the map.
const gridPoints = [];
for (let x = 1; x < COLS; x++) gridPoints.push(x, 0, 0.01, x, -ROWS, 0.01);
for (let y = 1; y < ROWS; y++) gridPoints.push(0, -y, 0.01, COLS, -y, 0.01);
const gridGeometry = new THREE.BufferGeometry();
gridGeometry.setAttribute(
"position",
new THREE.Float32BufferAttribute(gridPoints, 3),
);
scene.add(new THREE.LineSegments(gridGeometry, mat.grid));
scene.add(lineFrom([0, 0, COLS, 0, COLS, -ROWS, 0, -ROWS], mat.ink, true));
// Start: a filled dot. Goal: a ring with a cross. Cursor: a square.
const dot = new THREE.CircleGeometry(0.28, 24);
const startMark = add(new THREE.Mesh(dot, mat.inkFill), "start", 0.3);
const goalMark = add(new THREE.Group(), "goal", 0.3);
goalMark.add(lineFrom(circle(0.3), mat.ink, true));
goalMark.add(lineFrom([-0.42, 0, 0.42, 0], mat.ink));
goalMark.add(lineFrom([0, -0.42, 0, 0.42], mat.ink));
const box = [-0.4, -0.4, 0.4, -0.4, 0.4, 0.4, -0.4, 0.4];
const cursorMark = add(lineFrom(box, mat.accent, true), "cursor", 0.5);
const blank = new Array(COLS * ROWS * 2).fill(0);
const pathLine = add(lineFrom(blank, mat.accent), "path", 0.2);
pathLine.frustumCulled = false; // its points change
const robot = add(new THREE.Group(), "robot", 0.4);
robot.add(new THREE.Mesh(new THREE.CircleGeometry(0.22, 20), mat.paper));
robot.add(lineFrom(circle(0.22, 20), mat.accent, true));
robot.add(lineFrom([0, 0, 0.22, 0], mat.accent));
applyColors();
// ---- Fit the drawing to the canvas, also after a resize ----
function resize() {
const width = canvas.clientWidth || window.innerWidth;
const height = canvas.clientHeight || window.innerHeight;
renderer.setSize(width, height, false);
view = fitView(width, height);
Object.assign(camera, view);
camera.updateProjectionMatrix();
}
window.addEventListener("resize", resize);
resize();
window
.matchMedia("(prefers-color-scheme: dark)")
.addEventListener("change", applyColors);
// ---- Search controls ----
function clearSearch() {
search = null;
route = null;
setRunning(false);
dirty = true;
}
function ensureSearch() {
if (!search) search = createSearch(walls, COLS, ROWS, start, goal, options());
}
function setRunning(on) {
running = on && (!search || search.status === "searching");
$("run").textContent = running ? "Pause" : "Run";
saved = 0;
}
function step() {
ensureSearch();
stepSearch(search);
dirty = true;
if (search.status !== "searching") finish();
}
function finish() {
setRunning(false);
if (search.path) {
route = search.path.map((i) => cellCenter(i));
routeLength = pathCost(search.path);
driven = 0;
}
}
$("step").addEventListener("click", step);
function toggleRun() {
if (search && search.status !== "searching") clearSearch(); // run again
setRunning(!running);
}
$("run").addEventListener("click", toggleRun);
$("reset").addEventListener("click", clearSearch);
$("heuristic").addEventListener("change", clearSearch);
$("moves").addEventListener("change", clearSearch);
$("speed").addEventListener("input", () => {
$("speed-out").textContent = $("speed").value + " steps/s";
});
$("clear").addEventListener("click", () => {
walls = new Uint8Array(COLS * ROWS);
clearSearch();
});
$("random").addEventListener("click", () => {
walls = randomWalls(COLS, ROWS, 0.28, Math.random, [start, goal]);
clearSearch();
});
// ---- Editing the map: mouse, pen or finger ----
let drag = null; // "start", "goal", or the wall value being painted
function edit(i) {
if (i < 0) return;
if (drag === "start" || drag === "goal") {
if (walls[i] || i === start || i === goal) return;
if (drag === "start") start = i;
else goal = i;
} else if (i !== start && i !== goal && walls[i] !== drag) {
walls[i] = drag;
} else return;
clearSearch();
}
function cellFromEvent(event) {
const rect = canvas.getBoundingClientRect();
const p = screenToWorld(
event.clientX - rect.left,
event.clientY - rect.top,
rect.width,
rect.height,
view,
);
return cellAt(p.x, p.y);
}
canvas.addEventListener("pointerdown", (event) => {
const i = cellFromEvent(event);
if (i < 0) return;
canvas.setPointerCapture?.(event.pointerId);
drag = i === start ? "start" : i === goal ? "goal" : walls[i] ? 0 : 1;
cursor = i;
showCursor = false;
edit(i);
});
canvas.addEventListener("pointermove", (event) => {
if (drag !== null) edit(cellFromEvent(event));
});
const stopDrag = () => (drag = null);
canvas.addEventListener("pointerup", stopDrag);
canvas.addEventListener("pointercancel", stopDrag);
// ---- Editing with the keyboard (when the grid has focus) ----
const ARROWS = {
ArrowLeft: [-1, 0],
ArrowRight: [1, 0],
ArrowUp: [0, -1],
ArrowDown: [0, 1],
};
// Arrows, Space and Enter by position (event.code), letters by what the key
// types (event.key), so they work on every keyboard layout.
canvas.addEventListener("keydown", (event) => {
const arrow = ARROWS[event.code];
const key = event.key.toLowerCase();
const x = cursor % COLS;
const y = Math.floor(cursor / COLS);
showCursor = true;
if (arrow) {
const nx = Math.min(COLS - 1, Math.max(0, x + arrow[0]));
const ny = Math.min(ROWS - 1, Math.max(0, y + arrow[1]));
cursor = ny * COLS + nx;
} else if (event.code === "Space") {
drag = walls[cursor] ? 0 : 1;
edit(cursor);
drag = null;
} else if (key === "s" || key === "g") {
drag = key === "s" ? "start" : "goal";
edit(cursor);
drag = null;
} else if (event.code === "Enter") {
step();
} else if (key === "r") {
toggleRun();
} else return;
event.preventDefault(); // arrows and Space must not scroll the page
});
canvas.addEventListener("focus", () => (showCursor = true));
canvas.addEventListener("blur", () => (showCursor = false));
// ---- The animation loop ----
const timer = new THREE.Timer();
timer.connect(document);
let lastStatus = "";
let lastReadout = "";
function paintCells() {
for (let i = 0; i < COLS * ROWS; i++) {
cells.setColorAt(i, tint[cellKind(i, search, walls, start, goal)]);
}
cells.instanceColor.needsUpdate = true;
const path = search && search.path ? search.path : [];
const position = pathLine.geometry.attributes.position;
path.forEach((i, k) => {
const c = cellCenter(i);
position.setXY(k, c.x, c.y);
});
position.needsUpdate = true;
pathLine.geometry.setDrawRange(0, path.length);
dirty = false;
}
function statusText() {
if (!search) return "";
if (search.status === "searching") return "Searching…";
if (search.status === "no path")
return "No path: the goal cannot be reached.";
return `Path found: ${search.path.length - 1} steps, cost ${pathCost(search.path).toFixed(2)}`;
}
function update(dt) {
if (running) {
saved += dt * Number($("speed").value);
while (saved >= 1 && running) {
saved -= 1;
step();
}
}
if (route) driven += DRIVE_SPEED * dt;
}
function draw() {
if (dirty) paintCells();
const s = cellCenter(start);
const g = cellCenter(goal);
const c = cellCenter(cursor);
startMark.position.set(s.x, s.y, startMark.position.z);
goalMark.position.set(g.x, g.y, goalMark.position.z);
cursorMark.position.set(c.x, c.y, cursorMark.position.z);
cursorMark.visible = showCursor;
robot.visible = route !== null;
if (route) {
const at = pointAlong(route, driven);
robot.position.set(at.x, at.y, robot.position.z);
robot.rotation.z = at.heading;
// At the goal: wait a second, then drive the path again.
if (at.done && driven > routeLength + DRIVE_SPEED) driven = 0;
}
const status = statusText();
if (status !== lastStatus) statusLine.textContent = lastStatus = status;
const text = readoutText(search, options());
if (text !== lastReadout) $("readout").textContent = lastReadout = text;
}
renderer.setAnimationLoop((time) => {
timer.update(time);
const dt = Math.min(timer.getDelta(), MAX_STEP);
update(dt);
draw();
renderer.render(scene, camera);
});What to try
- Step through the cup. On the demo map, press Step a few times and watch the open set (tinted) and the closed set (gray) grow. A* heads straight for the goal, runs into the bottom of the cup, and explores the inside of the cup first, before it finds the way around: the heuristic made the inside look promising.
- Compare Zero (Dijkstra) with Euclidean. Run the demo map with each and compare the "expanded" counter. With the default 8-connected moves the Python version above counts 339 against 134 for the same path cost of 23.14.
- Try Manhattan with 8-connected moves. The readout says "admissible no". On the demo map it still finds the cheapest route, but press Random walls a few times and compare its cost with Euclidean on the same walls: now and then Manhattan returns a longer path.
- Switch between 4- and 8-connected. The path changes from 29 straight steps (cost 29.00) to 19 steps with diagonals (cost 23.14), and no diagonal step cuts a wall corner.
- Wall in the goal. Draw a closed ring of walls around it and run: A* expands every cell it can reach, and the readout shows no path. Proving that there is no route costs the most work of all.
- Move the start into the cup. Drag the start inside the cup, or the goal behind it, and see how a different start changes how much of the map A* has to search.
The code under the frame is the complete simulation: HTML, CSS and
JavaScript, ready to paste into a CodePen Pen (the HTML goes inside
<body>). Its ROBOT MATH section at the top holds
the pure functions this page explains, such as
neighbors, createSearch,
stepSearch and the three heuristics; the
Python version above mirrors
them. In CodePen 2.0 the first line,
import * as THREE from "three", is enough: CodePen adds
Three.js itself (the simulation is built against r186,
three@0.186.1, as of October 2026). For the Classic
editor, see
If you use the Classic editor.
Sampling-based planners
Grids work well for a robot on a floor: two coordinates, perhaps a heading. They do not scale to more dimensions. A grid of 100 values per joint for a six-joint arm has 1006, a trillion, cells, and nobody can store or search that. Sampling-based planners avoid building the space at all. They pick random configurations, ask only "is this one free of collisions?", and connect the free ones into a graph or a tree.
The best-known is the rapidly-exploring random tree (RRT), introduced by Steven M. LaValle in 1998 (technical report TR 98-11, Computer Science Department, Iowa State University). The tree starts at the start configuration and grows in a loop:
- Pick a random point in the space (sometimes: the goal itself).
- Find the tree node nearest to it.
- Take a short step from that node toward the point; if the step is free of collisions, add the new node to the tree.
- Stop when a node gets close enough to reach the goal.
Because random points fall mostly in large unexplored regions, the tree is pulled outward quickly and fills the space. RRT is probabilistically complete: if a route exists, the chance of finding it approaches certainty as it keeps sampling. It is not optimal: the route it finds is usually long and jagged, and is shortened and smoothed afterward. RRT* is a later variant that keeps improving the route toward the shortest one as it runs longer.
The example plans across a 10 × 10 m hall with two walls. Its randomness is seeded, so the three runs repeat exactly; only counts and rounded lengths are printed:
import math
import random
# A 10 m x 10 m hall with two walls, given as rectangles (x1, y1, x2, y2).
WALLS = [(3.0, 0.0, 3.5, 7.0), (6.5, 3.0, 7.0, 10.0)]
START, GOAL = (1.0, 1.0), (9.0, 9.0)
STEP = 0.5 # how far the tree grows toward each sample, m
def blocked(p):
return any(x1 <= p[0] <= x2 and y1 <= p[1] <= y2 for x1, y1, x2, y2 in WALLS)
def segment_free(a, b):
"""Check 20 points along the segment (good enough for 0.5 m walls)."""
return not any(blocked((a[0] + (b[0] - a[0]) * t / 20, a[1] + (b[1] - a[1]) * t / 20))
for t in range(21))
def rrt(seed, max_iterations=5000):
random.seed(seed)
nodes, parent = [START], {0: None}
for iteration in range(1, max_iterations + 1):
# Sample a random point (10% of the time: the goal itself).
sample = GOAL if random.random() < 0.1 else (random.uniform(0, 10), random.uniform(0, 10))
nearest = min(range(len(nodes)), key=lambda i: math.dist(nodes[i], sample))
near = nodes[nearest]
d = math.dist(near, sample)
if d == 0:
continue
new = sample if d <= STEP else (near[0] + (sample[0] - near[0]) * STEP / d,
near[1] + (sample[1] - near[1]) * STEP / d)
if not segment_free(near, new):
continue
nodes.append(new)
parent[len(nodes) - 1] = nearest
if math.dist(new, GOAL) <= STEP and segment_free(new, GOAL):
nodes.append(GOAL)
parent[len(nodes) - 1] = len(nodes) - 2
length, i = 0.0, len(nodes) - 1
while parent[i] is not None:
length += math.dist(nodes[i], nodes[parent[i]])
i = parent[i]
return iteration, len(nodes), length
return max_iterations, len(nodes), None
for seed in (1, 2, 3):
iterations, size, length = rrt(seed)
print(f"seed {seed}: path found after {iterations} samples, tree of {size} nodes, "
f"length {length:.1f} m")seed 1: path found after 442 samples, tree of 246 nodes, length 25.5 m seed 2: path found after 771 samples, tree of 446 nodes, length 31.4 m seed 3: path found after 796 samples, tree of 476 nodes, length 28.6 m
Every run finds a way through, each a different one. The shortest possible route, over the first wall's end and under the second's, is about 18.6 m (worked out by hand from the wall corners); the trees' routes are roughly 35% to 70% longer, which is typical before smoothing. Motion planners for robot arms commonly use sampling planners of this family, because an arm's configuration space is far too big for a grid.
Obstacle avoidance
The global path is planned on the map, and the map is never complete. Obstacle avoidance is the local, fast reaction to obstacles the sensors see right now: slowing down, steering around a pallet, or stopping and waiting for a person to pass, while staying as close as possible to the planned route.
A classic idea is the potential field, introduced for arms and mobile robots by Oussama Khatib in 1986 ("Real-Time Obstacle Avoidance for Manipulators and Mobile Robots", International Journal of Robotics Research): the goal pulls the robot like a magnet, every nearby obstacle pushes it away, and the robot follows the sum of the forces. It is simple and fast, and it has a famous flaw, shown here with a cup that opens toward the robot:
import math
# Potential field: the goal pulls, every obstacle point pushes when it is close.
GOAL = (10.0, 0.0)
# A cup that opens toward the robot: a back wall at x = 5 and two sides.
OBSTACLES = ([(5.0, y / 2) for y in range(-4, 5)] +
[(x / 2, 2.0) for x in range(6, 10)] + [(x / 2, -2.0) for x in range(6, 10)])
REACH = 1.5 # obstacles farther away than this do not push
def force(p):
fx, fy = GOAL[0] - p[0], GOAL[1] - p[1] # attraction, grows with distance
for ox, oy in OBSTACLES:
d = math.dist(p, (ox, oy))
if d < REACH:
push = 3.0 * (1 / d - 1 / REACH) / d ** 2
fx += push * (p[0] - ox) / d
fy += push * (p[1] - oy) / d
return fx, fy
def follow(start, steps=2000, step=0.05):
p = start
for _ in range(steps):
fx, fy = force(p)
size = math.hypot(fx, fy)
if size < 1e-6:
break
p = (p[0] + step * fx / size, p[1] + step * fy / size)
if math.dist(p, GOAL) < step:
return p, "reached the goal"
return p, "stuck"
for start in [(0.0, 0.0), (0.0, 4.0)]:
end, result = follow(start)
print(f"start {start}: {result} at ({end[0]:.2f}, {end[1]:.2f})")start (0.0, 0.0): stuck at (4.20, 0.00) start (0.0, 4.0): reached the goal at (9.97, 0.02)
Started in line with the cup, the robot drives in and stops where the pull of the goal and the push of the back wall cancel: a local minimum. Started beside it, the robot slides past and reaches the goal. It is the same trap as the cup on the simulation's demo map, and the contrast is the point: A* searched the inside of the cup too, but because it keeps the whole open set it backs out and finds the way around, while a purely local method has no memory and no map. That is why robots pair a global planner with a local one. Real local planners, the dynamic window approach (DWA) among them, are more refined than a potential field, but share its limit: they only see the neighborhood.