Let the robot's configuration space be \( \mathcal{X} \subset \mathbb{R}^d \), free space \( \mathcal{X}_{\mathrm{free}} \subset \mathcal{X} \), start configuration \( x_{\mathrm{init}} \in \mathcal{X}_{\mathrm{free}} \), and goal region \( \mathcal{X}_{\mathrm{goal}} \subset \mathcal{X}_{\mathrm{free}} \). A path is a continuous curve \( \sigma : [0,1] \to \mathcal{X}_{\mathrm{free}} \) with \( \sigma(0) = x_{\mathrm{init}} \), \( \sigma(1) \in \mathcal{X}_{\mathrm{goal}} \).

A cost functional \( J : \Sigma_{\mathrm{free}} \to \mathbb{R}_{\ge 0} \) (e.g., path length, energy) induces the optimal cost

\[ c^\ast \;=\; \inf_{\sigma \in \Sigma_{\mathrm{free}}} J(\sigma) \]

where \( \Sigma_{\mathrm{free}} \) is the set of all feasible paths. A sampling-based algorithm generates an increasingly dense graph or tree on \( \mathcal{X}_{\mathrm{free}} \). Let \( c_n \) be the cost of the best path returned after \( n \) random samples. The planner is asymptotically optimal if

\[ \mathbb{P}\!\left( \lim_{n \to \infty} c_n = c^\ast \right) = 1. \]

Intuitively, as the number of samples grows, the planner almost surely converges to the globally optimal path, not merely a feasible one. PRM* and RRT* modify PRM and RRT to ensure this behavior by:

• Using radius / k-nearest rules that shrink with \( n \).
• Ensuring graphs/trees are dense enough to approximate any feasible path.
• Performing local rewiring to reduce path costs.

flowchart TD
  S["Sample x_rand in X_free"] --> G["Graph/Tree G_n"]
  G --> C["Connect x_rand to neighbors \nwith r_n or k_n"]
  C --> F["Feasible path? yes/no"]
  F -->|yes| E["Compute best cost c_n"]
  C --> R["Local improvement \n(rewire / update parents)"]
  R --> G
  E --> N["Increase n; update r_n or k_n"]
  N --> S
        

The classical convergence analysis for PRM* and RRT* relies on assumptions similar to those you have already seen for probabilistic completeness:

• Bounded C-space: \( \mathcal{X} \subset \mathbb{R}^d \) compact, with Lebesgue measure \( \mu(\mathcal{X}) < \infty \).
• Positive-volume free space: \( \mu(\mathcal{X}_{\mathrm{free}}) > 0 \).
• Cost functional regularity: for path length, the cost is Lipschitz with respect to the Euclidean metric:

\[ \exists L > 0 \text{ such that } |J(\sigma_1) - J(\sigma_2)| \le L \cdot \sup_{t \in [0,1]} \left\|\sigma_1(t) - \sigma_2(t)\right\|. \]

• \( \delta \)-robust feasible optimal path: there exists an optimal (or arbitrarily close to optimal) path \( \sigma^\ast \) and some \( \delta > 0 \) such that its \( \delta \)-tube remains collision free:

\[ \mathcal{B}_\delta(\sigma^\ast) \;=\; \bigcup_{t \in [0,1]} \left\{ x \in \mathcal{X} \,\middle|\, \left\| x - \sigma^\ast(t) \right\| < \delta \right\} \;\subset\; \mathcal{X}_{\mathrm{free}}. \]

Under these conditions, the sequence of random graphs constructed by PRM* or RRT* behaves like a random geometric graph whose connectivity and shortest-path properties are well understood.

Recall PRM builds a roadmap by uniformly sampling free configurations and connecting each new sample to nearby neighbors if the connecting segment is collision free. The original PRM uses a fixed connection radius or a fixed number of neighbors, which is not sufficient for asymptotic optimality in general. PRM* modifies this as follows.

PRM* connection radius rule. For a roadmap with

• \( n \) vertices (samples),
• \( d \) the dimension of \( \mathcal{X} \),
• \( \mu(\mathcal{X}_{\mathrm{free}}) \) the volume of the free space,

PRM* uses a radius of the form

\[ r_n = \gamma_{\mathrm{PRM}} \left( \frac{\log n}{n} \right)^{1/d}, \]

where \( \gamma_{\mathrm{PRM}} \) is chosen sufficiently large, often

\[ \gamma_{\mathrm{PRM}} > 2 \left(1 + \frac{1}{d}\right)^{1/d} \left( \frac{\mu(\mathcal{X}_{\mathrm{free}})}{\zeta_d} \right)^{1/d}, \]

and \( \zeta_d \) is the volume of the unit ball in \( \mathbb{R}^d \).

