Path Planning Algorithms: A*, RRT, and Practical Selection

Your robot sits in a warehouse corner. The destination is three aisles away, blocked by pallets and equipment. Which algorithm gets it there efficiently—A*, RRT, or something else? The answer matters more than you'd think. Choose poorly and your system wastes compute cycles, gets stuck, or arrives too late. Choose right and it scales from single robots to fleet operations.

Path planning is the bridge between where a robot is and where it needs to go. But "bridge" is deceptively simple. The algorithm you pick determines latency, success rate, memory footprint, and whether your mobile manipulator actually reaches its goal.

A*: The Optimal Deterministic Solution

A* is the gold standard when you have a complete map and can afford computation upfront. It's a best-first search that uses a heuristic function to guide exploration toward the goal.

How It Works

A* expands nodes based on f(n) = g(n) + h(n), where:

• g(n) is the cost from start to current node
• h(n) is the estimated cost from current node to goal

With a good heuristic (like Euclidean distance), A* typically explores fewer nodes than Dijkstra's algorithm while guaranteeing optimality.

Example Implementation

pythonCopy
import heapq
from math import sqrt

class AStarPlanner:
    def __init__(self, grid, start, goal):
        self.grid = grid
        self.start = start
        self.goal = goal
        self.open_set = [(0, start)]
        self.came_from = {}
        self.g_score = {start: 0}
    
    def heuristic(self, pos):
        return sqrt((pos[0] - self.goal[0])**2 + (pos[1] - self.goal[1])**2)
    
    def plan(self):
        while self.open_set:
            _, current = heapq.heappop(self.open_set)
            if current == self.goal:
                return self._reconstruct_path(current)
            
            for neighbor in self._get_neighbors(current):
                tentative_g = self.g_score[current] + 1
                if neighbor not in self.g_score or tentative_g < self.g_score[neighbor]:
                    self.came_from[neighbor] = current
                    self.g_score[neighbor] = tentative_g
                    f_score = tentative_g + self.heuristic(neighbor)
                    heapq.heappush(self.open_set, (f_score, neighbor))
        
        return None  # No path found
    
    def _get_neighbors(self, pos):
        # Return valid adjacent cells
        pass
    
    def _reconstruct_path(self, current):
        path = [current]
        while current in self.came_from:
            current = self.came_from[current]
            path.append(current)
        return path[::-1]

When to Use A*

• Static, fully-known environments (warehouse with fixed layout)
• High-dimensional problems where exploration is expensive
• You need the shortest path, not just any path
• Real-time replanning with grid maps

Rapidly Exploring Random Trees (RRT): The Sampling Approach

RRT takes a fundamentally different approach. Instead of searching a discretized grid, it randomly samples the configuration space and connects new points to the nearest existing node in the tree.

Key Strengths

RRT excels in high-dimensional spaces (robotic arms, humanoids) and handles complex obstacles naturally. It finds a solution fast, though not necessarily the optimal one.

Basic Structure

pythonCopy
import random
from math import sqrt

class RRTPlanner:
    def __init__(self, start, goal, bounds, obstacle_check, step_size=1.0):
        self.start = start
        self.goal = goal
        self.bounds = bounds
        self.obstacle_check = obstacle_check
        self.step_size = step_size
        self.tree = {start: None}
    
    def plan(self, max_iterations=5000):
        for _ in range(max_iterations):
            rand_point = self._random_config()
            nearest = self._nearest_node(rand_point)
            new_point = self._steer(nearest, rand_point)
            
            if self.obstacle_check(nearest, new_point):
                self.tree[new_point] = nearest
                if self._distance(new_point, self.goal) < self.step_size:
                    return self._extract_path(new_point)
        
        return None
    
    def _random_config(self):
        return tuple(random.uniform(self.bounds[i][0], self.bounds[i][1]) 
                     for i in range(len(self.bounds)))
    
    def _nearest_node(self, point):
        return min(self.tree.keys(), key=lambda n: self._distance(n, point))
    
    def _steer(self, from_point, to_point):
        dist = self._distance(from_point, to_point)
        if dist < self.step_size:
            return to_point
        direction = tuple((to_point[i] - from_point[i]) / dist for i in range(len(from_point)))
        return tuple(from_point[i] + direction[i] * self.step_size for i in range(len(from_point)))
    
    def _distance(self, p1, p2):
        return sqrt(sum((p1[i] - p2[i])**2 for i in range(len(p1))))
    
    def _extract_path(self, node):
        path = []
        while node is not None:
            path.append(node)
            node = self.tree[node]
        return path[::-1]

When to Use RRT

• High-dimensional configuration spaces (robot arms, drones)
• Unknown or partially-known environments
• Quick solution needed over optimal solution
• Complex geometries that are hard to discretize

Comparing Trade-offs

| Metric | A* | RRT | |--------|-----|-----| | Optimality | Yes | No | | Dimensionality | 2–3D typical | 6D+ | | Map requirement | Complete | Partial/none | | Computation | Front-loaded | Incremental | | Obstacle handling | Discrete | Continuous |

At LavaPi, we often combine both: A* for ground robots with known maps, RRT* (an improved variant) for manipulator trajectory planning. The choice isn't binary—it's contextual.

The Real Takeaway

A* wins in structured, predictable spaces. RRT wins in complexity. Most production systems use hybrid approaches or variants like RRT* and Theta*. Understand your problem space, measure performance, and iterate. That's the only algorithm that always works.