Implementation of RRT* (Rapidly-exploring Random Tree) for Motion Planning

1. Executive Summary & Problem Formulation

When plotting a route from Point A to Point B on a 2D map, software engineers instantly deploy the A (A-Star) search algorithm. A operates on discrete graphs: you slice the map into a $1000 \times 1000$ pixel grid, define movement costs to neighboring cells, and the algorithm mathematically guarantees the shortest path.

However, A shatters the moment you attempt to navigate a 6 Degree-of-Freedom (DOF) robotic arm through a crowded factory floor. The robot's configuration space (C-Space) consists of 6 continuous joint angles. If you discretize each joint into just 100 increments, your search grid contains $100^6 = 1,000,000,000,000$ nodes. Attempting to run A across a trillion-node graph exhausts all system RAM and compute time instantly. This is the "Curse of Dimensionality."

To navigate continuous, high-dimensional spaces, modern robotics abandons grid search in favor of Sampling-Based Algorithms.

The Rapidly-exploring Random Tree (RRT) algorithm operates by blindly casting random points into the configuration space and growing a tree toward those points. It explores high-dimensional voids aggressively. While standard RRT finds a path quickly, the path is famously jagged, chaotic, and sub-optimal.

RRT (RRT-Star) alters the algorithm by introducing a "rewiring" step. As the tree explores, it continuously checks if newly added nodes can act as cheaper parent routes for existing nearby nodes. This mathematically guarantees asymptotic optimality: given infinite time, RRT will converge precisely on the absolutely shortest, most efficient physical path.

This guide explores the architectural mechanics of RRT* and implements the algorithm in Python, utilizing spatial indexing to solve the nearest-neighbor search bottleneck.

2. Mathematical & Architectural Theory

The Core RRT Expansion Loop

The basic RRT algorithm constructs a mathematical tree $\mathcal{T}$ starting from the origin point $\mathbf{x}_{init}$.

In a continuous loop, it executes three steps: 1. Sample: Generate a completely random coordinate $\mathbf{x}_{rand}$ in the configuration space. 2. Nearest: Search the existing tree $\mathcal{T}$ to find the node $\mathbf{x}_{nearest}$ that is physically closest to $\mathbf{x}_{rand}$. 3. Steer: Project a new node $\mathbf{x}_{new}$ by moving a fixed distance $\Delta q$ from $\mathbf{x}_{nearest}$ directly toward the random point $\mathbf{x}_{rand}$. 4. Collision Check: Evaluate the mathematical line segment between $\mathbf{x}_{nearest}$ and $\mathbf{x}_{new}$. If the line intersects any physical obstacles, discard the node. Otherwise, add $\mathbf{x}_{new}$ to the tree.

By aggressively steering toward random points, the tree rapidly explores the geometry of the map without getting trapped in local minima (like U-shaped walls), which frequently defeat gradient-descent algorithms.

The RRT* Rewiring Mechanism

Standard RRT stops there, resulting in a jagged, zigzagging path. RRT* introduces cost tracking and neighborhood rewiring.

When $\mathbf{x}_{new}$ is validated, RRT* calculates its cost: $C(\mathbf{x}_{new}) = C(\mathbf{x}_{parent}) + \text{distance}(\mathbf{x}_{parent}, \mathbf{x}_{new})$.

Instead of permanently attaching $\mathbf{x}_{new}$ to $\mathbf{x}_{nearest}$, RRT* defines a search radius $r$. It collects a set of all existing tree nodes within that radius (the neighborhood $\mathcal{X}_{near}$). It then performs two optimization sweeps: 1. Choose Best Parent: RRT* evaluates every node in $\mathcal{X}_{near}$. Could one of these nodes provide a cheaper total route from the origin to $\mathbf{x}_{new}$? If so, it assigns the cheapest neighbor as the parent. 2. Rewire Neighbors: RRT flips the logic. Could $\mathbf{x}_{new}$ act as a cheaper parent for any of the nodes currently in $\mathcal{X}_{near}$? If connecting an existing node through $\mathbf{x}_{new}$ lowers its total cost, RRT deletes the existing edge and "rewires" the neighbor to $\mathbf{x}_{new}$, instantly propagating the cost reduction down the entire branch.

As the tree grows dense, this continuous geometric rewiring straightens out the jagged branches, pulling the path tighter and tighter around obstacle corners like a taut rubber band.

The KD-Tree Nearest Neighbor Bottleneck

The fatal bottleneck in any RRT implementation is Step 2: finding the nearest node. If the tree contains 50,000 nodes, iterating through an array 50,000 times to calculate the Euclidean distance for every single random sample kills the frame rate.

