Roadmaps

Sample the free space once, link it into a graph, and answer every later question with a graph search. Probabilistic roadmaps, narrow passages, and when a tree is better.

U9.8PlanningUniversity35 min

Do this lesson in the simulator

Print it: worksheet (PDF) · answers (PDF) · as a web page

The RRT in U9.6 grows a tree from the start towards one goal, and throws it away afterwards. Ask for a second goal and it starts again from nothing. A robot that works in the same room all day, fetching and returning, is doing that over and over on a map that has not changed.

A probabilistic roadmap (PRM, Kavraki and others, 1996; Lynch and Park section 10.5) spends the effort once. It samples the free space, links the samples into a graph, and keeps the graph. Every question after that is a graph search, and on a graph of a few hundred nodes that is far cheaper than the build. This lesson measures both.

Two phases

build, once:
    sample N points in the free space
    link each point to its K nearest neighbours, where the straight edge between them is clear
query, as often as you like:
    add the start and the goal as nodes, each linked to its K nearest the same way
    search the graph (Dijkstra, U9.3) from start to goal
    if the search never reaches the goal, say so: the roadmap has no route

The build knows nothing about any start or goal. It covers the free space and stops. The query points arrive later. Each one joins the graph when a query needs it, and stays, so the roadmap grows by two nodes a query; the samples the build made are never touched again.

The edge test is the clear() from U9.7, on the same inflated map: both ends free is not enough, the whole segment must be.

from bugbot import *
import math, random
connect()

random.seed(4)
INFLATE = 10.0
N_SAMPLES, K = 150, 8
WALLS = [(70.0, 0.0, 8.0, 115.0), (125.0, 85.0, 8.0, 115.0)]

def free(x, y):
    if x < INFLATE or y < INFLATE or x > 200 - INFLATE or y > 200 - INFLATE:
        return False
    return not any(ox - INFLATE <= x <= ox + ow + INFLATE and oy - INFLATE <= y <= oy + oh + INFLATE
                   for ox, oy, ow, oh in WALLS)

def clear(a, b):
    d = math.hypot(b[0] - a[0], b[1] - a[1])
    n = max(2, int(d / 1.5))
    return all(free(a[0] + (b[0] - a[0]) * k / n, a[1] + (b[1] - a[1]) * k / n) for k in range(n + 1))

nodes = []
while len(nodes) < N_SAMPLES:
    p = (random.uniform(0, 200), random.uniform(0, 200))
    if free(*p):
        nodes.append(p)
adj = {i: {} for i in range(len(nodes))}          # adj[i][j] is the length of the edge from i to j
for i, a in enumerate(nodes):
    near = sorted(range(len(nodes)), key=lambda j: math.hypot(nodes[j][0] - a[0], nodes[j][1] - a[1]))[1:K + 1]
    for j in near:
        if j not in adj[i] and clear(a, nodes[j]):
            adj[i][j] = adj[j][i] = math.hypot(nodes[j][0] - a[0], nodes[j][1] - a[1])
