RRT, in depth: rapidly-exploring random trees from first principles, Voronoi bias, a measured 2D planner, RRT-Connect, RRT* rewiring and kinodynamic planning

The planning problem

Describe the robot by its configuration q: x and y for a point robot, x, y and heading for a car, seven joint angles for an arm. The set of all configurations is the configuration space C. The configurations that collide form C_obs, and the rest are C_free. Planning means finding a continuous path in C_free from q_start to the goal region.

Grid search such as A* is excellent in two or three dimensions. Its cell count grows exponentially with dimension, though, and it needs C_obs to be computed explicitly. Sampling-based planners avoid both problems: they only query a collision checker at the points and edges they try. The price is a weaker guarantee. RRT is probabilistically complete: if a path exists, the probability of finding one tends to 1 as samples grow. It can never prove that no path exists, and its first path is usually far from shortest.

The algorithm and why it explores

The loop is five steps. Sample a random configuration x_rand; with a small probability p, use the goal instead (goal bias). Find the tree node x_near closest to x_rand. Move from x_near towards x_rand by at most a step size eta to get x_new. If the edge from x_near to x_new is collision-free, add x_new with parent x_near. If x_new is within tolerance of the goal and can see it, follow parent pointers back to the start.

Why it explores rapidly. Take the Voronoi diagram of the tree's nodes. A uniform sample falls in a node's cell with probability proportional to that cell's area, and the node owning the cell is the one that gets extended. Nodes on the frontier, next to large unexplored regions, own the largest cells. So the tree is pulled outward into unexplored space without any explicit frontier logic. This is called Voronoi bias. It is also why RRT struggles in narrow passages: the cells that lead into a thin corridor are small, so they are rarely chosen.

A complete 2D planner

The planner below works in a 10 by 10 world with circular obstacles. Edges are checked exactly with a segment-to-circle distance test. For segment geometry in general, see segment intersection.

import math, random

OBST = [(3.0, 3.0, 1.5), (6.5, 6.0, 1.8), (3.0, 7.5, 1.2), (7.5, 2.5, 1.2)]

def seg_hits_circle(p, q, c):
    (x1, y1), (x2, y2), (cx, cy, r) = p, q, c
    dx, dy = x2 - x1, y2 - y1
    L2 = dx * dx + dy * dy
    t = 0.0 if L2 == 0 else max(0.0, min(1.0, ((cx-x1)*dx + (cy-y1)*dy) / L2))
    return (x1 + t*dx - cx) ** 2 + (y1 + t*dy - cy) ** 2 <= r * r

def free(p, q):
    return not any(seg_hits_circle(p, q, c) for c in OBST)

def rrt(start, goal, step=0.5, goal_bias=0.05, goal_tol=0.5, max_iter=5000, seed=0):
    rng = random.Random(seed)
    nodes, parent = [start], [None]
    for it in range(max_iter):
        s = goal if rng.random() < goal_bias else (rng.uniform(0, 10), rng.uniform(0, 10))
        i = min(range(len(nodes)), key=lambda k: math.dist(nodes[k], s))
        d = math.dist(nodes[i], s)
        if d == 0:
            continue
        t = min(1.0, step / d)
        new = (nodes[i][0] + t * (s[0] - nodes[i][0]), nodes[i][1] + t * (s[1] - nodes[i][1]))
        if not free(nodes[i], new):
            continue
        nodes.append(new); parent.append(i)
        if math.dist(new, goal) <= goal_tol and free(new, goal):
            nodes.append(goal); parent.append(len(nodes) - 2)
            path, k = [], len(nodes) - 1
            while k is not None:
                path.append(nodes[k]); k = parent[k]
            return path[::-1]
    return None

def shortcut(path, tries=200, seed=0):
    rng, path = random.Random(seed), list(path)
    for _ in range(tries):
        i, j = sorted(rng.sample(range(len(path)), 2))
        if j - i > 1 and free(path[i], path[j]):
            path = path[:i + 1] + path[j:]
    return path