Key ideas behind the proof.

1. Cover the optimal path. Choose a sequence of overlapping balls of radius \( \frac{1}{2}\min(\delta, r_n) \) centered on points along \( \sigma^\ast \). The number of balls grows linearly with the path length but is finite for each fixed radius.
2. Almost-sure coverage by samples. Because samples are i.i.d. uniform on \( \mathcal{X}_{\mathrm{free}} \) and each ball has positive volume, the probability that each ball eventually contains at least one sample is 1 (Borel–Cantelli lemma).
3. Connectivity between neighboring balls. If the radius \( r_n \) shrinks at the rate above, asymptotically the random geometric graph is connected within the \( \delta \)-tube around \( \sigma^\ast \), and the straight-line connections between representatives of consecutive balls are collision free.
4. Path cost convergence. The resulting discrete path approximates \( \sigma^\ast \) arbitrarily closely in the uniform norm, and by Lipschitz continuity of the cost functional we get

\[ \lim_{n \to \infty} J(\sigma_n) = J(\sigma^\ast) = c^\ast \quad \text{almost surely.} \]

Thus PRM* is asymptotically optimal. Note that the radius \( r_n \) shrinks to 0, but slowly enough that the graph remains connected.

Recall that standard RRT grows a tree by extending from the nearest node toward a random sample. The tree biases exploration but does not attempt to improve existing paths, so its solution quality often plateaus. RRT* adds a rewiring step that locally optimizes the tree.

RRT* (informal).

1. Sample \( x_{\mathrm{rand}} \sim \mathrm{Unif}(\mathcal{X}_{\mathrm{free}}) \).
2. Find the nearest existing vertex \( x_{\mathrm{nearest}} \) in the tree and steer toward \( x_{\mathrm{rand}} \) (respecting a step size) to obtain \( x_{\mathrm{new}} \).
3. Compute a nearby set of vertices:

\[ X_{\mathrm{near}}(n) = \left\{ x \in V_n \,\middle|\, \|x - x_{\mathrm{new}}\| \le r_n \right\}, \quad r_n = \gamma_{\mathrm{RRT^\ast}} \left( \frac{\log n}{n} \right)^{1/d}. \]

4. Choose as parent the node in \( X_{\mathrm{near}} \) that minimizes cost-to-come plus edge cost:

\[ x_{\mathrm{parent}} = \arg\min_{x \in X_{\mathrm{near}}} \left\{ J_n(x) + c\big(\text{segment}(x, x_{\mathrm{new}})\big) \right\}. \]

5. Add the edge \( (x_{\mathrm{parent}}, x_{\mathrm{new}}) \) if collision free. Initialize \( J_n(x_{\mathrm{new}}) \) accordingly.
6. For each \( x \in X_{\mathrm{near}} \), check whether going through \( x_{\mathrm{new}} \) is cheaper; if so, rewire:

\[ \text{if } J_n(x_{\mathrm{new}}) + c\big(\text{segment}(x_{\mathrm{new}}, x)\big) < J_n(x) \text{ then change parent of } x \text{ to } x_{\mathrm{new}}. \]

flowchart TD
  R["Sample x_rand"] --> Nn["Find nearest x_nearest"]
  Nn --> St["Steer to x_new"]
  St --> Ne["Find neighbors within r_n"]
  Ne --> Par["Choose best parent by cost"]
  Par --> Add["Add x_new to tree"]
  Add --> Rew["For each neighbor: cheaper via x_new?"]
  Rew -->|yes| Upd["Rewire neighbor parent"]
  Rew -->|no| Keep["Keep current parent"]
  Upd --> Rew
  Keep --> R
        

Why RRT* is asymptotically optimal (sketch).

• The radius \( r_n \) ensures the tree becomes dense in \( \mathcal{X}_{\mathrm{free}} \).
• As in PRM*, cover an optimal path by balls with radius proportional to \( r_n \). With probability 1, eventually each ball contains vertices of the tree.
• The rewiring step ensures that whenever a cheaper connection exists through a new sample, the tree locally improves; the sequence \( J_n(x_{\mathrm{goal}}) \) of best goal costs is nonincreasing and bounded below by \( c^\ast \).
• Combining density of the tree with local rewiring yields \( \lim_{n \to \infty} J_n(x_{\mathrm{goal}}) = c^\ast \) almost surely.

We implement a simple RRT* in Python for a holonomic point robot in \( \mathbb{R}^2 \). Obstacles are axis-aligned rectangles. This code intentionally uses plain NumPy; in practice you might integrate with ompl Python bindings or moveit_commander for manipulators.

import numpy as np
from dataclasses import dataclass

np.random.seed(0)

@dataclass
class Node:
    x: float
    y: float
    parent: int
    cost: float

