Classical asymptotically optimal (AO) planners such as PRM* and RRT* operate by incrementally constructing a random geometric graph (RGG) over samples \( \{x_i\}_{i=1}^n \subset \mathcal{X}_{\text{free}} \) and searching this graph for a minimum-cost path between \( x_{\text{start}} \) and \( x_{\text{goal}} \). The underlying optimal motion planning problem can be written as

\[ \sigma^\star = \arg\min_{\sigma} J(\sigma) \quad \text{s.t. } \sigma:[0,1]\rightarrow \mathcal{X}_{\text{free}},\; \sigma(0)=x_{\text{start}},\;\sigma(1)=x_{\text{goal}} , \]

where the cost functional \( J(\sigma) \) is typically either path length

\[ J(\sigma) = \int_0^1 \left\|\frac{\mathrm{d}\sigma}{\mathrm{d}s}(s)\right\|_2 \,\mathrm{d}s , \]

or an energy-like integral. Modern algorithms such as BIT* and FMT* sharpen the efficiency of PRM*/RRT* by:

• Viewing planning as a graph search over an implicit RGG.
• Using a heuristic cost estimate \( f(x) = g(x) + h(x) \) with \( g(x) \) the tree cost-to-come and \( h(x) \) an admissible heuristic to goal.
• Restricting sampling to an informed subset of configuration space after an initial solution is found.

flowchart TD
  A["Sampling-based planning"] --> B["Classical AO planners (PRM*, RRT*)"]
  A --> C["Modern AO variants"]
  C --> D["BIT* \n(batch informed tree)"]
  C --> E["FMT* \n(fast marching tree)"]
  C --> F["Informed RRT* \n(focused sampling)"]
  D --> D1["Search edges ordered by \nf = g + h"]
  E --> E1["Dynamic-programming style \nexpansion on RGG"]
  F --> F1["Sample only inside \n'good' region"]
        

The main idea is to reduce the number of expensive collision checks by ordering both vertex and edge evaluations using heuristic information and by focusing samples in regions that may improve the current best solution cost \( c_{\text{best}} \).

Let \( \mathcal{X} \subset \mathbb{R}^d \) denote a compact configuration space with obstacle subset \( \mathcal{X}_{\text{obs}} \) and free space \( \mathcal{X}_{\text{free}} = \mathcal{X} \setminus \mathcal{X}_{\text{obs}} \). A typical RGG over \( n \) samples is

\[ \mathcal{G}_n = (\mathcal{V}_n,\mathcal{E}_n),\quad \mathcal{V}_n = \{x_1,\dots,x_n\}\subset \mathcal{X}_{\text{free}},\quad \mathcal{E}_n = \big\{ (x_i,x_j) : \|x_i - x_j\|_2 \le r_n \big\}, \]

where \( r_n \) is a connection radius. Under mild regularity assumptions on \( \mathcal{X}_{\text{free}} \), asymptotic optimality of PRM*/RRT*-type constructions is guaranteed if

\[ r_n \ge \gamma \left(\frac{\log n}{n}\right)^{1/d} \]

for some constant \( \gamma > \gamma^\star \) that depends on the dimension and volume of \( \mathcal{X}_{\text{free}} \). The sequence of graph-optimal path costs \( c_n^\star \) satisfies

\[ \mathbb{P}\big( \lim_{n\rightarrow\infty} c_n^\star = c^\star \big) = 1, \]

where \( c^\star \) is the optimal continuous cost. Modern planners (BIT*, FMT*) construct and search such RGGs more efficiently by:

• Batch sampling: samples are drawn in batches, and the RGG is updated incrementally.
• Implicit graphs: edges are generated lazily when needed by the search rather than precomputed.
• Heuristic ordering: edges with lower optimistic cost are evaluated earlier.

Importantly, their AO proofs rely on the same scaling of \( r_n \) as PRM*/RRT*. The difference is only in how edges are processed, not in which edges exist in the limit \( n\rightarrow \infty \).

BIT* can be seen as an A*-like search over an implicit RGG that is grown in batches of samples. Let \( \mathcal{X}_{\text{samples}}^{(k)} \) denote the set of samples after batch \( k \) and \( \mathcal{T}^{(k)} \) the current tree. For each vertex \( x \) in the tree we maintain:

• Cost-to-come: \( g(x) \), the path cost from \( x_{\text{start}} \) to \( x \) along \( \mathcal{T}^{(k)} \).
• Heuristic: \( h(x) \), an admissible lower bound on the cost from \( x \) to goal; e.g. \( h(x) = \|x - x_{\text{goal}}\|_2 \) for path length.
• Total estimate: \( f(x) = g(x) + h(x) \).

BIT* maintains a priority queue of edges \( (x_{\text{parent}}, x_{\text{child}}) \) ordered by the optimistic cost of a solution passing through that edge. An admissible lower bound on that cost is