We must map the vertices into a K-Dimensional Tree (KD-Tree) or an R-Tree spatial index. A KD-Tree recursively partitions the space, reducing the time complexity of the nearest-neighbor search from $\mathcal{O}(N)$ down to $\mathcal{O}(\log N)$. Without spatial indexing, RRT* is useless in real-time robotics.

3. Concrete Implementation: The RRT* Python Architecture

Below is a highly optimized Python implementation of 2D RRT*. Instead of relying on slow Python arrays, it utilizes scipy.spatial.KDTree to aggressively optimize the neighborhood radius search and nearest node identification. We define obstacles as simple bounding circles for rapid mathematical collision checking.

import numpy as np
from scipy.spatial import KDTree
import math

class Node:
    def __init__(self, x: float, y: float):
        self.x = x
        self.y = y
        self.cost = 0.0
        self.parent_idx = -1

class RRTStar:
    def __init__(self, start: tuple, goal: tuple, obstacles: list, bounds: tuple, expand_dis: float = 2.0):
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.obstacles = obstacles # List of (x, y, radius)
        self.x_bounds, self.y_bounds = bounds
        
        self.expand_dis = expand_dis
        # Search radius shrinks dynamically in rigorous implementations, 
        # but is fixed here for clarity
        self.rewire_radius = 5.0 
        self.goal_sample_rate = 0.1 # 10% chance to cast a point directly at the goal

        self.node_list = [self.start]

    def _get_random_point(self) -> tuple:
        """Generates a random coordinate or biases toward the goal."""
        if np.random.rand() < self.goal_sample_rate:
            return (self.goal.x, self.goal.y)
        
        rx = np.random.uniform(self.x_bounds[0], self.x_bounds[1])
        ry = np.random.uniform(self.y_bounds[0], self.y_bounds[1])
        return (rx, ry)

    def _steer(self, from_node: Node, to_point: tuple) -> Node:
        """Projects a new node a fixed distance towards the target point."""
        new_node = Node(from_node.x, from_node.y)
        d, theta = self._calc_distance_and_angle(new_node, to_point)

        # Truncate step size if the point is too far
        step = min(self.expand_dis, d)
        new_node.x += step * math.cos(theta)
        new_node.y += step * math.sin(theta)
        new_node.cost = from_node.cost + step
        
        return new_node

    def _check_collision(self, node_a: Node, node_b: Node) -> bool:
        """
        Evaluates the line segment for obstacle intersection using point-line geometry.
        Returns True if the path is SAFE (no collision).
        """
        for (ox, oy, oradius) in self.obstacles:
            # Vector math to find the shortest distance from obstacle center to line segment
            dx = node_b.x - node_a.x
            dy = node_b.y - node_a.y
            length_sq = dx**2 + dy**2
            
            if length_sq == 0:
                continue
                
            t = max(0, min(1, ((ox - node_a.x) * dx + (oy - node_a.y) * dy) / length_sq))
            proj_x = node_a.x + t * dx
            proj_y = node_a.y + t * dy
            
            dist_to_obstacle = math.hypot(ox - proj_x, oy - proj_y)
            if dist_to_obstacle <= oradius:
                return False # Collision detected
        return True # Safe

    def _calc_distance_and_angle(self, node_a: Node, point: tuple) -> tuple:
        dx = point[0] - node_a.x
        dy = point[1] - node_a.y
        return math.hypot(dx, dy), math.atan2(dy, dx)

    def plan(self, max_iter: int = 2000):
        """Executes the main RRT* Expansion Loop."""
        for i in range(max_iter):
            rnd_point = self._get_random_point()
            
            # Rebuild KD-Tree every iteration (In production, use dynamic insertion R-Trees)
            node_coords = np.array([(n.x, n.y) for n in self.node_list])
            tree = KDTree(node_coords)
            
            # Find nearest node
            _, nearest_idx = tree.query(rnd_point)
            nearest_node = self.node_list[nearest_idx]

            # Steer
            new_node = self._steer(nearest_node, rnd_point)

            # Check primary collision
            if not self._check_collision(nearest_node, new_node):
                continue

            # Query the neighborhood for rewiring
            near_indices = tree.query_ball_point((new_node.x, new_node.y), self.rewire_radius)
            
            # --- REWIRE PHASE 1: Choose Best Parent ---
            best_parent_idx = nearest_idx
            min_cost = new_node.cost
            
            for near_idx in near_indices:
                near_node = self.node_list[near_idx]
                d, _ = self._calc_distance_and_angle(near_node, (new_node.x, new_node.y))
                potential_cost = near_node.cost + d
                
                if potential_cost < min_cost:
                    if self._check_collision(near_node, new_node):
                        min_cost = potential_cost
                        best_parent_idx = near_idx

            new_node.parent_idx = best_parent_idx
            new_node.cost = min_cost
            self.node_list.append(new_node)
            new_node_idx = len(self.node_list) - 1

            # --- REWIRE PHASE 2: Rewire Neighbors ---
            for near_idx in near_indices:
                near_node = self.node_list[near_idx]
                d, _ = self._calc_distance_and_angle(new_node, (near_node.x, near_node.y))
                potential_cost = new_node.cost + d
                
                if potential_cost < near_node.cost:
                    if self._check_collision(new_node, near_node):
                        # The path through the new node is cheaper. Rewire!
                        near_node.parent_idx = new_node_idx
                        near_node.cost = potential_cost
                        # Note: In a complete implementation, this cost reduction must 
                        # be recursively propagated to all children of near_node.

            # Check if we reached the goal radius
            if math.hypot(new_node.x - self.goal.x, new_node.y - self.goal.y) <= self.expand_dis:
                # We reached the goal. The algorithm can continue running to refine the path.
                pass

        print(f"RRT* Search exhausted after {max_iter} iterations.")
        return self._extract_path()

    def _extract_path(self) -> list:
        # Find the node closest to the goal
        node_coords = np.array([(n.x, n.y) for n in self.node_list])
        tree = KDTree(node_coords)
        _, best_idx = tree.query((self.goal.x, self.goal.y))
        
        path = []
        curr_idx = best_idx
        while curr_idx != -1:
            node = self.node_list[curr_idx]
            path.append((node.x, node.y))
            curr_idx = node.parent_idx
            
        return path[::-1] # Reverse to get Start -> Goal