class RRTStar:
    def __init__(self,
                 x_lim,
                 y_lim,
                 start,
                 goal,
                 step_size=0.1,
                 goal_radius=0.2,
                 max_iter=5000):
        self.x_min, self.x_max = x_lim
        self.y_min, self.y_max = y_lim
        self.start = Node(start[0], start[1], parent=-1, cost=0.0)
        self.goal = np.array(goal, dtype=float)
        self.step_size = step_size
        self.goal_radius = goal_radius
        self.max_iter = max_iter
        self.nodes = [self.start]
        self.obstacles = []  # list of (xmin, ymin, xmax, ymax)

    def add_obstacle(self, rect):
        # rect = (xmin, ymin, xmax, ymax)
        self.obstacles.append(rect)

    def sample_free(self):
        return np.array([
            np.random.uniform(self.x_min, self.x_max),
            np.random.uniform(self.y_min, self.y_max)
        ])

    def dist(self, p, q):
        return np.linalg.norm(p - q)

    def nearest(self, x_rand):
        dists = [self.dist(np.array([n.x, n.y]), x_rand) for n in self.nodes]
        return int(np.argmin(dists))

    def steer(self, x_nearest, x_rand):
        direction = x_rand - x_nearest
        norm = np.linalg.norm(direction)
        if norm <= self.step_size:
            return x_rand
        return x_nearest + self.step_size * direction / norm

    def line_collision_free(self, p, q, resolution=10):
        # simple discrete collision check along segment
        for i in range(resolution + 1):
            alpha = i / resolution
            x = (1 - alpha) * p + alpha * q
            if self.in_obstacle(x):
                return False
        return True

    def in_obstacle(self, x):
        px, py = x
        for (xmin, ymin, xmax, ymax) in self.obstacles:
            if xmin <= px <= xmax and ymin <= py <= ymax:
                return True
        return False

    def near(self, x_new, gamma_rrt=1.0):
        n = len(self.nodes)
        d = 2  # dimension
        if n == 1:
            return [0]
        r_n = min(
            gamma_rrt * (np.log(n) / n)**(1.0 / d),
            self.step_size * 10.0
        )
        idxs = []
        for i, node in enumerate(self.nodes):
            p = np.array([node.x, node.y])
            if self.dist(p, x_new) <= r_n:
                idxs.append(i)
        return idxs

    def cost_to(self, idx):
        return self.nodes[idx].cost

    def plan(self):
        goal_idx = None
        for k in range(self.max_iter):
            x_rand = self.sample_free()
            idx_nearest = self.nearest(x_rand)
            x_near = np.array([self.nodes[idx_nearest].x,
                               self.nodes[idx_nearest].y])
            x_new = self.steer(x_near, x_rand)
            if self.in_obstacle(x_new):
                continue
            if not self.line_collision_free(x_near, x_new):
                continue

            neighbor_idxs = self.near(x_new)
            # choose best parent
            best_parent = idx_nearest
            best_cost = self.cost_to(idx_nearest) + self.dist(x_near, x_new)
            for i in neighbor_idxs:
                xi = np.array([self.nodes[i].x, self.nodes[i].y])
                if self.line_collision_free(xi, x_new):
                    cand_cost = self.cost_to(i) + self.dist(xi, x_new)
                    if cand_cost < best_cost:
                        best_parent = i
                        best_cost = cand_cost

            new_node = Node(x_new[0], x_new[1], parent=best_parent,
                            cost=best_cost)
            self.nodes.append(new_node)
            new_idx = len(self.nodes) - 1

            # rewire neighbors
            for i in neighbor_idxs:
                if i == best_parent:
                    continue
                xi = np.array([self.nodes[i].x, self.nodes[i].y])
                if not self.line_collision_free(x_new, xi):
                    continue
                cand_cost = best_cost + self.dist(x_new, xi)
                if cand_cost < self.nodes[i].cost:
                    self.nodes[i].parent = new_idx
                    self.nodes[i].cost = cand_cost

            # check if we reached goal region
            if self.dist(x_new, self.goal) <= self.goal_radius:
                if (goal_idx is None or
                    best_cost + self.dist(x_new, self.goal) <
                    self.nodes[goal_idx].cost):
                    goal_idx = new_idx

        return self.extract_path(goal_idx)

    def extract_path(self, goal_idx):
        if goal_idx is None:
            return None
        path = [self.goal.copy()]
        idx = goal_idx
        while idx != -1:
            n = self.nodes[idx]
            path.append(np.array([n.x, n.y]))
            idx = n.parent
        path.reverse()
        return np.array(path)

