🔝​

Sampling-Based Planning

• Sampling-Based Planning
  • Books
  • Prerequisites
  • General Motivation
  • Course Content
    • Introduction
      • From a Physical Maze to an Abstract Graph
    • History of Sampling Motion Planning[1]
      • The Early Days: Exhaustive Search and the Complexity Wall
      • A Reactive Detour: Artificial Potential Fields
      • The Breakthrough: Searching the Graph
      • Dijkstra’s Algorithm: The Cost-Conscious Explorer
      • Final step: The Limits of Grids and the Curse of Dimensionality
        • Exercise: Growth in Planning Complexity
        • 1) Grid Search Scaling
          • 2) 2-DoF Arm (2D Planar)
          • 3) 4-DoF Arm Extension
    • Sampling-Based Methods
      • The Sampling Function: The Engine of Exploration[1], [6]
        • Distance Metrics: Workspace vs Configuration Space
        • Exercise: Narrow Passage Probability and Sample Budget
      • The Local Planner: The Reality Check
      • (Optional) Extending the Local Planner: Kinodynamic Planning
        • 1. What Are Kinodynamic Constraints?
        • 2. From Straight Lines to Dynamic Feasibility
          • Method 1: Analytical Steering Function (Two-Point BVP)
          • Method 2: Forward Simulation (Sampling in Control Space)
        • 3. Why It Matters
      • Coding Exercise 1: Sampling Free Configurations
      • Coding Exercise 2: Collision-Checking Graph Building
        • Reflection
    • The Roadmap Approach: Probabilistic Roadmaps (PRM)
      • Construction Phase
      • Query Phase
        • Takeaways
        • Discussion and Reflection
        • Exercise: PRM — k-NN vs Radius Connections
    • The Tree-Growing Approach: Rapidly-Exploring Random Trees (RRT)
      • Core Idea
        • Takeaways
        • Discussion and Reflections
        • Exercise: RRT Step Size — Coverage and Cost
    • Planning for Optimality
      • Post-Processing
        • Path Shortcutting[12]
      • Asymptotically-Optimal Planners
        • Extending RRT Toward Optimality: RRT*[8]
          • Intuition and Key Ideas
        • Extending PRM Toward Optimality: PRM*
      • Discussion: The Price of Optimality
      • Exercises
        • 1. Conceptual: Shortcutting vs. Rewiring
        • 2. Applied: RRT* ChooseParent
        • 3. Applied: RRT* Rewire
      • Summary Table: PRM vs PRM* vs RRT vs RRT*
      • Rules of thumb
    • Final Project
  • Credits:
  • References

Books

• Planning Algorithms – Steven LaValle (2006) Free Online
• Sampling-Based Motion Planning: A Comparative Review – Andreas Orthey, Constantinos Chamzas, Lydia E. Kavraki, arXiv:2309.13119 [cs.RO], 2023. arXiv link

Prerequisites

• Basic probability theory
• Robot kinematics and configuration space
• Graph search algorithms

General Motivation

Course Content

Introduction

At its heart, motion planning asks a simple question: how does a robot decide to move from point A to point B without hitting anything? While the question seems straightforward, the answer is profoundly complex. For a robot with many joints, the number of possible positions - its “configuration space” - is astronomically large. The key to solving this intractable problem lies not in brute force, but in intelligent representation: the art of simplifying a complex physical world into an abstract map that a computer can understand. To explore this foundational idea, we will begin with a classic, intuitive example: a small, wheeled robot navigating a 2D maze.

The robot’s goal is simple: travel from a starting point (blue) to an end point (green). It has no “bird’s-eye view” and can only sense its immediate surroundings. How can we give this robot the ability to find its way? We must first teach it how to think about the maze.

From a Physical Maze to an Abstract Graph

The robot’s world of “floors” and “walls” is too literal. We can create a more powerful mental model by simplifying the maze into its two core components: the locations of interest where the robot can actually be, and the possible moves or direct paths between adjacent locations. By focusing on these ideas, we can transform the physical maze into an abstract map of connections. Each open square becomes a point, and each possible move becomes a line connecting the corresponding points. What we have just created is a graph.

This abstraction is visualized below, showing how a continuous physical space is transformed into a discrete network of nodes and connections:

This is a monumental leap. The robot’s problem is no longer about navigating a physical space; it’s about finding a path within this abstract network. This representation is the common language for nearly all planning algorithms that follow.

History of Sampling Motion Planning[1]

With our graph representation in hand, we can now explore the history of motion planning, using our maze to understand how different strategies evolved.

The Early Days: Exhaustive Search and the Complexity Wall

Early attempts at motion planning in the 1980s tried to be mathematically perfect. They involved precisely defining the robot’s shape and all obstacles, a concept known as Configuration Space [2]. While powerful, these “complete” algorithms tried to solve the problem for every possible path. For our maze, this would be like listing every single possible sequence of moves. It was quickly proven that for complex robots, this problem is NP-hard [3]. In simple terms, this means that as the problem gets bigger, the time required to find a perfect solution explodes exponentially. Our simple maze is solvable, but a slightly larger maze or a robot with more joints would be computationally impossible to solve this way. The quest for mathematical perfection had hit a wall.

