explainer
RRT*: improve a path by rewiring the tree
Follow RRT* as it chooses cheaper parents, rewires nearby nodes, and updates every descendant's cost. Compare the first path with later improvements and understand what asymptotic optimality does and does not promise.
What you will learn
- Calculate a candidate parent's total path cost.
- Explain how a cheaper connection changes a tree without creating a cycle.
- Propagate a rewired node's cost change through all its descendants.
- Compare the first complete route with the best route after more samples.
- Separate asymptotic optimality from a finite run's measured result.
Before you start
A first collision-free path can take a long detour. RRT* keeps working after finding that path and uses new samples to repair expensive connections.
In the example below, the first route measures 6.709187 m. After 500 sample attempts, the best route measures 5.523152 m. You will trace the cost calculation behind that improvement and see why it does not certify the shortest possible path.
Keep searching after the first route
The basic RRT lesson grows a tree toward sampled targets and stops at its first checked goal connection. That gives a feasible route for the stated geometry.
RRT* adds two opportunities to reduce cost: choosing a cheaper parent for a new node and changing a nearby node's parent. It continues sampling until its budget ends. Karaman and Frazzoli introduced this approach when analyzing the cost limits of sampling-based planners.
Here the cost is the total Euclidean length of the robot center's path. For a tree node v with parent p, store:
C(v) = C(p) + ‖v − p‖₂
C(start) = 0
The robot is a translating disk with radius 0.2 m inside a 6 m square. A node records its center position. This model optimizes geometric distance; it leaves timing, turning dynamics, and actuator limits for other models.
Nonholonomic constraints explain why a wheeled robot's allowed velocity also matters. Its local connections must follow that motion model as well as clear the obstacles.
Propose a new configuration
Each attempt starts with the same ingredients as RRT:
- Sample a target position.
- Find the existing tree node nearest to that target.
- Move toward the target by at most the extension limit η.
- Check the whole proposed segment for collision.
Reject the proposal if that initial segment fails. Otherwise, consider the proposed endpoint for insertion. LaValle's discussion of exploration trees develops the nearest-point and growth ideas behind this process.
The widget samples the exact goal on 10% of attempts. Other targets lie in the square inset by the robot radius. Targets inside an inflated obstacle can still produce useful extensions that stop before reaching it.
Every attempt consumes three values from a seeded pseudorandom sequence, including goal-directed attempts. Keeping the settings fixed and raising the budget therefore extends the same run. Changing the extension length changes the tree that grows from those targets.
Proposals of at most 10⁻¹⁰ m do not add a node. Once the tree contains the exact goal, repeated goal samples can produce these zero-length proposals. The rejected count includes them along with colliding extensions.
Choose a parent by total route cost
The nearest node provides the first valid parent candidate. RRT* also considers nodes within a neighborhood around the proposed endpoint x_new.
For each candidate p, add the parent's current cost to the proposed edge length:
candidate(p) = C(p) + ‖x_new − p‖₂
A node with C = 2 m and a 0.3 m connection offers total cost 2.3 m. A farther node with C = 1.4 m and a 0.6 m connection offers 2.0 m. If both segments are clear, the farther node gives the cheaper route.
The widget checks every edge that could improve the current choice. It retains the nearest-node connection as a fallback even when the shrinking neighborhood excludes it. OMPL's RRTstar documentation describes choosing parents through cost ordering and collision checks.
This decision minimizes cost over the candidates considered at this insertion. It does not solve the full continuous planning problem.
Replace a costly branch connection
After inserting x_new, check whether reaching a nearby node v through it would be cheaper:
C(x_new) + ‖v − x_new‖₂ < C(v)
If the edge passes its collision check, replace v's parent with x_new. The root keeps its zero cost and never receives a parent. A node also cannot attach beneath one of its own descendants, which would create a cycle.
For a worked calculation, use a separate obstacle-free room with a 2.5 m connection limit. This miniature has the following tree:
| Node | Position (m) | Parent | Cost from S (m) |
|---|---|---|---|
| S | (1, 1) | None | 0 |
| A | (1, 3) | S | 2 |
| B | (3, 3) | A | 4 |
| C | (4, 3) | B | 5 |
| N | (2, 1) | S | 1 |
The new connection N → B has length √((3 − 2)² + (3 − 1)²) = √5 m. It gives B cost 1 + √5 ≈ 3.236068 m, improving its old cost of 4 m by about 0.763932 m.
Only B's parent changes, from A to N. This calculation uses a larger connection limit than the interactive room so the coordinates stay simple.
Update every descendant
C still attaches to B through the same 1 m edge. Its route now reaches B more cheaply, so C's cost must fall too:
C(B) = 1 + √5 m
C(C) = 2 + √5 m ≈ 4.236068 m
Every deeper descendant needs the same treatment. Leaving an old cost on C would make later parent comparisons use an incorrect route length. OMPL explicitly updates child costs when a parent's cost changes.
The implementation walks the affected subtree in parent-before-child order. For each node, it recomputes the parent's cost plus their unchanged edge length. Its descendant-update count records this work separately from the number of rewires.
Choose the neighborhood radius
The example uses a radius that shrinks with the tree size. In two dimensions, its formula is:
rₙ = min(η, γ√(ln(n)/n))
γ = 8 m
Here n counts the root, existing nodes, and the proposed new node. At n = 2 with η = 1 m, the cap gives rₙ = 1 m. At n = 1000, the same formula gives about 0.664903 m.
The exponent becomes 1/d in d dimensions. The neighborhood rule and a sufficiently large constant matter to the RRT* asymptotic analysis. The widget uses γ = 8 m as an illustrative setting; it does not estimate or certify a theorem's threshold for this sampling process.
The extension limit caps new and replacement edge lengths. A checked nearest edge remains available for initial parent selection when its length exceeds rₙ. Rewiring uses only nodes inside the radius.
Recheck the best complete route
A node near the goal becomes a goal candidate only when its final segment is no longer than η and passes the full collision check. For every such candidate v, evaluate:
goal_cost(v) = C(v) + ‖goal − v‖₂
best_cost = minᵥ goal_cost(v)
A rewire can improve an old candidate through one of its ancestors. The widget therefore recalculates the best complete cost after each insertion and its rewires. The best cost stays flat or decreases as the same run continues.
The dashed route preserves a copy of the first solution's coordinates. Later parent changes cannot reshape that comparison route. The amber route follows the current best parent chain and includes its checked final connection to the exact goal.
Compare the first route with later progress
The default room has one circular obstacle of radius 1 m. The robot's radius expands its forbidden center region to radius 1.2 m. Collision checking supplies the continuous segment test for this geometry.
With seed 7 and extension length 1 m, the first complete route appears at attempt 16. Change only the sample budget to compare prefixes of that run:
| Attempt budget | Best path (m) | Successful rewires |
|---|---|---|
| 50 | 6.709187 | 5 |
| 500 | 5.523152 | 433 |
| 1000 | 5.498180 | 760 |
The 500-attempt run saves 1.186035 m compared with its first solution, using unrounded costs for the subtraction. Its 433 rewires also trigger 874 descendant cost updates. A node can rewire more than once, so the rewire count can exceed the number of tree nodes.
Change the seed to 42. The first route changes to 5.828089 m, and the 500-attempt best changes to 5.521753 m. Sample order affects finite results; one successful run does not establish a general performance ranking.
Choose Solid barrier to see a failed run. Inflated obstacles join the top and bottom walls, separating the two endpoints in this geometry. The planner reports only that it exhausted its budget; the geometric barrier argument supplies the separate impossibility claim.
Read the optimality claim carefully
Asymptotic optimality means the returned cost converges to the optimum with probability one as samples grow without bound, under the theorem's assumptions. The analysis requires appropriate sampling and neighborhood rules, a suitable cost function, and feasible paths that satisfy its robustness conditions. Karaman and Frazzoli state those conditions and prove the limiting result.
This finite widget uses a pseudorandom generator, a fixed 10% goal-sampling mixture, floating-point arithmetic, and a maximum of 1000 attempts. The mixture differs from uniform free-space sampling. Neither the plotted decrease nor the chosen radius establishes the theorem for this implementation.
Its checks do establish concrete properties of the computed tree: every retained edge clears the disk geometry, parent chains reach the start, and stored costs agree with those chains. The best route remains a candidate solution at the budget limit.
A path with shorter geometric length can still need more work before execution. Path smoothing can remove unnecessary corners through checked shortcuts, while trajectory time scaling addresses speed and acceleration along a chosen path. Each operation has its own constraints.
Reproduce a rewire in Python
This standard-library example reproduces the obstacle-free miniature. It isolates parent replacement and descendant cost propagation; it does not run the room's sampler. The proposed N → B edge fits the miniature's 2.5 m limit, and the room has no obstacles.
from math import dist
points = {"S": (1, 1), "A": (1, 3), "B": (3, 3),
"C": (4, 3), "N": (2, 1)}
parent = {"S": None, "A": "S", "B": "A", "C": "B", "N": "S"}
cost = {"S": 0.0, "A": 2.0, "B": 4.0, "C": 5.0, "N": 1.0}
def rewire(node, new_parent):
if node == "S":
return False
ancestor = new_parent
while ancestor is not None:
if ancestor == node:
return False
ancestor = parent[ancestor]
edge = dist(points[node], points[new_parent])
candidate = cost[new_parent] + edge
if edge > 2.5 or candidate >= cost[node]:
return False
parent[node] = new_parent
cost[node] = candidate
queue = [node]
for ancestor in queue:
for child in points:
if parent[child] == ancestor:
cost[child] = cost[ancestor] + dist(points[ancestor], points[child])
queue.append(child)
return True
print(f"before: B={cost['B']:.6f}, C={cost['C']:.6f}")
print("rewired:", rewire("B", "N"))
print(f"after: B={cost['B']:.6f}, C={cost['C']:.6f}")
print("parents:", parent["B"], parent["C"])
print("cycle accepted:", rewire("N", "C"))
Expected output:
before: B=4.000000, C=5.000000
rewired: True
after: B=3.236068, C=4.236068
parents: N B
cycle accepted: False
The final call would place N beneath its own descendant C. The ancestor walk rejects it before changing either parents or costs.
Try it yourself
Exercise 1: carry a saving down the tree. A node costs 5.2 m from the start. A new parent costs 3.4 m, and their clear connection measures 0.6 m. The node has a child 0.8 m farther along a clear edge; calculate both new costs and the child's saving.
Show the descendant calculation
The node's new cost is 3.4 + 0.6 = 4.0 m. Its child originally cost 5.2 + 0.8 = 6.0 m, and now costs 4.0 + 0.8 = 4.8 m.
Both costs fall by 1.2 m. The child keeps its parent, but its stored route cost must change.
Exercise 2: update an old goal candidate. Candidate P costs 4.7 m from the start and has a clear 0.6 m goal edge. Candidate Q costs 4.9 m and has a clear 0.2 m goal edge. Which route leads initially, and what happens if a rewire lowers P's cost by 0.5 m?
Show the incumbent comparison
Initially, P offers 4.7 + 0.6 = 5.3 m, while Q offers 4.9 + 0.2 = 5.1 m. Q supplies the incumbent.
After the rewire, P offers 4.2 + 0.6 = 4.8 m and becomes the new best candidate. Its existing goal edge remains clear; only the route cost leading to it changed.
Sources and further study
- Karaman and Frazzoli: Sampling-based Algorithms for Optimal Motion Planning, including RRT* parent selection, rewiring, neighborhood rules, and asymptotic analysis.
- OMPL: RRTstar planner, including collision-aware parent selection and descendant cost updates.
- LaValle: Planning Algorithms, Section 5.5, for the exploration-tree foundation.
To reuse sampled configurations across several start and goal pairs, continue with probabilistic roadmaps.