\[ \underline{f}(x_{\text{parent}}, x_{\text{child}}) = g(x_{\text{parent}}) + \underline{c}(x_{\text{parent}}, x_{\text{child}}) + h(x_{\text{child}}), \]

where \( \underline{c} \) is a lower bound on the local edge cost (often simply the Euclidean distance \( \|x_{\text{parent}} - x_{\text{child}}\|_2 \)). Only edges with \( \underline{f} < c_{\text{best}} \) are considered, where \( c_{\text{best}} \) is the cost of the best solution found so far. This pruning ensures that any edge that cannot possibly improve the current solution is never collision-checked.

flowchart TD
  S["Start: initial batch of samples"] --> G["Build/extend RGG over samples"]
  G --> Q["Initialize edge queue \nwith promising edges"]
  Q --> L{"Queue empty or \nf_min >= c_best?"}
  L -->|yes| BATCH["Sample new batch, \nupdate RGG, \ntighten radius r_n"]
  L -->|no| E["Pop best edge \n(v_parent, v_child) from queue"]
  E --> C{"Edge feasible and \nbetter g(v_child)?"}
  C -->|no| Q
  C -->|yes| U["Add/update v_child in tree, \nupdate g(), rewire neighbors"]
  U --> Q
        

BIT* is anytime: after each successful tree extension, the current best solution cost \( c_{\text{best}}^{(k)} \) is updated. Because the search is ordered by an admissible optimistic cost, this sequence is monotonically nonincreasing:

\[ c_{\text{best}}^{(1)} \ge c_{\text{best}}^{(2)} \ge \cdots \ge c^\star \quad \text{(almost surely as the number of batches grows).} \]

When the connection radius within each batch satisfies the AO scaling discussed in Section 2 and the heuristic is admissible (never overestimates), the resulting sequence of best costs converges almost surely to the optimal cost \( c^\star \).

FMT* constructs a tree over a batch of samples by performing a dynamic-programming style expansion on the RGG. It maintains three sets:

• \( V_{\text{open}} \): frontier nodes (candidates for expansion).
• \( V_{\text{closed}} \): already expanded nodes.
• \( V_{\text{unvisited}} \): remaining samples.

Initialization places \( x_{\text{start}} \) in \( V_{\text{open}} \) and all other samples in \( V_{\text{unvisited}} \). At each iteration, FMT* selects

\[ x_{\min} = \arg\min_{x \in V_{\text{open}}} g(x), \]

and considers its neighbors in \( V_{\text{unvisited}} \):

\[ \mathcal{N}_{\text{unvisited}}(x_{\min}) = \big\{ y \in V_{\text{unvisited}} : \|y - x_{\min}\|_2 \le r_n \big\}. \]

For each such neighbor \( y \), FMT* computes a cost candidate through any open neighbor:

\[ g_{\text{cand}}(y) = \min_{x \in V_{\text{open}} \cap \mathcal{N}_{\text{open}}(y)} \big( g(x) + c(x,y) \big), \]

where \( \mathcal{N}_{\text{open}}(y) \) is the neighbor set of \( y \) currently in \( V_{\text{open}} \). FMT* then:

1. Selects a parent \( x_{\text{parent}} \) that realizes \( g_{\text{cand}}(y) \).
2. Checks the edge \( (x_{\text{parent}},y) \) for collision.
3. If collision-free and better than any existing cost, inserts \( y \) into \( V_{\text{open}} \) as a child of \( x_{\text{parent}} \).

Finally, \( x_{\min} \) is moved from \( V_{\text{open}} \) to \( V_{\text{closed}} \). The algorithm terminates when the goal region leaves the open set or when no open nodes remain.

FMT* differs from PRM* in that it never rewires the tree: costs are monotone along expansion, similar to Dijkstra's algorithm on the RGG. This yields good practical performance and a number of collision checks on the order of the number of edges in the final tree, while retaining asymptotic optimality under the same \( r_n \) scaling.

In uniform-sampling planners, early iterations often spend most samples far from any near-optimal path. Once a feasible solution with cost \( c_{\text{best}} \) is found, we can restrict subsequent sampling to an informed subset that can still yield improvements.

For the path-length objective in Euclidean configuration space, any path from \( x_{\text{start}} \) to \( x_{\text{goal}} \) that passes through a point \( x \) must have cost at least

\[ \underline{J}(x) = \|x - x_{\text{start}}\|_2 + \|x - x_{\text{goal}}\|_2 . \]

Therefore, only configurations satisfying \( \underline{J}(x) \le c_{\text{best}} \) can possibly yield a better solution. The informed set is