# Example usage
if __name__ == "__main__":
    planner = RRTStar(x_lim=(0.0, 1.0),
                      y_lim=(0.0, 1.0),
                      start=(0.1, 0.1),
                      goal=(0.9, 0.9))
    planner.add_obstacle((0.3, 0.3, 0.7, 0.4))
    path = planner.plan()
    print("Path:", path)
      

In high-dimensional C-spaces for manipulators, one typically replaces the explicit 2D collision checks with calls to a kinematics and collision library (e.g., pybullet, moveit_commander, or ompl bindings) and uses a configuration-space geodesic metric rather than Euclidean distance in task space.

In practice, asymptotically optimal planners are often used via the Open Motion Planning Library (OMPL), which implements RRT*, PRM*, and many variants. Below is a minimal C++ snippet that sets up a 2D RRT* planner.

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/ProblemDefinition.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>

namespace ob = ompl::base;
namespace og = ompl::geometric;

// Simple axis-aligned rectangular obstacle
struct Rect
{
    double xmin, ymin, xmax, ymax;
};

bool isStateValid(const ob::State *state)
{
    const auto *s =
        state->as<ob::RealVectorStateSpace::StateType>();
    double x = (*s)[0];
    double y = (*s)[1];

    // example: one obstacle
    Rect obs{0.3, 0.3, 0.7, 0.4};
    bool inside =
        (x >= obs.xmin && x <= obs.xmax &&
         y >= obs.ymin && y <= obs.ymax);
    return !inside;
}

int main()
{
    auto space =
        std::make_shared<ob::RealVectorStateSpace>(2);
    ob::RealVectorBounds bounds(2);
    bounds.setLow(0, 0.0);
    bounds.setHigh(0, 1.0);
    bounds.setLow(1, 0.0);
    bounds.setHigh(1, 1.0);
    space->setBounds(bounds);

    og::SimpleSetup ss(space);
    ss.setStateValidityChecker(&isStateValid);

    ob::ScopedState<ob::RealVectorStateSpace> start(space);
    start[0] = 0.1; start[1] = 0.1;
    ob::ScopedState<ob::RealVectorStateSpace> goal(space);
    goal[0] = 0.9; goal[1] = 0.9;
    ss.setStartAndGoalStates(start, goal, 0.05);

    auto planner =
        std::make_shared<og::RRTstar>(ss.getSpaceInformation());
    // optionally tune range and rewiring factor
    planner->setRange(0.1);
    ss.setPlanner(planner);

    ob::PlannerStatus solved =
        ss.solve(1.0); // 1 second

    if (solved)
    {
        auto path = ss.getSolutionPath();
        path.printAsMatrix(std::cout);
    }
    return 0;
}
      

For PRM*, OMPL provides og::PRMstar; replacing the planner instantiation with std::make_shared<og::PRMstar> yields a roadmap planner whose connection radius is adjusted per the PRM* theory. When used for manipulators, RealVectorStateSpace is typically replaced with joint-space spaces and coupled to kinematics and collision checkers (e.g., via MoveIt!).

There is no single standard Java robotics planning library analogous to OMPL, but RRT* can be implemented using general-purpose data structures. Below is a minimal skeleton for 2D RRT*; this can be embedded in a Java robotics stack (e.g., controlling a mobile base in simulation).

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

class Node {
    double x, y;
    int parent;
    double cost;
    Node(double x, double y, int parent, double cost) {
        this.x = x;
        this.y = y;
        this.parent = parent;
        this.cost = cost;
    }
}

public class RRTStar2D {
    private double xMin, xMax, yMin, yMax;
    private double stepSize;
    private double goalRadius;
    private int maxIter;
    private double[][] obstacles; // each row: [xmin, ymin, xmax, ymax]
    private List<Node> nodes;
    private double[] goal;
    private Random rng;

    public RRTStar2D(double xMin, double xMax,
                     double yMin, double yMax,
                     double[] start, double[] goal,
                     double stepSize,
                     double goalRadius,
                     int maxIter) {
        this.xMin = xMin; this.xMax = xMax;
        this.yMin = yMin; this.yMax = yMax;
        this.stepSize = stepSize;
        this.goalRadius = goalRadius;
        this.maxIter = maxIter;
        this.nodes = new ArrayList<>();
        this.nodes.add(new Node(start[0], start[1], -1, 0.0));
        this.goal = goal.clone();
        this.rng = new Random(0);
        this.obstacles = new double[0][4];
    }

    public void setObstacles(double[][] obs) {
        this.obstacles = obs;
    }

    private double[] sampleFree() {
        double x = xMin + rng.nextDouble() * (xMax - xMin);
        double y = yMin + rng.nextDouble() * (yMax - yMin);
        return new double[]{x, y};
    }

