Dijkstra and the cost of a step

When steps cost different amounts, the cheapest route is not the shortest one, and a queue sorted by cost finds it.

U9.3PlanningUniversity35 min

Do this lesson in the simulator

Breadth first assumes every step costs the same. Almost nothing about a real robot does.

  • A diagonal step across a grid is sqrt(2) times as long as a straight one.
  • Driving within a few centimetres of a wall is riskier than driving down the middle, and a plan that admits this produces a robot that does not clip corners.
  • Some of the floor is carpet, or a ramp, or has a cable across it.
  • Turning costs time, so a route with fewer turns can beat a shorter one.

All of these are the same idea: put a number on each edge, and look for the cheapest path rather than the shortest one.

The algorithm

Dijkstra is breadth first with the plain queue replaced by a priority queue, ordered by cost so far.

best = {start: 0.0}
queue = [(0.0, start)]
while queue:
    cost, cur = heapq.heappop(queue)
    if cur in done:
        continue
    done.add(cur)
    if cur == goal:
        break
    for nxt in free_neighbours(cur):
        price = cost + cost_of_entering(nxt, from_=cur)
        if price < best.get(nxt, INFINITY):
            best[nxt] = price
            came[nxt] = cur
            heapq.heappush(queue, (price, nxt))

Two details that matter and are easy to get wrong.

A node is finished the first time it comes off the queue. Not the first time it is pushed. A cell can be pushed several times with different costs, and only the smallest one is correct. That is why done exists.

Stale entries are left in the queue. Rather than find and update an entry, push a second one and skip it later if the node is already done. This is the lazy deletion trick, it costs a little memory, and it is what almost every implementation does.

Correctness rests on one assumption: no edge may have a negative cost. With only non-negative costs, the first time a node is popped its cost is final, because nothing later in the queue can ever be cheaper. A negative edge breaks that argument, and Dijkstra silently returns wrong answers. Robot cost maps are naturally non-negative, so this rarely bites here, but it is the reason the algorithm works at all.

Complexity with a binary heap is O((V + E) log V).

A cost map with clearance in it

The cost used in the task says: entering a cell costs the length of the step in centimetres, times three if that cell's centre is closer than 20 cm to a wall.

def price(step_cm, cell):
    return step_cm * (3.0 if gap(cell) < 20.0 else 1.0)

That single line changes the character of the route. The cheapest path now drives down the middle of the corridor instead of hugging the inside of the turn, because the extra distance is cheaper than the penalty. Nothing about the algorithm changed. The cost function is the interface, and designing it is most of the engineering.

Real systems use a smooth version rather than a step: cost falls off exponentially with distance from the nearest obstacle, over the same grid the occupancy map lives in. That produces a distance transform, and computing one over the whole map is a standard preprocessing step in every serious navigation stack.

from bugbot import *
import math
connect()

WALLS = [(70.0, 0.0, 8.0, 115.0), (125.0, 85.0, 8.0, 115.0)]

def gap(x, y):
    """How far a point is from the nearest wall: zero inside one."""
    best = 1e9
    for ox, oy, ow, oh in WALLS:
        dx = max(ox - x, 0.0, x - (ox + ow))
        dy = max(oy - y, 0.0, y - (oy + oh))
        best = min(best, math.hypot(dx, dy))
    return best

for y in (30, 60, 90, 120):
    print("y =", y, "".join("." if gap(x, y) >= 20 else "x" for x in range(0, 200, 5)))

Run this in the simulator

The crosses are the expensive band around each wall, and the dots between them are the lane the planner will prefer.

Uniform cost search, and the name

Dijkstra as written here stops as soon as the goal is popped, which is sometimes called uniform cost search to distinguish it from the textbook version that computes the distance to every node. They are the same algorithm; only the stopping rule differs. Computing the whole field is genuinely useful when many goals share one start, or when a robot wants a policy for reaching a place from anywhere, in which case run it backwards from the goal once and store the result.

Task: the cheapest way across

Same grid as U9.2, now with eight neighbours and the cost map above. Print cost:, the total cost of the cheapest path, and min gap:, the smallest distance from any cell on that path to a wall.

from bugbot import *
import heapq
import math
connect()

CELL = 5.0
N = 40
INFLATE = 8.0
NEAR = 20.0
WALLS = [(70.0, 0.0, 8.0, 115.0), (125.0, 85.0, 8.0, 115.0)]

Challenges

  1. Run it again with the penalty turned off (multiplier 1 everywhere) and compare both the cost and the minimum gap. How much did the clearance cost you in distance?
  2. Make the penalty smooth: 1 + 4 * max(0, 1 - gap/25). Does the path move further from the walls, or just sit differently?
  3. Add a cost for turning by penalising a step whose direction differs from the one before it. What does that need you to change about what a node is?