Lab 06-01: RRT Path Planning

Objectives

By the end of this lab, you will be able to:

• Implement the Rapidly-exploring Random Tree (RRT) algorithm
• Understand collision checking in configuration space
• Visualize tree growth and path extraction
• Apply RRT to robot arm motion planning

Prerequisites

• Completed Module 03 (Kinematics)
• Understanding of configuration space concepts
• Python programming with NumPy

Materials

Type	Name	Tier	Notes
software	MuJoCo 3.0+	required	Physics simulation and collision
software	Python 3.10+	required	Programming environment
software	NumPy, Matplotlib	required	Computation and visualization
simulation	2dof-arm-obstacles.xml	required	Arm with obstacle environment

Background

Rapidly-exploring Random Trees (RRT)

RRT is a sampling-based planner that builds a tree of configurations:

1. Sample a random configuration q_rand
2. Find the nearest node in the tree q_near
3. Extend from q_near toward q_rand by distance δ
4. If the extension is collision-free, add to tree
5. Repeat until goal is reached

RRT is probabilistically complete but not optimal. Extensions like RRT* add rewiring for asymptotic optimality.

Instructions

Step 1: Set Up the Environment

Load a model with obstacles:

import mujoco
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle, Circle

# Load model with obstacles
model = mujoco.MjModel.from_xml_path("textbook/assets/robot-models/mujoco/2dof-arm-obstacles.xml")
data = mujoco.MjData(model)

# Robot parameters
n_joints = model.njnt
joint_limits = model.jnt_range[:n_joints].copy()

print(f"Joint limits:")
for i in range(n_joints):
    print(f"  Joint {i}: [{np.degrees(joint_limits[i, 0]):.1f}, {np.degrees(joint_limits[i, 1]):.1f}] deg")

# Link lengths (for visualization)
link_lengths = [0.5, 0.4]  # meters

Expected: Joint limits loaded from model (e.g., ±180° for each joint).

Step 2: Collision Checking

Implement collision detection using MuJoCo:

def check_collision(model, data, q):
    """
    Check if configuration q is in collision.

    Args:
        model, data: MuJoCo model and data
        q: Joint configuration [q0, q1, ...]

    Returns:
        True if in collision, False otherwise
    """
    # Set configuration
    data.qpos[:len(q)] = q
    data.qvel[:] = 0

    # Forward kinematics to update geometry
    mujoco.mj_forward(model, data)

    # Check contacts
    n_contacts = data.ncon
```python
```python
    if n_contacts > 0:

        # Filter self-collision (if needed)
        for i in range(n_contacts):
            contact = data.contact[i]
            # Check if contact is with obstacle (not self-collision)
            geom1 = contact.geom1
            geom2 = contact.geom2
            # You may need to filter based on geom names/types
            return True
    return False

def check_collision_simple(q, obstacles):
    """
    Simple geometric collision check for 2D arm.

    Args:
        q: [q0, q1] joint angles
        obstacles: List of (x, y, radius) obstacles

    Returns:
        True if in collision
    """
    # Forward kinematics
    l1, l2 = 0.5, 0.4
    x1 = l1 * np.cos(q[0])
    y1 = l1 * np.sin(q[0])
    x2 = x1 + l2 * np.cos(q[0] + q[1])
    y2 = y1 + l2 * np.sin(q[0] + q[1])

    # Check line segments against obstacles
    n_samples = 20
    for obstacle in obstacles:
        ox, oy, r = obstacle

        # Check link 1
        for t in np.linspace(0, 1, n_samples):
            px, py = t * x1, t * y1
```python
```python
            if (px - ox)**2 + (py - oy)**2 < r**2:

                return True

        # Check link 2
        for t in np.linspace(0, 1, n_samples):
            px = x1 + t * (x2 - x1)
            py = y1 + t * (y2 - y1)
```python
```python
            if (px - ox)**2 + (py - oy)**2 < r**2:

                return True

    return False

# Define obstacles
obstacles = [
    (0.4, 0.3, 0.1),   # (x, y, radius)
    (0.2, 0.5, 0.08),
    (-0.3, 0.4, 0.12)
]

# Test collision checking
test_config = np.array([0.5, 0.3])
in_collision = check_collision_simple(test_config, obstacles)
print(f"Config {np.degrees(test_config)} in collision: {in_collision}")

Expected: Collision checker returns True for configurations where arm intersects obstacles.

Step 3: Implement RRT Core Algorithm

Build the RRT tree:

class RRTNode:
    """Node in RRT tree."""

    def __init__(self, q, parent=None):
        self.q = np.array(q)
        self.parent = parent
        self.children = []

class RRT:
    """Rapidly-exploring Random Tree planner."""

    def __init__(self, q_start, q_goal, joint_limits, obstacles,
                 step_size=0.1, goal_bias=0.1, max_iterations=5000):
        """
        Initialize RRT.

        Args:
            q_start: Start configuration
            q_goal: Goal configuration
            joint_limits: (n, 2) array of [min, max] per joint
            obstacles: List of obstacles
            step_size: Maximum extension step (radians)
            goal_bias: Probability of sampling goal
            max_iterations: Maximum tree growth iterations
        """
        self.q_start = np.array(q_start)
        self.q_goal = np.array(q_goal)
        self.joint_limits = np.array(joint_limits)
        self.obstacles = obstacles
        self.step_size = step_size
        self.goal_bias = goal_bias
        self.max_iterations = max_iterations

        # Tree
        self.root = RRTNode(q_start)
        self.nodes = [self.root]

    def sample_random(self):
        """Sample random configuration (with goal bias)."""
```python
```python
        if np.random.random() < self.goal_bias:

            return self.q_goal.copy()
        else:
            q = np.zeros(len(self.q_start))
            for i in range(len(q)):
                q[i] = np.random.uniform(self.joint_limits[i, 0],
                                         self.joint_limits[i, 1])
            return q

    def nearest(self, q):
        """Find nearest node in tree."""
        min_dist = float('inf')
        nearest_node = None
        for node in self.nodes:
            dist = np.linalg.norm(node.q - q)
```python
```python
            if dist < min_dist:

                min_dist = dist
                nearest_node = node
        return nearest_node

    def steer(self, q_from, q_to):
        """Steer from q_from toward q_to by step_size."""
        direction = q_to - q_from
        dist = np.linalg.norm(direction)
```python
```python
        if dist < self.step_size:

            return q_to.copy()
        else:
            return q_from + direction / dist * self.step_size

    def is_collision_free(self, q1, q2, n_checks=10):
        """Check if path from q1 to q2 is collision-free."""
        for t in np.linspace(0, 1, n_checks):
            q = q1 + t * (q2 - q1)
            if check_collision_simple(q, self.obstacles):
                return False
        return True

    def plan(self):
        """
        Build tree and find path to goal.

        Returns:
            path: List of configurations from start to goal, or None if failed
        """
        goal_threshold = 0.1  # radians

        for i in range(self.max_iterations):
            # Sample
            q_rand = self.sample_random()

            # Find nearest
            q_near_node = self.nearest(q_rand)

            # Steer
            q_new = self.steer(q_near_node.q, q_rand)

            # Check collision
            if self.is_collision_free(q_near_node.q, q_new):
                # Add node
                new_node = RRTNode(q_new, parent=q_near_node)
                q_near_node.children.append(new_node)
                self.nodes.append(new_node)

                # Check if goal reached
```python
```python
                if np.linalg.norm(q_new - self.q_goal) < goal_threshold:

                    # Try to connect directly to goal
                    if self.is_collision_free(q_new, self.q_goal):
                        goal_node = RRTNode(self.q_goal, parent=new_node)
                        new_node.children.append(goal_node)
                        self.nodes.append(goal_node)
                        print(f"Goal reached in {i+1} iterations!")
                        return self.extract_path(goal_node)

        print(f"Failed to find path in {self.max_iterations} iterations")
        return None

    def extract_path(self, goal_node):
        """Extract path from root to goal."""
        path = []
        node = goal_node
        while node is not None:
            path.append(node.q)
            node = node.parent
        return path[::-1]  # Reverse to get start→goal order

# Define problem
q_start = np.array([0.0, 0.0])
q_goal = np.array([np.pi/2, -np.pi/4])

# Run RRT
rrt = RRT(q_start, q_goal, joint_limits, obstacles,
          step_size=0.15, goal_bias=0.1, max_iterations=3000)

path = rrt.plan()
if path:
    print(f"Path found with {len(path)} waypoints")
    print(f"Tree has {len(rrt.nodes)} nodes")

Expected: RRT finds a collision-free path from start to goal.

Step 4: Visualize Tree and Path

Create visualization in both configuration space and workspace:

def forward_kinematics(q, link_lengths=[0.5, 0.4]):
    """Compute end-effector and joint positions."""
    positions = [(0, 0)]
    x, y = 0, 0
    angle = 0
    for i, (qi, l) in enumerate(zip(q, link_lengths)):
        angle += qi
        x += l * np.cos(angle)
        y += l * np.sin(angle)
        positions.append((x, y))
    return positions