    private double dist(double[] p, double[] q) {
        double dx = p[0] - q[0];
        double dy = p[1] - q[1];
        return Math.sqrt(dx * dx + dy * dy);
    }

    private int nearest(double[] xRand) {
        double best = Double.POSITIVE_INFINITY;
        int idx = 0;
        for (int i = 0; i < nodes.size(); ++i) {
            Node n = nodes.get(i);
            double d = dist(new double[]{n.x, n.y}, xRand);
            if (d < best) {
                best = d;
                idx = i;
            }
        }
        return idx;
    }

    private double[] steer(double[] xNearest, double[] xRand) {
        double dx = xRand[0] - xNearest[0];
        double dy = xRand[1] - xNearest[1];
        double norm = Math.sqrt(dx * dx + dy * dy);
        if (norm <= stepSize) {
            return xRand.clone();
        }
        double scale = stepSize / norm;
        return new double[]{
            xNearest[0] + scale * dx,
            xNearest[1] + scale * dy
        };
    }

    private boolean inObstacle(double[] p) {
        double px = p[0], py = p[1];
        for (double[] obs : obstacles) {
            if (px >= obs[0] && px <= obs[2] &&
                py >= obs[1] && py <= obs[3]) {
                return true;
            }
        }
        return false;
    }

    private boolean lineCollisionFree(double[] p, double[] q, int steps) {
        for (int i = 0; i <= steps; ++i) {
            double alpha = (double) i / (double) steps;
            double x = (1.0 - alpha) * p[0] + alpha * q[0];
            double y = (1.0 - alpha) * p[1] + alpha * q[1];
            if (inObstacle(new double[]{x, y})) {
                return false;
            }
        }
        return true;
    }

    private List<Integer> near(double[] xNew, double gammaRrt) {
        int n = nodes.size();
        List<Integer> idxs = new ArrayList<>();
        if (n == 1) {
            idxs.add(0);
            return idxs;
        }
        int d = 2;
        double rN = gammaRrt * Math.pow(Math.log(n) / (double) n, 1.0 / d);
        rN = Math.min(rN, stepSize * 10.0);
        for (int i = 0; i < n; ++i) {
            Node node = nodes.get(i);
            double dxy = dist(new double[]{node.x, node.y}, xNew);
            if (dxy <= rN) {
                idxs.add(i);
            }
        }
        return idxs;
    }

    public List<double[]> plan() {
        Integer goalIdx = null;
        for (int k = 0; k < maxIter; ++k) {
            double[] xRand = sampleFree();
            int idxNearest = nearest(xRand);
            Node xNear = nodes.get(idxNearest);
            double[] xNearArr = new double[]{xNear.x, xNear.y};
            double[] xNew = steer(xNearArr, xRand);
            if (inObstacle(xNew)) continue;
            if (!lineCollisionFree(xNearArr, xNew, 10)) continue;

            List<Integer> neighbors = near(xNew, 1.0);
            int bestParent = idxNearest;
            double bestCost = xNear.cost + dist(xNearArr, xNew);
            for (int i : neighbors) {
                Node ni = nodes.get(i);
                double[] xi = new double[]{ni.x, ni.y};
                if (!lineCollisionFree(xi, xNew, 10)) continue;
                double cand = ni.cost + dist(xi, xNew);
                if (cand < bestCost) {
                    bestCost = cand;
                    bestParent = i;
                }
            }
            Node newNode = new Node(xNew[0], xNew[1], bestParent, bestCost);
            nodes.add(newNode);
            int newIdx = nodes.size() - 1;

            // rewire
            for (int i : neighbors) {
                if (i == bestParent) continue;
                Node ni = nodes.get(i);
                double[] xi = new double[]{ni.x, ni.y};
                if (!lineCollisionFree(xNew, xi, 10)) continue;
                double cand = bestCost + dist(xNew, xi);
                if (cand < ni.cost) {
                    ni.parent = newIdx;
                    ni.cost = cand;
                }
            }

            // goal check
            if (dist(xNew, goal) <= goalRadius) {
                if (goalIdx == null ||
                    bestCost + dist(xNew, goal)
                        < nodes.get(goalIdx).cost) {
                    goalIdx = newIdx;
                }
            }
        }
        return extractPath(goalIdx);
    }

    private List<double[]> extractPath(Integer goalIdx) {
        List<double[]> path = new ArrayList<>();
        if (goalIdx == null) return path;
        path.add(goal.clone());
        int idx = goalIdx;
        while (idx != -1) {
            Node n = nodes.get(idx);
            path.add(0, new double[]{n.x, n.y});
            idx = n.parent;
        }
        return path;
    }
}
      

