Week 9 — Advanced Motion Planning Day 3 of 7 | Difficulty: ⭐⭐⭐⭐⭐
The Sampling Revolution
In the 1990s, robotics faced a crisis: industrial arms had 6+ joints, but no planner could handle the resulting high-dimensional C-space. Grid-based A* would require $100^6 = 10^{12}$ cells.
Sampling-based planners solved this by giving up on exactness and embracing randomness:
- They do not discretize C-space explicitly
- They sample random configurations and check collision
- They build a graph or tree implicitly, only where needed
The result: planners that scale to 100+ dimensional C-spaces.
Rapidly-Exploring Random Tree (RRT)
RRT grows a tree from the start configuration toward the goal by sampling random points in C-space:
import numpy as np
def rrt(start, goal, c_space_bounds, max_iter=10000, step_size=0.1):
"""
start, goal: np arrays of configuration
c_space_bounds: [(min, max), ...] for each DOF
"""
tree = {tuple(start): None} # node -> parent
nodes = [start]
for _ in range(max_iter):
# 1. Sample random configuration
if np.random.rand() < 0.05:
random_q = goal # bias toward goal
else:
random_q = np.random.uniform(
[b[0] for b in c_space_bounds],
[b[1] for b in c_space_bounds]
)
# 2. Find nearest node in tree
nearest = min(nodes, key=lambda n: np.linalg.norm(n - random_q))
# 3. Steer toward random point
direction = random_q - nearest
distance = np.linalg.norm(direction)
if distance > step_size:
new_q = nearest + direction / distance * step_size
else:
new_q = random_q
# 4. Collision check the edge
if is_collision_free_edge(nearest, new_q):
tree[tuple(new_q)] = tuple(nearest)
nodes.append(new_q)
# 5. Check if we can connect to goal
if np.linalg.norm(new_q - goal) < step_size and is_collision_free_edge(new_q, goal):
tree[tuple(goal)] = tuple(new_q)
return reconstruct_path(tree, goal)
return None # Failed
Why RRT Works
RRT has a Voronoi bias: nodes on the frontier of the tree have the largest Voronoi regions, so they are most likely to be the “nearest” to a random sample. This causes rapid, balanced exploration.
Key Data: RRT solves 6-DOF arm planning in ~0.1–1 seconds on modern CPUs. It is probabilistically complete: the probability of finding a solution approaches 1 as iterations → ∞ (if a solution exists).
RRT* (Asymptotically Optimal)
Standard RRT finds a feasible path, not necessarily the shortest. RRT* (Karaman & Frazzoli, 2011) fixes this:
- After adding a new node, search for nearby nodes within a ball of radius $r$
- If connecting through the new node produces a shorter path to any neighbor, rewire the tree
- Repeat until the tree converges to the shortest path
$$r = \gamma \left( \frac{\log n}{n} \right)^{1/d}$$
Where $n$ = number of nodes, $d$ = C-space dimension.
Key Data: RRT* is asymptotically optimal — the solution converges to the optimal path as iterations → ∞. Rewiring adds only ~20% overhead compared to standard RRT.
Probabilistic Roadmap (PRM)
PRM is a two-phase planner:
Phase 1: Learning
- Sample $N$ random configurations in $C_{free}$
- Connect each sample to its $k$ nearest neighbors with collision-free edges
- Store the resulting graph (the “roadmap”)
Phase 2: Query
- Connect the start and goal to the roadmap
- Run Dijkstra/A* on the roadmap to find a path
PRM excels in multi-query scenarios (e.g., a fixed warehouse where thousands of robots need paths between the same set of stations). The roadmap is built once and reused.
| RRT / RRT* | PRM | |
|---|---|---|
| Structure | Single tree (or two) | Pre-built graph |
| Queries | Single-query | Multi-query |
| Optimality | RRT: no; RRT*: asymptotic | Depends on roadmap density |
| Best for | One-off planning, changing environments | Fixed environments, repeated tasks |
Review Days 1–2
Day 1: C-space fundamentals. Day 2: Search-based planning (A*, Hybrid A*). Today: Sampling-based methods for the high-dimensional cases where grids fail.
Preview Day 4
Day 4: Trajectory optimization — CHOMP, STOMP, and TrajOpt for smooth, dynamically feasible motion.
FAQ
Q: Why does RRT use random sampling instead of systematic exploration?
A: Randomness avoids the curse of dimensionality. A systematic grid in 6D with 100 bins requires $10^{12}$ points. Random sampling concentrates computation where it matters — near obstacles and the goal.
Details: The Voronoi bias naturally balances exploration (frontier) and exploitation (goal bias). Random sampling also means RRT performance has variance — running it multiple times and keeping the best result improves reliability.
Q: What is the “step_size” parameter in RRT?
A: It controls the maximum distance between connected nodes. Too large: edges may pass through obstacles (collision checker may miss thin obstacles). Too small: slow convergence. Typical values: 1–10% of C-space diameter.
Details: Adaptive step sizes can help — shrink near obstacles, grow in open space. Alternatively, use a local planner (e.g., straight-line interpolation with dense collision checks) for long edges.
Q: Can RRT handle non-holonomic constraints?
A: Yes — Kinodynamic RRT samples controls instead of configurations. Each tree extension simulates the robot’s dynamics forward for a short time. This is slower but necessary for cars, drones with inertia, or any system with velocity/acceleration limits.
Details: Standard RRT samples a configuration $q_{rand}$ and steers toward it. Kinodynamic RRT samples a control $u$ and integrates $\dot{q} = f(q, u)$ forward. The result is a trajectory that is dynamically feasible by construction.