We assume a configuration space \( \mathcal{C} \subset \mathbb{R}^d \), typically the joint space of a manipulator, decomposed into \( \mathcal{C}_{\text{free}} \) and \( \mathcal{C}_{\text{obs}} \):

\[ \mathcal{C}_{\text{free}} = \{ q \in \mathcal{C} \mid \text{robot at configuration } q \text{ is collision-free} \}, \quad \mathcal{C}_{\text{obs}} = \mathcal{C} \setminus \mathcal{C}_{\text{free}}. \]

A motion planning query specifies start and goal configurations \( q_{\text{start}}, q_{\text{goal}} \in \mathcal{C}_{\text{free}} \) and asks for a continuous path \( \sigma : [0,1] \to \mathcal{C}_{\text{free}} \) such that \( \sigma(0) = q_{\text{start}} \), \( \sigma(1) = q_{\text{goal}} \). Often we minimize a cost functional

\[ J(\sigma) = \int_0^1 c(\sigma(t), \dot{\sigma}(t))\,dt, \]

though classical RRT is feasibility-oriented (it does not explicitly optimize \(J\)).

RRTs are defined with respect to a metric \( d : \mathcal{C} \times \mathcal{C} \to \mathbb{R}_{\ge 0} \). In practice, common choices include:

• Joint-space Euclidean: \( d(q,q') = \| q - q' \|_2 \).
• Weighted joint-space: \( d(q,q') = \sqrt{ (q-q')^\top W (q-q') } \), with a positive-definite weight matrix \( W \) approximating inertia or task priorities.

RRT is driven by a probability measure \( \mu \) on \( \mathcal{C} \), typically uniform on a bounding box containing \( \mathcal{C}_{\text{free}} \). At iteration \( k \), the algorithm samples \( q_{\text{rand}} \sim \mu \), finds a nearest node in its current tree, and extends a small step toward \( q_{\text{rand}} \).

Formally, let \( T_k = (V_k, E_k) \) denote the tree at iteration \(k\), with \( V_k \subset \mathcal{C}_{\text{free}} \) and directed edges \( E_k \subset V_k \times V_k \) connecting parent to child along collision-free straight-line segments (or more general local motions).

The RRT iteration is driven by a steer operator that moves a bounded distance toward a random sample. For a step size parameter \( \eta > 0 \), define

\[ \operatorname{Steer}(q_{\text{near}}, q_{\text{rand}}; \eta) = \begin{cases} q_{\text{near}} + \displaystyle \eta \dfrac{q_{\text{rand}} - q_{\text{near}}}{\|q_{\text{rand}} - q_{\text{near}}\|_2}, & \text{if } \|q_{\text{rand}} - q_{\text{near}}\|_2 > \eta, \\[1.0em] q_{\text{rand}}, & \text{otherwise.} \end{cases} \]

Let \( \text{CollisionFree}(q,q') \) be a predicate that checks whether the straight-line segment between \(q\) and \(q'\) lies in \( \mathcal{C}_{\text{free}} \) (typically via interpolation and geometric collision checks).

A standard single-tree RRT can be described as:

1. Initialize \( V_0 = \{ q_{\text{start}} \} \), \( E_0 = \emptyset \).
2. For iterations \( k = 0,1,2,\dots \):
   1. Sample \( q_{\text{rand}} \sim \mu \) (occasionally biasing toward \( q_{\text{goal}} \)).
   2. Find \( q_{\text{near}} = \arg\min_{v \in V_k} d(v, q_{\text{rand}}) \).
   3. Compute \( q_{\text{new}} = \operatorname{Steer}(q_{\text{near}}, q_{\text{rand}}; \eta) \).
   4. If \( \text{CollisionFree}(q_{\text{near}}, q_{\text{new}}) \) then \( V_{k+1} = V_k \cup \{ q_{\text{new}} \} \) and \( E_{k+1} = E_k \cup \{ (q_{\text{near}}, q_{\text{new}}) \} \).
   5. If \( d(q_{\text{new}}, q_{\text{goal}}) \le \varepsilon \) (goal radius), stop and return the path traced backwards along parents.

flowchart TD
  A["Initialize tree with q_start"] --> B["Sample q_rand in C"]
  B --> C["Find nearest node q_near in tree"]
  C --> D["Compute q_new = Steer(q_near, q_rand, eta)"]
  D --> E{"Edge (q_near, q_new) \nis collision-free?"}
  E -->|yes| F["Add q_new and edge to tree"]
  E -->|no| B
  F --> G{"Is q_new in \ngoal region?"}
  G -->|yes| H["Extract path and terminate"]
  G -->|no| B
        

The exploration bias of RRT is often described via Voronoi regions. The Voronoi cell of a node \( x \in V_k \) is

\[ \operatorname{Vor}_k(x) = \{ q \in \mathcal{C}_{\text{free}} \mid d(q,x) \le d(q,v) \ \forall v \in V_k \}. \]

Because \( q_{\text{near}} \) is chosen as the nearest neighbor of \( q_{\text{rand}} \), the probability that a node \(x\) is selected for expansion at iteration \(k\) is \( \mu(\operatorname{Vor}_k(x)) \). Regions that remain large (poorly explored) have larger Voronoi volume and are more likely to be expanded, giving RRTs their characteristic rapid exploration property.

Informally, a planner is probabilistically complete if it finds a solution with probability approaching 1 as the number of iterations goes to infinity, whenever a robust solution exists.

Assume there exists a collision-free path \( \sigma : [0,1] \to \mathcal{C}_{\text{free}} \) with clearance \( \delta > 0 \), i.e.

\[ \operatorname{dist}(\sigma(t), \mathcal{C}_{\text{obs}}) \ge \delta \quad \forall t \in [0,1], \]

and suppose \( \mu \) has a density bounded below on \( \mathcal{C}_{\text{free}} \). Cover \( \sigma \) by a finite sequence of overlapping balls \( B_1, \dots, B_m \) of radius \( \delta/2 \) so that \( q_{\text{start}} \in B_1 \) and \( q_{\text{goal}} \in B_m \).

For each \( i \), define an event that the tree contains a node in \( B_i \) and subsequently extends to reach \( B_{i+1} \). With suitable bounds on the step size \( \eta \) relative to \( \delta \), it can be shown that each extension has probability at least \( p_i > 0 \) of succeeding in any given iteration. A simple bound on the failure probability after \( N \) iterations is then

\[ \mathbb{P}(\text{RRT fails after } N \text{ samples}) \le \sum_{i=1}^{m-1} (1 - p_i)^N \xrightarrow[N \to \infty]{} 0. \]

Hence, the probability of eventually reaching the goal region tends to 1, which is probabilistic completeness. Note that this argument assumes:

• A path with positive clearance \( \delta \).
• Non-zero sampling probability in any open subset of \( \mathcal{C}_{\text{free}} \).
• Sufficiently small \( \eta \) to ensure local steering remains inside \( \mathcal{C}_{\text{free}} \) when moving within the clearance tube around \( \sigma \).

RRTs are not asymptotically optimal: the cost of the best path found does not converge to the global optimum in general, even as \( N \to \infty \). Asymptotically optimal variants (e.g., RRT*) will be treated in a later lesson.

RRT-Connect modifies the single-step extension of RRT into an aggressive connect operation that repeatedly extends toward a sample until hitting an obstacle. For a tree \(T\) and target configuration \(q_{\text{rand}}\), define:

\[ \operatorname{Extend}(T, q_{\text{rand}}) = \begin{cases} \text{Trapped}, & \text{if first step is in collision},\\ \text{Reached}, & \text{if we exactly reach } q_{\text{rand}},\\ \text{Advanced}, & \text{otherwise.} \end{cases} \]

The CONNECT operation repeatedly calls \( \operatorname{Extend} \) in a loop:

\[ \operatorname{Connect}(T, q_{\text{rand}}): \quad \text{while } \operatorname{Extend}(T, q_{\text{rand}}) = \text{Advanced} \text{ do continue}. \]

The key idea is to traverse as far as possible along the ray from the tree toward \( q_{\text{rand}} \) in a single iteration, rather than taking only one bounded step. This significantly reduces the number of iterations required in open regions, while maintaining probabilistic completeness.

RRT-Connect is almost always used with two trees, one rooted at \( q_{\text{start}} \) and one at \( q_{\text{goal}} \), as described next.

Bidirectional RRT maintains two trees \( T^{\text{start}}_k \) and \( T^{\text{goal}}_k \), rooted at \( q_{\text{start}} \) and \( q_{\text{goal}} \), respectively. At each iteration, a random sample is used to grow one tree, and then the other tree attempts to connect to the newly added node.

The RRT-Connect algorithm can be summarized as:

1. Initialize \( T^{\text{start}} \) with \( q_{\text{start}} \), \( T^{\text{goal}} \) with \( q_{\text{goal}} \).
2. Repeat until success or iteration limit:
   1. Sample \( q_{\text{rand}} \sim \mu \).
   2. \( \operatorname{Extend}(T^{\text{start}}, q_{\text{rand}}) \).
   3. Let \( q_{\text{new}} \) be the last added configuration in \( T^{\text{start}} \) (if any).
   4. \( \operatorname{Connect}(T^{\text{goal}}, q_{\text{new}}) \).
   5. If trees meet (a node in one tree coincides with or is within \( \varepsilon \) of a node in the other tree), extract the path by concatenating root-to-meeting-node paths in each tree.
   6. Swap the roles of \( T^{\text{start}} \) and \( T^{\text{goal}} \) for the next iteration (to balance growth).

flowchart TD
  S["Tree Ts rooted at q_start"] --> A["Sample q_rand"]
  G["Tree Tg rooted at q_goal"] --> A
  A --> B["Extend Ts toward q_rand (single or multiple steps)"]
  B --> C["Let q_new be last node added to Ts"]
  C --> D["Connect Tg toward q_new (aggressive)"]
  D --> E{"Did Ts and Tg meet?"}
  E -->|yes| F["Extract path from q_start to q_goal"]
  E -->|no| H["Swap Ts and Tg and continue"]
        

Computationally, each iteration requires nearest-neighbor queries in both trees and several collision checks. Let \( n_k = |V^{\text{start}}_k| + |V^{\text{goal}}_k| \). A naive nearest-neighbor search has cost \( O(n_k) \) per query, so the time per iteration is \( O(n_k) \). Using a spatial index (e.g., a kd-tree) gives expected \( O(\log n_k) \) query time and significantly improves scalability for high-dimensional manipulators.

From a theoretical standpoint, bidirectional RRT and RRT-Connect can also be shown to be probabilistically complete under assumptions similar to single-tree RRT, though the detailed proofs are more delicate due to interactions between trees.

Below is a minimal 2D implementation sketch illustrating single-tree RRT and the core of RRT-Connect. In practice, one would integrate with a robotics library such as the ompl Python bindings (Open Motion Planning Library) or ROS MoveIt planning pipelines.

import numpy as np
from dataclasses import dataclass
from typing import List, Optional, Tuple

# 2D configuration
Config = np.ndarray  # shape (2,)

@dataclass
class Node:
    q: Config
    parent: Optional[int]  # index of parent node in the tree list

@dataclass
class Tree:
    nodes: List[Node]

    def add_node(self, q: Config, parent_idx: Optional[int]) -> int:
        self.nodes.append(Node(q=q, parent=parent_idx))
        return len(self.nodes) - 1

    def nearest(self, q: Config) -> int:
        dists = [np.linalg.norm(n.q - q) for n in self.nodes]
        return int(np.argmin(dists))

def steer(q_near: Config, q_rand: Config, eta: float) -> Config:
    direction = q_rand - q_near
    dist = np.linalg.norm(direction)
    if dist <= eta:
        return q_rand
    return q_near + eta * direction / dist

def collision_free(q1: Config, q2: Config, obstacles) -> bool:
    """
    obstacles: list of axis-aligned rectangles [xmin, xmax, ymin, ymax]
    Simple discrete check; for real robots, integrate with FCL / MoveIt etc.
    """
    n_steps = 20
    for i in range(n_steps + 1):
        alpha = i / n_steps
        q = (1 - alpha) * q1 + alpha * q2
        x, y = q
        for (xmin, xmax, ymin, ymax) in obstacles:
            if xmin <= x <= xmax and ymin <= y <= ymax:
                return False
    return True

def sample_config(bounds: Tuple[Tuple[float,float], Tuple[float,float]]) -> Config:
    (xmin, xmax), (ymin, ymax) = bounds
    return np.array([
        np.random.uniform(xmin, xmax),
        np.random.uniform(ymin, ymax)
    ])

def build_rrt(q_start: Config,
              q_goal: Config,
              bounds,
              obstacles,
              eta: float = 0.2,
              goal_radius: float = 0.1,
              max_iter: int = 5000) -> Optional[List[Config]]:
    tree = Tree(nodes=[])
    tree.add_node(q_start, parent_idx=None)

    for k in range(max_iter):
        # Goal bias: occasionally sample the goal directly
        if np.random.rand() < 0.1:
            q_rand = q_goal
        else:
            q_rand = sample_config(bounds)

        idx_near = tree.nearest(q_rand)
        q_near = tree.nodes[idx_near].q
        q_new = steer(q_near, q_rand, eta)

        if collision_free(q_near, q_new, obstacles):
            idx_new = tree.add_node(q_new, parent_idx=idx_near)

            if np.linalg.norm(q_new - q_goal) <= goal_radius:
                # Reconstruct path
                path = []
                cur = idx_new
                while cur is not None:
                    path.append(tree.nodes[cur].q)
                    cur = tree.nodes[cur].parent
                path.reverse()
                return path
    return None

def extend(tree: Tree, q_rand: Config, eta: float, obstacles) -> Tuple[str, Optional[int]]:
    """
    RRT-Connect style extend: single step toward q_rand.
    Returns (status, new_index).
    """
    idx_near = tree.nearest(q_rand)
    q_near = tree.nodes[idx_near].q
    q_new = steer(q_near, q_rand, eta)
    if not collision_free(q_near, q_new, obstacles):
        return "Trapped", None
    idx_new = tree.add_node(q_new, idx_near)
    if np.allclose(q_new, q_rand):
        return "Reached", idx_new
    return "Advanced", idx_new

def connect(tree: Tree, q_target: Config, eta: float, obstacles) -> Tuple[str, Optional[int]]:
    """
    Aggressively extend tree toward q_target until trapped or reached.
    """
    status = "Advanced"
    last_idx = None
    while status == "Advanced":
        status, last_idx = extend(tree, q_target, eta, obstacles)
    return status, last_idx

# Example usage (2D point robot):
if __name__ == "__main__":
    np.random.seed(0)
    q_start = np.array([0.1, 0.1])
    q_goal = np.array([0.9, 0.9])
    bounds = ((0.0, 1.0), (0.0, 1.0))
    obstacles = [
        (0.3, 0.7, 0.4, 0.6),  # rectangular obstacle
    ]
    path = build_rrt(q_start, q_goal, bounds, obstacles)
    if path is None:
        print("No path found")
    else:
        print("Path length:", len(path))
      

For manipulators, the configuration \( q \) would be a joint vector and the collision checker would call a 3D geometry engine (e.g., FCL via MoveIt). Nearest neighbors are efficiently implemented using scipy.spatial.KDTree or sklearn.neighbors.KDTree.

C++ is the standard language for high-performance planners. The Open Motion Planning Library (OMPL) provides optimized implementations of RRT, RRT-Connect, and many variants, and is used underneath ROS MoveIt. Below is a minimal RRT skeleton in C++-style pseudocode for a configuration space represented as Eigen::VectorXd.

#include <vector>
#include <random>
#include <limits>
#include <Eigen/Dense>

using Config = Eigen::VectorXd;

struct Node {
    Config q;
    int parent; // -1 for root
};

struct Tree {
    std::vector<Node> nodes;

    int addNode(const Config& q, int parent) {
        nodes.push_back(Node{q, parent});
        return static_cast<int>(nodes.size()) - 1;
    }

    int nearest(const Config& q) const {
        double best = std::numeric_limits<double>::infinity();
        int best_idx = -1;
        for (int i = 0; i < static_cast<int>(nodes.size()); ++i) {
            double d = (nodes[i].q - q).norm();
            if (d < best) {
                best = d;
                best_idx = i;
            }
        }
        return best_idx;
    }
};

Config steer(const Config& q_near, const Config& q_rand, double eta) {
    Config d = q_rand - q_near;
    double dist = d.norm();
    if (dist <= eta) {
        return q_rand;
    }
    return q_near + (eta / dist) * d;
}

// Collision checker signature; use FCL, bullet, etc. in practice.
bool collisionFree(const Config& q1, const Config& q2);

enum class ExtendStatus { Trapped, Advanced, Reached };

ExtendStatus extend(Tree& T, const Config& q_rand, double eta, int& new_idx) {
    int idx_near = T.nearest(q_rand);
    Config q_near = T.nodes[idx_near].q;
    Config q_new = steer(q_near, q_rand, eta);
    if (!collisionFree(q_near, q_new)) {
        return ExtendStatus::Trapped;
    }
    new_idx = T.addNode(q_new, idx_near);
    if ((q_new - q_rand).norm() < 1e-6) {
        return ExtendStatus::Reached;
    }
    return ExtendStatus::Advanced;
}
      

In OMPL, one would instead construct an ompl::base::SpaceInformation object describing \( \mathcal{C}_{\text{free}} \), then instantiate ompl::geometric::RRT or ompl::geometric::RRTConnect with this space, rather than writing the tree logic from scratch.

Java-based planning is common in simulation environments or educational frameworks. Below is a simple RRT implementation for a 2D point robot; collision checking is abstracted into an interface so it can be plugged into a physics engine or a Java-based robotics toolkit.

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

class Config2D {
    public double x, y;
    public Config2D(double x, double y) { this.x = x; this.y = y; }
    public Config2D add(Config2D other) { return new Config2D(x + other.x, y + other.y); }
    public Config2D sub(Config2D other) { return new Config2D(x - other.x, y - other.y); }
    public Config2D mul(double s) { return new Config2D(s * x, s * y); }
    public double norm() { return Math.sqrt(x * x + y * y); }
}

class Node {
    public Config2D q;
    public int parent; // -1 for root
    public Node(Config2D q, int parent) { this.q = q; this.parent = parent; }
}

interface CollisionChecker {
    boolean collisionFree(Config2D q1, Config2D q2);
}

class Tree {
    public List<Node> nodes = new ArrayList<>();

    public int addNode(Config2D q, int parent) {
        nodes.add(new Node(q, parent));
        return nodes.size() - 1;
    }

    public int nearest(Config2D q) {
        double best = Double.POSITIVE_INFINITY;
        int bestIdx = -1;
        for (int i = 0; i < nodes.size(); ++i) {
            Config2D qi = nodes.get(i).q;
            double dx = qi.x - q.x;
            double dy = qi.y - q.y;
            double d = Math.sqrt(dx * dx + dy * dy);
            if (d < best) {
                best = d;
                bestIdx = i;
            }
        }
        return bestIdx;
    }
}

public class RRT2D {
    private Random rng = new Random();
    private double xmin, xmax, ymin, ymax;

    public RRT2D(double xmin, double xmax, double ymin, double ymax) {
        this.xmin = xmin; this.xmax = xmax;
        this.ymin = ymin; this.ymax = ymax;
    }

    private Config2D sample() {
        double x = xmin + rng.nextDouble() * (xmax - xmin);
        double y = ymin + rng.nextDouble() * (ymax - ymin);
        return new Config2D(x, y);
    }

    private Config2D steer(Config2D qNear, Config2D qRand, double eta) {
        Config2D d = qRand.sub(qNear);
        double dist = d.norm();
        if (dist <= eta) return qRand;
        return qNear.add(d.mul(eta / dist));
    }

    public List<Config2D> plan(Config2D qStart,
                                 Config2D qGoal,
                                 CollisionChecker checker,
                                 double eta,
                                 double goalRadius,
                                 int maxIter) {
        Tree tree = new Tree();
        tree.addNode(qStart, -1);

        for (int k = 0; k < maxIter; ++k) {
            Config2D qRand = (rng.nextDouble() < 0.1) ? qGoal : sample();
            int idxNear = tree.nearest(qRand);
            Config2D qNear = tree.nodes.get(idxNear).q;
            Config2D qNew = steer(qNear, qRand, eta);
            if (checker.collisionFree(qNear, qNew)) {
                int idxNew = tree.addNode(qNew, idxNear);
                double dx = qNew.x - qGoal.x;
                double dy = qNew.y - qGoal.y;
                if (Math.sqrt(dx * dx + dy * dy) <= goalRadius) {
                    // reconstruct
                    ArrayList<Config2D> path = new ArrayList<>();
                    int cur = idxNew;
                    while (cur != -1) {
                        path.add(0, tree.nodes.get(cur).q);
                        cur = tree.nodes.get(cur).parent;
                    }
                    return path;
                }
            }
        }
        return null; // failure
    }
}
      

For Java-based robot stacks, one can integrate this planner with visualization (e.g., Processing, jMonkeyEngine) or with JNI bindings to C++ libraries such as OMPL for industrial-scale problems.

MATLAB offers both custom implementations and high-level planners in the Navigation and Robotics System Toolboxes (e.g., plannerRRT, manipulatorRRT). Below is a basic 2D variant and an indication of how to embed it in a Simulink MATLAB Function block.

function path = rrt2d(qStart, qGoal, bounds, obstacles, eta, goalRadius, maxIter)
% qStart, qGoal: 1x2 row vectors
% bounds: [xmin xmax ymin ymax]
% obstacles: Nx4 rectangles [xmin xmax ymin ymax]

nodes = qStart;
parent = -1;  % parent(1) = -1 (root)

for k = 1:maxIter
    % Goal bias
    if rand() < 0.1
        qRand = qGoal;
    else
        qRand = [ ...
            bounds(1) + rand() * (bounds(2) - bounds(1)), ...
            bounds(3) + rand() * (bounds(4) - bounds(3)) ];
    end

    idxNear = nearestNode(nodes, qRand);
    qNear = nodes(idxNear, :);
    qNew = steer(qNear, qRand, eta);

    if collisionFree(qNear, qNew, obstacles)
        nodes(end+1, :) = qNew; %#ok<AGROW>
        parent(end+1, 1) = idxNear; %#ok<AGROW>
        if norm(qNew - qGoal) <= goalRadius
            % Reconstruct path
            idx = size(nodes,1);
            path = zeros(0, 2);
            while idx ~= -1
                path = [nodes(idx,:); path]; %#ok<AGROW>
                idx = parent(idx);
            end
            return;
        end
    end
end

path = []; % failure
end

function idx = nearestNode(nodes, q)
    diffs = nodes - q;
    d2 = sum(diffs.^2, 2);
    [~, idx] = min(d2);
end

function qNew = steer(qNear, qRand, eta)
    d = qRand - qNear;
    dist = norm(d);
    if dist <= eta
        qNew = qRand;
    else
        qNew = qNear + (eta / dist) * d;
    end
end

function free = collisionFree(q1, q2, obstacles)
    nSteps = 20;
    free = true;
    for i = 0:nSteps
        alpha = i / nSteps;
        q = (1 - alpha) * q1 + alpha * q2;
        x = q(1); y = q(2);
        for j = 1:size(obstacles,1)
            xmin = obstacles(j,1); xmax = obstacles(j,2);
            ymin = obstacles(j,3); ymax = obstacles(j,4);
            if x >= xmin && x <= xmax && y >= ymin && y <= ymax
                free = false;
                return;
            end
        end
    end
end
      

To use this inside Simulink, wrap the call to rrt2d in a MATLAB Function block that takes the current scene description as input and outputs a path; this block can be triggered at a slower planning rate than the controller blocks. For manipulators, the Robotics System Toolbox offers the manipulatorRRT class that already implements RRT and RRT-Connect-like planners for rigid-body trees.

Mathematica can be used to prototype sampling-based planners using high-level list and geometric operations. Below is a concise 2D RRT example with a simple rectangular obstacle representation.

ClearAll[steer, nearestIndex, collisionFree, buildRRT];

steer[qNear_, qRand_, eta_] := Module[{d = qRand - qNear, dist},
  dist = Norm[d];
  If[dist <= eta, qRand, qNear + (eta/dist) d]
];

nearestIndex[nodes_, q_] := Module[{dists},
  dists = Norm /@ (nodes - q);
  First@Ordering[dists, 1]
];

collisionFree[q1_, q2_, obstacles_] := Module[
  {nSteps = 20, pts, x, y},
  pts = Table[(1 - a) q1 + a q2, {a, 0., 1., 1./nSteps}];
  Do[
    {x, y} = pt;
    Do[
      With[{xmin = obs[[1]], xmax = obs[[2]], ymin = obs[[3]], ymax = obs[[4]]},
        If[x >= xmin && x <= xmax && y >= ymin && y <= ymax,
          Return[False]
        ]
      ],
      {obs, obstacles}
    ],
    {pt, pts}
  ];
  True
];

buildRRT[qStart_, qGoal_, bounds_, obstacles_, eta_, goalRadius_, maxIter_] :=
 Module[
  {nodes = {qStart}, parents = {-1}, qRand, idxNear, qNear, qNew, k},
  For[k = 1, k <= maxIter, k++,
    qRand = If[RandomReal[] < 0.1,
      qGoal,
      {RandomReal[{bounds[[1]], bounds[[2]]}],
       RandomReal[{bounds[[3]], bounds[[4]]}]}
    ];
    idxNear = nearestIndex[nodes, qRand];
    qNear = nodes[[idxNear]];
    qNew = steer[qNear, qRand, eta];
    If[collisionFree[qNear, qNew, obstacles],
      AppendTo[nodes, qNew];
      AppendTo[parents, idxNear];
      If[Norm[qNew - qGoal] <= goalRadius,
        (* reconstruct path *)
        Module[{idx = Length[nodes], path = {}},
          While[idx != -1,
            path = Prepend[path, nodes[[idx]]];
            idx = parents[[idx]];
          ];
          Return[path];
        ];
      ];
    ];
  ];
  {}
];

(* Example usage *)
qStart = {0.1, 0.1};
qGoal = {0.9, 0.9};
bounds = {0., 1., 0., 1.};
obstacles = ;

path = buildRRT[qStart, qGoal, bounds, obstacles, 0.2, 0.1, 4000];
      

Mathematica's visualization tools (Graphics, Line, Point) can then be used to render the random tree and resulting path.

Problem 1 (Voronoi Bias Formalization). Let \( T_k = (V_k, E_k) \) be the RRT after \(k\) iterations, and let \( \{\operatorname{Vor}_k(v)\}_{v \in V_k} \) be its Voronoi partition under metric \(d\). Show that for uniform sampling measure \( \mu \) on \( \mathcal{C}_{\text{free}} \), the probability that node \(v\) is chosen for expansion at iteration \(k+1\) is \( \mu(\operatorname{Vor}_k(v)) \).

Solution. By definition, RRT chooses \( q_{\text{near}} \) as the nearest neighbor of the random sample \( q_{\text{rand}} \). The region of points \( q \) such that \( v \) is the nearest neighbor is exactly \( \operatorname{Vor}_k(v) \). Since \( q_{\text{rand}} \) is sampled according to \( \mu \), we have

\[ \mathbb{P}(v \text{ chosen at iteration } k+1) = \mathbb{P}(q_{\text{rand}} \in \operatorname{Vor}_k(v)) = \mu(\operatorname{Vor}_k(v)). \]

Thus large Voronoi cells are expanded with higher probability, which drives exploration.

Problem 2 (Step Size and Clearance Tube). Consider a robust path \( \sigma \) with clearance \( \delta > 0 \) in \( \mathbb{R}^d \). Suppose RRT uses a fixed step size \( \eta \). Show that if \( 0 < \eta \le \delta/2 \), then any local extension between points on \( \sigma \) contained inside the clearance tube remains in \( \mathcal{C}_{\text{free}} \).

Solution. Let \( q, q' \) be two configurations on \( \sigma \) within distance \( \eta \). Since both lie at distance at least \( \delta \) from \( \mathcal{C}_{\text{obs}} \), the straight-line segment joining them lies within the intersection of two balls of radius \( \delta \) around \( q \) and \( q' \). Every point on the segment is at most distance \( \eta \le \delta/2 \) from each endpoint; by the triangle inequality, its distance to \( \mathcal{C}_{\text{obs}} \) is at least

\[ \delta - \eta \ge \delta - \frac{\delta}{2} = \frac{\delta}{2} > 0. \]

Hence the entire segment lies in \( \mathcal{C}_{\text{free}} \), so any RRT step strictly within this tube remains collision-free.

Problem 3 (Single-Tree vs Bidirectional Expected Depth). Consider a simple 1D planning problem on the interval \([0,1]\) with \( q_{\text{start}} = 0 \), \( q_{\text{goal}} = 1 \), no obstacles, and RRT with step size \( \eta \ll 1 \). Compare the expected number of edges on the path from start to goal between: (a) single-tree RRT growing from 0 to 1; and (b) bidirectional RRT with trees rooted at 0 and 1 that always connect to the nearest node in the other tree. Give a qualitative argument.

Solution. In the single-tree case, the path from 0 to 1 follows many small steps determined by the random sampling sequence and the metric, and its depth is typically on the order of \( 1/\eta \). In the bidirectional case, once the two trees are close, a single connect operation can bridge the remaining distance; roughly half of the path is built from each side. So the depth from 0 to 1 is close to \( \frac{1}{2\eta} + \frac{1}{2\eta} = 1/\eta \), but the expected distance (in the tree) from the start to the meeting point is about \( 1/(2\eta) \), which can reduce search effort when backtracking or optimizing along the path.

Problem 4 (Nearest-Neighbor Complexity). Suppose an RRT has \( n \) nodes in \( \mathbb{R}^d \). Compare the per-iteration complexity of: (a) naive nearest search; and (b) kd-tree nearest search. Derive an asymptotic bound on total planning complexity if the planner performs \( N \) iterations.

Solution. In the naive case, nearest neighbor search checks all \( n \) nodes, requiring \( O(nd) \) operations per query. With a balanced kd-tree, expected query time is \( O(\log n) \) (with a dimension-dependent constant). Over \( N \) iterations, the total cost is roughly:

• Naive: \( O(d \sum_{k=1}^N k) = O(d N^2) \).
• kd-tree: \( O(\sum_{k=1}^N \log k) = O(N \log N) \).

For high-DOF manipulators where \( N \) is large, the asymptotic improvement is substantial.

Problem 5 (Failure of Asymptotic Optimality). Give an example configuration space and obstacle layout where RRT will almost surely not converge to the optimal path as the number of samples goes to infinity, and explain intuitively why.

Solution. Consider a planar environment where the start and goal are separated by a wide open corridor and a narrow, but shorter, corridor (shortest path passes through the narrow corridor). RRT tends to expand wide-open regions (larger Voronoi cells); the narrow corridor has low measure. Although RRT is probabilistically complete and will eventually find a feasible path through the narrow corridor with probability 1, the algorithm does not attempt to improve the best path once found and does not rewire the tree. The proportion of samples that lead to improved paths through the narrow corridor vanishes as \( N \to \infty \), so there is no guarantee that the path cost converges to the true optimum.

In this lesson we formally introduced Rapidly-Exploring Random Trees (RRT), highlighting their Voronoi-biased exploration of configuration space and providing a probabilistic completeness sketch under standard robustness assumptions. We then extended the basic algorithm to RRT-Connect and bidirectional RRT, which aggressively expand trees and attempt to connect trees rooted at start and goal, greatly improving practical performance. Implementation sketches across Python, C++, Java, MATLAB/Simulink, and Mathematica illustrated how to embed these planners into realistic robotics stacks with appropriate collision checking and nearest-neighbor data structures. In the next lesson, we will move from feasibility to optimality, studying asymptotically optimal planners such as RRT* and PRM*.