We consider a 6-DOF revolute manipulator with joint vector \( q \in \mathbb{R}^6 \), joint limits \( q_i^{\min}, q_i^{\max} \), a known forward kinematics mapping from previous courses, and a static workspace with obstacles. The objective is to compute a collision-free, near-optimal joint-space path connecting a start configuration \( q_{\mathrm{start}} \) to a goal configuration \( q_{\mathrm{goal}} \).

The lab decomposes into the following stages:

• Define the configuration space and joint limits for the 6-DOF arm.
• Implement forward kinematics and collision checking for robot vs. obstacles.
• Implement sampling, metric, and steering primitives in configuration space.
• Implement RRT* and PRM using these primitives.
• Export the resulting joint-space path to a simulator or controller.

flowchart TD
  A["Define 6-DOF arm model and joint limits"] --> B["Implement forward kinematics and collision checker"]
  B --> C["Implement C-space sampling, metric, and steering"]
  C --> D["RRT* planner on C_free"]
  C --> E["PRM planner on C_free"]
  D --> F["Extract joint-space path"]
  E --> F
  F --> G["Time-parameterize and send to simulator/controller"]
        

In all implementations, the planner operates purely in configuration space; dynamics and time-parameterization can be handled subsequently by your trajectory-generation or control stack.

Let the manipulator joint vector be \( q = [q_1,\dots,q_6]^\top \) where each \( q_i \) is a revolute joint angle. The nominal configuration space is the 6-dimensional hyper-rectangle

\[ \mathcal{C} = \prod_{i=1}^6 [q_i^{\min}, q_i^{\max}] \subset \mathbb{R}^6. \]

The forward kinematics mapping (from your kinematics course) is

\[ f(q) = \big(T_1(q),\dots,T_L(q)\big), \]

where \( T_\ell(q) \in SE(3) \) is the homogeneous transform of link \( \ell \) with respect to a world frame, and \( L \) is the number of links. We assume \( f \) is continuous in \( q \).

Model each link by a geometric primitive (e.g., capsule or box). Let \( \mathcal{B}(q) \subset \mathbb{R}^3 \) denote all points occupied by the robot at configuration \( q \), and let \( \mathcal{O} \subset \mathbb{R}^3 \) be the union of all obstacle solids. Then the configuration-space obstacle set is

\[ \mathcal{C}_{\mathrm{obs}} = \big\{ q \in \mathcal{C} \,\big|\, \mathcal{B}(q) \cap \mathcal{O} \neq \emptyset \big\}, \]

and the free space is

\[ \mathcal{C}_{\mathrm{free}} = \mathcal{C} \setminus \mathcal{C}_{\mathrm{obs}}. \]

A path is a continuous curve \( \sigma : [0,1] \to \mathcal{C}_{\mathrm{free}} \) such that \( \sigma(0) = q_{\mathrm{start}} \) and \( \sigma(1) = q_{\mathrm{goal}} \). A common choice in this lab is to use piecewise-linear joint-space paths:

\[ \sigma(t) = (1-t)\,q_k + t\,q_{k+1} \quad \text{for } t \in [t_k, t_{k+1}], \]

where the breakpoints \( q_k \in \mathcal{C}_{\mathrm{free}} \) are nodes of the tree (RRT*) or roadmap (PRM).

Collision checking is encapsulated in a predicate \( \text{Valid}(q) \) returning true if \( q \in \mathcal{C}_{\mathrm{free}} \). A useful continuous quantity is the configuration clearance

\[ \operatorname{clear}(q) = \inf_{x \in \mathcal{B}(q)} \inf_{y \in \mathcal{O}} \lVert x - y \rVert_2. \]

A configuration is collision-free if \( \operatorname{clear}(q) \) is strictly positive. In practice we use a discretized test:

• Use forward kinematics to obtain link frames for a given \( q \).
• Approximate each link by a small number of points or capsules.
• Compute distances from these primitives to each obstacle primitive.
• Declare collision if any signed distance is nonpositive.

