Sampling: the RRT
Throwing darts at the free space instead of enumerating it, and what probabilistic completeness does and does not promise.
Do this lesson in the simulatorA grid enumerates the free space. That is fine for a mat: 1,600 cells, searched in milliseconds. It stops being fine quickly.
- A robot on a mat that also cares which way it is facing:
(x, y, heading), and at 5 cm and 5 degrees that is 115,200 cells. - A six-jointed arm at 5 degrees per joint: 72 to the power of 6, about 139 billion.
The number of cells grows exponentially with the number of dimensions, and there is no cleverness that avoids it. Above three or four dimensions, enumerating the configuration space is finished as an idea.
Sampling planners give up on enumerating it. They throw points into the free space at random and connect the ones that can see each other. They do not represent the free space; they take a sparse random sketch of it and search that.
The RRT
The Rapidly-exploring Random Tree, from LaValle in 1998, grows one tree from the start.
repeat:
sample a random point in the space (now and then, the goal itself)
find the node of the tree nearest to it
step a fixed distance from that node towards the sample
if the little segment is collision free, add it as a new node
stop when a node can see the goal
The behaviour the name promises comes from the nearest-node rule. A sample falling in an unexplored region will be nearest to a node on the frontier, so the tree grows towards where it has not been. Formally the probability of a node being extended is proportional to the area of its Voronoi region, which means the tree is pulled outwards into empty space. It explores fast and deliberately, without being told to.
Three knobs:
- Step size. Too small and the tree takes for ever. Too large and the segments cannot fit through gaps.
- Goal bias. Sample the goal instead of a random point maybe 5 to 10 percent of the time. Without any bias the tree wanders; with too much it becomes greedy best first and gets stuck against a wall.
- Collision checking of the edge. The single most common bug. Both endpoints can be in free space while the segment between them crosses a wall. Check along the segment, at a spacing finer than the thinnest obstacle.
What it promises
Probabilistic completeness. If a route exists, the probability of finding one tends to 1 as the number of samples tends to infinity. That is a weaker promise than completeness, and the difference matters: if there is no route, an RRT runs until you stop it and can never tell you so.
Not optimal, and not close. A plain RRT returns whatever jagged path the tree happened to grow. On the map in this module the best route is about 330 cm and a raw RRT path is commonly 350 to 530 cm. Worse, more samples do not fix it: the plain RRT provably converges to something that is not optimal.
Two standard responses:
- Shortcutting. After the fact, repeatedly pick two points on the path and, if the straight line between them is clear, splice out everything in between. Cheap, effective, and it recovers most of the loss.
- RRT*. A genuine variant: when a node is added, rewire nearby nodes to go through it if that is cheaper. This is asymptotically optimal, converging to the best path as samples grow, at meaningfully more cost per sample. The other family, the PRM, samples once into a roadmap graph and then answers many queries against it, which is the right shape when the map is fixed and the start and goal keep changing.
Randomness has consequences
Two runs give two different paths. For a robot that is unsettling: the same task, twice, and it drives differently. Debugging a failure is harder because it may not reproduce. Fix the seed while developing, and be aware that a planner which usually finds a route in 200 samples will occasionally take 2,000, so a real system needs a sample budget and a plan for what to do when it is spent.
from bugbot import *
import math, random
connect()
INFLATE, STEP = 10.0, 12.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)
# how far the tree reaches after a few hundred samples, with no goal in mind at all
nodes = [(30.0, 30.0)]
for step in range(400):
sx, sy = random.uniform(0, 200), random.uniform(0, 200)
nx, ny = min(nodes, key=lambda p: (p[0] - sx) ** 2 + (p[1] - sy) ** 2)
d = math.hypot(sx - nx, sy - ny)
px, py = nx + (sx - nx) / d * min(STEP, d), ny + (sy - ny) / d * min(STEP, d)
if free(px, py):
nodes.append((px, py))
print("nodes:", len(nodes))
print("furthest reached:", round(max(math.hypot(p[0] - 30, p[1] - 30) for p in nodes), 1), "cm from the start")
Note that this sketch checks only the new point, not the segment. It is the bug named above, left in on purpose. Fix it before using it for anything.
Task: grow a tree into the free space
Build an RRT from (30, 30) to (170, 170) through the same two walls, inflated by 10 cm. Print nodes:, how many nodes the tree had when it reached the goal, and path:, the length in centimetres of the route through the tree.
from bugbot import *
import math
import random
connect()
INFLATE = 10.0
STEP = 12.0
BIAS = 0.1
START = (30.0, 30.0)
GOAL = (170.0, 170.0)
WALLS = [(70.0, 0.0, 8.0, 115.0), (125.0, 85.0, 8.0, 115.0)]
Challenges
- Shortcut the finished path and print the length before and after. How much of the loss does it recover?
- Run with the goal bias at 0, 0.05, 0.5 and 1.0, ten times each, and report the mean number of nodes. Explain both ends of the curve.
- Take the edge check out, so only the new point is tested. Find a seed where the path goes through a wall.