4. Edge Cases, Optimization & Memory Considerations

Cost Propagation Bug

In the code above, Phase 2 rewires a neighbor to point to the new node, updating the neighbor's cost (near_node.cost = potential_cost). However, if that near_node already had "children" extending further out into the map, those children now have mathematically invalid costs. Their costs were calculated based on the old, expensive route.

To maintain mathematical integrity, every time a node is rewired, you must execute a recursive depth-first sweep down that specific branch of the tree, subtracting the cost delta from every single child node. Failure to propagate costs destroys the asymptotic optimality guarantee of RRT*.

Dynamic KD-Tree Rebuilding

Python's scipy.spatial.KDTree is static. When we append a new node to the tree, we cannot inject it into the existing scipy KDTree. We are forced to recreate the entire KDTree from scratch every single iteration (tree = KDTree(node_coords)).

While KDTree construction is highly optimized in C ($\mathcal{O}(N \log N)$), recreating a 10,000-node tree 1,000 times per second dominates CPU profiling. For production C++ systems, robotics engineers use dynamic spatial indexes, like FLANN (Fast Library for Approximate Nearest Neighbors) or Boost.Geometry's R-Tree, which support $\mathcal{O}(\log N)$ insertion without requiring a full rebuild.

Kinematic Constraints (Non-Holonomic RRT*)

The implementation above assumes the robot can move instantly in any direction (a holonomic system). A car cannot move sideways. If the steer function generates a path moving $90^\circ$ sideways, the car physically cannot follow it.

To plan paths for cars (Ackermann steering), the _steer function must not project a straight line. It must project Dubins Curves or Reeds-Shepp curves—geometric arcs defining the minimum turning radius of the vehicle. The collision checker must then evaluate the entire sweep of that curve against obstacles, vastly increasing the computational complexity of the inner loop.

5. Benchmarks & Practical Engineering Takeaways

We benchmarked A, RRT, and RRT navigating a highly constrained maze in a continuous 2D plane ($1000 \times 1000$ coordinate space).

Engineering Guidelines

• Always prune the tree: If RRT* runs for 5 minutes continuously rewiring, the RAM footprint explodes. Implement a pruning threshold. If the absolute best path to the goal currently costs $500$, any node in the tree whose base cost is $>500$ can mathematically never be part of a better path. Delete it instantly to shrink the spatial index.
• Goal Biasing: Pure random sampling is extremely slow at finding narrow doorways. Inject a heuristic: $10\%$ of the time, force _get_random_point() to return the exact coordinate of the Goal. This acts as a gravitational pull, dragging the branches toward the finish line aggressively.
• Any-Time Execution: Do not wait for RRT to finish 10,000 iterations before telling the robot to move. RRT is an "Any-Time" algorithm. Let it run for $500$ iterations to find a jagged, sub-optimal path, and immediately command the robot to start moving along the first segment. While the robot is driving, keep the algorithm running in a background thread to continuously smooth and refine the remaining waypoints.

6. References & Cross-Links

• Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. International Journal of Robotics Research.
• LaValle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning. Technical Report.
• Susam, A. (2026). Unscented Kalman Filters for Non-Linear Kinematic State Estimation. Read Article.
• Susam, A. (2026). Designing Robust PID Controllers for Underactuated Robotic Systems. Read Article.