For manipulator planning, one would replace the 2D coordinates with joint vectors, connect to a collision detection engine (e.g. through JNI bindings), and use a configuration-space metric reflecting joint limits and weights.

MATLAB's Robotics System Toolbox includes RRT and RRT* planners, but it is instructive to see a simple from-scratch implementation for a point robot in 2D. This function can be wrapped in a Simulink MATLAB Function block to serve as a motion-planning module in a larger control diagram.

function path = rrt_star_2d(xlim, ylim, start, goal, obstacles)
% xlim, ylim: [min max]
% start, goal: [x y]
% obstacles: Mx4, rows [xmin ymin xmax ymax]

stepSize    = 0.1;
goalRadius  = 0.2;
maxIter     = 3000;
gamma_rrt   = 1.0;

nodes(1).x     = start(1);
nodes(1).y     = start(2);
nodes(1).parent = -1;
nodes(1).cost   = 0.0;

for k = 1:maxIter
    x_rand = [ ...
        rand()*(xlim(2)-xlim(1)) + xlim(1), ...
        rand()*(ylim(2)-ylim(1)) + ylim(1) ...
    ];

    idx_near = nearest_node(nodes, x_rand);
    x_near = [nodes(idx_near).x, nodes(idx_near).y];
    x_new  = steer(x_near, x_rand, stepSize);

    if in_obstacle(x_new, obstacles)
        continue;
    end
    if ~line_collision_free(x_near, x_new, obstacles)
        continue;
    end

    nbr_idx = near_nodes(nodes, x_new, gamma_rrt, stepSize);
    if isempty(nbr_idx)
        nbr_idx = idx_near;
    end

    best_parent = idx_near;
    best_cost = nodes(idx_near).cost + norm(x_new - x_near);

    for i = nbr_idx
        xi = [nodes(i).x, nodes(i).y];
        if line_collision_free(xi, x_new, obstacles)
            cand = nodes(i).cost + norm(x_new - xi);
            if cand < best_cost
                best_cost = cand;
                best_parent = i;
            end
        end
    end

    new_idx = numel(nodes) + 1;
    nodes(new_idx).x = x_new(1);
    nodes(new_idx).y = x_new(2);
    nodes(new_idx).parent = best_parent;
    nodes(new_idx).cost = best_cost;

    % rewire
    for i = nbr_idx
        if i == best_parent
            continue;
        end
        xi = [nodes(i).x, nodes(i).y];
        if line_collision_free(x_new, xi, obstacles)
            cand = best_cost + norm(x_new - xi);
            if cand < nodes(i).cost
                nodes(i).parent = new_idx;
                nodes(i).cost = cand;
            end
        end
    end

    if norm(x_new - goal) <= goalRadius
        goal_idx = new_idx; %#ok<NASGU>
        break;
    end
end

% extract path if goal reached
if exist('goal_idx', 'var')
    path = extract_path(nodes, goal, goal_idx);
else
    path = [];
end
end

function idx = nearest_node(nodes, x)
dmin = inf;
idx = 1;
for i = 1:numel(nodes)
    p = [nodes(i).x, nodes(i).y];
    d = norm(p - x);
    if d < dmin
        dmin = d;
        idx = i;
    end
end
end

function x_new = steer(x_near, x_rand, stepSize)
v = x_rand - x_near;
nrm = norm(v);
if nrm <= stepSize
    x_new = x_rand;
else
    x_new = x_near + stepSize * v / nrm;
end
end

function inside = in_obstacle(x, obstacles)
px = x(1); py = x(2);
inside = false;
for i = 1:size(obstacles,1)
    xmin = obstacles(i,1);
    ymin = obstacles(i,2);
    xmax = obstacles(i,3);
    ymax = obstacles(i,4);
    if px >= xmin && px <= xmax && ...
       py >= ymin && py <= ymax
        inside = true;
        return;
    end
end
end

function free = line_collision_free(p, q, obstacles)
steps = 10;
free = true;
for i = 0:steps
    alpha = i / steps;
    x = (1-alpha)*p + alpha*q;
    if in_obstacle(x, obstacles)
        free = false;
        return;
    end
end
end

function idxs = near_nodes(nodes, x_new, gamma_rrt, stepSize)
n = numel(nodes);
if n == 1
    idxs = 1;
    return;
end
d = 2;
r_n = gamma_rrt * (log(n)/n)^(1/d);
r_n = min(r_n, 10*stepSize);
idxs = [];
for i = 1:n
    p = [nodes(i).x, nodes(i).y];
    if norm(p - x_new) <= r_n
        idxs(end+1) = i; %#ok<AGROW>
    end
end
end

function path = extract_path(nodes, goal, goal_idx)
pts = goal;
idx = goal_idx;
while idx ~= -1
    n = nodes(idx);
    pts = [ [n.x; n.y], pts ];
    idx = n.parent;
