Dijkstra and A*

What it computes

The cheapest path across an occupancy grid from a start cell to a goal cell. Each cell has up to 8 neighbours: a straight step costs 1 and a diagonal one $\sqrt 2$.

The rover is a 6.4 cm disc, not a point, so the program first grows every wall by its radius (plus 5 mm). In that grown map, the rover's configuration space, the rover is a point: any path through free cells is safe to drive, even a diagonal step past the corner of a blocked cell.

Dijkstra and A*

Both pop cells from a priority queue and relax their neighbours. Dijkstra orders the queue by $g$, the cost from the start, so it spreads out in every direction. A* orders it by $f = g + h$, where $h$ guesses the cost still to go, so it heads for the goal. If $h$ never overestimates (it is admissible), the path is optimal once the goal comes off the queue. With $h = 0$, A* is Dijkstra.

Ignoring walls, the cheapest route takes $\min(|\Delta r|, |\Delta c|)$ diagonal steps and straight ones for the rest. That's the octile distance:

$$h = \max(|\Delta r|, |\Delta c|) + (\sqrt 2 - 1)\,\min(|\Delta r|, |\Delta c|)$$

The algorithm

g[start] = 0; heap = [(h(start, goal), start)]
while heap:
    pop the cell with the smallest f
    if it's already expanded: skip it
    expand it; if it's the goal: stop
    for each free neighbour n on the grid:
        if g[cell] + step < g[n]:
            g[n] = g[cell] + step; parent[n] = cell
            push (g[n] + h(n, goal), n)
follow parent back from the goal

Tools

heapq.heappush(heap, (f, cell)) and heapq.heappop(heap). Cells are (row, col) tuples, and grid[row, col] is 1 where it's blocked.

See it in context

Motion Planning's Configuration space step builds the arm's C-space, and its bonus step searches a roadmap with Dijkstra.

Your task

Implement octile(a, b) and astar(grid, start, goal, h), which returns (path, expanded): the cells from start to goal, and every cell it expanded, in order. The program plans through the maze with $h = 0$ and with octile, shows both searches in the Images tab, then drives the A* path. The grader also runs both on 20 grids of its own.