Worked example. Plan from (0.5, 0.5) to (9.5, 9.5). The straight line, of length 12.73, passes through two obstacles. With seed 0 the planner succeeds after 175 iterations and 124 nodes. Its path has 34 waypoints and length 16.03, and it zig-zags because every edge points at a random sample. Random shortcutting (try to connect two random waypoints directly and drop everything between them) cuts it to 4 waypoints and length 13.56. Over 20 seeds the median was 191.5 iterations (range 138 to 312), with median raw length 16.04 and median shortcut length 13.75. Always post-process an RRT path: a raw tree path is never what you should send to a controller.

Tuning: goal bias, step size, tolerance

Three parameters dominate.

Goal bias p. Over 50 seeds on the map above, iterations to first solution were:

Some bias helps a lot. Too much turns the planner into a greedy straight-line attempt that keeps hitting the same obstacle, and the worst case gets worse. Values between 0.05 and 0.2 are a sensible start; tune on your own maps.

Step size eta. Large steps cross open space quickly but fail more collision checks near obstacles and cannot enter gaps narrower than the step allows. Small steps give fine trees and many more nearest-neighbour queries. Set eta to a few percent of the workspace diameter, then check behaviour in the tightest passage you care about.

Goal tolerance and sampling bounds. A tight tolerance needs many goal-biased tries. Bounds much larger than the reachable workspace waste samples.

Where the time goes: collisions and neighbours

Profile any real planner and you will find two hot spots.

Collision checking usually costs the most. For robots made of meshes it is done by a geometry library using bounding volume hierarchies. Edges are often checked by sampling points along the motion at a fixed resolution. That is a correctness risk: if the resolution is coarser than the thinnest obstacle, the edge can tunnel through it. Set the resolution from the smallest obstacle and the largest per-step motion of any robot point. Lazy variants skip edge checks until a candidate path exists, then check only that path and remove the bad edges.

Nearest-neighbour search is O(n) per iteration with a linear scan, so a run of n iterations is O(n^2). Above a few thousand nodes, use a k-d tree or an approximate index. The metric matters as much as the index. A heading angle wraps around, so 359 degrees is close to 1 degree. Translation and rotation must be weighted into one distance, and a bad weighting makes the tree extend from the wrong nodes.

RRT-Connect

RRT-Connect (Kuffner and LaValle, 2000) grows two trees, one from the start and one from the goal. Each iteration extends one tree towards a random sample. The other tree then tries to connect to the new node by stepping greedily towards it until it arrives or hits an obstacle. Then the trees swap roles. In open spaces it is usually much faster than single-tree RRT, and it is the default choice for single-query manipulation planning. It needs a goal that is a single configuration, or a sampled set of them, and like RRT it makes no promise about path quality.

RRT* and optimal paths

Plain RRT is not asymptotically optimal. Karaman and Frazzoli (2011) showed that the cost of its best path converges to a suboptimal value with probability one, because nodes never change parent. RRT* adds two steps after creating x_new. First, choose parent: among the nodes within radius r, connect x_new to the one that gives the lowest cost-from-start through a collision-free edge. Second, rewire: for each of those neighbours, if going through x_new is cheaper, change its parent to x_new. The radius shrinks like gamma (log n / n)^(1/d) in d dimensions. That keeps the expected number of neighbours logarithmic while keeping asymptotic optimality, provided gamma is above a threshold that depends on the free-space volume and the dimension.