A Reactive Detour: Artificial Potential Fields

One clever idea was to stop planning and start reacting. The Artificial Potential Fields method[4], popular in the late 1980s, treated the robot like a marble rolling on a contoured surface. The goal would be a low point, pulling the robot towards it, while obstacles (walls) would be high points, pushing the robot away.

The resulting movement, which behaves like a ball rolling down a contoured surface, is illustrated in the animation below:

This worked well for simple, open environments. However, in a maze, the robot could easily get stuck in a dead-end (a “local minimum”) without ever reaching the goal. It was a step forward in creating dynamic motion, but it wasn’t a reliable planner.

The Breakthrough: Searching the Graph

Before adopting a complex strategy, one might consider a simple heuristic. For our maze, a possible solution could be the “wall-follower” rule: always keep a wall to your left (or right). This can solve simple mazes, but it’s not a general solution. It can fail in mazes with islands or complex layouts, and it provides no guarantees about finding the shortest or most efficient path. What was needed was a systematic, robust method.

The most successful solution returned to our graph representation. If the maze is a graph, then planning is simply a matter of finding the best path through that graph. This led to the utilization of graph search algorithms, which remain a cornerstone of robotics today.

Dijkstra’s Algorithm: The Cost-Conscious Explorer

Let’s make our maze more interesting. Imagine some floor tiles are sand, taking more energy to cross. We can represent this by making our graph weighted—an edge over pavement might have a weight of 1, while an edge over sand has a weight of 5. Now, we don’t just want any path; we want the cheapest path. Dijkstra’s Algorithm is the classic and definitive solution for this. It operates by starting at starting point and exploring outwards like a ripple in a pond. Crucially, it always expands from the vertex that has the lowest total cost discovered so far. It meticulously builds a map of the cheapest way to get to every reachable vertex from the start and doesn’t stop until it has found the cheapest path to the goal. The result is a guaranteed optimal path in terms of total weight. Its weakness is that it’s “uninformed” - it explores in all directions equally, because it has no sense of direction.

Its complexity is described as O(E + V log V), meaning its runtime depends on the number of edges E and vertices V in the graph. For a grid, this is very efficient. But as we’ll see, for more complex problems, creating the graph itself is the real challenge. The process of Dijkstra’s algorithm meticulously expanding outward from the start node is demonstrated in the visualization below:

(For a formal treatment of other graph properties and search algorithms, please refer to the upcoming chapter on Graph Theory in the Advanced Mathematical Foundations section)

Final step: The Limits of Grids and the Curse of Dimensionality

The grid-based approach with Dijkstra’s algorithm feels powerful, a method that guarantees the best possible path on the grid we’ve defined. But what happens when the problem gets more complicated? The simple truth is that this exhaustive approach fails due to the The Curse of Dimensionality.

Let’s move from our simple 2D robot to a more realistic one, like a robotic arm used in manufacturing. A common arm might have 7 joints (7-DOF). To define the robot’s complete pose, we need to know the angle of every single one of those 7 joints. This 7-dimensional space of all possible joint angles is the robot’s Configuration Space (C-space).

Now, imagine trying to create a grid for this C-space. To keep it simple, let’s say we only divide each joint’s range of motion into 10 discrete steps. For one joint, that’s 10 grid cells. For two joints, it’s $10x10 = 100$ cells. For our 7-DOF arm, it becomes $10^7 = 10 000 000$ cells. If we wanted a more reasonable resolution, say 100 steps per joint, we would need $100^7 = 10^{14}$ cells. No modern computer has enough memory to store such a grid, let alone run Dijkstra’s on it.

This problem isn’t unique to robot arms. Imagine a self-driving car navigating a city. If the map is very large and the resolution is high (e.g., centimeter-level precision to avoid small obstacles), the number of grid cells again becomes astronomically large. This exponential explosion in the number of states as we add more dimensions (or higher resolution) is the Curse of Dimensionality. It makes grid-based methods computationally impossible for almost all real-world robotics problems. We need a fundamentally different way to think about the problem.

Sampling-Based Methods

Since we can’t possibly map out the entire C-space, what if we don’t even try? This is the fundamental shift in thinking that leads to Sampling-Based Motion Planning (SBMP). Instead of exhaustively checking every possible location, we can simply generate random configurations in the C-space and check if they are valid (i.e., not in collision).

The core intuition is that if we take enough random samples, we can build a sparse but effective map that captures the essential connectivity of the free space. We don’t need to know about every single point; we just need to know enough to find a way from start to goal. This approach trades the guarantee of finding the absolute best path for the ability to find a feasible path, quickly, in incredibly complex, high-dimensional spaces. Early on, these planners proved their power by solving highly constrained problems that were previously intractable, such as the famous “alpha puzzle”:

This approach is built upon two simple but powerful components, which we will now explore in detail.

The Sampling Function: The Engine of Exploration[1], [6]

A sampling function is responsible for generating an infinite sequence of configurations:

$$ S = { q_1, q_2, q_3, \dots } $$

from the configuration space $\mathcal{C}$.

