Implementing the RRT Algorithm in Julia: A Step-by-Step Robotics Guide

1. Fundamentals: How RRT Explores Space

The brilliance of RRT lies in its bias toward unexplored territory. Instead of methodically checking every adjacent pixel, RRT expands a tree structure by randomly sampling coordinates across the workspace.

The algorithm follows a simple recursive loop:

1. Sample (q_rand): Pick a random coordinate in the environment.

2. Nearest Neighbor (q_near): Search the existing tree to find the node closest to q_rand.

3. Steer (q_new): Move a short, fixed distance ($\Delta t$) from q_near directly toward q_rand. This creates a candidate node, q_new.

4. Collision Check: Verify if the path segment between q_near and q_new intersects an obstacle. If the path is clear, q_new is added to the tree with an edge linking it to q_near.

The Voronoi Bias

Why does random sampling work so well? Larger, unexplored areas of your workspace contain larger Voronoi cells (the regions of space closest to an outer node). Because these empty areas are larger, a random sample is statistically much more likely to land inside them, instinctively pulling the tree outward into uncharted space.

2. Step-by-Step Implementation in Julia

Let's implement a clean, lightweight 2D RRT planner. We will avoid bulky external packages to keep our execution logic highly transparent.

Step 1: Define the Data Structures

We need a structure to represent our nodes and the tree itself. Create a file named rrt_planner.jl:

Julia

using LinearAlgebra

# Represent a point in 2D space
struct Point2D
    x::Float64
    y::Float64
end

# Node structure to track parent-child relationships for path backtracking
class RRTNode
    point::Point2D
    parent_idx::Int # Index of the parent node in the tree list
end

Step 2: Distance and Steering Functions

Next, we calculate Euclidean distance and implement the steering logic to step exactly $\Delta t$ units toward our random sample.

Julia

function euclidean_distance(p1::Point2D, p2::Point2D)
    return sqrt((p1.x - p2.x)^2 + (p1.y - p2.y)^2)

$$\text{Distance} = \sqrt{(x_2 - x_1)^2 + (y_2 - y_1)^2}$$

Julia

function steer(q_near::Point2D, q_rand::Point2D, step_size::Float64)
    dist = euclidean_distance(q_near, q_rand)
    if dist <= step_size
        return q_rand
    end
    # Calculate direction vector components
    theta = atan(q_rand.y - q_near.y, q_rand.x - q_near.x)
    return Point2D(q_near.x + step_size * cos(theta), q_near.y + step_size * sin(theta))
end

Step 3: Define Obstacles and Collision Checking

For simplicity, we will model obstacles as static circles defined by a center coordinate and a radius.

Julia

struct CircleObstacle
    center::Point2D
    radius::Float64
end

function is_collision_free(p1::Point2D, p2::Point2D, obstacles::Vector{CircleObstacle})
    # Check intermediate steps along the new edge segment
    steps = 10
    for i in 0:steps
        t = i / steps
        # Interpolate point along the line segment
        check_pt = Point2D(p1.x + t * (p2.x - p1.x), p1.y + t * (p2.y - p1.y))
        
        for obs in obstacles
            if euclidean_distance(check_pt, obs.center) <= obs.radius
                return false # Collision detected
            end
        end
    end
    return true # Path segment is clear
end

Step 4: The Core RRT Execution Loop

Now, let's assemble the core planning loop. We will add a small Goal Bias (e.g., 5%), forcing the planner to occasionally sample the exact goal coordinate to speed up convergence.

Julia

function plan_rrt(start::Point2D, goal::Point2D, obstacles::Vector{CircleObstacle}, 
                  bounds::Tuple{Float64, Float64}, max_iter::Int, step_size::Float64)
    
    tree = [RRTNode(start, 0)]
    goal_threshold = 0.5

    for iter in 1:max_iter
        # 1. Sample with goal bias
        q_rand = (rand() < 0.05) ? goal : Point2D(rand() * bounds[1], rand() * bounds[2])

        # 2. Find nearest node in the tree
        min_dist = Inf
        near_idx = 1
        for (idx, node) in enumerate(tree)
            d = euclidean_distance(node.point, q_rand)
            if d < min_dist
                min_dist = d
                near_idx = idx
            end
        end
        q_near = tree[near_idx].point

        # 3. Steer toward sample
        q_new = steer(q_near, q_rand, step_size)

        # 4. Check collisions and insert node
        if is_collision_free(q_near, q_new, obstacles)
            push!(tree, RRTNode(q_new, near_idx))
            
            # Check if we are close enough to the goal
            if euclidean_distance(q_new, goal) < goal_threshold
                println("Goal reached in $iter iterations!")
                return tree # Return full tree to reconstruct path
            end
        end
    end
    println("Path planning timed out.")
    return nothing
end

Step 5: Backtracking the Path

To extract the final coordinates, walk backward from the final node using the parent_idx attributes:

Julia

function reconstruct_path(tree::Vector{RRTNode})
    path = Point2D[]
    current_node = tree[end]
    while current_node.parent_idx != 0
        pushfirst!(path, current_node.point)
        current_node = tree[current_node.parent_idx]
    end
    pushfirst!(path, tree[1].point) # Add start node
    return path
end

3. Running a Test Scenario

Let's execute our planner across a $10 \times 10$ environment containing a central obstacle block:

Julia

# Initialize points
start_pos = Point2D(1.0, 1.0)
goal_pos  = Point2D(9.0, 9.0)

# Build a defensive wall of circular obstacles
obs_fleet = [
    CircleObstacle(Point2D(5.0, 5.0), 1.5),
    CircleObstacle(Point2D(4.0, 6.0), 1.0)
]

# Run planner
resulting_tree = plan_rrt(start_pos, goal_pos, obs_fleet, (10.0, 10.0), 2000, 0.3)

if resulting_tree !== nothing
    final_path = reconstruct_path(resulting_tree)
    println("Generated Waypoints:")
    for (i, pt) in enumerate(final_path)
        println("Waypoint $i: ($(round(pt.x, digits=2)), $(round(pt.y, digits=2)))")
    end
end

Conclusion: Next Steps to Optimality

Vanilla RRT is excellent at finding an initial feasible path quickly, but it lacks optimization metrics—meaning your path will often look jagged.

To bridge this gap in production environments, developers update this foundation into RRT*. RRT* introduces a "rewire" radius loop that continuously checks nearby nodes to find a cheaper path cost, transforming your random tree into an asymptotically optimal planner.

Sampling-based Motion Planning (RRT, RRT*) - MIT 16.410

This lecture session from MIT provides a rigorous academic breakdown of sampling theory, completeness guarantees, and the core performance differences between RRT and RRT*.