\[ \mathcal{X}_{\text{inf}}(c_{\text{best}}) = \big\{ x \in \mathcal{X}_{\text{free}} : \|x - x_{\text{start}}\|_2 + \|x - x_{\text{goal}}\|_2 \le c_{\text{best}} \big\}. \]

In \( \mathbb{R}^d \), this set is a prolate hyperspheroid with foci at \( x_{\text{start}} \) and \( x_{\text{goal}} \). Let \( d_f = \|x_{\text{start}} - x_{\text{goal}}\|_2 \) be the distance between foci, and define the semi-major axis \( a = \tfrac{1}{2} c_{\text{best}} \) and semi-minor axes

\[ b = \sqrt{a^2 - \left(\tfrac{1}{2} d_f\right)^2} . \]

The volume of the hyperspheroid is then

\[ \operatorname{vol}\big(\mathcal{X}_{\text{inf}}(c_{\text{best}})\big) = \zeta_d \, a \, b^{d-1},\quad \zeta_d = \frac{\pi^{d/2}}{\Gamma\!\big(\tfrac{d}{2} + 1\big)} , \]

where \( \zeta_d \) is the volume of the unit ball in \( \mathbb{R}^d \). As \( c_{\text{best}} \downarrow d_f \), we have \( a \downarrow \tfrac{1}{2} d_f \) and \( b \downarrow 0 \), so the informed set volume shrinks dramatically, focusing samples tightly around near-optimal paths.

Informed RRT* and informed variants of BIT*/FMT* replace uniform sampling in \( \mathcal{X}_{\text{free}} \) with sampling in \( \mathcal{X}_{\text{inf}}(c_{\text{best}}) \). The AO proofs extend by observing that \( \mathcal{X}_{\text{inf}}(c_{\text{best}}) \) still contains all optimal points when \( c_{\text{best}} > c^\star \) and that uniform sampling restricted to this set still yields dense coverage along optimal paths as the number of samples grows.

Below is a compact Python implementation sketch illustrating informed sampling and BIT*-style edge ordering in a 2D configuration space (e.g., planar arm end-effector). Collision checking and nearest-neighbor search are simplified; for a full implementation, one would use spatial indexes (e.g., k-d trees) and robotics libraries such as ompl or moveit_commander.

import math
import random
from typing import List, Tuple, Optional

Point = Tuple[float, float]

class Node:
    def __init__(self, x: Point, parent: Optional["Node"] = None, g: float = math.inf):
        self.x = x
        self.parent = parent
        self.g = g  # cost-to-come

def euclidean(a: Point, b: Point) -> float:
    return math.hypot(a[0] - b[0], a[1] - b[1])

def line_collision_free(a: Point, b: Point, obstacles) -> bool:
    """
    Placeholder: check segment a-b against obstacles.
    'obstacles' could be polygons, circles, etc.
    """
    # TODO: implement robust collision checking
    return True

def sample_uniform(bounds: Tuple[float, float, float, float]) -> Point:
    xmin, xmax, ymin, ymax = bounds
    return (random.uniform(xmin, xmax), random.uniform(ymin, ymax))

def sample_informed(start: Point, goal: Point, c_best: float,
                    bounds: Tuple[float, float, float, float]) -> Point:
    """
    Sample inside the prolate ellipse defined by start, goal and c_best.
    If no solution yet (c_best == inf), fall back to uniform sampling.
    """
    if not math.isfinite(c_best):
        return sample_uniform(bounds)

    xs, ys = start
    xg, yg = goal
    d_f = euclidean(start, goal)
    if c_best <= d_f:
        # Numerically degenerate: just return start
        return start

    # Coordinate transform: align major axis with x-axis in local frame.
    cx, cy = (xs + xg) / 2.0, (ys + yg) / 2.0
    theta = math.atan2(yg - ys, xg - xs)
    a = c_best / 2.0
    c = d_f / 2.0
    b = math.sqrt(max(a * a - c * c, 0.0))

    # Sample inside unit ball, then scale
    while True:
        u = random.uniform(-1.0, 1.0)
        v = random.uniform(-1.0, 1.0)
        if u * u + v * v <= 1.0:
            break
    # Stretch to ellipse
    lx, ly = a * u, b * v

    # Rotate back and translate to world coordinates
    cos_th, sin_th = math.cos(theta), math.sin(theta)
    wx = cx + cos_th * lx - sin_th * ly
    wy = cy + sin_th * lx + cos_th * ly

    # Optionally clamp to workspace bounds
    xmin, xmax, ymin, ymax = bounds
    wx = min(max(wx, xmin), xmax)
    wy = min(max(wy, ymin), ymax)
    return (wx, wy)

def nearest(nodes: List[Node], x: Point) -> Node:
    return min(nodes, key=lambda n: euclidean(n.x, x))

def neighbors(nodes: List[Node], x: Point, r: float) -> List[Node]:
    return [n for n in nodes if euclidean(n.x, x) <= r]