For a planner to offer theoretical guarantees, this sequence must be dense in $\mathcal{C}$, meaning that for any point in the space, there exists a sampled point arbitrarily close to it.

Distance Metrics: Workspace vs Configuration Space

_{How close are 2 configurations of a robot?. YouTube video, Jan 23, 2018. Available at: https://www.youtube.com/watch?v=B8I43AEerUU&list=PLYZT24lofrjXcuu1iBNWu-NprW2wZD3zu&index=46}

The most straightforward method is unbiased (uniform) sampling, which draws configurations from the C-space such that each has an equal probability of being chosen.

While simple, uniform sampling can be inefficient, in environments with many obstacles or narrow passages, most random samples will fall inside obstacles, wasting computational effort.

To improve efficiency, biased sampling strategies were developed to focus on regions more likely to be useful. One such strategy is obstacle-based sampling, which increases the probability of sampling near obstacle boundaries. The intuition is that the most difficult parts of a path are often found when navigating around obstacles, so focusing effort there can speed up finding a solution in cluttered spaces or through narrow gaps.

Another approach is clearance-based sampling, which prioritizes samples that are far away from obstacles. This leads to safer, smoother paths that are easier for a real robot to execute. This can be achieved by first taking a uniform sample and then performing a short random walk away from the nearest obstacle to improve its clearance.

Finally, deterministic sampling uses low-dispersion sequences (like Halton or Sukharev grid sequences) instead of pseudo-random numbers. These sequences are designed to cover the space more evenly than random sampling, reducing large gaps and improving the reliability and predictability of the planner.

The Local Planner: The Reality Check

A local planner determines whether a simple path between two nearby configurations $q_1$ and $q_2$ is collision-free.

The most common approach is the Straight-Line Planner, which parameterizes the path as:

$$ \tau(s) = (1 - s)\, q_1 + s\, q_2, \quad s \in [0, 1] $$

The local planner discretizes this path into intermediate configurations and checks each for collisions.
If all intermediate steps are collision-free, the connection is accepted as valid.

For robots with differential constraints (e.g., cars or drones), a more advanced Steering Function is required, one that computes control inputs to move the robot from $q_1$ to $q_2$ while respecting its dynamics.

By combining these two functions, one to propose potential locations and one to verify connections between them, these algorithms gain a powerful property known as Probabilistic Completeness.

(Optional) Extending the Local Planner: Kinodynamic Planning

The straight-line local planner works well for purely geometric planning problems, where the robot can move freely in any direction. However, most real-world robots, cars, drones, manipulators, or legged robots, cannot do this. They have kinodynamic constraints, meaning their motion depends not only on position but also on velocity, acceleration, and actuator limits.

1. What Are Kinodynamic Constraints?

In real systems, motion is governed by the robot’s equations of motion, often written as:

$$ \dot{x} = f(x, u) $$

where:

• $x$ is the state vector (e.g., position, velocity, orientation)
• $u$ is the control input (e.g., motor torque, thrust, or steering angle)

The goal of kinodynamic planning is to find a feasible trajectory $x(t)$ and a sequence of control inputs $u(t)$ that move the robot from $x_{\text{start}}$ to $x_{\text{goal}}$ while avoiding obstacles and satisfying physical limits.

2. From Straight Lines to Dynamic Feasibility

The simple local planner assumes the robot can follow a straight line between any two nearby configurations. For a robot with dynamics, this is no longer true — the system may not be able to stop, turn, or move sideways. Instead, the planner must connect states using feasible motions that respect the robot’s dynamic model.

There are two general approaches to doing this:

Method 1: Analytical Steering Function (Two-Point BVP)

A steering function computes a control law that moves the robot exactly from one state to another by solving a two-point boundary value problem (BVP).

How it works:

1. Given two states $x_1$ and $x_2$, solve for a control input $u(t)$ such that integrating $\dot{x} = f(x, u)$ moves $x_1 \rightarrow x_2$.
2. The resulting trajectory is guaranteed to be dynamically feasible.

Examples:

• The Dubins Car (forward-only, minimum turning radius)
• The Reeds–Shepp Car (can drive forward and backward)

Pros:
Produces optimal, physically valid connections.

Cons:
Analytical steering functions exist only for a few simple systems.
For most robots, the BVP is extremely difficult to solve.

Method 2: Forward Simulation (Sampling in Control Space)

Instead of trying to solve a complex boundary-value problem, this approach samples control inputs and simulates the system forward in time, a much more general technique.

How it works:

1. Choose a state $x_{\text{near}}$ already in the tree.
2. Sample a random control $u_{\text{rand}} \in U$ (e.g., random steering or thrust command).

3. Simulate the dynamics forward for a short time $\Delta t$:

   $$ x_{\text{new}} = \text{Simulate}(x_{\text{near}}, u_{\text{rand}}, \Delta t) $$

4. If the simulated motion stays collision-free, accept the new state and continue.

This “forward-propagation” idea enables planners to handle nonholonomic and dynamic systems
without needing a closed-form steering law.

3. Why It Matters

Adding kinodynamic feasibility fundamentally changes the planning problem:

• State space dimension increases (position + velocity + other dynamics)
• Local connections become directional and irreversible
• Paths are trajectories in time, not just curves in space

These challenges make the local planner the core difficulty in motion planning for dynamic robots. Later chapters will show how global planners such as PRM and RRT extend these ideas
to build complete kinodynamic planning algorithms.

Coding Exercise 1: Sampling Free Configurations

In this exercise, you will implement a sampling function for a 2D maze:

def sampling(grid, num_samples, size_of_grid)

The function should return a list of num_samples valid positions (x, y) inside the maze.
Each returned point must correspond to a location the robot can occupy without colliding with obstacles.

After you implement the function, click Run ▶ below to visualize your sampled points on the maze.

def sampling(grid, num_samples, size_of_grid, robot_radius=0.25): """ Sample 'num_samples' continuous (x,y) in free space with clearance. A point is accepted iff its distance to the nearest wall cell (axis-aligned unit square) is >= robot_radius. grid: 2D np.ndarray, 1=wall, 0=free size_of_grid: (W, H) in grid units (cells) """ import numpy as np import math H, W = grid.shape assert (W, H) == size_of_grid, "size_of_grid must match grid" samples = [] attempts = 0 max_attempts = max(1000, num_samples * 100) while len(samples) < num_samples and attempts < max_attempts: attempts += 1 # 1. Randomly sample a continuous (x, y) within the maze bounds # 2. Check if it lands inside a free cell (grid[r, c] == 0) # 3. Compute its minimum distance to the nearest wall cell # 4. Keep it only if that distance >= robot_radius # (Hint: distance to a wall cell [xx, xx+1]x[yy, yy+1] can be computed manually) return samples

Samples: Run ▶ Show Solution 💡

✅ Solution:

def sampling(grid, num_samples, size_of_grid, robot_radius=0.25):
    import numpy as np, math
    H, W = grid.shape
    samples = []
    attempts = 0
    max_attempts = max(1000, num_samples * 100)
    while len(samples) < num_samples and attempts < max_attempts:
        attempts += 1
        x, y = np.random.uniform(0, W), np.random.uniform(0, H)
        c, r = int(x), int(y)
        if grid[r, c] == 1:
            continue
        pad = int(math.ceil(robot_radius)) + 1
        dmin = float("inf")
        for yy in range(max(0, r - pad), min(H - 1, r + pad) + 1):
            for xx in range(max(0, c - pad), min(W - 1, c + pad) + 1):
                if grid[yy, xx] == 1:
                    dx = max(xx - x, 0, x - (xx + 1))
                    dy = max(yy - y, 0, y - (yy + 1))
                    d = math.hypot(dx, dy)
                    if d < dmin:
                        dmin = d
        if dmin >= robot_radius:
            samples.append((x, y))
    return samples
    

Output:

Coding Exercise 2: Collision-Checking Graph Building

In this exercise, you will implement a function to build a collision-free graph connecting sampled configurations in a 2D maze:

def build_graph(grid, nodes, robot_radius=0.25, k=8, max_edge_len=None)

The function should return a list of undirected edges [(i, j)], where each edge connects two valid configurations without intersecting walls.
Each edge must satisfy the robot’s clearance constraints.

After you implement the function, click Run ▶ below to visualize your constructed roadmap over the maze.

def build_graph(grid, nodes, robot_radius=0.25, k=8, max_edge_len=None): """ Build an undirected graph with collision-checked straight-line edges. Inputs ------ grid : np.ndarray (H, W) with 1=wall, 0=free nodes : list[(x, y)] continuous coordinates in free space robot_radius : float minimum clearance required from walls k : int connect up to k nearest neighbors per node max_edge_len : float or None if set, only connect if distance <= max_edge_len Returns ------- edges : list[(i, j)] with i < j """ import numpy as np N = len(nodes) edges = [] # TODO: # 1) Compute pairwise distances (or use a k-NN approach). # 2) For each node i, consider nearest neighbors j (skip i==j). # - If max_edge_len is not None and dist(i,j) > max_edge_len: skip # - Query helper: clr = segment_clearance(grid, nodes[i], nodes[j]) # 0 => collision # - If clr >= robot_radius: accept edge (min(i,j), max(i,j)) once # - Stop after adding up to k neighbors for node i return edges

Samples: k: Max edge len: Robot radius: Run ▶ Show Solution 💡

✅ One Possible Solution (for reference):

def build_graph(grid, nodes, robot_radius=0.25, k=8, max_edge_len=None):
    import numpy as np
    pts = np.array(nodes, dtype=float)
    N = len(pts)
    edges = []
    seen = set()

    # pairwise distances (O(N^2))
    dmat = np.sqrt(((pts[:, None, :] - pts[None, :, :])**2).sum(axis=2))

    for i in range(N):
        order = np.argsort(dmat[i])
        added = 0
        for j in order:
            if j == i:
                continue
            dij = dmat[i, j]
            if max_edge_len is not None and dij > float(max_edge_len):
                continue
            if added >= int(k):
                break
            clr = segment_clearance(grid, tuple(pts[i]), tuple(pts[j]))
            if clr >= float(robot_radius):
                a, b = (i, j) if i < j else (j, i)
                if (a, b) not in seen:
                    edges.append((a, b))
                    seen.add((a, b))
                    added += 1
    return edges
    

Output:

In the following chapters, we’ll explore several of sampling-based algorithms in depth.
You’ll learn how they work, their pros and cons, and when to use each one depending on your environment and planning needs.
We’ll also discuss different variations and extensions designed to improve performance, guarantee completeness, or adapt to complex robot dynamics.

The Roadmap Approach: Probabilistic Roadmaps (PRM)

The first major algorithm built on this sampling paradigm is the Probabilistic Roadmap (PRM)[9].
Its philosophy is simple and powerful:

“Build a map first, then ask for directions.”

This makes it a multi-query planner, meaning it invests time upfront to build a comprehensive roadmap of the free configuration space ($\mathcal{C}_{\text{free}}$), which can then be reused to solve many different planning problems (e.g., moving between different start and goal points) almost instantly.
This is ideal for static environments, like a factory floor where the obstacles don’t move.

Construction Phase

PRM operates in two distinct phases. The first is the Construction Phase, where the map is built. This is an automated version of the exercises you just completed.

1. Sample a configuration.
   Draw a random configuration $ q_{\text{rand}} $ uniformly from the free configuration space (C-space).

2. Validate the sample.
   If $ q_{\text{rand}} $ is collision-free, add it to the roadmap as a vertex; otherwise discard it.

3. Find neighbors.
   Compute the $ k $ nearest existing vertices to $ q_{\text{rand}} $ using a chosen distance metric, typically Euclidean distance.

4. Call the local planner.
   For each neighbor $ q_{\text{near}} $, invoke a straight-line local planner to test whether the segment
   $ \overline{q_{\text{rand}} q_{\text{near}}} $ is collision-free.

5. Add edges.
   If the local connection is valid, add an undirected edge between $ q_{\text{rand}} $ and $ q_{\text{near}} $.

6. Repeat.
   Iterate steps 1–5 for a fixed number of samples (or until a stopping criterion is met) to gradually build a rich network that captures the connectivity of the robot’s free space.

The pseudocode below follows this logic line by line: it starts by initializing an empty graph, then iteratively performs sampling, collision-checking, neighbor search, and local connections.

procedure BUILD-ROADMAP(num_samples)
    G ← (V, E) with V = ∅, E = ∅               ▷ Initialize empty roadmap:
                                               ▷ V = vertices (samples), 
                                                E = edges (connections)

    for i = 1 to num_samples do                ▷ Repeat construction steps
        q_rand ← sample from 𝒞_free            ▷ Step 1: draw a random 
                                            collision-free configuration
        V.add(q_rand)                          ▷ Step 2: add it as a new vertex

        N(q_rand) ← k-nearest neighbors of q_rand in V  
                                               ▷ Step 3: find its k closest existing 
                                              vertices

        for each q_near in N(q_rand) do        ▷ Attempt local connections
            if LocalPlanner(q_rand, q_near) is collision-free then
                                              ▷ Step 4: check straight-line validity
                E.add((q_rand, q_near))       ▷ Step 5: add an undirected edge

    return G                                  ▷ Return the completed roadmap

Query Phase

The second phase is the Query Phase.
Now that we have our roadmap, solving a specific problem is easy.

1. Insert query configurations.
   Take the start $ q_{\text{start}} $ and goal $ q_{\text{goal}} $ configurations.

2. Connect to the roadmap.
   Use the same $ k $-nearest neighbor search and local planner to connect $ q_{\text{start}} $ and $ q_{\text{goal}} $
   to nearby roadmap vertices (adding edges where collision-free).

3. Search the graph.
   Run a shortest-path algorithm (e.g., Dijkstra’s) on the augmented roadmap to find a path
   from $ q_{\text{start}} $ to $ q_{\text{goal}} $.

4. Extract the trajectory.
   Concatenate the straight-line local-planner segments along the found graph path to obtain
   a feasible trajectory in configuration space (C-space).

A major strength of PRM is that if we get a new query (e.g., move from $q_{\text{start}}$ to a new $q_{\text{goal}_2}$), we only need to repeat this trivial query phase, the expensive map construction is already done.

The animation below provides a visual walkthrough of the PRM construction phase, showing how random samples are converted into a connected graph, it then follows with the query phase, resulting in complete path from start to goal:

_{PRM: Probabilistic Roadmap Method in 3D and with 7-DOF robot arm. YouTube video, Nov 23, 2020. Available at: https://www.youtube.com/watch?v=tlFVbHENPCI&t=568s}

Takeaways

• In addition to the usual k-nearest neighbors, the video highlights a radius-based connection rule, where nodes are linked only if they lie within a fixed spatial threshold. A larger radius increases roadmap density and shortens paths, but also raises computational cost.

• The roadmap can be reused if obstacles remain static, but when they move, one must recompute collisions and rebuild edges, even if the sample points themselves are retained.

• The video compares Dijkstra’s algorithm with A*:
  A* expands fewer nodes when the heuristic (Euclidean distance in C-space) is informative, but behaves like Dijkstra in cluttered or poorly guided spaces.

• Increasing the number of samples or the connection radius reduces average path length, at the expense of longer preprocessing time.

• The 3D and 7-DOF scenes show how PRM approximates complex manifolds in joint space and scales gracefully without explicitly constructing obstacle regions.

Discussion and Reflection

• How should distance metrics and interpolation be adapted for toroidal joint spaces to avoid discontinuities?
• What are the trade-offs between k-nearest and radius-based connection strategies?
• Why is it misleading to show the full configuration-space obstacles when teaching PRM, as the video notes?
• Can you design an experiment to observe diminishing returns in path length improvement as sample count increases?

The Tree-Growing Approach: Rapidly-Exploring Random Trees (RRT)

What if we only need to find a single path quickly, and don’t want to spend time building a comprehensive map of the whole space? This is the problem the Rapidly-Exploring Random Tree (RRT)[11] algorithm solves. Its philosophy is:

“Explore purposefully from the start.”

RRT is a single-query planner that grows a tree structure rooted at the start configuration, incrementally expanding into unexplored regions of the C-space until it finds the goal.

Core Idea

The growth heuristic is simple and powerful:

1. Sample a random configuration $q_{\text{rand}}$ from the entire C-space (note: not just the free space).
2. Find the node already in the tree that is nearest to this random sample — call it $q_{\text{near}}$.
3. Instead of trying to connect all the way to $q_{\text{rand}}$, the algorithm extends a new branch from $q_{\text{near}}$ in the direction of $q_{\text{rand}}$, but only for a small, predefined step distance $\varepsilon$. This new point is $q_{\text{new}}$.
4. The local planner checks if the small path segment from $q_{\text{near}}$ to $q_{\text{new}}$ is collision-free.
   If it is, $q_{\text{new}}$ is added to the tree as a new vertex with an edge connecting it back to $q_{\text{near}}$.

To summarize, RRT repeatedly samples a random configuration, finds the closest tree node, takes a small step toward the sample, and adds this new point if the short motion is collision-free. The pseudocode below follows this logic line by line.

procedure BUILD-RRT(q_start)
    T.initialize(q_start)                   ▷ Initialize the tree T with the 
                                              start configuration q_start
                                              as its root node

    for i = 1 to num_iterations do          ▷ Grow the tree by repeating 
                                            the expansion
        q_rand ← sample from 𝒞              ▷ Step 1: sample a random configuration
                                              anywhere in the full C-space
        q_near ← NearestNode(q_rand, T)     ▷ Step 2: find the node in T whose 
                                              configuration is closest to q_rand
        q_new ← Extend(q_near, q_rand)      ▷ Step 3: move a small step ε from q_near 
                                             toward q_rand to create a candidate
                                             node q_new
        if LocalPlanner(q_near, q_new) is collision-free then
                                            ▷ Step 4: check that the short motion  
                                            from q_near to q_new is collision-free
            T.add_node(q_new)               ▷ Add q_new as a new vertex in the tree
            T.add_edge(q_near, q_new)       ▷ Connect q_new back to its parent q_near

    return T                                ▷ Return the final exploration tree

The following animation provides a visual walkthrough of the RRT graph building and path finding:

_{RRT, RRT & Random Trees. YouTube video, Nov 21, 2018. Available at: https://www.youtube.com/watch?v=Ob3BIJkQJEw*}

Takeaways

• A purely random tree expands from arbitrary existing nodes and often remains clustered near the start. In contrast, RRT always extends the nearest node toward a random sample, driving rapid outward exploration.

• RRT quickly finds a feasible path, but the result is usually jagged and non-optimal.
  The basic RRT does not improve its path even if more nodes are added.

• RRT* (to be covered later): Introduces rewiring, allowing the algorithm to gradually shorten and smooth the path.
  As more nodes are added, the solution asymptotically approaches the true optimal path.

• Sampling the goal with some probability (e.g., 5–10%) helps reach the goal faster in open spaces,
  but excessive bias can trap the tree in local minima or concave regions.

• RRT’s local, forward-projection nature makes it efficient in high-dimensional and obstacle-rich environments, unlike PRM, which must solve many global connection checks.

Discussion and Reflections

1. In what types of environments can goal bias help or hinder progress?
2. Why can RRT* improve path optimality while RRT cannot?
3. Compare RRT’s forward projection to PRM’s global roadmap in terms of computational trade-offs.
4. What post-processing methods could smooth a jagged RRT path while maintaining feasibility?

Planning for Optimality

The paths found by PRM and RRT are feasible, but they are rarely good. Due to their random nature, the resulting paths are often jagged, inefficient, and unnatural. In many applications, especially in robotics, finding a path that is short, smooth, or energy-efficient is just as important as finding a path at all.

This chapter explores two main strategies for finding higher-quality paths:

• Post-Processing: Taking a “bad” path and improving it.
• Asymptotically-Optimal Planners: Using algorithms that are guaranteed to find the optimal path, given enough time.

Post-Processing

Before 2010, optimality was commonly handled as a post-processing step. The planner would quickly find any feasible path, and then a separate optimization algorithm would try to improve it.[1]

Path Shortcutting[12]

The simplest and most common post-processing technique is path shortcutting. The idea is to iteratively improve the path by replacing segments with shorter, collision-free “shortcuts.”

procedure SHORTCUT-PATH(path)
    for i = 1 to N_ITERATIONS do               ▷ Repeat many shortcut attempts

        t1, t2 ← RandomTimes(path) with t1 < t2
                                               ▷ Step 1: pick two random arc-length
                                                 positions along the current path
        q1 ← path(t1)                          ▷ Evaluate the first point
        q2 ← path(t2)                          ▷ Evaluate the second point

        if LocalPlanner(q1, q2) is collision-free then
                                               ▷ Step 2: check whether a straight-line
                                                 shortcut between q1 and q2 is valid

            path.replace(from=q1, to=q2, with=NewSegment(q1, q2))
                                               ▷ Step 3: shorten the path by replacing
                                                 the old section with this shortcut

    return path                                ▷ Return the smoothed path

This is a gradient-free optimization method that is surprisingly effective. It is not guaranteed to find the true optimal path, but it can dramatically reduce path length and remove unnecessary detours with very little computational effort.

Asymptotically-Optimal Planners

In 2011, a breakthrough occurred with the development of planners that are asymptotically optimal.[1]

This led to the “star” versions of the main algorithms: PRM* and RRT*.

Extending RRT Toward Optimality: RRT*[8]

While the Rapidly-Exploring Random Tree (RRT) is highly effective at finding feasible paths in complex, high-dimensional spaces, it does not account for path quality. In many robotic applications, such as manipulator motion, drone navigation, or autonomous driving, path quality matters just as much as feasibility. A poor trajectory may increase energy consumption, execution time, or even risk of collision when tracking the path dynamically.

To address this, RRT* extends the original RRT framework to not only explore the configuration space but also to improve the solution quality over time.
It introduces two critical operations, ChooseParent and Rewire, which enable local cost optimization and global convergence to the optimal path.
The resulting planner is asymptotically optimal, meaning that as the number of samples grows, the cost of the best path found converges to the true minimum.

Intuition and Key Ideas

RRT* follows the same exploratory logic as RRT — sampling random configurations and incrementally growing a tree that covers the free configuration space.
However, instead of greedily attaching each new node to its nearest neighbor, it performs a local optimization every time a new node is added:

1. ChooseParent — optimal insertion:
   When a new node $q_{\text{new}}$ is created, RRT* looks at all existing nodes within a neighborhood of radius $r(n)$ and selects as its parent the one that minimizes the total path cost from the start.
   This ensures that $q_{\text{new}}$ is always connected through the most efficient local route.

2. Rewire — cost propagation:
   Once $q_{\text{new}}$ is added, it may also improve existing connections.
   RRT* checks whether connecting any nearby node through $q_{\text{new}}$ would yield a cheaper total cost.
   If so, it “rewires” that neighbor to use $q_{\text{new}}$ as its new parent, effectively reshaping the tree toward a more optimal structure.

The pseudocode below follows this logic line by line.

procedure BUILD-RRT*(q_start)
    T.initialize(q_start)                      ▷ Start tree with q_start

    for i = 1 to num_iterations do             ▷ Grow the tree
        q_rand ← sample from 𝒞                 ▷ Random configuration
        q_near ← NearestNode(q_rand, T)        ▷ Nearest existing node
        q_new  ← Extend(q_near, q_rand)        ▷ Step toward q_rand

        if LocalPlanner(q_near, q_new) is collision-free then
                                               ▷ Add only if short step is valid

            N_near ← NearNodes(q_new, T, radius=r(n))
                                               ▷ Nearby nodes within radius r(n)

            // --- ChooseParent: locally optimal insertion ---
            q_min ← q_near                     ▷ Default parent
            c_min ← Cost(q_near) + Cost(q_near, q_new)
                                               ▷ Cost via q_near

            for each q_neighbor in N_near do
                c_new ← Cost(q_neighbor) + Cost(q_neighbor, q_new)
                                               ▷ Try cheaper parent
                if c_new < c_min and
                   LocalPlanner(q_neighbor, q_new) then
                                               ▷ Only if edge is valid
                    q_min ← q_neighbor
                    c_min ← c_new

            T.add_node(q_new)                  ▷ Insert q_new
            T.add_edge(q_min, q_new)           ▷ Connect to best parent

            // --- Rewire: locally optimal tree repair ---
            for each q_neighbor in N_near do
                c_old ← Cost(q_neighbor)       ▷ Old cost
                c_new ← Cost(q_new) + Cost(q_new, q_neighbor)
                                               ▷ Cost via q_new
                if c_new < c_old and
                   LocalPlanner(q_new, q_neighbor) then
                                               ▷ Rewire if shorter and valid
                    T.rewire(q_neighbor, new_parent=q_new)

    return T                                  ▷ Return optimal tree

The search radius $r(n)$ gradually decreases with the number of samples $n$, ensuring both convergence and computational tractability.
Through repeated sampling and local rewiring, RRT* progressively smooths out the tree, pulling all feasible paths toward the globally optimal solution:

Extending PRM Toward Optimality: PRM*

The primary limitation of the classic Probabilistic Roadmap (PRM) algorithm is its lack of guaranteed optimality. The connection rule based on a fixed number of neighbors ($k$) prevents the graph from fully exploring longer, potentially more optimal connections. The resulting feasible path is often suboptimal, limited by the sparse, static structure of the roadmap.

PRM* addresses this by modifying the construction phase to guarantee asymptotic optimality, ensuring the path cost converges to the true optimal cost as the number of samples increases.

The guarantee of optimality in PRM* comes from replacing the fixed $k$-Nearest Neighbors rule with a dynamic, shrinking search radius $r(n)$, as shown in the following pseudocode:

procedure BUILD-PRM*(num_samples)
    G.initialize()                            ▷ Create empty graph G = (V, E)

    for n = 1 to num_samples do               ▷ Add samples one by one
        q_rand ← sample from 𝒞_free           ▷ Step 1: draw a collision-free sample
        V.add(q_rand)                         ▷ Add new vertex to the roadmap

        r_n ← r(n)                            ▷ Step 2: connection radius
                                                 r(n) = γ (log n / n)^(1/d)

        N_near ← NearNodes(q_rand, V, radius=r_n)
                                              ▷ Step 3: all neighbors within r_n

        for each q_near in N_near do          ▷ Try to connect nearby vertices
            if LocalPlanner(q_rand, q_near) is collision-free then
                                              ▷ Step 4: check if the straight segment
                                                 between q_rand and q_near is valid
                E.add((q_rand, q_near))       ▷ Step 5: add undirected edge

    return G = (V, E)                         ▷ Final optimal roadmap

This change ensures that new, shorter connections can be established across the entire free space as the roadmap grows, continuously refining the path toward the optimum.

The animation below showcases the performance difference between the standard PRM algorithm and its asymptotically optimal variant, PRM*:

Discussion: The Price of Optimality

• PRM is fast. It connects each sample to a fixed number of neighbors and quickly produces a feasible roadmap.

• PRM* is slower. It uses a growing connection radius, requiring many more neighbor checks. But as samples increase, its paths converge to the optimal one.

• RRT is fast. It finds a feasible path quickly and stops, making it ideal for fast exploration.
• RRT* is slow. It keeps refining the tree using ChooseParent and Rewire, performing many more nearest-neighbor and collision checks. Over time, it converges to the true optimal path.

Exercises

Summary Table: PRM vs PRM* vs RRT vs RRT*

Rules of thumb

• Use PRM when: many queries, static environment.
• Use RRT when: you need one quick feasible path.
• Use RRT* when: quality matters and time isn’t critical.
• Use PRM* when: you are building a high-quality global map (e.g., industrial cells).

Final Project

• Implement and compare RM, RRT, RRT on a chosen benchmark.
• Evaluate success rate, runtime, and path quality.
• Extend to one special case (kinodynamic, narrow passage, or uncertainty).

Credits:

This course page was created by Hanka Goralija, EPFL under the supervision of Prof. Aude Billard, and funded by IEEE RAS and EPFL.

References

1. Orthey, A., Chamzas, C., & Kavraki, L. E. (2023). Sampling-Based Motion Planning: A Comparative Review. arXiv preprint arXiv:2309.13119 [cs.RO]. Available at: https://arxiv.org/abs/2309.13119

2. Lozano-Pérez, T., & Wesley, M. A. (1979). An algorithm for planning collision-free paths among polyhedral obstacles. Communications of the ACM, 22(10), 560–570.

3. Canny, J., & Reif, J. (1987). New lower bound techniques for robot motion planning problems. In Proceedings of the 28th Annual Symposium on Foundations of Computer Science (SFCS 1987), pp. 49–60. IEEE.

4. Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots. In Autonomous Robot Vehicles. Springer.

5. Choset, H. (with Ji Yeong Lee, G. D. Hager & Z. Dodds). (n.d.). Robotic Motion Planning: Potential Functions. 16-735 Robotics Institute, Carnegie Mellon University. Retrieved from https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf

6. LaValle, S. M. (2012). Motion Planning for Dynamic Environments, Part II: Motion Planning – Finding the Path. ICRA 2012 Tutorial, University of Illinois at Urbana-Champaign. Retrieved from https://msl.cs.uiuc.edu/~lavalle/ (ICRA 2012 Tutorial, May 14 2012).

7. LaValle, S. M. (2006). Planning Algorithms. Cambridge University Press. Available at: https://lavalle.pl/planning/

8. Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7), 846–894. DOI: 10.1177/0278364911406761

9. Kavraki, L. E., Švestka, P., Latombe, J.-C., & Overmars, M. H. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 12(4), 566–580. DOI: 10.1109/70.508439

10. Okabe, A., Boots, B., Sugihara, K., & Chiu, S. N. (2000). Spatial Tessellations: Concepts and Applications of Voronoi Diagrams (2nd ed.). Wiley.

11. LaValle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning. Technical Report TR 98-11, Computer Science Department, Iowa State University.

12. Geraerts, R., & Overmars, M. H. (2007). Creating high-quality paths for motion planning. International Journal of Robotics Research, 26(8), 845–863.

Back to Top