Robot Perception, Planning & Manipulation · Lesson 10
RRT and RRT* Concepts
Est. 60–75 min
Why
Why this lesson matters
An RRT grows a tree from the start configuration using random samples. It uses the same two checks you built in Lessons 7–8: “is this pose free?” and “is this motion free?”. It needs no roadmap, so it is ideal for single queries in a world that keeps changing.
But the first path an RRT finds is often long and zig-zag, and waiting longer does not fix it. RRT* adds two small steps — choose the cheapest parent and rewire neighbours — so the path keeps getting better as the tree grows. In this lesson you will run both on the desk arm and see the numbers for yourself, then run a small RRT on the Arduino UNO.
Concept
Growing a tree through free space
1. The RRT loop
RRTs were developed by Steven M. LaValle and James J. Kuffner Jr. The tree is rooted at the start configuration. Each round: draw a random sample, find the nearest node in the tree, and try to connect them. If the connection passes entirely through free space, the new state is added to the tree.
- Sample a random configuration x (sometimes, with a small probability, use the goal itself — the goal bias).
- Nearest: find the tree node closest to x.
- Steer: if x is far, take only one step of fixed size (the growth factor, 10° in our code) towards it.
- Check: is the motion from the nearest node to the new point free? (Lesson 8 motion check.)
- Add the new node with the nearest node as its parent. If it is close to the goal, follow parents back to the start: that is your path.
2. Why it explores so fast: the Voronoi bias
With uniform sampling, the chance that a node is chosen for expansion is proportional to the size of its Voronoi region — the area of space closer to it than to any other node. Nodes on the frontier of the tree have the biggest Voronoi regions, so the tree preferentially grows towards large unexplored areas. Nobody programmed “go explore”; it falls out of “extend the nearest node”.

3. Goal bias and step size
| Knob | What it does | Our value |
|---|---|---|
| Step size (growth factor) | Maximum length of each new branch; controls the rate of growth | 10° in joint space |
| Goal bias | Probability of sampling the goal itself; higher = more greedy towards the goal | 10% |
| Goal tolerance | How close a node must be to count as “reached” | 5° |
| Edge check step | Lesson 8 motion check resolution | 1° |
4. The problem with plain RRT: the first path stays
Karaman and Frazzoli (2010) proved that, under mild technical conditions, the cost of the best path in an RRT converges almost surely to a non-optimal value. In simple words: running RRT longer does not make the path short. The tree never goes back to fix old branches.
5. RRT*: choose parent, then rewire
In the same work they introduced RRT*, a tree version of their RRG algorithm that keeps its asymptotic optimality: as the number of samples grows, the best path cost converges to the optimum. In our implementation (rrt_lab.py), each new node does two extra things inside a radius of 25°:
- Choose parent: connect the new node to whichever neighbour gives the cheapest total cost from the start (if that motion is free) — not just the nearest one.
- Rewire: for each neighbour, if going *through the new node* is cheaper than its current route and the motion is free, change its parent to the new node (and lower the cost of its whole subtree).
Visual
Visualise it: a tree that explores
A tree grows from the start: sample, nearest, step, check, add. Frontier branches grow fastest because they own the biggest empty regions. (Educational explorer, not an industrial planner.)
Interactive · RRT planning simulator · Educational
Click Grow — educational RRT samples toward free space.
Toy 2D RRT/RRT* — not OMPL / not MoveIt / not industrial sampling planners.
Code lab
Code lab: RRT and RRT* on the desk arm
rrt_lab.py uses the same desk arm and bottle as Lessons 6 and 9 (bottle at (19, 10) cm, start (−29.4°, 54.1°), goal (51.8°, 59.4°)). Part 1 runs plain RRT with three seeds; part 2 runs RRT* with the same seeds for 300, 600 and 1200 iterations; part 3 averages 10 seeds. Path cost = joint travel in degrees.
"""rrt_lab.py — Stage 13 L10 (AIHUB STATION lab)
RRT and RRT* for the 2-link desk arm, same scene as L06/L09 (example values, metres):
links 0.15 m and 0.12 m, link radius 1.5 cm, bottle r 3.5 cm at (0.19, 0.10).
Joint limits q1 -180..180, q2 -150..150 deg.
Run: python3 rrt_lab.py
"""
import math, random
L1, L2, LINK_R = 0.15, 0.12, 0.015
BOTTLE, BOTTLE_R = (0.19, 0.10), 0.035
Q1, Q2 = (-180.0, 180.0), (-150.0, 150.0)
START, GOAL = (-29.4, 54.1), (51.8, 59.4)
STEP, GOAL_BIAS, GOAL_TOL, RADIUS = 10.0, 0.10, 5.0, 25.0
def fk(q1, q2):
a, b = math.radians(q1), math.radians(q1 + q2)
e = (L1*math.cos(a), L1*math.sin(a))
return e, (e[0] + L2*math.cos(b), e[1] + L2*math.sin(b))
def seg_dist(a, b, p):
ax, ay = a; bx, by = b; px, py = p
L = (bx-ax)**2 + (by-ay)**2
s = max(0, min(1, ((px-ax)*(bx-ax) + (py-ay)*(by-ay)) / L))
return math.hypot(ax + s*(bx-ax) - px, ay + s*(by-ay) - py)
def free(q, pad=0.003):
if not (Q1[0] <= q[0] <= Q1[1] and Q2[0] <= q[1] <= Q2[1]): return False
e, t = fk(*q)
return min(seg_dist((0, 0), e, BOTTLE), seg_dist(e, t, BOTTLE)) >= BOTTLE_R + LINK_R + pad
def dist(a, b): return math.hypot(a[0]-b[0], a[1]-b[1])
def edge_free(a, b, step=1.0):
n = max(1, math.ceil(max(abs(a[0]-b[0]), abs(a[1]-b[1])) / step))
return all(free((a[0] + (b[0]-a[0])*i/n, a[1] + (b[1]-a[1])*i/n)) for i in range(1, n + 1))
def steer(a, b):
d = dist(a, b)
if d <= STEP: return b
return (a[0] + (b[0]-a[0])*STEP/d, a[1] + (b[1]-a[1])*STEP/d)
def plan(iters, star, seed):
rng = random.Random(seed)
nodes, parent, cost = [START], [None], [0.0]
best, first = None, None
for it in range(1, iters + 1):
x = GOAL if rng.random() < GOAL_BIAS else (rng.uniform(*Q1), rng.uniform(*Q2))
near_i = min(range(len(nodes)), key=lambda i: dist(nodes[i], x)) # nearest node
new = steer(nodes[near_i], x) # grow one step
if not edge_free(nodes[near_i], new): continue
p, c = near_i, cost[near_i] + dist(nodes[near_i], new)
if star:
nbrs = [i for i in range(len(nodes)) if dist(nodes[i], new) <= RADIUS]
for i in nbrs: # choose the cheapest parent
ci = cost[i] + dist(nodes[i], new)
if ci < c and edge_free(nodes[i], new): p, c = i, ci
nodes.append(new); parent.append(p); cost.append(c); k = len(nodes) - 1
if star:
for i in nbrs: # rewire neighbours through new
ci = c + dist(new, nodes[i])
if ci < cost[i] and edge_free(new, nodes[i]):
delta = cost[i] - ci; parent[i] = k
stack = [i] # update the subtree costs
while stack:
j = stack.pop(); cost[j] -= delta
stack += [m for m in range(len(nodes)) if parent[m] == j]
for i in (range(len(nodes)) if star else [k]):
if dist(nodes[i], GOAL) <= GOAL_TOL:
total = cost[i] + dist(nodes[i], GOAL)
if best is None or total < best: best = total
if first is None: first = (it, total, len(nodes))
return first, best, len(nodes)
print("1. RRT: grow a tree from the start until it reaches the goal")
for seed in (1, 2, 3):
first, best, n = plan(600, False, seed)
print(f" seed {seed}: first reached goal at iteration {first[0]} ({first[2]} nodes), path cost {first[1]:.0f} deg;"
f" after 600 iterations best {best:.0f} deg")
print("\n2. RRT*: same sampling, plus 'choose the cheapest parent' and 'rewire' within 25 deg")
for seed in (1, 2, 3):
row = []
for iters in (300, 600, 1200):
first, best, n = plan(iters, True, seed)
row.append(f"{best:.0f}" if best else "not yet")
print(f" seed {seed}: best path cost after 300 / 600 / 1200 iterations: {' / '.join(row)} deg")
print("\n3. Averages over 10 seeds at 1200 iterations")
for star in (False, True):
bests = [b for b in (plan(1200, star, s)[1] for s in range(10)) if b]
print(f" {'RRT*' if star else 'RRT '}: solved {len(bests)}/10, mean best cost {sum(bests)/len(bests):.0f} deg, min {min(bests):.0f}, max {max(bests):.0f}")
print(f" (straight-line joint distance start->goal is {dist(START, GOAL):.0f} deg, but that line hits the bottle)")Output (run with python3 rrt_lab.py)
1. RRT: grow a tree from the start until it reaches the goal seed 1: first reached goal at iteration 373 (226 nodes), path cost 219 deg; after 600 iterations best 219 deg seed 2: first reached goal at iteration 261 (164 nodes), path cost 211 deg; after 600 iterations best 211 deg seed 3: first reached goal at iteration 525 (379 nodes), path cost 498 deg; after 600 iterations best 498 deg 2. RRT*: same sampling, plus 'choose the cheapest parent' and 'rewire' within 25 deg seed 1: best path cost after 300 / 600 / 1200 iterations: not yet / 177 / 173 deg seed 2: best path cost after 300 / 600 / 1200 iterations: 184 / 158 / 154 deg seed 3: best path cost after 300 / 600 / 1200 iterations: not yet / 441 / 164 deg 3. Averages over 10 seeds at 1200 iterations RRT : solved 10/10, mean best cost 333 deg, min 192, max 624 RRT*: solved 10/10, mean best cost 165 deg, min 150, max 198 (straight-line joint distance start->goal is 81 deg, but that line hits the bottle)
Try it: press “Grow ×8” several times. Watch how the frontier branches grow fastest — the Voronoi bias at work. (Educational explorer, not an industrial planner.)
Interactive · RRT planning simulator · Educational
Click Grow — educational RRT samples toward free space.
Toy 2D RRT/RRT* — not OMPL / not MoveIt / not industrial sampling planners.
Bench build: a tiny RRT on the Arduino UNO
tiny_rrt.ino grows an RRT of up to 120 nodes on the UNO when you press GO, from the current pose to GOAL (150°, 90°), with 10° steps, 10% goal bias and 1° motion checks against a keep-out box in joint space (measured on your bench with a foam cup, plus 5° padding). It then shortcuts the path — jumping straight to the farthest later node with a free motion — and plays it slowly. Press GO again to plan back to HOME (30°, 90°).
| Part | Connection | Note |
|---|---|---|
| Shoulder servo (SG90-class) | signal → D9 | servo degrees used directly (0…180) |
| Elbow servo (SG90-class) | signal → D10 | same |
| Servo supply | red → +5–6 V, brown → supply GND | separate supply (about 2 A); supply GND joined to UNO GND |
| GO button | D3 → button → GND | INPUT_PULLUP |
| A0 | leave unconnected | floating input seeds the random generator |
// tiny_rrt.ino — Stage 13 L10 (AIHUB STATION lab)
// A small RRT (Rapidly-exploring Random Tree) running ON the Arduino UNO.
// Press GO: the UNO grows a tree from the current pose towards GOAL,
// 10-degree steps, 10% goal bias, every motion checked every 1 degree
// against a keep-out box in JOINT space. Then it shortcuts the path and plays it.
//
// Wiring (Arduino UNO):
// Shoulder servo signal -> D9, elbow servo signal -> D10
// Servo red/brown -> SEPARATE 5-6 V supply (about 2 A), GND joined to UNO GND
// GO button: D3 -> button -> GND (INPUT_PULLUP)
// Serial Monitor 115200 shows tree size and path
#include <Servo.h>
const int MAXN = 120; // tree nodes that fit comfortably in 2 KB RAM
const float STEP = 10.0; // growth step in degrees
const int GOAL_BIAS = 10; // percent of samples that are the goal itself
// Keep-out box in joint space (servo degrees), measured on YOUR bench.
const int BOX_Q1_MIN = 70, BOX_Q1_MAX = 110, BOX_Q2_MIN = 60, BOX_Q2_MAX = 180;
const int PAD = 5;
int GOAL_A = 150, GOAL_B = 90; // first goal; after arriving we swap goal and HOME
Servo s1, s2;
float ta[MAXN], tb[MAXN];
int8_t par[MAXN];
int curA = 30, curB = 90;
bool poseFree(float a, float b) {
if (a < 0 || a > 180 || b < 0 || b > 180) return false;
return !(a > BOX_Q1_MIN - PAD && a < BOX_Q1_MAX + PAD && b > BOX_Q2_MIN - PAD && b < BOX_Q2_MAX + PAD);
}
bool motionFree(float a0, float b0, float a1, float b1) { // check every 1 degree
int n = max(1, (int)ceil(max(fabs(a1 - a0), fabs(b1 - b0))));
for (int k = 1; k <= n; k++)
if (!poseFree(a0 + (a1 - a0) * k / n, b0 + (b1 - b0) * k / n)) return false;
return true;
}
void moveTo(int a, int b) { // synchronised slow move, 1 deg per 15 ms
int steps = max(abs(a - curA), abs(b - curB));
int a0 = curA, b0 = curB;
for (int k = 1; k <= steps; k++) {
s1.write(a0 + (long)(a - a0) * k / steps);
s2.write(b0 + (long)(b - b0) * k / steps);
delay(15);
}
curA = a; curB = b;
}
int growRRT() { // returns index of node that reached the goal, or -1
int n = 1; ta[0] = curA; tb[0] = curB; par[0] = -1;
for (int it = 0; it < 2000 && n < MAXN; it++) {
float xa, xb;
if (random(100) < GOAL_BIAS) { xa = GOAL_A; xb = GOAL_B; }
else { xa = random(0, 181); xb = random(0, 181); }
int near = 0; float best = 1e9; // nearest node
for (int i = 0; i < n; i++) {
float d = sq(ta[i] - xa) + sq(tb[i] - xb);
if (d < best) { best = d; near = i; }
}
float d = sqrt(best), na = xa, nb = xb; // steer one step
if (d > STEP) { na = ta[near] + (xa - ta[near]) * STEP / d; nb = tb[near] + (xb - tb[near]) * STEP / d; }
if (!motionFree(ta[near], tb[near], na, nb)) continue;
ta[n] = na; tb[n] = nb; par[n] = near; n++;
if (motionFree(na, nb, GOAL_A, GOAL_B) && sqrt(sq(na - GOAL_A) + sq(nb - GOAL_B)) <= STEP) {
Serial.print(F("tree reached goal: ")); Serial.print(n); Serial.print(F(" nodes, iteration ")); Serial.println(it);
return n - 1;
}
}
Serial.print(F("no path: tree has ")); Serial.print(n); Serial.println(F(" nodes"));
return -1;
}
void setup() {
Serial.begin(115200);
pinMode(3, INPUT_PULLUP);
randomSeed(analogRead(A0));
s1.attach(9); s2.attach(10);
s1.write(curA); s2.write(curB);
Serial.println(F("tiny_rrt ready - press GO"));
}
void loop() {
if (digitalRead(3) != LOW) return;
delay(30);
int g = growRRT();
if (g < 0) { delay(500); return; } // never move without a checked path
int path[MAXN], len = 0;
for (int i = g; i >= 0; i = par[i]) path[len++] = i; // goal -> start
// shortcut: from each point, jump to the farthest later point with a free motion
int i = len - 1; float a = ta[path[i]], b = tb[path[i]];
Serial.print(F("path: "));
while (i > 0) {
int j = 0;
while (j < i - 1 && !motionFree(a, b, ta[path[j]], tb[path[j]])) j++;
a = ta[path[j]]; b = tb[path[j]]; i = j;
Serial.print((int)a); Serial.print(','); Serial.print((int)b); Serial.print(F(" "));
moveTo((int)a, (int)b);
}
Serial.println();
if (motionFree(curA, curB, GOAL_A, GOAL_B)) moveTo(GOAL_A, GOAL_B);
Serial.println(F("at goal - press GO again to plan back"));
int t = GOAL_A; GOAL_A = (t == 150) ? 30 : 150; GOAL_B = 90; // next goal: the other side
delay(800);
}tiny_rrt ready - press GO
tree reached goal: <nodes> nodes, iteration <n>
path: <q1>,<q2> <q1>,<q2> ...
at goal - press GO again to plan back
or, if the tree did not reach the goal in 2000 tries:
no path: tree has <nodes> nodes (the arm does not move)Debugging
| Symptom | Likely cause | Fix |
|---|---|---|
| Tree never reaches the goal | goal in collision, step too small, too few iterations | Check the goal pose first; larger step; more iterations |
| Tree keeps hitting the same obstacle | goal bias too high | Lower the goal bias (5–10%) |
| Path very zig-zag | plain RRT | Shortcut the path, or use RRT* |
| RRT* very slow | radius too big; subtree updates | Smaller radius; better nearest-neighbour data structure |
| Robot clips an obstacle | edge check too coarse | Use the Lesson 8 step bound |
| Bench: UNO resets or behaves strangely | RAM full, or servos on the UNO 5 V pin | Reduce MAXN; power servos from their own supply |
Real world
RRTs in the real world

- Autonomous robots: RRTs handle obstacles and differential constraints (nonholonomic and kinodynamic) and have been widely used in autonomous robotic motion planning.
- Many variants: RRT*, Informed RRT*, RRT*-Smart, LQR-RRT* and others build on the same tree idea.
- Robot arms: the same two checks (pose free, motion free) work for 6 or 7 joints; only the sampling space changes.
- Your lab: always report results over many seeds. One lucky run proves nothing.
Think
Think about it
Think
Your RRT found a path of 498° in 525 iterations. Your friend says “let it run for 10 000 iterations and the path will get shorter.” Is she right?
Experiment
Try it in the lab
About 60 minutes. Python 3 for the script; Arduino UNO, two servos on a 2-link arm with their own supply, a push button and a foam cup for the bench.
Experiment
Grow trees, compare costs
You will need
- `rrt_lab.py` (Python 3, no libraries)
- RRT planning simulator (in your browser)
- Arduino UNO, 2 × SG90-class servos on a 2-link arm, separate 5–6 V supply, push button, jumper wires
- Foam cup as a soft obstacle
Steps
- Run the script. Note the first-path iteration and cost for each RRT seed.
- Change GOAL_BIAS to 0.0 and 0.5. How does the first-path iteration change?
- Change RADIUS for RRT* to 10 and 40. How do cost and run time change?
- Add a counter of collision checks to `plan()`. How many more checks does RRT* use than RRT?
- Bench: measure your keep-out box, add 5° padding, and press GO 10 times (5 trips each way). Log nodes and iterations.
- Bench: disable shortcutting and compare the motion time.
Interactive · RRT planning simulator · Educational
Click Grow — educational RRT samples toward free space.
Toy 2D RRT/RRT* — not OMPL / not MoveIt / not industrial sampling planners.
RRT explores quickly from the start and is great for one query in a changing world. Plain RRT paths do not improve with time; RRT* fixes this by repairing the tree locally.
Knowledge check
Check your understanding
Knowledge check
1. An RRT tree is rooted at…
2. In each RRT round, the random sample is connected to…
3. Why does an RRT grow towards unexplored areas?
4. The growth factor (step size) controls…
5. A very high goal bias can…
6. If you run plain RRT much longer, its best path cost…
7. RRT* improves paths by…
8. In our 10-seed test at 1200 iterations, mean costs were…
Mini assignment
Hands-on assignment
Mini assignment
Tree race on your desk
Compare RRT and RRT* on your own desk scene, and run a tiny RRT on the bench.
Deliverables
- Your measured obstacle(s), start and goal added to `rrt_lab.py` (or a copy).
- A table over 10 seeds: first-path iteration, cost at 300/600/1200 iterations, and collision checks, for RRT and RRT*.
- A shortcutting step applied to the RRT path, with cost before and after.
- A bench log of 10 `tiny_rrt.ino` runs with your keep-out box: success count and tree sizes.
Do this, step by step
- Run
rrt_lab.pyand compare RRT and RRT* costs. - Add your measured obstacle, start and goal to a copy of the script.
- Fill the 10-seed table: first-path iteration and cost at 300, 600 and 1200 iterations.
- Add shortcutting to the RRT path and write the cost before and after.
- Bench: upload
tiny_rrt.inowith your keep-out box and log 10 runs: success count and tree size.
Stuck? Hints, one at a time
- If the tree never reaches the goal, check the goal itself is collision-free.
- RRT* gets shorter paths because it rewires nearby nodes. If RRT* cost does not drop, check the rewire step uses
edge_free(). MAXNis 120 nodes on the UNO. If runs fail, try a biggerSTEPor moreGOAL_BIAS, not more nodes.
Challenges (pick by level)
- 🟢 Beginner: On graph paper, grow an RRT by hand: roll dice for random points, always extend the nearest node by 1 cm. Stop when you touch the goal.
- 🟡 Intermediate: Add shortcutting to
rrt_lab.pyand compare shortcut-RRT with RRT* over 10 seeds. - 🟠 Advanced: Grow two trees, one from start and one from goal, and try to connect them every round. Compare iterations to the first path.
- 🔴 Expert: Add a third joint (wrist) with the self-collision check from Lesson 7 and run RRT* in 3D joint space.
- ⚫ Research: Read the abstract of Karaman and Frazzoli, “Incremental Sampling-based Algorithms for Optimal Motion Planning” (arXiv 1005.0416). Explain in one page what “asymptotically optimal” means and why plain RRT is not.
Next step
You can now plan a collision-free motion for the arm. But a motion is only half of manipulation — the arm must actually hold something. In Lesson 11 — Robot End Effectors and Grippers you will meet the tools at the end of the arm.
Look back: Lesson 9: Sampling-Based Planning. Next: Lesson 11: Robot End Effectors and Grippers. Related: Stage 14 — Advanced Robotics & RL.
Sources
- Wikipedia — Rapidly exploring random tree — LaValle and Kuffner; growth from start, nearest node, growth factor, goal bias, Voronoi bias; nonholonomic/kinodynamic constraints; RRT* variants.
- arXiv 1005.0416 — Karaman & Frazzoli, Incremental Sampling-based Algorithms for Optimal Motion Planning — RRT best-path cost converges to a non-optimal value; RRG and RRT* asymptotic optimality.
- Arduino Servo library (GitHub) — controlling RC hobby servos.
- Photos: Wikimedia Commons (credits under each image).
Self-attested for now — keep artifacts in your lab notebook.
Complete
Today's goal
0 of 1 done today
Mark this lesson complete when you have worked the experiment, knowledge check, and assignment (even roughly).