print("nodes:", len(nodes), " edges:", sum(len(v) for v in adj.values()) // 2)

# how many pieces is it in? a depth-first walk (a stack) from each node not yet seen
seen, pieces = set(), 0
for s in range(len(nodes)):
    if s not in seen:
        pieces += 1
        stack = [s]
        while stack:
            u = stack.pop()
            if u not in seen:
                seen.add(u)
                stack.extend(adj[u])
print("connected pieces:", pieces)
draw("roadmap", nodes, "blue", "dots", 2)

Run this in the simulator

The blue dots on the mat are the roadmap's nodes. The figure further down shows its edges as well: 150 samples and 708 edges, in one connected piece, running through both gaps.

Two numbers to watch when you build one: the edge count, which says how well linked it is, and the number of connected pieces. One piece means any two free points the roadmap can reach can reach each other. Two pieces means some start and goal pairs have no route through it, even when one exists on the mat.

The graph is a dictionary of dictionaries: adj[i][j] is the length of the edge from node i to node j, and adj[i] on its own lists the neighbours of i. Looking up whether an edge is already there, j in adj[i], is then a single step rather than a walk along a list.

Asking it questions

Here is the query phase, on the same 150 samples. add_node(p) puts a point the build never saw into the roadmap: it finds the K nodes nearest to p, keeps the edges to the ones it can see, and returns the new node's number. query(a, b) calls it for both ends, then runs Dijkstra. If the search runs out of nodes before it reaches the goal, there is no route on this roadmap, and query returns None rather than falling over. The cell also times the build and each query.

from bugbot import *
import heapq, math, random, time
connect()

random.seed(4)
INFLATE = 10.0
N_SAMPLES, K = 150, 8
START = (30.0, 30.0)
GOALS = [(170.0, 170.0), (170.0, 30.0)]
WALLS = [(70.0, 0.0, 8.0, 115.0), (125.0, 85.0, 8.0, 115.0)]

def free(x, y):
    if x < INFLATE or y < INFLATE or x > 200 - INFLATE or y > 200 - INFLATE:
        return False
    return not any(ox - INFLATE <= x <= ox + ow + INFLATE and oy - INFLATE <= y <= oy + oh + INFLATE
                   for ox, oy, ow, oh in WALLS)

def clear(a, b):
    d = math.hypot(b[0] - a[0], b[1] - a[1])
    n = max(2, int(d / 1.5))
    return all(free(a[0] + (b[0] - a[0]) * k / n, a[1] + (b[1] - a[1]) * k / n) for k in range(n + 1))

def nearest(p, count):
    """The indices of the `count` nodes closest to p, nearest first."""
    return sorted(range(len(nodes)), key=lambda j: math.hypot(nodes[j][0] - p[0], nodes[j][1] - p[1]))[:count]

# build, once: samples only
t0 = time.perf_counter()
nodes = []
while len(nodes) < N_SAMPLES:
    p = (random.uniform(0, 200), random.uniform(0, 200))
    if free(*p):
        nodes.append(p)
adj = {i: {} for i in range(len(nodes))}
for i, a in enumerate(nodes):
    for j in nearest(a, K + 1)[1:]:                # [0] is the node itself
        if j not in adj[i] and clear(a, nodes[j]):
            d = math.hypot(nodes[j][0] - a[0], nodes[j][1] - a[1])
            adj[i][j] = adj[j][i] = d
build_ms = 1000 * (time.perf_counter() - t0)

def add_node(p):
    """Put a query point into the roadmap, linked to its K nearest nodes that it can see. Returns its index."""
    near = nearest(p, K)
    i = len(nodes)
    nodes.append(p)
    adj[i] = {}
    for j in near:
        if clear(p, nodes[j]):
            d = math.hypot(nodes[j][0] - p[0], nodes[j][1] - p[1])
            adj[i][j] = adj[j][i] = d
    return i

def query(a, b):
    """Dijkstra from point a to point b on the roadmap: (route, length), or None if the roadmap has no route."""
    s, g = add_node(a), add_node(b)
    dist, came, q = {s: 0.0}, {s: None}, [(0.0, s)]
    while q:
        d, u = heapq.heappop(q)
        if u == g:
            break
        if d > dist[u]:
            continue
        for v, w in adj[u].items():
            if d + w < dist.get(v, 1e18):
                dist[v], came[v] = d + w, u
                heapq.heappush(q, (d + w, v))
    if g not in dist:
        return None
    path, u = [], g
    while u is not None:
        path.append(nodes[u])
        u = came[u]
    return path[::-1], dist[g]

print("build:", round(build_ms), "ms for", N_SAMPLES, "nodes")
for k, (a, b) in enumerate(((START, GOALS[0]), (GOALS[0], GOALS[1]))):
    t0 = time.perf_counter()
    route, length = query(a, b)
    ms = 1000 * (time.perf_counter() - t0)
    print("query", k + 1, "length:", round(length, 1), "cm through", len(route) - 2, "roadmap nodes, in", round(ms, 1), "ms")
    draw("route %d" % (k + 1), route, "green" if k == 0 else "red", "line")

Run this in the simulator

The first route is 386.7 cm. The shortest possible, hugging the grown corners of both walls, is about 311. A PRM is not optimal: the route can only go node to node, so it zigzags, and the best path through 150 random points is not the best path on the mat. The shortcut pass from U9.7 applies to it unchanged and takes most of the zigzag out.

Now look at the times. The exact figures depend on the machine, and the page runs more slowly than a laptop, but the build takes some milliseconds and each query a fraction of one, tens of times less. The second query needed no new sampling and no new graph, only K clear() checks for each new point and one search. With 150 samples the build is cheap anyway; it is the ratio that matters, because the build grows faster than a query as the roadmap gets bigger, and a robot asks for routes many times on the one map.

Dark grey: the walls; dashed: grown by 10 cm, the configuration space the samples live in. The thin lines are the roadmap's 708 edges, plus the few each query point added when it joined. Green is the first query, 387 cm to the far corner; red is the second, 149 cm on to the bottom right, answered on the same graph with no new sampling.

How many neighbours?

K is the one setting the roadmap has besides N. Too few and the graph falls apart; too many and every node pays for edges it never needs. Here are four values on the same 150 samples.

from bugbot import *
import heapq, math, random, time
connect()

INFLATE = 10.0
N_SAMPLES = 150
START, GOAL = (30.0, 30.0), (170.0, 170.0)
WALLS = [(70.0, 0.0, 8.0, 115.0), (125.0, 85.0, 8.0, 115.0)]

def free(x, y):
    if x < INFLATE or y < INFLATE or x > 200 - INFLATE or y > 200 - INFLATE:
        return False
    return not any(ox - INFLATE <= x <= ox + ow + INFLATE and oy - INFLATE <= y <= oy + oh + INFLATE
                   for ox, oy, ow, oh in WALLS)

def clear(a, b):
    d = math.hypot(b[0] - a[0], b[1] - a[1])
    n = max(2, int(d / 1.5))
    return all(free(a[0] + (b[0] - a[0]) * k / n, a[1] + (b[1] - a[1]) * k / n) for k in range(n + 1))

def link(nodes, adj, i, k):
    """Link node i to whichever of its k nearest nodes it can see."""
    a = nodes[i]
    near = sorted(range(len(nodes)), key=lambda j: math.hypot(nodes[j][0] - a[0], nodes[j][1] - a[1]))[1:k + 1]
    for j in near:
        if j not in adj[i] and clear(a, nodes[j]):
            adj[i][j] = adj[j][i] = math.hypot(nodes[j][0] - a[0], nodes[j][1] - a[1])

def shortest(adj, s, g):
    dist, q = {s: 0.0}, [(0.0, s)]
    while q:
        d, u = heapq.heappop(q)
        if u == g:
            return d
        if d > dist[u]:
            continue
        for v, w in adj[u].items():
            if d + w < dist.get(v, 1e18):
                dist[v] = d + w
                heapq.heappush(q, (d + w, v))
    return None

for K in (2, 4, 8, 16):
    random.seed(4)                                 # the same 150 samples every time; only K changes
    nodes = []
    while len(nodes) < N_SAMPLES:
        p = (random.uniform(0, 200), random.uniform(0, 200))
        if free(*p):
            nodes.append(p)
    t0 = time.perf_counter()
    adj = {i: {} for i in range(len(nodes))}
    for i in range(len(nodes)):
        link(nodes, adj, i, K)
    ms = 1000 * (time.perf_counter() - t0)
    edges = sum(len(v) for v in adj.values()) // 2
    for p in (START, GOAL):                        # the two query points, linked the same way
        nodes.append(p)
        adj[len(nodes) - 1] = {}
        link(nodes, adj, len(nodes) - 1, K)
    length = shortest(adj, len(nodes) - 2, len(nodes) - 1)
    print("K", K, " edges:", edges, " build:", round(ms), "ms",
          " route:", "none" if length is None else round(length, 1))

Run this in the simulator

With K = 2 there are 187 edges and no route at all: the graph is in 14 pieces. K = 4 connects it, but the route through it is 519.4 cm. K = 8 gives 386.7, and K = 16 gives 361.0 for nearly twice the edges, each one a clear() check at build time. Past a point more edges buy little.

Linking to a fixed number of neighbours is one choice. The other is to link each node to every node within a radius r. The classic analysis of PRMs (Kavraki and others) is for radius linking, and the fixed-K version does not share all its guarantees, as the narrow passage below shows. The variant called PRM* (Karaman and Frazzoli, 2011) lets K grow with the number of samples, as a constant times log N; that keeps the graph connected as N grows, and makes the routes tend to the shortest ones.

The narrow passage

The two gaps on this mat, above the first wall and below the second, are wide. Narrow the first one and the roadmap starts to fail. For the roadmap to get through a gap, some edge has to run through it, and an edge only exists between two samples that are near each other and can see each other through the gap. The chance of that depends on how much of the free area lies in and around the gap. If no edge gets through the gap, the roadmap has two pieces, and the start and the goal are in different ones.

60 seeds at each size. With the lesson's wide gaps and K = 8, 30 samples connect start and goal 65% of the time. Raise the first wall so its gap is only 10 cm wide once the walls are grown, and the same 30 manage 13%; with K held at 8, even 480 reach only 92%. Let K grow with log N, as PRM* does (here 2e log N; any constant above e(1 + 1/d), which is 1.5e on a flat map, will do: 15 at 15 samples, 34 at 480) and 240 samples already connect 98% of the time.

More samples fix it, slowly: the chance of missing a passage falls away exponentially with N, but the passage's share of the area sets how fast. That is the probabilistic completeness of U9.6 again, and the proof of it is for radius linking. Fixed K has no such promise: with K held at 8 the narrow curve climbs slowly, and is still short of certain at 480 samples. Letting K grow as log N, as PRM* does, closes the gap. It is also why real PRMs sample more densely near obstacles, or along the medial axis, rather than uniformly.

Roadmap or tree?

	roadmap (PRM)	tree (RRT)
queries	many, on the same map	one
up-front cost	build the whole graph	none
cost per query	link two points in, then a graph search	grow a new tree
suits	a fixed room, many trips	a new or changing map, one trip
weak spot	narrow passages split the graph	narrow passages slow the tree

Most real systems mix them: a roadmap for the parts of the world that do not change, and a quick tree or local repair for the parts that do.

Task: one roadmap, three trips

Build a roadmap once, then drive three queries on it without touching a wall: from the start to the green corner, from there to the bottom right bay, and from there to a third goal that nobody knows until the program runs.

The build. 150 free samples, each linked to its K = 8 nearest neighbours that clear() says it can see. Only samples: the start and the goals are not in it. Straight after the build, print nodes: and how many nodes the roadmap has, which is 150.
add_node(p). Takes a point p = (x, y) in mat centimetres, adds it to the roadmap linked to its K nearest nodes that it can see, and returns its node number.
query(a, b). Takes two points, adds both with add_node, and runs Dijkstra between them. Returns (route, length), where route is the list of (x, y) points from a to b and length is its length in centimetres, or None if the search never reaches b. When a query comes back None, throw the samples away, build a fresh roadmap of 150 and ask again.
The third goal. The simulator picks it afresh every run, somewhere in the free space to the left of the first wall and at least 30 cm from where the robot starts, and the program reads it with input(): two lines, x then y, in mat centimetres. The starter reads it into GOAL3, and the mat shows it as a red square once the run starts. It is not in the build either; it joins the roadmap like any other query point.
The trips. Visit the corner, then the bay, then finish within 8 cm of GOAL3, one query each. Start each query from here(), where the robot actually is. Before driving each one, print query 1 length:, query 2 length: and query 3 length: with the route's length.

free(), clear(), here(), GOAL3 and follow(route), which drives a list of waypoints the way U9.7 did, are written for you.

from bugbot import *
import heapq, math, random
connect()

DT = 0.1
INFLATE = 10.0
N_SAMPLES, K = 150, 8
V_MAX, V_LAT = 20.0, 15.0
START = (30.0, 30.0)
GOALS = [(170.0, 170.0), (170.0, 30.0)]        # the corner, then the bay
WALLS = [(70.0, 0.0, 8.0, 115.0), (125.0, 85.0, 8.0, 115.0)]

def free(x, y):
    if x < INFLATE or y < INFLATE or x > 200 - INFLATE or y > 200 - INFLATE:
        return False
    return not any(ox - INFLATE <= x <= ox + ow + INFLATE and oy - INFLATE <= y <= oy + oh + INFLATE
                   for ox, oy, ow, oh in WALLS)

def clear(a, b):
    d = math.hypot(b[0] - a[0], b[1] - a[1])
    n = max(2, int(d / 1.5))
    return all(free(a[0] + (b[0] - a[0]) * k / n, a[1] + (b[1] - a[1]) * k / n) for k in range(n + 1))

GOAL3 = (float(input()), float(input()))       # the third goal: the simulator picks it every run, x then y in mat cm

def here():
    px, py = position()
    return START[0] + px, START[1] + py

def follow(route):
    """Drive a list of (x, y) waypoints in order, holding the heading at zero."""
    for wx, wy in route[1:]:
        for tick in range(400):
            x, y = here()
            dx, dy = wx - x, wy - y
            gap = math.hypot(dx, dy)
            if gap < 4.0:
                break
            speed = min(13.0, 3.0 + 0.5 * gap)
            vx, vy = speed * dx / gap, speed * dy / gap
            h = math.radians(heading())
            spin = (heading() + 180) % 360 - 180
            drive(100 * (vx * math.sin(h) + vy * math.cos(h)) / V_MAX, 100 * (vx * math.cos(h) - vy * math.sin(h)) / V_LAT,
                  max(-30.0, min(30.0, -0.8 * spin)))
            wait(DT)
    stop()

# build the roadmap once from samples alone, then query it three times

Trip one (370 cm planned) to the green corner, trip two (146 cm) down the right-hand side to the bay, then trip three (231 cm) back under the second wall and over the first to a goal the simulator picked for this run, here (48, 91). All three are answered on the roadmap built at the start. The robot follows the node-to-node route, zigzags and all, and touches nothing.

Challenges

Add the shortcut pass from U9.7 to each query's route. How much shorter is each trip?
Move the first wall's top end up to y = 170, so the gap above it is only 10 cm once the walls are grown. How many samples does the roadmap need before it connects on most seeds? Then try radius linking, or K growing as log N, and compare.
Lazy PRM. Build the graph without checking any edges, and only check the edges on a route when a query uses it, throwing out the bad ones and searching again. When is that faster?