def bitstar_planner(start: Point, goal: Point,
                    bounds: Tuple[float, float, float, float],
                    obstacles,
                    n_batches: int = 20,
                    batch_size: int = 100,
                    r0: float = 1.0) -> Optional[Node]:
    """
    Extremely simplified BIT*-like planner with informed sampling.
    """
    root = Node(start, parent=None, g=0.0)
    tree_nodes: List[Node] = [root]
    best_goal: Optional[Node] = None
    c_best = math.inf

    for k in range(n_batches):
        # 1. Sample new batch
        samples: List[Point] = []
        for _ in range(batch_size):
            x_samp = sample_informed(start, goal, c_best, bounds)
            samples.append(x_samp)

        # 2. Define connection radius for this batch
        n_total = len(tree_nodes) + len(samples)
        r_n = r0 * math.pow(math.log(max(n_total, 2)) / n_total, 1.0 / 2.0)

        # 3. Populate edge queue: (f_lower, parent_node, x_sample)
        edge_queue: List[Tuple[float, Node, Point]] = []
        for x in samples:
            # consider neighbors in current tree
            for parent in neighbors(tree_nodes, x, r_n):
                g_parent = parent.g
                c_lb = euclidean(parent.x, x)
                h_x = euclidean(x, goal)
                f_lb = g_parent + c_lb + h_x
                if f_lb < c_best:
                    edge_queue.append((f_lb, parent, x))

        # 4. Process edges in order of optimistic cost
        edge_queue.sort(key=lambda tup: tup[0])
        for f_lb, parent, x in edge_queue:
            if f_lb >= c_best:
                break  # cannot improve current solution

            # Attempt to connect if not already in tree
            if any(euclidean(n.x, x) < 1e-9 for n in tree_nodes):
                continue

            if not line_collision_free(parent.x, x, obstacles):
                continue

            g_new = parent.g + euclidean(parent.x, x)
            new_node = Node(x, parent=parent, g=g_new)
            tree_nodes.append(new_node)

            # Check if we reached goal region (here: distance threshold)
            if euclidean(x, goal) <= r_n and line_collision_free(x, goal, obstacles):
                g_goal = g_new + euclidean(x, goal)
                if g_goal < c_best:
                    goal_node = Node(goal, parent=new_node, g=g_goal)
                    tree_nodes.append(goal_node)
                    best_goal = goal_node
                    c_best = g_goal
                    # continue processing; better solution may exist

    return best_goal  # May be None if no solution
      

The same basic structure extends to higher-dimensional configuration spaces (e.g., joint angles of a manipulator) by replacing Point with vectors and using forward kinematics together with robot-specific collision checking.

The Open Motion Planning Library (OMPL) provides ready-made implementations of BIT* and FMT* in C++, and is widely used in robotics frameworks such as MoveIt. The snippet below shows how to instantiate BIT* for a simple 2D planning problem; for manipulators, one would use an ompl::base::RealVectorStateSpace of higher dimension or a joint-space state space type.

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/ScopedState.h>
#include <ompl/base/ProblemDefinition.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/bitstar/BITstar.h>
#include <ompl/geometric/planners/fmt/FMT.h>

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

bool isStateValid(const ob::State *state) {
    const auto *s = state->as<ob::RealVectorStateSpace::StateType>();
    double x = (*s)[0];
    double y = (*s)[1];
    // TODO: collision check in workspace
    (void)x; (void)y;
    return true;
}

int main() {
    // 1. Define a 2D state space
    auto space(std::make_shared<ob::RealVectorStateSpace>(2));
    ob::RealVectorBounds bounds(2);
    bounds.setLow(0, -5.0);
    bounds.setHigh(0,  5.0);
    bounds.setLow(1, -5.0);
    bounds.setHigh(1,  5.0);
    space->setBounds(bounds);

    // 2. SimpleSetup wraps common boilerplate
    og::SimpleSetup ss(space);
    ss.setStateValidityChecker(&isStateValid);

    ob::ScopedState<> start(space), goal(space);
    start[0] = -4.0; start[1] = -4.0;
    goal[0]  =  4.0; goal[1]  =  4.0;
    ss.setStartAndGoalStates(start, goal);

    // 3. Choose planner: BIT* or FMT*
    auto si = ss.getSpaceInformation();
    auto planner = std::make_shared<og::BITstar>(si);
    // For FMT*, use: auto planner = std::make_shared<og::FMT>(si);

    ss.setPlanner(planner);

    // 4. Solve with time budget
    ob::PlannerStatus solved = ss.solve(5.0);
    if (solved) {
        ss.simplifySolution();
        auto path = ss.getSolutionPath();
        path.printAsMatrix(std::cout);
    }
    return 0;
}
      