def plot_rrt_results(rrt, path, obstacles):
    """Visualize RRT in C-space and workspace."""
    fig, axes = plt.subplots(1, 2, figsize=(14, 6))

    # C-space plot
    ax = axes[0]

    # Plot tree edges
    for node in rrt.nodes:
        if node.parent is not None:
            ax.plot([node.parent.q[0], node.q[0]],
                   [node.parent.q[1], node.q[1]],
                   'b-', alpha=0.3, linewidth=0.5)

    # Plot nodes
    q_nodes = np.array([n.q for n in rrt.nodes])
    ax.scatter(q_nodes[:, 0], q_nodes[:, 1], s=5, c='blue', alpha=0.5)

    # Plot path
    if path:
        path_array = np.array(path)
        ax.plot(path_array[:, 0], path_array[:, 1], 'r-', linewidth=3, label='Path')

    ax.scatter([rrt.q_start[0]], [rrt.q_start[1]], s=100, c='green', marker='s', label='Start')
    ax.scatter([rrt.q_goal[0]], [rrt.q_goal[1]], s=100, c='red', marker='*', label='Goal')

    ax.set_xlabel('q1 (rad)')
    ax.set_ylabel('q2 (rad)')
    ax.set_title(f'C-Space: RRT Tree ({len(rrt.nodes)} nodes)')
    ax.legend()
    ax.set_aspect('equal')
    ax.grid(True)

    # Workspace plot
    ax = axes[1]

    # Draw obstacles
    for (ox, oy, r) in obstacles:
        circle = Circle((ox, oy), r, color='gray', alpha=0.7)
        ax.add_patch(circle)

    # Draw arm at start
    start_pos = forward_kinematics(rrt.q_start)
    ax.plot([p[0] for p in start_pos], [p[1] for p in start_pos],
            'g-', linewidth=3, label='Start pose')

    # Draw arm at goal
    goal_pos = forward_kinematics(rrt.q_goal)
    ax.plot([p[0] for p in goal_pos], [p[1] for p in goal_pos],
            'r--', linewidth=3, label='Goal pose')

    # Draw path waypoints
    if path:
        for i, q in enumerate(path):
            pos = forward_kinematics(q)
            alpha = 0.2 + 0.8 * i / len(path)
            ax.plot([p[0] for p in pos], [p[1] for p in pos],
                    'b-', alpha=alpha, linewidth=1)

    ax.set_xlabel('X (m)')
    ax.set_ylabel('Y (m)')
    ax.set_title('Workspace: Arm Configuration')
    ax.set_xlim(-1.0, 1.0)
    ax.set_ylim(-1.0, 1.0)
    ax.set_aspect('equal')
    ax.legend()
    ax.grid(True)

    plt.tight_layout()
    plt.savefig('rrt_results.png', dpi=150)
    plt.show()

plot_rrt_results(rrt, path, obstacles)

Expected: Two plots showing tree structure in C-space and arm motion in workspace.

Step 5: Path Smoothing

Smooth the path to remove unnecessary waypoints:

def smooth_path(path, obstacles, max_iterations=100):
    """
    Shortcut-based path smoothing.

    Randomly picks two points and tries to connect them directly.
    """
```python
```python
    if not path or len(path) < 3:

        return path

    path = [np.array(q) for q in path]
    smoothed = path.copy()

    for _ in range(max_iterations):
```python
```python
        if len(smoothed) < 3:

            break

        # Pick two random indices
        i = np.random.randint(0, len(smoothed) - 2)
        j = np.random.randint(i + 2, len(smoothed))

        # Check if direct connection is collision-free
        collision_free = True
        for t in np.linspace(0, 1, 20):
            q = smoothed[i] + t * (smoothed[j] - smoothed[i])
            if check_collision_simple(q, obstacles):
                collision_free = False
                break

        if collision_free:
            # Remove intermediate points
            smoothed = smoothed[:i+1] + smoothed[j:]

    return smoothed

# Smooth the path
smoothed_path = smooth_path(path, obstacles, max_iterations=200)
print(f"Original path: {len(path)} waypoints")
print(f"Smoothed path: {len(smoothed_path)} waypoints")

# Visualize comparison
fig, ax = plt.subplots(figsize=(8, 8))

# Draw obstacles
for (ox, oy, r) in obstacles:
    circle = Circle((ox, oy), r, color='gray', alpha=0.7)
    ax.add_patch(circle)