end
path = pts;
end
      

In Simulink, you can expose parameters such as stepSize, goalRadius, and maxIter as block parameters, feeding in obstacle lists and goal states from other subsystems (e.g., perception or high-level task planners).

Mathematica's symbolic and graph capabilities are useful for experimenting with roadmap-based planners. Below is a compact PRM*-style roadmap builder for 2D free space with circular obstacles.

ClearAll["Global`*"];

(* Obstacles: list of {cx, cy, radius} *)
obstacles = {
   {0.5, 0.5, 0.15}
};

inObstacle[{x_, y_}] := 
 Module[{cx, cy, r},
  Or @@ ( (cx = #[[1]]; cy = #[[2]]; r = #[[3]];
           (x - cx)^2 + (y - cy)^2 <= r^2) & /@ obstacles)
  ];

sampleFree[] := 
 Module[{p},
  p = {RandomReal[{0, 1}], RandomReal[{0, 1}]};
  While[inObstacle[p],
   p = {RandomReal[{0, 1}], RandomReal[{0, 1}]};
  ];
  p
  ];

lineFree[p_, q_, steps_: 15] :=
 Module[{pts},
  pts = Table[(1 - t) p + t q, {t, 0, 1, 1./steps}];
  Not@Or @@ (inObstacle /@ pts)
  ];

dist[p_, q_] := Norm[p - q];

prmStar[n_] := Module[
  {samples, edges = {}, d = 2, gamma = 1.0, rN, i, j},
  samples = Table[sampleFree[], {n}];
  rN = gamma*(Log[n]/n)^(1./d);
  For[i = 1, i <= n, i++,
   For[j = i + 1, j <= n, j++,
    If[dist[samples[[i]], samples[[j]]] <= rN &&
       lineFree[samples[[i]], samples[[j]]],
     AppendTo[edges, UndirectedEdge[i, j]];
    ];
   ];
  ];
  Graph[Range[n], edges, 
   VertexCoordinates -> samples,
   VertexSize -> Small
  ]
  ];

g = prmStar[200];
Graph[g]
      

Extending this sketch to a full PRM* planner involves adding designated start and goal vertices, computing shortest paths on the resulting Graph, and tuning the constant in the connection radius according to the theoretical bounds.

Problem 1 (Definition of Asymptotic Optimality): Let \( c_n \) be the random variable corresponding to the best path cost output by a planner after \( n \) samples, and \( c^\ast \) the optimal cost. Show that the following are equivalent:

1. \( \mathbb{P}(\lim_{n\to\infty} c_n = c^\ast) = 1 \).
2. For all \( \varepsilon > 0 \), \( \mathbb{P}(\limsup_{n\to\infty} \{ |c_n - c^\ast| > \varepsilon \}) = 0 \).

Solution:

By definition, convergence almost surely of \( c_n \) to \( c^\ast \) means that the set

\[ A = \left\{ \omega \,\middle|\, \lim_{n\to\infty} c_n(\omega) = c^\ast \right\} \]

has probability 1. For each \( \varepsilon > 0 \), the event

\[ B_\varepsilon = \left\{ \omega \,\middle|\, \limsup_{n\to\infty} |c_n(\omega) - c^\ast| > \varepsilon \right\} \]

consists of those outcomes for which infinitely many \( n \) satisfy \( |c_n - c^\ast| > \varepsilon \). If \( \omega \in A \), then \( |c_n(\omega) - c^\ast| \to 0 \), so for each \( \varepsilon \) there exists \( N(\omega,\varepsilon) \) such that for all \( n \ge N \) one has \( |c_n(\omega) - c^\ast| \le \varepsilon \). Thus \( \omega \notin B_\varepsilon \), and hence \( A \subset B_\varepsilon^c \) for all \( \varepsilon \).

Therefore \( \mathbb{P}(B_\varepsilon) \le 1 - \mathbb{P}(A) = 0 \), so \( \mathbb{P}(B_\varepsilon) = 0 \). Conversely, if \( \mathbb{P}(B_\varepsilon) = 0 \) for all \( \varepsilon \), then almost surely \( \limsup_{n} |c_n - c^\ast| = 0 \), which is equivalent to \( c_n \to c^\ast \) almost surely. Thus the two conditions are equivalent.

Problem 2 (Radius Scaling for PRM*): Consider PRM* in \( \mathbb{R}^d \) with \( n \) samples and radius \( r_n = \gamma (\log n / n)^{1/d} \). Show that \( r_n \to 0 \) and \( n r_n^d / \log n \to \gamma^d \) as \( n \to \infty \). Why is this scaling natural from the perspective of random geometric graphs?