def rrt_star(start, goal, n_iter=3000, step=0.5, gamma=6.0, goal_tol=0.5, seed=0):
    rng = random.Random(seed)
    nodes, parent, cost, kids = [start], [None], [0.0], [set()]
    def propagate(k):                      # a rewire changes every descendant's cost
        stack = [k]
        while stack:
            u = stack.pop()
            for v in kids[u]:
                cost[v] = cost[u] + math.dist(nodes[u], nodes[v]); stack.append(v)
    for _ in range(n_iter):
        s = goal if rng.random() < 0.05 else (rng.uniform(0, 10), rng.uniform(0, 10))
        i = min(range(len(nodes)), key=lambda k: math.dist(nodes[k], s))
        d = math.dist(nodes[i], s)
        if d == 0: continue
        t = min(1.0, step / d)
        new = (nodes[i][0] + t*(s[0] - nodes[i][0]), nodes[i][1] + t*(s[1] - nodes[i][1]))
        if not free(nodes[i], new): continue
        n = len(nodes)
        r = min(gamma * math.sqrt(math.log(n + 1) / (n + 1)), 2 * step)   # d = 2
        near = [k for k in range(n) if math.dist(nodes[k], new) <= r]
        c_best, p_best = cost[i] + math.dist(nodes[i], new), i            # choose parent
        for k in near:
            c = cost[k] + math.dist(nodes[k], new)
            if c < c_best and free(nodes[k], new):
                c_best, p_best = c, k
        nodes.append(new); parent.append(p_best); cost.append(c_best); kids.append(set())
        kids[p_best].add(n)
        for k in near:                                                    # rewire
            c = c_best + math.dist(new, nodes[k])
            if c < cost[k] and free(new, nodes[k]):
                kids[parent[k]].discard(k); kids[n].add(k)
                parent[k], cost[k] = n, c
                propagate(k)
    ends = [k for k in range(len(nodes))
            if math.dist(nodes[k], goal) <= goal_tol and free(nodes[k], goal)]
    return min((cost[k] + math.dist(nodes[k], goal) for k in ends), default=None)

The propagate call is easy to forget. Without it, descendants keep stale costs, later choose-parent decisions use wrong numbers, and the tree improves more slowly than it should. On the same map and five seeds, the median best-path cost was 13.97 after 500 iterations, 13.91 after 1,000, 13.82 after 3,000 and 13.60 after 6,000. Compare plain RRT plus shortcutting at 13.75 over 20 seeds; with five and twenty seeds the comparison is rough. Shortcutting is cheap and often good enough, but it can only straighten the route RRT happened to find, around the same obstacles. RRT* keeps improving and can switch routes. Once a first solution of cost c exists, Informed RRT* (Gammell and colleagues, 2014) samples only inside the ellipse of points whose straight-line detour through them costs less than c, which speeds convergence a great deal.

Kinodynamic planning and libraries

A car cannot move sideways, and a drone has momentum. In kinodynamic RRT the steer step simulates the system forward under a sampled or chosen control for a short duration, so every edge is a feasible trajectory. Nodes then hold full state, including velocity. The difficulty moves into the metric. Euclidean distance between states says little about how hard one state is to reach from another, and a poor metric makes the tree extend from nodes that cannot reach the sample. Use a cost-to-go approximation where one exists. Plain RRT, which never rewires, is the usual choice here, because RRT* needs a steering function that connects two states exactly, and many systems lack one.

Planning libraries such as OMPL ship RRT, RRT-Connect, RRT* and Informed RRT*, together with state spaces, collision interfaces and path simplifiers. Use a library in production. Write your own only to learn, or when your state space is unusual.

Failure modes

• Narrow passages. The Voronoi cells that lead into a corridor are tiny, so the tree rarely enters it and success rates collapse. Use bridge or obstacle-biased sampling, or bidirectional search.
• Edge tunnelling. A discretized edge check skips a thin obstacle. Derive the resolution from geometry, and add a test map containing a one-cell wall.
• No timeout semantics. RRT cannot report that no path exists. Give each query a time or iteration budget, return a typed "no solution in budget" result, and let the caller choose between retrying, relaxing the goal and escalating.
• Non-reproducible runs. A planner seeded from the clock produces a bug that only occurs on some runs. Log the seed with every query.
• Jerky output. Raw tree paths are full of corners. Shortcut them, then smooth with time parameterization that respects velocity and acceleration limits before execution.
• Dynamic obstacles. A path planned once against a static snapshot goes stale. Replan on a cycle, or pair RRT with an incremental replanner such as Lifelong Planning A* on a coarse grid.

Trade-offs

What to do next

1. Run the planner above. Add a narrow corridor to OBST and measure how the success rate inside 5,000 iterations falls as the corridor narrows.
2. Implement RRT-Connect on the same map and compare median iterations with single-tree RRT.
3. Replace the linear nearest-neighbour scan with a k-d tree and time 20,000 iterations.
4. Run rrt_star with and without propagate and plot cost against iterations.
5. For a real robot, prototype in a planning library, log seeds and budgets, and add a thin-wall regression test for collision resolution.