# Draw original path
for q in path:
    pos = forward_kinematics(q)
    ax.plot([p[0] for p in pos], [p[1] for p in pos], 'b-', alpha=0.3, linewidth=1)

# Draw smoothed path
for q in smoothed_path:
    pos = forward_kinematics(q)
    ax.plot([p[0] for p in pos], [p[1] for p in pos], 'r-', linewidth=2)

ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_title('Path Smoothing Comparison')
ax.set_xlim(-1.0, 1.0)
ax.set_ylim(-1.0, 1.0)
ax.set_aspect('equal')
ax.grid(True)

plt.savefig('path_smoothing.png', dpi=150)
plt.show()

Expected: Smoothed path has fewer waypoints but remains collision-free.

Step 6: Execute Path in Simulation

Execute the planned path using a simple controller:

def interpolate_path(path, num_points=100):
    """Linearly interpolate path to get dense waypoints."""
```python
```python
    if len(path) < 2:

        return path

    # Compute total path length
    segments = []
    for i in range(len(path) - 1):
        dist = np.linalg.norm(np.array(path[i+1]) - np.array(path[i]))
        segments.append(dist)
    total_length = sum(segments)

    # Generate interpolated points
    interpolated = []
    for i in range(len(path) - 1):
        n_segment = max(2, int(num_points * segments[i] / total_length))
```python
```python
        for t in np.linspace(0, 1, n_segment, endpoint=(i == len(path) - 2)):

            q = np.array(path[i]) + t * (np.array(path[i+1]) - np.array(path[i]))
            interpolated.append(q)

    return interpolated

def execute_path(model, data, path, kp=50, kd=10):
    """Execute path with simple PD control."""
    dt = model.opt.timestep
    interpolated = interpolate_path(path, num_points=200)

    trajectory = {
        'time': [],
        'position': [],
        'target': [],
        'torque': []
    }

    for i, target in enumerate(interpolated):
        # Run for multiple steps per waypoint
        for _ in range(5):
            q = data.qpos[:2].copy()
            qd = data.qvel[:2].copy()

            # PD control
            error = target - q
            tau = kp * error - kd * qd
            tau = np.clip(tau, -50, 50)

            data.ctrl[:2] = tau
            mujoco.mj_step(model, data)

            trajectory['time'].append(data.time)
            trajectory['position'].append(q.copy())
            trajectory['target'].append(target.copy())
            trajectory['torque'].append(tau.copy())

    return trajectory

# Execute the smoothed path
mujoco.mj_resetData(model, data)
data.qpos[:2] = smoothed_path[0]

trajectory = execute_path(model, data, smoothed_path)

print(f"Execution complete. Final position: {np.degrees(trajectory['position'][-1])} deg")
print(f"Target position: {np.degrees(smoothed_path[-1])} deg")

Expected: Robot follows path to reach goal configuration.

Expected Outcomes

After completing this lab, you should have:

1. Code artifacts:

   • rrt.py: RRT planner implementation
   • collision_check.py: Collision checking utilities
   • path_smoothing.py: Path post-processing

2. Visual outputs:

   • rrt_results.png: Tree and path visualization
   • path_smoothing.png: Before/after smoothing comparison

3. Understanding:

   • Sampling-based planning principles
   • Configuration space vs. workspace
   • Trade-offs in RRT parameters

Rubric

Criterion	Points	Description
Collision Checking	15	Correct detection of arm-obstacle intersection
RRT Implementation	30	Proper sampling, steering, tree growth
Path Finding	20	Successfully finds collision-free path
Visualization	15	Clear C-space and workspace plots
Path Smoothing	20	Reduces waypoints while maintaining collision-free
Total	100

Common Errors

Error: RRT takes too long or fails to find path Solution: Increase step_size, increase goal_bias, or increase max_iterations.

Error: Path appears to go through obstacles Solution: Increase n_checks in collision checking, or check collision geometry.

Error: Smoothed path collides with obstacles Solution: Increase interpolation density in smoothing collision check.

Extensions

For advanced students:

1. RRT*: Add rewiring for asymptotic optimality
2. Bidirectional RRT: Grow trees from start and goal
3. Informed RRT*: Use ellipsoidal sampling after first solution
4. Higher-dimensional planning: Extend to 6-DOF robot arm

Related Content

• Theory: Module 06 theory.md, Section 6.2 (Sampling-Based Planning)
• Next Lab: Lab 06-02 (A* with Heuristics)
• Application: See Amazon Digit case study for warehouse navigation planning