For an edge between configurations \( q, q' \in \mathcal{C}_{\mathrm{free}} \) we discretize the straight-line interpolation

\[ \sigma(t) = (1-t)\,q + t\,q',\quad t \in [0,1], \]

into samples \( \sigma(t_k) \) at

\[ t_k = \frac{k}{K}, \quad K = \left\lceil \frac{d(q,q')}{\delta} \right\rceil, \]

where \( d \) is a configuration-space metric and \( \delta \) is a user-chosen step limit. If all intermediate samples are valid, we accept the edge.

RRT* maintains a tree \( T_n = (V_n, E_n) \) whose vertices lie in \( \mathcal{C}_{\mathrm{free}} \), with root \( q_{\mathrm{start}} \). For node \( x \in V_n \), let \( J_n(x) \) denote the cost-to-come (path length) from the root along the unique tree path. Using the metric \( d \), the cost of a piecewise-linear path \( \sigma \) with nodes \( q_0,\dots,q_K \) is

\[ J(\sigma) = \sum_{k=0}^{K-1} d(q_k, q_{k+1}). \]

As in previous lessons, the RRT* connection radius is chosen as

\[ r_n = \min\Big\{ \eta,\; \gamma_{\mathrm{RRT*}} \big( \tfrac{\log n}{n} \big)^{1/6} \Big\}, \]

where \( n = |V_n| \), \( \eta \) is a maximum steering length, and \( \gamma_{\mathrm{RRT*}} \) is a constant chosen larger than the critical threshold from the RRT* convergence theorem.

For each iteration:

1. Sample \( q_{\mathrm{rand}} \sim \mathcal{U}(\mathcal{C}) \).
2. Find nearest vertex \( x_{\mathrm{near}} \in V_n \).
3. Steer from \( q_{\mathrm{near}} \) toward \( q_{\mathrm{rand}} \) with step limit \( \eta \) to obtain \( q_{\mathrm{new}} \).
4. If the edge from \( q_{\mathrm{near}} \) to \( q_{\mathrm{new}} \) is collision-free, compute the neighbor set \( \mathcal{X}_{\mathrm{near}} \subset V_n \) within radius \( r_n \).
5. Choose as parent the neighbor minimizing \( J_n(x) + c(x,q_{\mathrm{new}}) \), where \( c \) is edge cost. Insert \( q_{\mathrm{new}} \) accordingly.
6. Rewire neighbors when connecting via \( q_{\mathrm{new}} \) decreases their cost, subject to collision checks.

flowchart TD
  S["Sample q_rand in C"] --> N["Nearest node x_near"]
  N --> ST["Steer from q_near toward q_rand (step <= eta)"]
  ST --> V["Collision-free edge?"]
  V -->|no| S
  V -->|yes| NB["Compute neighbors within radius r_n"]
  NB --> P["Choose parent with minimal cost-to-come"]
  P --> INS["Insert q_new into tree"]
  INS --> RW["Rewire cheaper neighbors through q_new"]
  RW --> GOAL["Check if q_new is in goal region"]
        

As the number of samples grows, and under the correct choice of \( r_n \), RRT* converges almost surely to an optimal path in \( \mathcal{C}_{\mathrm{free}} \), provided the problem is robustly feasible.

The Probabilistic Roadmap (PRM) algorithm builds an undirected graph \( G_n = (V_n, E_n) \) in \( \mathcal{C}_{\mathrm{free}} \):

1. Sample \( n \) configurations \( q^{(1)},\dots,q^{(n)} \).
2. Discard samples not in \( \mathcal{C}_{\mathrm{free}} \).
3. Connect each sample to its neighbors (within radius \( r_n \) or to the \( k_n \) nearest) if the connecting edge is collision-free.

With radius-based connection, one typically uses

\[ r_n = \gamma_{\mathrm{PRM}} \Big( \tfrac{\log n}{n} \Big)^{1/6}, \]

with \( \gamma_{\mathrm{PRM}} \) larger than the theoretical threshold ensuring asymptotic optimality (see Lesson 4). Given \( G_n \), one inserts \( q_{\mathrm{start}} \) and \( q_{\mathrm{goal}} \), connects them to nearby nodes, and runs a graph shortest-path algorithm (e.g. Dijkstra) to obtain an approximate optimal path.

Note that a single roadmap can be reused for many start/goal pairs as long as the obstacles and joint limits remain unchanged, which is useful for repetitive manipulator tasks.

Both RRT* and PRM require:

• Sampling \( \mathcal{C} \)
• Metrics and steering
• Collision checking for configurations and edges

A weighted Euclidean metric in joint space is

\[ d(q,q') = \Bigg( \sum_{i=1}^6 w_i^2 (q_i - q_i')^2 \Bigg)^{1/2}, \quad w_i \gt 0. \]

Here the weights \( w_i \) compensate for different joint ranges or for how much a joint influences end-effector motion. A straight-line steering function in joint space is

\[ \operatorname{Steer}(q,q',\eta) = q + \alpha (q'-q), \quad \alpha = \min \Big\{ 1,\; \frac{\eta}{d(q,q')} \Big\}, \]

where \( \eta \) is the maximum step length. Discretized collision checking along this segment uses the scheme from Section 3.

Below is a minimal from-scratch Python implementation of RRT* and a basic PRM for a 6-DOF arm. To keep the code focused on planning, the functions forward_kinematics and in_collision are placeholders where you should plug in your own manipulator model and collision engine (e.g., using mesh distances or capsule approximations).

import numpy as np
from dataclasses import dataclass

# Joint limits for 6-DOF arm (example values, radians)
Q_MIN = np.array([-np.pi, -np.pi, -np.pi, -np.pi, -np.pi, -np.pi])
Q_MAX = np.array([ np.pi,  np.pi,  np.pi,  np.pi,  np.pi,  np.pi])

@dataclass
class Node:
    q: np.ndarray
    parent: int
    cost: float  # cost-to-come

def sample_config():
    return Q_MIN + (Q_MAX - Q_MIN) * np.random.rand(6)

def distance(q1, q2, w=None):
    if w is None:
        w = np.ones(6)
    diff = (q1 - q2) * w
    return float(np.linalg.norm(diff))

def forward_kinematics(q):
    """
    Placeholder: return link frames or sample points on the robot.
    Use your existing DH parameters or rigid-body model here.
    """
    raise NotImplementedError

def in_collision(q, obstacles):
    """
    Placeholder: use forward_kinematics(q) and distance checks vs obstacles.
    Return True if the robot at configuration q intersects any obstacle.
    """
    raise NotImplementedError

def edge_collision_free(q1, q2, obstacles, step=0.05):
    d = distance(q1, q2)
    if d == 0.0:
        return not in_collision(q1, obstacles)
    steps = int(np.ceil(d / step))
    for k in range(steps + 1):
        alpha = k / max(steps, 1)
        q = (1.0 - alpha) * q1 + alpha * q2
        if in_collision(q, obstacles):
            return False
    return True

def steer(q_from, q_to, eta):
    d = distance(q_from, q_to)
    if d <= eta:
        return q_to
    alpha = eta / d
    return q_from + alpha * (q_to - q_from)

def nearest(nodes, q_rand):
    d_min = float("inf")
    idx_min = -1
    for i, node in enumerate(nodes):
        d = distance(node.q, q_rand)
        if d < d_min:
            d_min = d
            idx_min = i
    return idx_min

def near_indices(nodes, q_new, gamma, eta):
    n = len(nodes)
    if n == 0:
        return []
    r = min(eta, gamma * (np.log(n + 1) / (n + 1)) ** (1.0 / 6.0))
    idxs = []
    for i, node in enumerate(nodes):
        if distance(node.q, q_new) <= r:
            idxs.append(i)
    return idxs

def extract_path(nodes, idx_goal):
    path = []
    idx = idx_goal
    while idx != -1:
        path.append(nodes[idx].q)
        idx = nodes[idx].parent
    path.reverse()
    return np.array(path)

def rrt_star(q_start, q_goal, obstacles,
             eta=0.3, gamma=2.0,
             max_iter=5000, goal_radius=0.1):
    nodes = [Node(q=np.array(q_start, dtype=float), parent=-1, cost=0.0)]
    goal_index = None

    for it in range(max_iter):
        q_rand = sample_config()
        i_near = nearest(nodes, q_rand)
        q_near = nodes[i_near].q
        q_new = steer(q_near, q_rand, eta)

        if in_collision(q_new, obstacles):
            continue
        if not edge_collision_free(q_near, q_new, obstacles):
            continue

        # Choose parent with minimal cost-to-come
        idxs_near = near_indices(nodes, q_new, gamma, eta)
        cost_best = nodes[i_near].cost + distance(nodes[i_near].q, q_new)
        parent_best = i_near
        for j in idxs_near:
            if not edge_collision_free(nodes[j].q, q_new, obstacles):
                continue
            c = nodes[j].cost + distance(nodes[j].q, q_new)
            if c < cost_best:
                cost_best = c
                parent_best = j

        nodes.append(Node(q=q_new, parent=parent_best, cost=cost_best))
        i_new = len(nodes) - 1

        # Rewire neighbors through q_new if cheaper
        for j in idxs_near:
            if j == parent_best:
                continue
            if not edge_collision_free(q_new, nodes[j].q, obstacles):
                continue
            c_through_new = nodes[i_new].cost + distance(q_new, nodes[j].q)
            if c_through_new + 1e-9 < nodes[j].cost:
                nodes[j].parent = i_new
                nodes[j].cost = c_through_new

        # Check goal
        if distance(q_new, q_goal) <= goal_radius:
            if edge_collision_free(q_new, q_goal, obstacles):
                nodes.append(
                    Node(q=np.array(q_goal, dtype=float),
                         parent=i_new,
                         cost=nodes[i_new].cost + distance(q_new, q_goal))
                )
                goal_index = len(nodes) - 1
                break

    if goal_index is None:
        return None  # planning failed
    return extract_path(nodes, goal_index)

def prm(q_start, q_goal, obstacles,
        n_samples=1000, k_neighbors=10):
    """
    Simple PRM: uniform sampling, k-nearest neighbors with local collision check.
    Returns a list of configurations forming a path, or None.
    """
    samples = []
    for _ in range(n_samples):
        q = sample_config()
        if not in_collision(q, obstacles):
            samples.append(q)
    samples = [np.array(q, dtype=float) for q in samples]

    # build adjacency list
    n = len(samples)
    adj = [[] for _ in range(n)]

    for i in range(n):
        # find k nearest neighbors
        dists = [(j, distance(samples[i], samples[j])) for j in range(n) if j != i]
        dists.sort(key=lambda p: p[1])
        for j, d in dists[:k_neighbors]:
            if edge_collision_free(samples[i], samples[j], obstacles):
                adj[i].append(j)
                adj[j].append(i)

    # include start and goal as extra nodes
    nodes = [np.array(q_start, dtype=float)] + samples + [np.array(q_goal, dtype=float)]
    start_idx = 0
    goal_idx = len(nodes) - 1
    adj_ext = [[] for _ in range(len(nodes))]
    # copy PRM adjacency
    for i in range(n):
        for j in adj[i]:
            adj_ext[i + 1].append(j + 1)
    # connect start and goal to nearest neighbors
    def connect_special(idx_special, q_special):
        dists = [(i + 1, distance(q_special, samples[i])) for i in range(n)]
        dists.sort(key=lambda p: p[1])
        for j, d in dists[:k_neighbors]:
            if edge_collision_free(q_special, samples[j - 1], obstacles):
                adj_ext[idx_special].append(j)
                adj_ext[j].append(idx_special)

    connect_special(start_idx, nodes[start_idx])
    connect_special(goal_idx, nodes[goal_idx])

    # Dijkstra's algorithm
    import heapq
    INF = float("inf")
    dist_arr = [INF] * len(nodes)
    parent = [-1] * len(nodes)
    dist_arr[start_idx] = 0.0
    heap = [(0.0, start_idx)]
    while heap:
        d, u = heapq.heappop(heap)
        if d > dist_arr[u]:
            continue
        if u == goal_idx:
            break
        for v in adj_ext[u]:
            w = distance(nodes[u], nodes[v])
            nd = d + w
            if nd < dist_arr[v]:
                dist_arr[v] = nd
                parent[v] = u
                heapq.heappush(heap, (nd, v))

    if dist_arr[goal_idx] == INF:
        return None

    # reconstruct path
    path = []
    u = goal_idx
    while u != -1:
        path.append(nodes[u])
        u = parent[u]
    path.reverse()
    return np.array(path)
      

For a real manipulator, you would integrate this code with a robot model (e.g., pybullet, pinocchio, or your own DH-based implementation) to implement forward_kinematics and in_collision.

In C++, the Open Motion Planning Library (OMPL) provides efficient implementations of geometric RRT* and PRM. You only need to define the configuration space, bounds, and a state-validity checker for your 6-DOF arm.

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

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

struct Obstacles
{
    // store meshes or primitives here
};

bool isStateValid(const ob::State *state, const Obstacles &obs)
{
    const auto *q = state->as<ob::RealVectorStateSpace::StateType>();
    // Convert OMPL state to joint vector
    std::array<double, 6> q_vec;
    for (std::size_t i = 0; i < 6; ++i)
        q_vec[i] = (*q)[i];

    // TODO: forward kinematics and collision checking for q_vec vs obs
    // return false if in collision
    return true;
}

og::SimpleSetup makeRRTStarPlanner(const Obstacles &obs,
                                   const std::array<double, 6> &qMin,
                                   const std::array<double, 6> &qMax)
{
    auto space = std::make_shared<ob::RealVectorStateSpace>(6);
    ob::RealVectorBounds bounds(6);
    for (std::size_t i = 0; i < 6; ++i)
    {
        bounds.setLow(i, qMin[i]);
        bounds.setHigh(i, qMax[i]);
    }
    space->setBounds(bounds);

    og::SimpleSetup ss(space);
    ss.setStateValidityChecker(
        [&obs](const ob::State *state)
        {
            return isStateValid(state, obs);
        });

    auto planner = std::make_shared<og::RRTstar>(ss.getSpaceInformation());
    ss.setPlanner(planner);
    return ss;
}

og::SimpleSetup makePRMPlanner(const Obstacles &obs,
                               const std::array<double, 6> &qMin,
                               const std::array<double, 6> &qMax)
{
    auto space = std::make_shared<ob::RealVectorStateSpace>(6);
    ob::RealVectorBounds bounds(6);
    for (std::size_t i = 0; i < 6; ++i)
    {
        bounds.setLow(i, qMin[i]);
        bounds.setHigh(i, qMax[i]);
    }
    space->setBounds(bounds);

    og::SimpleSetup ss(space);
    ss.setStateValidityChecker(
        [&obs](const ob::State *state)
        {
            return isStateValid(state, obs);
        });

    auto planner = std::make_shared<og::PRM>(ss.getSpaceInformation());
    ss.setPlanner(planner);
    return ss;
}

int main()
{
    Obstacles obstacles;
    std::array<double, 6> qMin;
    std::array<double, 6> qMax;

    auto ss = makeRRTStarPlanner(obstacles, qMin, qMax);

    ob::ScopedState<> start(ss.getStateSpace());
    ob::ScopedState<> goal(ss.getStateSpace());
    for (std::size_t i = 0; i < 6; ++i)
    {
        start[i] = 0.0;
        goal[i] = 1.0; // example
    }
    ss.setStartAndGoalStates(start, goal);

    ob::PlannerStatus solved = ss.solve(5.0); // seconds
    if (solved)
    {
        og::PathGeometric path = ss.getSolutionPath();
        path.interpolate();
        path.printAsMatrix(std::cout);
    }
    return 0;
}
      

Replacing og::RRTstar by og::PRM in the setup yields a PRM planner. Libraries such as MoveIt integrate OMPL planners with detailed collision geometry and trajectory execution on real manipulators.

While Java has fewer robotics-specific planning libraries, a PRM can be implemented directly using basic data structures (e.g., ArrayList, adjacency lists). Below is a simplified PRM skeleton for a 6-DOF arm.

import java.util.*;
import java.util.stream.Collectors;

class Config6D {
    double[] q = new double[6];

    Config6D(double[] q) {
        if (q.length != 6) throw new IllegalArgumentException("q must be length 6");
        System.arraycopy(q, 0, this.q, 0, 6);
    }
}

class Edge {
    int u, v;
    double w;
    Edge(int u, int v, double w) {
        this.u = u;
        this.v = v;
        this.w = w;
    }
}

interface CollisionChecker {
    boolean inCollision(double[] q);
    boolean edgeCollisionFree(double[] q1, double[] q2);
}

public class PRM6D {
    private final double[] qMin;
    private final double[] qMax;
    private final CollisionChecker checker;
    private final Random rng = new Random();

    public PRM6D(double[] qMin, double[] qMax, CollisionChecker checker) {
        if (qMin.length != 6 || qMax.length != 6)
            throw new IllegalArgumentException("Joint limits must be length 6");
        this.qMin = qMin.clone();
        this.qMax = qMax.clone();
        this.checker = checker;
    }

    private double[] sampleConfig() {
        double[] q = new double[6];
        for (int i = 0; i < 6; ++i) {
            q[i] = qMin[i] + (qMax[i] - qMin[i]) * rng.nextDouble();
        }
        return q;
    }

    private double distance(double[] q1, double[] q2) {
        double s = 0.0;
        for (int i = 0; i < 6; ++i) {
            double d = q1[i] - q2[i];
            s += d * d;
        }
        return Math.sqrt(s);
    }

    public List<double[]> plan(double[] qStart, double[] qGoal,
                               int nSamples, int kNeighbors) {
        List<Config6D> nodes = new ArrayList<>();
        // Add PRM samples
        for (int i = 0; i < nSamples; ++i) {
            double[] q = sampleConfig();
            if (!checker.inCollision(q)) {
                nodes.add(new Config6D(q));
            }
        }
        // Add start and goal
        int startIdx = nodes.size();
        nodes.add(new Config6D(qStart.clone()));
        int goalIdx = nodes.size();
        nodes.add(new Config6D(qGoal.clone()));

        int n = nodes.size();
        List<List<Edge>> adj = new ArrayList<>(n);
        for (int i = 0; i < n; ++i) adj.add(new ArrayList<>());

        // Connect PRM nodes
        for (int i = 0; i < n - 2; ++i) {
            List<int[]> neigh = new ArrayList<>();
            for (int j = 0; j < n - 2; ++j) {
                if (i == j) continue;
                double d = distance(nodes.get(i).q, nodes.get(j).q);
                neigh.add(new int[]{j, Double.valueOf(d).hashCode()});
            }
            // sort by distance
            neigh.sort(Comparator.comparingInt(a -> a[1]));
            int count = 0;
            for (int[] pair : neigh) {
                if (count >= kNeighbors) break;
                int j = pair[0];
                if (checker.edgeCollisionFree(nodes.get(i).q, nodes.get(j).q)) {
                    double w = distance(nodes.get(i).q, nodes.get(j).q);
                    adj.get(i).add(new Edge(i, j, w));
                    adj.get(j).add(new Edge(j, i, w));
                    count++;
                }
            }
        }

        // Connect start and goal to nearest neighbors
        connectSpecial(nodes, adj, startIdx, kNeighbors);
        connectSpecial(nodes, adj, goalIdx, kNeighbors);

        // Dijkstra shortest path
        double[] dist = new double[n];
        int[] parent = new int[n];
        Arrays.fill(dist, Double.POSITIVE_INFINITY);
        Arrays.fill(parent, -1);
        dist[startIdx] = 0.0;

        PriorityQueue<int[]> pq =
            new PriorityQueue<>(Comparator.comparingDouble(a -> dist[a[0]]));
        pq.add(new int[]{startIdx});

        while (!pq.isEmpty()) {
            int u = pq.poll()[0];
            if (u == goalIdx) break;
            for (Edge e : adj.get(u)) {
                int v = e.v;
                double nd = dist[u] + e.w;
                if (nd < dist[v]) {
                    dist[v] = nd;
                    parent[v] = u;
                    pq.add(new int[]{v});
                }
            }
        }

        if (!Double.isFinite(dist[goalIdx])) {
            return null;
        }

        List<double[]> path = new ArrayList<>();
        int idx = goalIdx;
        while (idx != -1) {
            path.add(nodes.get(idx).q);
            idx = parent[idx];
        }
        Collections.reverse(path);
        return path;
    }

    private void connectSpecial(List<Config6D> nodes,
                                List<List<Edge>> adj,
                                int idxSpecial, int kNeighbors) {
        int n = nodes.size();
        List<double[]> candidates = new ArrayList<>();
        for (int i = 0; i < n - 2; ++i) {
            if (i == idxSpecial) continue;
            candidates.add(new double[]{i, distance(nodes.get(idxSpecial).q, nodes.get(i).q)});
        }
        candidates.sort(Comparator.comparingDouble(a -> a[1]));
        int count = 0;
        for (double[] pair : candidates) {
            if (count >= kNeighbors) break;
            int j = (int) pair[0];
            if (checker.edgeCollisionFree(nodes.get(idxSpecial).q, nodes.get(j).q)) {
                double w = pair[1];
                adj.get(idxSpecial).add(new Edge(idxSpecial, j, w));
                adj.get(j).add(new Edge(j, idxSpecial, w));
                count++;
            }
        }
    }
}
      

The CollisionChecker interface allows you to plug in a Java binding to an existing physics or geometry engine. This design mirrors the separation between planning and environment modeling emphasized throughout the course.

MATLAB's Robotics System Toolbox provides rigidBodyTree models, collision checking, and visualization, which you can combine with your own RRT*/PRM implementation. The snippet below illustrates a simple PRM for a 6-DOF manipulator; Simulink can then be used to execute the resulting joint-space trajectory.

% Define 6-DOF rigidBodyTree robot (e.g., from URDF)
robot = importrobot("my6dof.urdf");
robot.DataFormat = "row";
robot.Gravity = [0 0 -9.81];

% Joint limits
qlim = robot.homeConfiguration;
nJ = numel(qlim);
qMin = zeros(1, nJ);
qMax = zeros(1, nJ);
for i = 1:nJ
    qMin(i) = qlim(i).JointPositionLimits(1);
    qMax(i) = qlim(i).JointPositionLimits(2);
end

% Obstacles (example: sphere at position p with radius r)
obsCenter = [0.5 0.0 0.3];
obsRadius = 0.15;

function coll = inCollision(robot, q, obsCenter, obsRadius)
    % Simple point-sampling collision check on end-effector
    eePose = getTransform(robot, q, robot.BodyNames{end});
    p = eePose(1:3, 4).';
    coll = norm(p - obsCenter) <= obsRadius;
end

function ok = edgeCollisionFree(robot, q1, q2, obsCenter, obsRadius, step)
    if nargin < 6, step = 0.05; end
    dq = q2 - q1;
    L = norm(dq, 2);
    nSteps = ceil(L / step);
    ok = true;
    for k = 0:nSteps
        alpha = k / max(nSteps, 1);
        q = q1 + alpha * dq;
        if inCollision(robot, q, obsCenter, obsRadius)
            ok = false;
            return;
        end
    end
end

% PRM parameters
nSamples = 800;
kNeighbors = 10;
samples = zeros(nSamples, nJ);
validMask = false(nSamples, 1);

for i = 1:nSamples
    q = qMin + (qMax - qMin).*rand(1, nJ);
    if ~inCollision(robot, q, obsCenter, obsRadius)
        samples(i, :) = q;
        validMask(i) = true;
    end
end
samples = samples(validMask, :);
n = size(samples, 1);

% Build adjacency
adj = cell(n, 1);
for i = 1:n
    qi = samples(i, :);
    d = vecnorm((samples - qi).');
    [~, idx] = sort(d);
    cnt = 0;
    for j = idx(2:end)
        if cnt >= kNeighbors, break; end
        if edgeCollisionFree(robot, qi, samples(j, :), obsCenter, obsRadius)
            w = norm(samples(j, :) - qi);
            adj{i}(end+1, :) = [j, w]; %#ok<AGROW>
            adj{j}(end+1, :) = [i, w]; %#ok<AGROW>
            cnt = cnt + 1;
        end
    end
end

% Start/goal and graph search omitted for brevity:
% - Insert q_start and q_goal as additional nodes
% - Connect to neighbors via edgeCollisionFree
% - Run Dijkstra to get sequence of joint configurations pathQ

% To execute pathQ in Simulink:
% 1. Create a timeseries object tsQ = timeseries(pathQ, t);
% 2. Feed tsQ into a "Joint-Space Trajectory" block driving the robot model.
      

For RRT*, you would similarly implement tree expansion and rewiring in MATLAB, using the same inCollision and edgeCollisionFree primitives. Simulink then becomes a convenient platform for testing closed-loop tracking of the planned trajectories.

Mathematica is well suited for rapid prototyping of PRM via its graph and optimization primitives. The following code constructs a simple PRM in a 6D configuration space with a user-defined collision checker.

(* Joint limits *)
qMin = {-Pi, -Pi, -Pi, -Pi, -Pi, -Pi};
qMax = { Pi,  Pi,  Pi,  Pi,  Pi,  Pi};

sampleConfig[] :=
  Table[
    RandomReal[{qMin[[i]], qMax[[i]]}],
    {i, 1, 6}
  ];

distance[q1_List, q2_List] :=
  Sqrt[Total[(q1 - q2)^2]];

(* Placeholder collision tests; plug in your own robot model *)
inCollision[q_List] := False;

edgeCollisionFree[q1_List, q2_List, step_: 0.05] :=
 Module[{d = distance[q1, q2], n, k, alpha, q},
  If[d == 0.0, Return[Not@inCollision[q1]]];
  n = Ceiling[d/step];
  For[k = 0, k <= n, k++,
    alpha = N[k/Max[n, 1]];
    q = (1.0 - alpha) q1 + alpha q2;
    If[inCollision[q], Return[False]];
  ];
  True
];

buildPRM[nSamples_Integer, kNeighbors_Integer] :=
 Module[{samples, valid, n, edges, i, j, dists, sorted, cnt, G},
  samples = Table[sampleConfig[], {nSamples}];
  valid = Select[samples, Not@*inCollision];
  n = Length[valid];
  edges = {};

  For[i = 1, i <= n, i++,
    dists = Table[{j, distance[valid[[i]], valid[[j]]]},
                  {j, 1, n}];
    sorted = SortBy[DeleteCases[dists, {i, _}], Last];
    cnt = 0;
    Do[
      If[cnt >= kNeighbors, Break[]];
      j = sorted[[m, 1]];
      If[edgeCollisionFree[valid[[i]], valid[[j]]],
        AppendTo[edges, UndirectedEdge[i, j]];
        cnt++;
      ],
      {m, 1, Length[sorted]}
    ];
  ];
  G = Graph[edges, VertexCoordinates -> Automatic];
  << GraphUtilities`;
  << Combinatorica`;
  << Experimental`;
  << Statistics`DescriptiveStatistics`;
  << Statistics`MultiDescriptiveStatistics`;
  << Statistics`HypothesisTests`;
  << Statistics`NonlinearFit`;
  << Statistics`ConfidenceIntervals`;
  << Statistics`LinearRegression`;
  << LinearAlgebra`MatrixManipulation`;
  << Optimization`UnconstrainedProblems`;
  << Optimization`ConstrainedProblems`;
  << Optimization`QuadraticProgramming`;
  << Combinatorica`;

  << GraphUtilities`;
  << Experimental`;
  << Statistics`DescriptiveStatistics`;
  << Statistics`MultiDescriptiveStatistics`;

  << GraphUtilities`;

  Association[
    "Graph" -> G,
    "Samples" -> valid
  ]
];

(* After building PRM, add start and goal nodes and use FindShortestPath
   to get the sequence of indices, which can be mapped back to joint vectors. *)
      

For a complete implementation, you should replace inCollision with a function that uses your manipulator's forward kinematics and geometry, and then solve shortest paths using FindShortestPath on the PRM graph.

Problem 1 (RRT* Connection Radius in 6D): In general, under mild regularity assumptions on the free space, asymptotically optimal RRT* in dimension \( d \) chooses radius \( r_n = \gamma (\log n / n)^{1/d} \) with \( \gamma \) larger than a critical constant depending on the volume of \( \mathcal{C}_{\mathrm{free}} \). Specialize this expression to the 6-DOF manipulator, and explain how decreasing \( \gamma \) affects convergence and performance.

Solution:

\[ d = 6 \quad \Rightarrow \quad r_n = \gamma \Big( \tfrac{\log n}{n} \Big)^{1/6}. \]

For fixed \( n \), a smaller \( \gamma \) reduces the neighborhood radius, which decreases the number of neighbors considered in each iteration, thus reducing per-iteration complexity. However, if \( \gamma \) is chosen below the theoretical threshold, the random geometric graph induced by the RRT* rewiring may fail to contain near-optimal connections with high probability, and the algorithm may converge to a suboptimal path. Larger \( \gamma \) improves the likelihood of asymptotically optimal behavior but at the expense of more collision checks.

Problem 2 (Continuity of Workspace Path): Let \( f : \mathcal{C} \to \mathbb{R}^{3} \) be the end-effector position map of a 6-DOF manipulator, assumed continuous in \( q \). Consider the joint-space interpolation

\[ \sigma(t) = (1-t) q_0 + t q_1,\quad t \in [0,1]. \]

Show that the end-effector path \( x(t) = f(\sigma(t)) \) is continuous in \( t \). Why is this property important for the correctness of collision checking along straight-line segments?

Solution:

The map \( t \mapsto \sigma(t) \) is continuous because it is an affine combination of \( q_0 \) and \( q_1 \). Composition of continuous functions is continuous, so \( x(t) = f(\sigma(t)) \) is continuous in \( t \). This implies that if there exist \( t_a < t_b \) such that \( x(t_a) \) and \( x(t_b) \) are both collision-free and the clearance function is continuous, then any collision along the segment must occur within some intermediate interval. Therefore, by sampling sufficiently densely in \( t \), we can detect collisions along the segment with arbitrary precision, justifying the discretized edge checks used in RRT*/PRM.

Problem 3 (Expected Degree in 6D PRM): Assume that \( \mathcal{C}_{\mathrm{free}} \) is a full hyper-rectangle of volume \( V \) in \( \mathbb{R}^6 \), and that PRM samples \( n \) configurations i.i.d. uniformly. Using a radius-based connection strategy with radius \( r \), approximate the expected degree of a node in the roadmap. Use the volume of a 6D ball

\[ \operatorname{Vol}_6(r) = \frac{\pi^3}{6} r^6. \]

Solution:

For large \( n \), the expected number of neighbors of a given node is approximately the number of other points that fall in the 6D ball of radius \( r \) centered at that node, that is

\[ \mathbb{E}[\deg] \approx (n - 1) \frac{\operatorname{Vol}_6(r)}{V} = (n - 1) \frac{\pi^3 r^6}{6 V}. \]

This shows the strong dependence of graph density on the radius \( r \) in high dimensions. For fixed \( n \) and \( V \), small changes in \( r \) dramatically change the expected degree and thus affect connectivity and planning success.

Problem 4 (Discretization Step Along Edges): Suppose every collision-free configuration has clearance at least \( \epsilon \gt 0 \) from obstacles, and the mapping from configuration to any point on the robot is globally Lipschitz with constant \( L \) (i.e., moving by \( \Delta q \) in joint space moves any point on the robot by at most \( L \lVert \Delta q \rVert_2 \) in workspace). Show that if the discretization step in joint space satisfies \( \delta \le \epsilon / L \), then checking collisions on each sample along a straight-line edge guarantees that the entire edge is collision-free.

Solution:

Consider two consecutive configurations along the edge, \( q \) and \( q' \), such that \( \lVert q - q' \rVert_2 \le \delta \). For any intermediate configuration on the segment \( \tilde{q} = (1-\alpha) q + \alpha q' \), the Lipschitz property implies that each point on the robot moves by at most \( L \lVert q - q' \rVert_2 \le L \delta \) from its position at \( q \). If \( q \) has clearance at least \( \epsilon \), then every intermediate configuration has clearance at least \( \epsilon - L \delta \). If we enforce \( \delta \le \epsilon / L \), this quantity is nonnegative, so no intermediate configuration can collide with obstacles. Applying this argument segment-wise along the edge shows that the entire continuous edge is collision-free if all samples are.

In this lab you instantiated asymptotically optimal sampling-based planners (RRT* and PRM) for a 6-DOF manipulator in joint space. You formulated the configuration space and free space for the arm, built collision checking around your existing kinematic model, and implemented the metric, sampling, and steering primitives that both planners share. You then realized RRT* and PRM in several languages and toolchains (Python, C++, Java, MATLAB/Simulink, Wolfram Mathematica), emphasizing the separation between environment modeling and planner logic. These implementations provide a foundation for integrating sampling-based planners into more advanced manipulator planning pipelines in later chapters.