For a manipulator, isStateValid calls into robot-specific forward kinematics and collision detection (e.g., via a planning scene in MoveIt). The planner configuration (e.g., range, batch size) can be tuned via BIT* and FMT*'s parameter APIs.

There is no de-facto standard Java motion-planning library comparable to OMPL, but the same mathematical ideas can be implemented directly. Below is a skeleton for an informed RRT* planner in Java for a generic configuration type Config with user-defined distance and interpolation.

public interface ConfigSpace<C> {
    double distance(C a, C b);
    C interpolate(C a, C b, double alpha); // alpha in [0, 1]
    boolean collisionFree(C a, C b);
    C sampleUniform();
    C sampleInformed(double cBest, C start, C goal);
}

public class RRTStarNode<C> {
    public C x;
    public RRTStarNode<C> parent;
    public double g;
    public RRTStarNode(C x, RRTStarNode<C> parent, double g) {
        this.x = x;
        this.parent = parent;
        this.g = g;
    }
}

public class InformedRRTStar<C> {
    private final ConfigSpace<C> space;
    private final C start, goal;
    private final double step;
    private final double goalThreshold;
    private final java.util.List<RRTStarNode<C>> tree;
    private double cBest;

    public InformedRRTStar(ConfigSpace<C> space, C start, C goal,
                           double step, double goalThreshold) {
        this.space = space;
        this.start = start;
        this.goal = goal;
        this.step = step;
        this.goalThreshold = goalThreshold;
        this.tree = new java.util.ArrayList<>();
        this.tree.add(new RRTStarNode<>(start, null, 0.0));
        this.cBest = Double.POSITIVE_INFINITY;
    }

    private RRTStarNode<C> nearest(C x) {
        RRTStarNode<C> best = null;
        double bestDist = Double.POSITIVE_INFINITY;
        for (RRTStarNode<C> n : tree) {
            double d = space.distance(n.x, x);
            if (d < bestDist) {
                bestDist = d;
                best = n;
            }
        }
        return best;
    }

    public RRTStarNode<C> plan(int maxIterations, double radius) {
        RRTStarNode<C> bestGoal = null;
        for (int k = 0; k < maxIterations; ++k) {
            C xRand = space.sampleInformed(cBest, start, goal);
            RRTStarNode<C> xNear = nearest(xRand);

            double d = space.distance(xNear.x, xRand);
            double alpha = Math.min(1.0, step / Math.max(d, 1e-9));
            C xNew = space.interpolate(xNear.x, xRand, alpha);

            if (!space.collisionFree(xNear.x, xNew)) continue;

            // Compute cost-to-come candidate
            double gNew = xNear.g + space.distance(xNear.x, xNew);

            // Choose parent and rewire neighbors inside radius
            java.util.List<RRTStarNode<C>> neighbors = new java.util.ArrayList<>();
            for (RRTStarNode<C> n : tree) {
                if (space.distance(n.x, xNew) <= radius)
                    neighbors.add(n);
            }
            RRTStarNode<C> xParent = xNear;
            double gMin = gNew;
            for (RRTStarNode<C> n : neighbors) {
                double gCand = n.g + space.distance(n.x, xNew);
                if (gCand < gMin && space.collisionFree(n.x, xNew)) {
                    gMin = gCand;
                    xParent = n;
                }
            }
            RRTStarNode<C> xNode = new RRTStarNode<>(xNew, xParent, gMin);
            tree.add(xNode);

            // Rewire
            for (RRTStarNode<C> n : neighbors) {
                double gCand = gMin + space.distance(xNew, n.x);
                if (gCand + 1e-9 < n.g && space.collisionFree(xNew, n.x)) {
                    n.parent = xNode;
                    n.g = gCand;
                }
            }

            // Check for goal
            if (space.distance(xNew, goal) <= goalThreshold
                    && space.collisionFree(xNew, goal)) {
                double gGoal = gMin + space.distance(xNew, goal);
                if (gGoal < cBest) {
                    cBest = gGoal;
                    bestGoal = new RRTStarNode<>(goal, xNode, gGoal);
                    tree.add(bestGoal);
                }
            }
        }
        return bestGoal;
    }
}
      

In a manipulator application, Config would typically be a joint angle vector, and ConfigSpace would interface with a kinematics and collision library (for example, a Java binding to a robotics simulator).

MATLAB's Robotics System Toolbox provides RRT and RRT* implementations. A BIT*/FMT*-style planner can be prototyped using matrices to represent sample sets and adjacency queries. Below is a simplified MATLAB function that performs informed sampling and a BIT*-like edge expansion in 2D.

function goalNode = bitstar2d(start, goal, bounds, nBatches, batchSize, r0)
% start, goal: 1x2 row vectors
% bounds: [xmin xmax ymin ymax]
% Return: struct with fields x, parent, g

