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.
Do this lesson in the simulatorPrint 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, and join the graph only for the query that needs them.
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)
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 "orange", "line")
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.
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))
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.
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 it holds for radius linking. With K fixed at 8 the narrow curve rises much more slowly than that, and is still short of certain at the largest size in the figure. 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, printnodes:and how many nodes the roadmap has, which is 150. add_node(p). Takes a pointp = (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 withadd_node, and runs Dijkstra between them. Returns(route, length), whererouteis the list of(x, y)points fromatobandlengthis its length in centimetres, orNoneif the search never reachesb. When a query comes backNone, throw the samples away, build a fresh roadmap of 150 and ask again.- The trips. Start each query from
here(), where the robot actually is. Before driving each one, printquery 1 length:,query 2 length:andquery 3 length:with the route's length. - The third goal. Once the second trip is done, pick a random free point anywhere on the mat (
free()says whether a point is free), print it asgoal 3: x, y, then query the roadmap you already have and drive there. Leave the random module unseeded, so the third goal is somewhere new every run.
free(), clear(), here() 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))
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
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?