Implementing RRT Path Planning for Robotics Assignment

Introduction to RRT Path Planning

Path planning is a fundamental challenge in robotics, especially when navigating through environments with obstacles. The Rapidly-exploring Random Tree (RRT) algorithm, introduced by Steven M. LaValle, is a popular sampling-based method that efficiently explores high-dimensional configuration spaces. In this tutorial, we'll implement the single tree search version described in Section 5.5.3 of LaValle's book, applied to a robot approximated by lines and obstacles approximated by circles. This approach is widely used in autonomous vehicles, drone navigation, and even in AI-driven gaming where non-player characters need to move intelligently.

Understanding the Problem Setup

Consider a workspace where a robot (black) must move from an initial pose to a goal pose without intersecting obstacles (red). The robot is represented as a set of line segments, and obstacles are circles defined by their center coordinates and radii. Your task is to implement RRT to find a collision-free path. This mirrors real-world applications like warehouse robots avoiding obstacles or autonomous cars navigating around pedestrians.

Key Concepts

Configuration Space (C-space): The set of all possible robot poses. For a robot with three joints (θ1, θ2, θ3), each configuration is a point in a 3D space.
Obstacle Representation: Obstacles are circles, making collision detection straightforward: check if any line segment of the robot intersects any circle.
Single Tree Search: The RRT grows a tree from the start configuration by randomly sampling configurations and extending the tree toward them, avoiding obstacles.

RRT Algorithm Overview

The RRT algorithm works as follows:

Initialize the tree with the start configuration as the root node.
Sample a random configuration from the C-space with some probability (e.g., 5% bias toward the goal).
Find the nearest node in the tree to the sampled configuration using a distance metric (e.g., Euclidean distance).
Attempt to extend the tree from the nearest node toward the sampled configuration by a step size. If the resulting edge is collision-free, add the new node to the tree.
Repeat steps 2-4 until the tree reaches the goal configuration (within a tolerance) or a maximum number of iterations is reached.
If the goal is reached, trace back from the goal to the root to obtain the path.

Step-by-Step Implementation

1. Data Structures

Define classes for Configuration, Node, Obstacle, and Robot. The Robot can be represented as a list of line segments (pairs of points) for simplicity.

import math, random

class Config:
    def __init__(self, theta1, theta2, theta3):
        self.theta = [theta1, theta2, theta3]

class Node:
    def __init__(self, config):
        self.config = config
        self.parent = None

class Obstacle:
    def __init__(self, x, y, radius):
        self.x = x
        self.y = y
        self.radius = radius

2. Collision Detection

For each line segment of the robot, check if it intersects any obstacle circle. Use geometry: distance from line segment to circle center.

def line_circle_intersection(p1, p2, circle):
    # Returns True if segment p1-p2 intersects circle
    # Implementation using vector math
    dx = p2[0] - p1[0]
    dy = p2[1] - p1[1]
    fx = p1[0] - circle.x
    fy = p1[1] - circle.y
    a = dx*dx + dy*dy
    b = 2*(fx*dx + fy*dy)
    c = fx*fx + fy*fy - circle.radius*circle.radius
    discriminant = b*b - 4*a*c
    if discriminant < 0:
        return False
    discriminant = math.sqrt(discriminant)
    t1 = (-b - discriminant)/(2*a)
    t2 = (-b + discriminant)/(2*a)
    if (t1 >= 0 and t1 <= 1) or (t2 >= 0 and t2 <= 1):
        return True
    return False

3. RRT Core

Implement the main RRT loop. Sample configurations, find nearest neighbor, and extend.

def rrt(start, goal, obstacles, bounds, max_iter=1000, step_size=0.1, goal_bias=0.05):
    tree = [Node(start)]
    for i in range(max_iter):
        # Sample with goal bias
        if random.random() < goal_bias:
            sample = goal
        else:
            sample = Config(random.uniform(bounds[0][0], bounds[0][1]),
                            random.uniform(bounds[1][0], bounds[1][1]),
                            random.uniform(bounds[2][0], bounds[2][1]))
        # Find nearest node
        nearest = min(tree, key=lambda n: distance(n.config, sample))
        # Extend toward sample
        new_config = steer(nearest.config, sample, step_size)
        if not collision(nearest.config, new_config, obstacles):
            new_node = Node(new_config)
            new_node.parent = nearest
            tree.append(new_node)
            if distance(new_config, goal) < step_size:
                return extract_path(new_node)
    return None

4. Distance and Steer Functions

def distance(config1, config2):
    return math.sqrt(sum((a-b)**2 for a,b in zip(config1.theta, config2.theta)))

def steer(from_config, to_config, step_size):
    d = distance(from_config, to_config)
    if d < step_size:
        return to_config
    ratio = step_size / d
    new_theta = [f + (t-f)*ratio for f,t in zip(from_config.theta, to_config.theta)]
    return Config(*new_theta)

5. Path Extraction

def extract_path(node):
    path = []
    while node is not None:
        path.append(node.config)
        node = node.parent
    path.reverse()
    return path

Visualization

Use matplotlib to plot the robot, obstacles, and computed path. Represent the robot as lines connecting its joints (forward kinematics). For each configuration in the path, draw the robot in a subplot or animate.

import matplotlib.pyplot as plt

def plot_robot(config, robot_geometry):
    # robot_geometry: list of joint positions (x,y) relative to base
    # Use config.theta to compute absolute positions via forward kinematics
    pass

def plot_path(path, obstacles):
    fig, ax = plt.subplots()
    for obs in obstacles:
        circle = plt.Circle((obs.x, obs.y), obs.radius, color='red', alpha=0.5)
        ax.add_artist(circle)
    for config in path:
        # plot robot configuration
        pass
    plt.show()

Testing and Debugging

Test with simple scenarios. For example, a single obstacle in the middle. The RRT should find a path around it. If the algorithm fails, check collision detection and step size. Increase iterations if needed. The goal bias helps direct the search toward the goal, but too much bias may trap the tree in local minima.

Tips for Success

Use a reasonable step size relative to the workspace size.
Ensure collision detection is efficient; precompute robot line segments for each configuration.
For the graphical representation, show the tree edges and final path.
Test with the provided test data (released Nov 3rd). The maximum number of obstacles is four.

Connecting to Trends

RRT and its variants are used in self-driving cars (e.g., Waymo), drone delivery systems (Amazon Prime Air), and even in video games for NPC navigation. The recent surge in AI-powered robotics, like Boston Dynamics' robots, relies on such algorithms for real-time path planning. Understanding RRT gives you a foundation for more advanced techniques like RRT* (optimal) and informed RRT.

Conclusion

Implementing RRT for a line robot with circular obstacles is a classic exercise in robotics. By following this tutorial, you've learned the core concepts of sampling-based planning, collision detection, and tree growth. The provided code skeleton should help you complete your assignment. Remember to submit both your code and a graphical representation of the computed path. Good luck!