Rapidly Exploring Random Trees (RRT)
Majid Khonji
201
www.avlab.io
Khalifa University
ROBO
Course Structure
Module 1: Algorithm Fundamentals
Module 2: Path Planning in Discrete Spaces
Module 3: Path Planning in Continuous Spaces
2
ku.ac.ae
Path Planning in Continuous Spaces
3
ku.ac.ae
PRM
1. Sample many random robot positions
2. Check each: does robot collide with obstacles?
4. Connect nearby safe positions
Search the resulting graph (using UCS/A*/Dijkstra)��
4
Two parameters:
Last Lecture
ku.ac.ae
PRM Idea Map
5
PriorityQueue
Graph / DiGraph
A*
PRM
Heap
Adjacency List
Adjacency Matrix
Data Structures
Algorithms
Sorting
K-nearest neighbors
Collision Checking
Alg. Not Covered
QuickSort
MergeSort
HeapSort
Last Lecture
ku.ac.ae
Concepts: Completeness and Optimality
Completeness: Will the algorithm find a solution if one exists?
6
Optimality: How good is the solution?
ku.ac.ae
Previous Approaches
Grid-based methods (A*, Dijkstra):
Probabilistic Roadmaps (PRM)
7
ku.ac.ae
Rapidly Exploring Random Trees (RRT)
8
RRT
Key Idea: Randomly sample the configuration space and incrementally build a tree structure
Advantages:
9
ku.ac.ae
RRT Basic Concept
1. Start with initial configuration (tree root)
2. Randomly sample configuration space
3. Find nearest node in tree
4. Extend tree toward sample
5. Check for collisions
6. Repeat until goal is reached
Result: A tree that explores the free space
10
ku.ac.ae
RRT Algorithm: Pseudocode
BUILD_RRT(x_init)
1 T.init(x_init);
2 for k = 1 to K do
3 x_rand ← RANDOM_STATE();
4 EXTEND(T, x_rand);
5 Return T
11
EXTEND(T, x)
1 x_near ← NEAREST_NEIGHBOR(x, T);
2 x_new ← STEER(x_near, x);
3 if COLLISION_FREE(x_near, x_new) then
4 T.add_node(x_new);
5 T.add_edge(x_near, x_new);
6 if distance(x_new, goal) < α then
7 Return Reached;
8 else
9 Return Advanced;
10 Return Trapped;
ku.ac.ae
Extend
12
ku.ac.ae
Python Implementation: Setup
class Node:
"""Represents a node in the RRT tree"""
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
self.cost = 0.0
13
ku.ac.ae
RRT Class Setup
class RRT:
def __init__(self, start, goal, obstacles, bounds):
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.obstacles = obstacles # List of obstacles
self.bounds = bounds # [x_min, x_max, y_min, y_max]
self.nodes = [self.start]
self.step_size = 0.5
self.goal_threshold = 0.5
14
Key attributes: tree nodes, start/goal, obstacles, bounds�Parameters: step_size, goal_threshold
ku.ac.ae
RRT Class Setup (Type hints)
class RRT:
def __init__(
self,
start: tuple[float, float],
goal: tuple[float, float],
obstacles: list,
bounds: tuple[float, float, float, float]
):
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.obstacles = obstacles # List of obstacles
self.bounds = bounds # [x_min, x_max, y_min, y_max]
self.nodes = [self.start]
self.step_size = 0.5
self.goal_threshold = 0.5
15
ku.ac.ae
RANDOM_STATE()
Purpose: Sample a random configuration in the space
# Inside RRT Class
def sample_random_point(self):
"""RANDOM_STATE: Sample random point in configuration space"""
# Bias toward goal 10% of the time (optional heuristic)
if random.random() < 0.1:
return self.goal
# Sample uniformly in the bounded space
x = random.uniform(self.bounds[0], self.bounds[1])
y = random.uniform(self.bounds[2], self.bounds[3])
return Node(x, y)
BUILD_RRT(x_init)
1 T.init(x_init);
2 for k = 1 to K do
3 x_rand ← RANDOM_STATE();
4 EXTEND(T, x_rand);
5 Return T
EXTEND(T, x)
1 x_near ← NEAREST_NEIGHBOR(x, T);
2 x_new ← STEER(x_near, x);
3 if COLLISION_FREE(x_near, x_new) then
4 T.add_node(x_new);
5 T.add_edge(x_near, x_new);
6 if distance(x_new, goal) < α then
7 Return Reached;
8 else
9 Return Advanced;
10 Return Trapped;
16
Goal biasing helps convergence but not required for correctness
ku.ac.ae
NEAREST_NEIGHBOR(x, T)
def find_nearest_node(self, target_node):
"""NEAREST_NEIGHBOR: Find nearest node in � tree to target"""
min_dist = float('inf')
nearest = None
for node in self.nodes:
dist = self.distance(node, target_node)
if dist < min_dist:
min_dist = dist
nearest = node
return nearest
BUILD_RRT(x_init)
1 T.init(x_init);
2 for k = 1 to K do
3 x_rand ← RANDOM_STATE();
4 EXTEND(T, x_rand);
5 Return T
EXTEND(T, x)
1 x_near ← NEAREST_NEIGHBOR(x, T);
2 x_new ← STEER(x_near, x);
3 if COLLISION_FREE(x_near, x_new) then
4 T.add_node(x_new);
5 T.add_edge(x_near, x_new);
6 if distance(x_new, goal) < α then
7 Return Reached;
8 else
9 Return Advanced;
10 Return Trapped;
17
def distance(self, node1, node2):
"""Euclidean distance between nodes"""
dx = node1.x - node2.x
dy = node1.y - node2.y
return math.sqrt(dx*dx + dy*dy)
ku.ac.ae
STEER(x_near, x)
Purpose: Create new node by moving from x_near toward x
�def steer(self, from_node, to_node):
"""STEER: Move from from_node toward to_node"""
# Calculate direction
dx = to_node.x - from_node.x
dy = to_node.y - from_node.y
dist = math.sqrt(dx*dx + dy*dy)
# If target is close, go directly to it
if dist < self.step_size:
new_node = Node(to_node.x, to_node.y)
else:
# Move step_size in direction of target
theta = math.atan2(dy, dx)
new_x = from_node.x + self.step_size * math.cos(theta)
new_y = from_node.y + self.step_size * math.sin(theta)
new_node = Node(new_x, new_y)
new_node.parent = from_node
return new_node
BUILD_RRT(x_init)
1 T.init(x_init);
2 for k = 1 to K do
3 x_rand ← RANDOM_STATE();
4 EXTEND(T, x_rand);
5 Return T
EXTEND(T, x)
1 x_near ← NEAREST_NEIGHBOR(x, T);
2 x_new ← STEER(x_near, x);
3 if COLLISION_FREE(x_near, x_new) then
4 T.add_node(x_new);
5 T.add_edge(x_near, x_new);
6 if distance(x_new, goal) < α then
7 Return Reached;
8 else
9 Return Advanced;
10 Return Trapped;
18
step_size * sin(theta)
x_new
step_size * cos(theta)
x_near
x_rand
dx
dy
θ
step_size
x_new = (step_size * cos(theta), step_size * sin(theta))
ku.ac.ae
COLLISION_FREE(x_near, x_new)
def is_collision_free(self, from_node, to_node):
"""Check if path is collision-free"""
import numpy as np
# Sample points uniformly along the line segment
num_samples = 10
xs = np.linspace(from_node.x, to_node.x, num_samples)
ys = np.linspace(from_node.y, to_node.y, num_samples)
# Check each point for collision
for x, y in zip(xs, ys):
for obs in self.obstacles:
if obs.contains(x, y):
return False
return True
BUILD_RRT(x_init)
1 T.init(x_init);
2 for k = 1 to K do
3 x_rand ← RANDOM_STATE();
4 EXTEND(T, x_rand);
5 Return T
EXTEND(T, x)
1 x_near ← NEAREST_NEIGHBOR(x, T);
2 x_new ← STEER(x_near, x);
3 if COLLISION_FREE(x_near, x_new) then
4 T.add_node(x_new);
5 T.add_edge(x_near, x_new);
6 if distance(x_new, goal) < α then
7 Return Reached;
8 else
9 Return Advanced;
10 Return Trapped;
19
ku.ac.ae
Collision Checking Details
Why sample multiple points?
- Path is continuous, but we check discrete points
- More samples = more accurate but slower
- Trade-off between safety and speed
Alternative approaches:
- Analytical collision checking (for simple shapes)
- Swept volume methods
- Conservative bounds
20
ku.ac.ae
Obstacles for Collision Checking
from abc import ABC, abstractmethod
class Obstacle(ABC):
"""Abstract base class for obstacles"""
@abstractmethod
def contains(self, x, y):
"""Check if point (x, y) is inside obstacle"""
pass
class CircularObstacle(Obstacle):
"""Circular obstacle"""
def __init__(self, center, radius):
self.cx = center[0]
self.cy = center[1]
self.radius = radius
def contains(self, x, y):
"""Check if point (x, y) is inside obstacle"""
dx = x - self.cx
dy = y - self.cy
return math.sqrt(dx*dx + dy*dy) < self.radius
21
You can use shapely library for more sophisticated shapes (as we did with PRD).
class RectangularObstacle(Obstacle):
"""Rectangular obstacle"""
def __init__(self, x, y, width, height):
self.x, self.y = x, y
self.width, self.height = width, height
def contains(self, x, y):
"""Check if point (x, y) is inside obstacle"""
return (self.x <= x <= self.x + self.width and
self.y <= y <= self.y + self.height)
ku.ac.ae
Putting It Together: EXTEND
def extend(self, x_target):
"""EXTEND: Try to extend tree toward x_target"""
x_near = self.find_nearest_node(x_target)
x_new = self.steer(x_near, x_target)
if self.is_collision_free(x_near, x_new):
self.nodes.append(x_new)
if self.distance(x_new, self.goal) < self.goal_threshold:
return "Reached"
else:
return "Advanced"
return "Trapped"
BUILD_RRT(x_init)
1 T.init(x_init);
2 for k = 1 to K do
3 x_rand ← RANDOM_STATE();
4 EXTEND(T, x_rand);
5 Return T
EXTEND(T, x)
1 x_near ← NEAREST_NEIGHBOR(x, T);
2 x_new ← STEER(x_near, x);
3 if COLLISION_FREE(x_near, x_new) then
4 T.add_node(x_new);
5 T.add_edge(x_near, x_new);
6 if distance(x_new, goal) < α then
7 Return Reached;
8 else
9 Return Advanced;
10 Return Trapped;
22
Main algorithm
def build_rrt(self, max_iterations=5000):
"""BUILD_RRT: Main RRT algorithm"""
# Tree starts with just the start node (in __init__)
for k in range(max_iterations):
x_rand = self.sample_random_point()
status = self.extend(x_rand)
if status == "Reached":
return self.extract_path(self.nodes[-1])
return None # No path found within iteration limit
BUILD_RRT(x_init)
1 T.init(x_init);
2 for k = 1 to K do
3 x_rand ← RANDOM_STATE();
4 EXTEND(T, x_rand);
5 Return T
EXTEND(T, x)
1 x_near ← NEAREST_NEIGHBOR(x, T);
2 x_new ← STEER(x_near, x);
3 if COLLISION_FREE(x_near, x_new) then
4 T.add_node(x_new);
5 T.add_edge(x_near, x_new);
6 if distance(x_new, goal) < α then
7 Return Reached;
8 else
9 Return Advanced;
10 Return Trapped;
23
Path Extraction
def extract_path(self, goal_node):
"""Extract path from start to goal"""
path = []
current = goal_node
while current is not None:
path.append([current.x, current.y])
current = current.parent
return path[::-1] # Reverse to get start-to-goal order
24
ku.ac.ae
Example
# Define environment
start = [1, 1]
goal = [9, 9]
bounds = [0, 10, 0, 10]
# Create obstacles
obstacles = [
CircularObstacle([5, 5], 1.5),
CircularObstacle([3, 7], 1.0),
RectangularObstacle(6, 2, 2, 3),
RectangularObstacle(2, 3, 1.5, 2)
]
# Create RRT planner
rrt = RRT(start, goal, obstacles, bounds)
print("Planning path...")
path = rrt.build_rrt(max_iterations=5000)
if path:
print(f"Path found with {len(path)} waypoints!")
print(f"Explored {len(rrt.nodes)} nodes")
else:
print("No path found")
25
ku.ac.ae
Exercise
Given: Start at (1, 1), step size = 2.0, no obstacles
Random samples (in order):
1. (7, 3)
2. (4, 6)
3. (8, 8)
4. (2, 5)
5. (6, 2)
Task: For each sample, determine:
26
step_size * sin(theta)
x_new
step_size * cos(theta)
x_near
x_rand
dx
dy
θ
step_size
ku.ac.ae
Solution
27
ku.ac.ae
RRT Properties
Probabilistic Completeness:
- If a solution exists, RRT will find it given infinite time
- Probability → 1 as iterations → ∞
Time Complexity: O(n log n) per iteration
- n = number of nodes in tree
- Finding nearest neighbor dominates
Space Complexity: O(n)
- Stores all nodes in tree
28
ku.ac.ae