Solution:

First, \( r_n = \gamma (\log n / n)^{1/d} \) implies \( \log n / n \to 0 \), so \( r_n \to 0 \). Next,

\[ n r_n^d = n \gamma^d \left( \frac{\log n}{n} \right) = \gamma^d \log n. \]

Hence

\[ \frac{n r_n^d}{\log n} = \gamma^d \quad \text{for all } n. \]

In random geometric graphs, the expected degree of a vertex scales like \( n r_n^d \). The threshold for connectivity occurs when this quantity is on the order of \( \log n \). Thus choosing \( r_n \) proportional to \( (\log n/n)^{1/d} \) positions the graph close to the connectivity threshold, minimizing edges while preserving connectivity with high probability. This balance is precisely what PRM* exploits to be both sparse and asymptotically optimal.

Problem 3 (RRT vs. RRT* Cost Monotonicity): Let \( c_n^{\mathrm{RRT}} \) and \( c_n^{\mathrm{RRT^\ast}} \) be the best goal costs produced by RRT and RRT* after \( n \) samples in the same environment. Argue qualitatively why \( \{c_n^{\mathrm{RRT^\ast}}\}_n \) is a nonincreasing sequence, whereas \( \{c_n^{\mathrm{RRT}}\}_n \) need not be.

Solution:

In RRT*, each new sample can either:

• fail to connect to the goal region, in which case the best goal cost is unchanged, or
• connect to the goal via a path whose cost is no greater than the existing path, because the planner explicitly chooses the minimum-cost parent among neighbors.

Additionally, the rewiring step may reduce the cost to existing nodes, including those on the goal path. Thus \( c_{n+1}^{\mathrm{RRT^\ast}} \le c_n^{\mathrm{RRT^\ast}} \). In contrast, RRT chooses parents based only on nearest neighbor, without cost-based rewiring. New edges can create alternative goal paths with higher cost or leave the best cost unchanged, and there is no guarantee of monotonic improvement. Therefore the cost sequence for RRT is not necessarily nonincreasing.

Problem 4 (Effect of Step Size): Consider RRT* with fixed step size \( \eta > 0 \). Explain how choosing \( \eta \) too small or too large can affect (i) convergence speed and (ii) the quality of the asymptotic approximation to an optimal path, even though the asymptotic optimality theorem itself assumes an abstract steering function.

Solution:

If \( \eta \) is too small, the number of vertices required to approximate a fixed-length path within a given tolerance grows like \( \mathcal{O}(1/\eta) \), making the convergence to reasonable path costs very slow in practice; finite-time performance is poor. If \( \eta \) is too large, the ability of the tree to follow curvature or navigate narrow passages is reduced; the discrete paths obtained may oscillate around the continuous optimum, and the Lipschitz constant in the cost approximation bound effectively worsens. In the theoretical asymptotic optimality proofs, one often assumes an ideal steering function that can connect arbitrary nearby states exactly, which abstracts away step-size effects. In practice, choosing \( \eta \) comparable to the smallest obstacle feature size yields a more faithful approximation to the optimal path for a given computational budget.

Problem 5 (PRM vs. PRM* Edges): Suppose a PRM uses a fixed radius \( r > 0 \) that does not depend on \( n \). Argue informally how this differs from the PRM* scaling in terms of edge count and why such a planner is not guaranteed to be asymptotically optimal in all environments.

Solution:

With fixed \( r \), the expected degree of each vertex grows like \( n \) (because the volume of the ball of radius \( r \) is constant). Thus the total number of edges scales as \( \mathcal{O}(n^2) \), which is substantially denser than PRM*, and the roadmap is highly connected. However, this connectivity does not guarantee asymptotic optimality: one can construct environments with narrow optimal corridors where the local graph structure induced by a fixed radius fails to approximate the exact geodesics, even though the roadmap is globally connected. More technically, the relative scaling of edge density to \( \log n \) is not tuned to the connectivity threshold regime; edges may concentrate in wide open regions and sparsely cover the narrow regions critical for optimality. PRM* explicitly tunes its radius so that edge density and graph connectivity scale correctly, which leads to asymptotic optimality.

In this lesson we formalized asymptotic optimality for sampling-based motion planners and studied the two canonical examples: PRM* and RRT*. Both rely on random geometric graph constructions with connection radii shrinking like \( (\log n / n)^{1/d} \), balancing connectivity and sparsity. PRM* uses this to ensure that its roadmap approximates optimal paths, while RRT* incorporates a rewiring step to monotonically improve tree-based solutions. We then explored practical implementations in Python, C++ (OMPL), Java, MATLAB/Simulink, and Mathematica, preparing the ground for modern variants (e.g., informed and batch planners) in the next lesson.