if nargin < 6, r0 = 1.0; end
INF = 1e9;

root.x = start;
root.parent = 0;
root.g = 0.0;
tree = root;
bestCost = INF;
goalNode = [];

for k = 1:nBatches
    % Sample batch (no obstacles in this simple example)
    samples = zeros(batchSize, 2);
    for i = 1:batchSize
        samples(i,:) = sampleInformed(start, goal, bestCost, bounds);
    end

    nTotal = numel(tree) + batchSize;
    r = r0 * (log(max(nTotal, 2)) / nTotal)^(1/2);

    % Build edge queue: [f_lb, parentIndex, sampleIndex]
    edgeQueue = [];
    for i = 1:batchSize
        x = samples(i,:);
        for j = 1:numel(tree)
            xp = tree(j).x;
            d = norm(xp - x);
            if d <= r
                gParent = tree(j).g;
                h = norm(x - goal);
                f_lb = gParent + d + h;
                if f_lb < bestCost
                    edgeQueue = [edgeQueue; f_lb, j, i]; %#ok<AGROW>
                end
            end
        end
    end

    % Process edges by increasing f_lb
    if isempty(edgeQueue), continue; end
    edgeQueue = sortrows(edgeQueue, 1);

    for e = 1:size(edgeQueue, 1)
        f_lb = edgeQueue(e,1);
        if f_lb >= bestCost
            break;
        end
        parentIdx = edgeQueue(e,2);
        sampIdx   = edgeQueue(e,3);
        x = samples(sampIdx,:);

        % Check if x already in tree
        already = false;
        for j = 1:numel(tree)
            if norm(tree(j).x - x) < 1e-6
                already = true;
                break;
            end
        end
        if already, continue; end

        xp = tree(parentIdx).x;
        % TODO: collision check between xp and x
        gNew = tree(parentIdx).g + norm(xp - x);

        newNode.x = x;
        newNode.parent = parentIdx;
        newNode.g = gNew;
        tree(end + 1) = newNode; %#ok<AGROW>

        if norm(x - goal) <= r
            % Direct connection to goal
            gGoal = gNew + norm(x - goal);
            if gGoal < bestCost
                bestCost = gGoal;
                goalNode.x = goal;
                goalNode.parent = numel(tree);
                goalNode.g = gGoal;
            end
        end
    end
end
end

function x = sampleInformed(start, goal, cBest, bounds)
if ~isfinite(cBest)
    % uniform sampling
    xmin = bounds(1); xmax = bounds(2);
    ymin = bounds(3); ymax = bounds(4);
    x = [xmin + (xmax - xmin) * rand, ...
         ymin + (ymax - ymin) * rand];
    return;
end
% Elliptical informed sampling (similar to Python version)
xs = start(1); ys = start(2);
xg = goal(1);  yg = goal(2);
df = norm(start - goal);
if cBest <= df
    x = start;
    return;
end
cx = 0.5 * (xs + xg);
cy = 0.5 * (ys + yg);
theta = atan2(yg - ys, xg - xs);
a = 0.5 * cBest;
c = 0.5 * df;
b = sqrt(max(a^2 - c^2, 0));

while true
    u = 2 * rand - 1;
    v = 2 * rand - 1;
    if u^2 + v^2 <= 1
        break;
    end
end
lx = a * u;
ly = b * v;
R = [cos(theta), -sin(theta); sin(theta), cos(theta)];
w = R * [lx; ly];
x = [cx; cy] + w;
x = x(:).';
end
      

In Simulink, the above function can be wrapped in a MATLAB Function block that outputs a sequence of waypoints to a trajectory tracking subsystem, enabling closed-loop simulations of the planned path on a dynamic robot model.

Mathematica is convenient for prototyping sampling-based planners thanks to its symbolic and visualization capabilities. The following sketch implements informed sampling and a simple RRT*-like tree in 2D.

ClearAll[euclidean, sampleInformed, buildRRTStar];
euclidean[a_, b_] := Norm[a - b];

sampleUniform[{xmin_, xmax_, ymin_, ymax_}] := {
  RandomReal[{xmin, xmax}], RandomReal[{ymin, ymax}]
};

sampleInformed[start_, goal_, cBest_, bounds_] :=
 Module[{df, xmin, xmax, ymin, ymax, xs, ys, xg, yg, cx, cy, theta, a, c, b,
   u, v, lx, ly, rot, p},
  {xs, ys} = start; {xg, yg} = goal;
  df = euclidean[start, goal];
  {xmin, xmax, ymin, ymax} = bounds;
  If[!FiniteQ[cBest] || cBest <= df,
   Return[sampleUniform[bounds]];
  ];
  cx = (xs + xg)/2; cy = (ys + yg)/2;
  theta = ArcTan[xg - xs, yg - ys];
  a = cBest/2; c = df/2;
  b = Sqrt[Max[a^2 - c^2, 0]];
  {u, v} = 
   First@RandomPoint[Disk[], 1]; (* sample in unit disk *)
  lx = a*u; ly = b*v;
  rot = { {Cos[theta], -Sin[theta]}, {Sin[theta], Cos[theta]} };
  p = {cx, cy} + rot.{lx, ly};
  {
   Clip[p[[1]], {xmin, xmax}],
   Clip[p[[2]], {ymin, ymax}]
   }
  ];

