Stage 13 · Lesson 10/16

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.

  1. Sample a random configuration x (sometimes, with a small probability, use the goal itself — the goal bias).
  2. Nearest: find the tree node closest to x.
  3. Steer: if x is far, take only one step of fixed size (the growth factor, 10° in our code) towards it.
  4. Check: is the motion from the nearest node to the new point free? (Lesson 8 motion check.)
  5. 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.
Thin black branching lines spreading out from a single point in the middle of a white rectangle
An RRT early in its growth: a few long branches already reach out in every direction from the start in the centre. (A frame from the animation by Javed Hossain.)Photo: Javed Hossain · CC BY-SA 3.0 · Source: Wikimedia Commons · resized, converted to WebP

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”.

A square split into coloured polygons, each containing a black dot
A Voronoi diagram for a set of seed points: every coloured region holds the points closest to its seed. In an RRT the seeds are tree nodes; big regions at the frontier attract more random samples, so the tree grows outwards.Photo: Jahobr · CC0 · Source: Wikimedia Commons · resized, converted to WebP

3. Goal bias and step size

Too much goal bias and the tree keeps bumping into the obstacle in front of the goal; too little and it wanders. Our values are classroom examples, not tuned industrial settings.
KnobWhat it doesOur value
Step size (growth factor)Maximum length of each new branch; controls the rate of growth10° in joint space
Goal biasProbability of sampling the goal itself; higher = more greedy towards the goal10%
Goal toleranceHow close a node must be to count as “reached”5°
Edge check stepLesson 8 motion check resolution1°

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

Sampling-based planningRRT concepts

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 · Python
"""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

Sampling-based planningRRT concepts

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°).

Safety: servos get their own supply — never a GPIO pin or the UNO 5 V pin. Use a soft obstacle (a foam cup) until your keep-out box is measured and padded. Moves are slowed to 1° every 15 ms; keep fingers out of the arm’s reach and keep a hand near the supply switch.
PartConnectionNote
Shoulder servo (SG90-class)signal → D9servo degrees used directly (0…180)
Elbow servo (SG90-class)signal → D10same
Servo supplyred → +5–6 V, brown → supply GNDseparate supply (about 2 A); supply GND joined to UNO GND
GO buttonD3 → button → GNDINPUT_PULLUP
A0leave unconnectedfloating input seeds the random generator
tiny_rrt.ino · Arduino C++
// 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);
}
Serial Monitor (115200) — the shape of a run (numbers change every time) · Text
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

SymptomLikely causeFix
Tree never reaches the goalgoal in collision, step too small, too few iterationsCheck the goal pose first; larger step; more iterations
Tree keeps hitting the same obstaclegoal bias too highLower the goal bias (5–10%)
Path very zig-zagplain RRTShortcut the path, or use RRT*
RRT* very slowradius too big; subtree updatesSmaller radius; better nearest-neighbour data structure
Robot clips an obstacleedge check too coarseUse the Lesson 8 step bound
Bench: UNO resets or behaves strangelyRAM full, or servos on the UNO 5 V pinReduce MAXN; power servos from their own supply

Real world

RRTs in the real world

A white rectangle almost completely covered by a dense mesh of fine black branching lines
The same RRT after many more iterations: the tree has filled the whole space. RRTs are biased to grow into large unexplored areas. (A frame from the animation by Javed Hossain.)Photo: Javed Hossain · CC BY-SA 3.0 · Source: Wikimedia Commons · resized, converted to WebP
  • 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

  1. Run the script. Note the first-path iteration and cost for each RRT seed.
  2. Change GOAL_BIAS to 0.0 and 0.5. How does the first-path iteration change?
  3. Change RADIUS for RRT* to 10 and 40. How do cost and run time change?
  4. Add a counter of collision checks to `plan()`. How many more checks does RRT* use than RRT?
  5. Bench: measure your keep-out box, add 5° padding, and press GO 10 times (5 trips each way). Log nodes and iterations.
  6. Bench: disable shortcutting and compare the motion time.

Interactive · RRT planning simulator · Educational

Sampling-based planningRRT concepts

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

  1. Run rrt_lab.py and compare RRT and RRT* costs.
  2. Add your measured obstacle, start and goal to a copy of the script.
  3. Fill the 10-seed table: first-path iteration and cost at 300, 600 and 1200 iterations.
  4. Add shortcutting to the RRT path and write the cost before and after.
  5. Bench: upload tiny_rrt.ino with your keep-out box and log 10 runs: success count and tree size.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. If the tree never reaches the goal, check the goal itself is collision-free.
  2. RRT* gets shorter paths because it rewires nearby nodes. If RRT* cost does not drop, check the rewire step uses edge_free().
  3. MAXN is 120 nodes on the UNO. If runs fail, try a bigger STEP or more GOAL_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.py and 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

Self-attested for now — keep artifacts in your lab notebook.

Complete

Today's goal

0 of 1 done today

0-day streak
Daily goal:lessons, quizzes or assignments a day

Mark this lesson complete when you have worked the experiment, knowledge check, and assignment (even roughly).

Stuck somewhere?

Ask a doubt about this lesson

Saved with this lesson's id and your email so we can reply. Privacy · or email contact@aihubstation.in