buildRRTStar[start_, goal_, bounds_, maxIter_Integer, step_, radius_] :=
 Module[{tree, costs, parents, cBest, bestGoalIndex, n, xRand, xNear,
   xNew, d, alpha, gNew, neigh, i},
  tree = {start};
  costs = {0.0};
  parents = {0};
  cBest = Infinity;
  bestGoalIndex = -1;

  Do[
   xRand = sampleInformed[start, goal, cBest, bounds];
   (* nearest neighbor *)
   n = Length[tree];
   {d, i} = 
    MinimalBy[Table[{euclidean[tree[[j]], xRand], j}, {j, 1, n}], 
      First][[1]];
   xNear = tree[[i]];
   alpha = Min[1., step/Max[d, 10.^-9]];
   xNew = xNear + alpha (xRand - xNear);
   If[False, Continue[]]; (* placeholder for collision check *)
   gNew = costs[[i]] + euclidean[xNear, xNew];

   (* neighbors for RRT* rewiring *)
   neigh = 
    Select[Range[n], 
     euclidean[tree[[#]], xNew] <= radius && True &]; (* collision-free placeholder *)

   (* choose parent *)
   {gNew, i} =
    MinimalBy[
      Append[
       Table[{costs[[j]] + euclidean[tree[[j]], xNew], j}, {j, neigh}],
       {gNew, i}],
      First][[1]];
   AppendTo[tree, xNew];
   AppendTo[costs, gNew];
   AppendTo[parents, i];

   (* rewire neighbors *)
   Do[
    If[gNew + euclidean[xNew, tree[[j]]] + 10.^-9 < costs[[j]],
     parents[[j]] = Length[tree];
     costs[[j]] = gNew + euclidean[xNew, tree[[j]]];
     ],
    {j, neigh}
    ];

   (* goal check *)
   If[euclidean[xNew, goal] <= radius,
    If[gNew + euclidean[xNew, goal] < cBest,
     cBest = gNew + euclidean[xNew, goal];
     bestGoalIndex = Length[tree];
     ];
    ];
   ,
   {maxIter}
   ];

  <|
   "Tree" -> tree,
   "Costs" -> costs,
   "Parents" -> parents,
   "BestGoalIndex" -> bestGoalIndex,
   "BestCost" -> cBest
   |>
  ];
      

One can extract the best path by following "Parents" from "BestGoalIndex" back to the root, then visualize it with Graphics overlays on obstacle regions and the informed sampling region.

Problem 1 (Geometry of the Informed Set): Let \( x_{\text{start}}, x_{\text{goal}} \in \mathbb{R}^d \). For a given cost bound \( c_{\text{best}} \ge d_f \) with \( d_f = \|x_{\text{start}} - x_{\text{goal}}\|_2 \), show that the informed set \( \mathcal{X}_{\text{inf}}(c_{\text{best}}) \) defined in Section 5 is a prolate hyperspheroid with foci at \( x_{\text{start}} \) and \( x_{\text{goal}} \).

Solution: By definition, \( x \in \mathcal{X}_{\text{inf}}(c_{\text{best}}) \) iff

\[ \|x - x_{\text{start}}\|_2 + \|x - x_{\text{goal}}\|_2 \le c_{\text{best}}. \]

This is exactly the standard geometric definition of a (hyper-)ellipsoid with two foci at \( x_{\text{start}} \) and \( x_{\text{goal}} \), consisting of points for which the sum of distances to the foci is bounded by a constant. When \( c_{\text{best}} = d_f \), the locus degenerates to the line segment between the two points. For \( c_{\text{best}} > d_f \), the set is a volume region with semi-major axis \( a = \tfrac{1}{2} c_{\text{best}} \) and semi-minor axes \( b = \sqrt{a^2 - (d_f/2)^2} \), which is precisely a prolate hyperspheroid.

Problem 2 (Volume Ratio and Speed-Up Intuition): Assume a configuration space \( \mathcal{X} \subset \mathbb{R}^d \) has free volume \( V_{\text{free}} \) and the informed set volume is \( V_{\text{inf}}(c_{\text{best}}) \). Suppose samples are drawn uniformly from \( \mathcal{X}_{\text{inf}} \) instead of \( \mathcal{X}_{\text{free}} \). What is the probability that a single sample from each distribution lies in the informed set, and how does their ratio relate to expected convergence speed?

Solution: A uniform sample from \( \mathcal{X}_{\text{free}} \) lies in the informed set with probability \( p_{\text{uniform}} = V_{\text{inf}} / V_{\text{free}} \). Under informed sampling, every sample is in \( \mathcal{X}_{\text{inf}} \) by construction, so \( p_{\text{inf}} = 1 \). Thus the ratio of probabilities is

\[ \frac{p_{\text{inf}}}{p_{\text{uniform}}} = \frac{1}{V_{\text{inf}} / V_{\text{free}}} = \frac{V_{\text{free}}}{V_{\text{inf}}}. \]

When \( V_{\text{inf}} \ll V_{\text{free}} \), the expected number of samples required to obtain a point in the informed region scales inversely with this probability. Hence informed sampling can lead to a speed-up proportional to \( V_{\text{free}} / V_{\text{inf}} \) in discovering improving paths.

Problem 3 (Monotone Best-Cost Sequence in BIT*): In BIT*, let \( c_{\text{best}}^{(k)} \) denote the best solution cost after processing batch \( k \). Show that the sequence \( \{c_{\text{best}}^{(k)}\} \) is monotonically nonincreasing.

Solution: At batch \( k+1 \), BIT* adds new samples and edges to the search structure, but never removes valid paths discovered earlier. Hence, the set of feasible solution paths considered at batch \( k+1 \) strictly contains the set from batch \( k \). The new best cost is

\[ c_{\text{best}}^{(k+1)} = \min\big\{ c_{\text{best}}^{(k)},\, \text{costs of new feasible paths at batch } k+1 \big\} \le c_{\text{best}}^{(k)}. \]

Therefore, the sequence is nonincreasing. Because edge evaluation is ordered by an admissible optimistic cost, any newly discovered path at batch \( k+1 \) has cost at least \( c^\star \), so the sequence is bounded below and converges.

Problem 4 (RGG Connectivity Radius): Consider an RGG built from \( n \) i.i.d. uniform samples in a compact subset of \( \mathbb{R}^d \) with connection radius \( r_n = \gamma (\log n / n)^{1/d} \) for \( \gamma > 0 \). Argue (informally) why \( \gamma \) must be large enough (above a critical \( \gamma^\star \)) to ensure that the probability of graph connectivity tends to 1 as \( n\rightarrow\infty \).

Solution: Classical results on RGGs show that connectivity exhibits a sharp threshold in terms of \( r_n \). Intuitively, the average degree of a node scales as

\[ \mathbb{E}[\deg] \approx n \cdot \operatorname{vol}(B(0,r_n)) \propto n \cdot r_n^d \approx \gamma^d \log n. \]

If \( \gamma \) is too small, the average degree grows slower than a constant times \( \log n \), and with non-negligible probability isolated components persist as \( n \) grows. If \( \gamma \) exceeds a critical \( \gamma^\star \), the average degree grows faster than a threshold that ensures, via union bounds and concentration inequalities, that the probability of any isolated component tends to zero. BIT* and FMT* require \( \gamma > \gamma^\star \) so that the RGG approximates the connectivity of the continuous space.

Problem 5 (Collision-Check Complexity in FMT* vs PRM*): Suppose PRM* precomputes all edges within radius \( r_n \), resulting in \( O(n^2) \) potential edges and \( O(n^2) \) collision checks. FMT*, however, only checks edges involved in the tree construction. Explain why, for a given solution quality, FMT* typically requires asymptotically fewer collision checks than PRM*.

Solution: In PRM*, the planner eagerly collision-checks all edges that satisfy the distance threshold, regardless of whether they are ultimately used in the shortest path. The number of such edges scales roughly with \( n^2 r_n^d \approx n \log n \). FMT* instead performs a dynamic-programming expansion in which each node is inserted into the tree at most once, and only edges connecting frontier nodes to their best parent candidates are checked. The total number of such edges is on the order of the final tree size, which is \( O(n) \) in typical settings. Hence the number of collision checks in FMT* scales closer to \( O(n) \) than \( O(n \log n) \), giving a significant asymptotic improvement in high-sample regimes.

In this lesson we framed modern sampling-based motion planners as heuristic searches over implicit random geometric graphs. BIT* and FMT* improve over PRM*/RRT* by batch processing, heuristic edge ordering, and dynamic-programming style expansions that reduce collision-check complexity while preserving asymptotic optimality. Informed sampling exploits geometric structure of the cost functional to focus samples within a prolate hyperspheroid that contains all potentially better solutions, dramatically accelerating convergence in high-dimensional configuration spaces. Multi-language implementation sketches connected these mathematical ideas to practical robotic code in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica.