Let the configuration space be \( \mathcal{C} \subset \mathbb{R}^d \), partitioned as \( \mathcal{C} = \mathcal{C}_{\text{free}} \cup \mathcal{C}_{\text{obs}} \), where \( \mathcal{C}_{\text{free}} \) is the set of collision-free configurations (with respect to known obstacles in the workspace) and \( \mathcal{C}_{\text{obs}} \) the set of configurations in collision. A motion planning query is specified by start and goal configurations \( q_{\text{start}}, q_{\text{goal}} \in \mathcal{C}_{\text{free}} \).

A Probabilistic Roadmap is a random geometric graph \( G_n = (V_n, E_n) \) built in \( \mathcal{C}_{\text{free}} \):

• Sampling: draw i.i.d. samples \( q_1,\dots,q_n \sim \rho \) supported on \( \mathcal{C}_{\text{free}} \), where \( \rho \) is typically uniform or biased by heuristics.
• Connection: connect pairs \( (q_i, q_j) \) if they are within a distance threshold or are among each other's \( k \) nearest neighbors, and a local planner finds a collision-free path segment between them.
• Query: insert \( q_{\text{start}}, q_{\text{goal}} \) into \( G_n \), connect them locally, and run a graph search algorithm (e.g. Dijkstra or A* with a metric from Chapter 2) over \( G_n \).

Conceptually, PRM decouples global connectivity (captured by a sparse random graph) from local collision checking. Collision checking is expensive, but each test is independent and thus parallelizable.

flowchart TD
  S["Sample n configs in C_free"] --> C1["Connect neighbors by local planner"]
  C1 --> G["Roadmap graph G_n = (V_n, E_n)"]
  G --> Qs["Add q_start and q_goal"]
  Qs --> GS["Graph search (e.g., Dijkstra, A*)"]
  GS --> P["Path as sequence of configurations"]
        

PRM is a multi-query method: the roadmap \( G_n \) can be reused for multiple start/goal pairs in the same environment, amortizing the cost of roadmap construction over many planning queries.

We now formalize PRM in terms of random geometric graphs. Let \( \mu \) be the Lebesgue measure on \( \mathcal{C}_{\text{free}} \), normalized to a probability measure when convenient:

\[ \mu_{\text{free}}(A) = \frac{\mu(A \cap \mathcal{C}_{\text{free}})}{\mu(\mathcal{C}_{\text{free}})}, \quad A \subset \mathcal{C}. \]

Let \( d(\cdot,\cdot) \) be a metric in configuration space (e.g. weighted Euclidean, or a metric defined via joint-space norms introduced in Chapter 1). For a given sample set \( V_n = \{q_1,\dots,q_n\} \subset \mathcal{C}_{\text{free}} \), define the candidate neighbor set for each node \( q_i \) as

\[ \mathcal{N}_r(q_i) = \{ q_j \in V_n \setminus \{q_i\} : d(q_i, q_j) \le r_n \}, \]

where \( r_n > 0 \) is a (possibly \( n \)-dependent) connection radius. Alternatively, we may use a \( k_n \)-nearest neighbor scheme with \( k_n \) growing slowly with \( n \).

A local planner is a deterministic mapping

\[ \mathsf{Local} : \mathcal{C}_{\text{free}} \times \mathcal{C}_{\text{free}} \rightarrow \{\text{continuous paths } \sigma:[0,1]\rightarrow \mathcal{C}\}, \]

such that \( \sigma(0) = q_i \) and \( \sigma(1) = q_j \). In practice, \( \mathsf{Local}(q_i,q_j) \) is often the straight-line interpolation in configuration space:

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

An edge \( (q_i,q_j) \) is accepted iff \( \sigma(t) \in \mathcal{C}_{\text{free}} \) for all \( t \in [0,1] \). In practice one samples \( t_1,\dots,t_M \) along the segment and performs discrete collision checks in the workspace using the known kinematics of the robot.

The standard PRM algorithm can be summarized as:

1. Sample \( n \) configurations in \( \mathcal{C}_{\text{free}} \) using a suitable density \( \rho \).
2. For each \( q_i \), form \( \mathcal{N}_r(q_i) \) or the \( k_n \) nearest neighbors with respect to \( d \).
3. For each neighbor \( q_j \in \mathcal{N}_r(q_i) \), call the local planner and perform collision checking; if the local path is collision-free, add edge \( (q_i,q_j) \) to \( E_n \).
4. Insert \( q_{\text{start}} \) and \( q_{\text{goal}} \), connect them locally to nearby roadmap nodes, and run graph search.

In Lesson 1, probabilistic completeness (PC) was introduced informally: a planner is PC if the probability it fails to find a solution, when one exists, tends to zero as the number of samples grows. For PRM we make this precise under a robust feasibility assumption.

A continuous path \( \sigma : [0,1] \rightarrow \mathcal{C}_{\text{free}} \) is said to have clearance \( \delta > 0 \) if for all \( t \in [0,1] \),

\[ \operatorname{dist}(\sigma(t), \mathcal{C}_{\text{obs}}) = \inf_{q \in \mathcal{C}_{\text{obs}}} d(\sigma(t), q) \ge \delta. \]

A planning problem is robustly feasible if there exists at least one path from \( q_{\text{start}} \) to \( q_{\text{goal}} \) with clearance \( \delta > 0 \).

Theorem (PC of PRM, sketch): Assume:

• \( \mathcal{C}_{\text{free}} \) is an open subset of \( \mathbb{R}^d \) with finite measure.
• The problem is robustly feasible with path clearance \( \delta > 0 \).
• The connection radius \( r_n \) satisfies \( r_n \rightarrow 0 \) and \( n r_n^d / \log n \rightarrow \infty \) as \( n \rightarrow \infty \).

Then PRM is probabilistically complete: the probability that the roadmap contains a collision-free path from \( q_{\text{start}} \) to \( q_{\text{goal}} \) converges to 1 as \( n \rightarrow \infty \).

Proof sketch:

1. Covering the robust path. Take a robustly feasible path \( \sigma \) with clearance \( \delta \). Parameterize it by arc length and cover its image with a finite sequence of balls \( B_1,\dots,B_M \) of radius \( \varepsilon \in (0,\delta) \), each contained in \( \mathcal{C}_{\text{free}} \), such that consecutive balls overlap:

\[ B_i = B(\sigma(t_i), \varepsilon), \quad B_i \cap B_{i+1} \ne \emptyset, \quad i = 1,\dots,M-1. \]

2. Sampling in each ball. For each ball \( B_i \), define \( p_i = \mu_{\text{free}}(B_i) \), which is strictly positive. The probability no sample falls in \( B_i \) is

\[ \mathbb{P}[\text{no sample in } B_i] = (1 - p_i)^n \le \exp(-n p_i). \]

3. Union bound over all balls. By the union bound,

\[ \mathbb{P}[\exists\, i \text{ with no sample in } B_i] \le \sum_{i=1}^M \exp(-n p_i) \le M \exp(-n p_{\min}), \]

where \( p_{\min} = \min_i p_i > 0 \). Thus this probability decays exponentially in \( n \).

4. Connectivity of neighboring balls. If each \( B_i \) contains at least one sampled configuration and the connection radius \( r_n \) is larger than the overlap distance between consecutive balls (for large enough \( n \)), then the roadmap contains a path that follows the centers through overlapping balls. The scaling condition on \( r_n \) ensures that such local connections exist with high probability (a standard result from random geometric graph theory).

Combining these steps shows that the PRM converges (in probability) to a graph that includes a discrete approximation of the robust path.

PRM includes several tunable parameters: \( n \) (number of samples), \( r_n \) or \( k_n \) (connection threshold), and local planner resolution (number of intermediate collision checks).

For a configuration space of intrinsic dimension \( d \), random geometric graph results suggest a scaling for the connection radius of the form

\[ r_n = \gamma \left( \frac{\log n}{n} \right)^{1/d}, \quad \gamma > \gamma^\ast(d), \]

where \( \gamma^\ast(d) \) is a dimension-dependent constant that guarantees connectivity with high probability. In practice:

• Too small \( r_n \) yields a disconnected roadmap, especially in narrow passages.
• Too large \( r_n \) raises the degree of each node and increases collision checking cost quadratically in \( n \).

Using a \( k_n \)-nearest neighbor scheme yields expected node degree proportional to \( k_n \). To maintain PC, one takes \( k_n \) growing at least on the order of \( \log n \). As a rule of thumb:

• \( n \) proportional to the complexity of the free space (e.g., number of obstacle features).
• \( k_n \approx c \log n \) with \( c \) between 5 and 20 in moderate dimensions.

The dominant cost in PRM is collision checking: for each candidate edge \( (q_i,q_j) \) we may perform dozens of workspace collision tests using the robot's forward kinematics and geometry model. Lazy-PRM reduces the number of such checks by postponing them until they are strictly necessary.

In standard PRM, collision checks are performed during roadmap construction: many checked edges may never appear in any query path. Lazy-PRM delays edge validation until a candidate path is requested, dramatically reducing collision checking on easy instances.

The key idea is to build a hypothetical roadmap where all edges are initially considered feasible; collision checks are only performed on edges that lie on a candidate path returned by graph search.

Lazy-PRM construction (single-query view):

1. Sample \( n \) configurations in \( \mathcal{C}_{\text{free}} \) and connect neighbors based purely on distance (no collision checking yet).
2. Insert \( q_{\text{start}} \) and \( q_{\text{goal}} \), connect them to neighbors (still without collision checks).
3. Run a shortest-path search (e.g. with edge costs equal to \( d(q_i, q_j) \)) to obtain a candidate path \( \pi = (q_{\text{start}},\dots,q_{\text{goal}}) \).
4. Validate edges of \( \pi \) in sequence using the local planner and collision checking:
   • If all edges are valid, return \( \pi \).
   • If an invalid edge \( (q_i,q_j) \) is found, remove it permanently from the roadmap, and re-run the shortest-path search. Repeat until a valid path is found or no path exists.

flowchart TD
  R0["Build roadmap G with edges assumed valid"] --> Q["Run shortest-path search"]
  Q --> P0["Candidate path pi"]
  P0 --> V1["Validate edges of pi"]
  V1 -->|all valid| OUT["Return feasible path"]
  V1 -->|invalid edge found| REM["Remove invalid edge from G"]
  REM --> Q
        

If the underlying sample set and neighbor structure satisfy the same conditions as in the standard PRM, Lazy-PRM remains probabilistically complete: it is essentially a different strategy for when to pay the collision-checking cost, without altering the limiting graph structure.

The worst-case complexity (when many candidate paths fail) is comparable to standard PRM, but empirically the number of collision checks can be much smaller, particularly in settings where valid paths are relatively short or where many edges are never explored during queries.

We implement a simple 2D PRM / Lazy-PRM planner using NumPy and SciPy data structures. In practical robotics systems, planners are often accessed via OMPL (Open Motion Planning Library) Python bindings or MoveIt (for manipulators), but here we build a minimal educational implementation.

import numpy as np
from heapq import heappush, heappop

# ---------- Geometry and collision model (2D example) ----------

def sample_free(n_samples, bounds, obstacles):
    """
    Uniformly sample configurations in C_free in 2D.
    bounds = ((x_min, x_max), (y_min, y_max))
    obstacles is a list of axis-aligned boxes (x_min, y_min, x_max, y_max).
    """
    xs = np.random.uniform(bounds[0][0], bounds[0][1], size=n_samples * 2)
    ys = np.random.uniform(bounds[1][0], bounds[1][1], size=n_samples * 2)
    pts = []
    for x, y in zip(xs, ys):
        if not in_collision((x, y), obstacles):
            pts.append((x, y))
        if len(pts) == n_samples:
            break
    return np.array(pts)


def in_collision(q, obstacles):
    x, y = q
    for (xmin, ymin, xmax, ymax) in obstacles:
        if xmin <= x <= xmax and ymin <= y <= ymax:
            return True
    return False


def interpolate(q1, q2, n_steps):
    """Straight line interpolation in configuration space."""
    q1 = np.asarray(q1, dtype=float)
    q2 = np.asarray(q2, dtype=float)
    return [q1 + float(t) / float(n_steps) * (q2 - q1) for t in range(n_steps + 1)]


def edge_collision_free(q1, q2, obstacles, n_steps=20):
    """Check discrete collision along segment q1-q2."""
    for q in interpolate(q1, q2, n_steps):
        if in_collision(q, obstacles):
            return False
    return True


def euclidean(q1, q2):
    return float(np.linalg.norm(np.asarray(q1) - np.asarray(q2)))


# ---------- PRM Roadmap construction ----------

def build_prm(samples, r, obstacles):
    """
    Build PRM roadmap.
    samples: (N,2) array of points in C_free.
    r: connection radius.
    Returns adjacency list: graph[i] = list of (j, cost).
    """
    n = samples.shape[0]
    graph = [[] for _ in range(n)]
    for i in range(n):
        for j in range(i + 1, n):
            d = euclidean(samples[i], samples[j])
            if d <= r:
                if edge_collision_free(samples[i], samples[j], obstacles):
                    graph[i].append((j, d))
                    graph[j].append((i, d))
    return graph


def add_node_to_graph(q, samples, graph, r, obstacles):
    """
    Insert a new node q into an existing roadmap.
    Returns new index and updated samples and graph.
    """
    samples = np.vstack([samples, np.asarray(q)])
    idx = samples.shape[0] - 1
    graph.append([])
    for j in range(idx):
        d = euclidean(samples[idx], samples[j])
        if d <= r:
            if edge_collision_free(samples[idx], samples[j], obstacles):
                graph[idx].append((j, d))
                graph[j].append((idx, d))
    return idx, samples, graph


# ---------- Dijkstra shortest path ----------

def dijkstra(graph, start, goal):
    """
    graph: adjacency list with (neighbor, cost).
    start, goal: integer indices.
    """
    n = len(graph)
    dist = [np.inf] * n
    prev = [-1] * n
    dist[start] = 0.0
    pq = [(0.0, start)]
    while pq:
        d, u = heappop(pq)
        if u == goal:
            break
        if d > dist[u]:
            continue
        for v, w in graph[u]:
            nd = d + w
            if nd < dist[v]:
                dist[v] = nd
                prev[v] = u
                heappush(pq, (nd, v))
    if dist[goal] == np.inf:
        return None
    # reconstruct path
    path = []
    u = goal
    while u != -1:
        path.append(u)
        u = prev[u]
    path.reverse()
    return path


# ---------- Lazy-PRM modifications ----------

def build_lazy_prm(samples, r):
    """
    Build a 'hypothetical' roadmap: edges added based on distance only,
    with no collision checking yet.
    """
    n = samples.shape[0]
    graph = [[] for _ in range(n)]
    for i in range(n):
        for j in range(i + 1, n):
            d = euclidean(samples[i], samples[j])
            if d <= r:
                graph[i].append((j, d))
                graph[j].append((i, d))
    return graph


def validate_path_lazy(path, samples, graph, obstacles, n_steps=20):
    """
    Validate edges along a path. If an edge is invalid, remove it from graph.
    Returns (valid, first_invalid_edge) where first_invalid_edge is (u, v) or None.
    """
    for u, v in zip(path[:-1], path[1:]):
        if not edge_collision_free(samples[u], samples[v], obstacles, n_steps=n_steps):
            # remove edge symmetrically
            graph[u] = [(nbr, w) for (nbr, w) in graph[u] if nbr != v]
            graph[v] = [(nbr, w) for (nbr, w) in graph[v] if nbr != u]
            return False, (u, v)
    return True, None


def lazy_prm_query(samples, graph, q_start, q_goal, r, obstacles):
    """
    Lazy-PRM query on pre-sampled roadmap nodes.
    """
    # Add start and goal nodes without collision checking on edges.
    samples = np.vstack([samples, np.asarray(q_start), np.asarray(q_goal)])
    idx_start = samples.shape[0] - 2
    idx_goal = samples.shape[0] - 1
    graph.append([])
    graph.append([])
    # connect start and goal to neighbors by distance only
    for idx in [idx_start, idx_goal]:
        for j in range(idx):
            d = euclidean(samples[idx], samples[j])
            if d <= r:
                graph[idx].append((j, d))
                graph[j].append((idx, d))

    while True:
        path = dijkstra(graph, idx_start, idx_goal)
        if path is None:
            return None  # no path exists
        valid, bad_edge = validate_path_lazy(path, samples, graph, obstacles)
        if valid:
            return [samples[i] for i in path]
        # else loop again with reduced graph


# Example usage (for testing):
if __name__ == "__main__":
    np.random.seed(0)
    bounds = ((0.0, 1.0), (0.0, 1.0))
    obstacles = [(0.3, 0.3, 0.6, 0.7)]
    samples = sample_free(200, bounds, obstacles)
    r = 0.15

    # Standard PRM
    graph = build_prm(samples, r, obstacles)
    q_start = (0.1, 0.1)
    q_goal = (0.9, 0.9)
    idx_start, samples, graph = add_node_to_graph(q_start, samples, graph, r, obstacles)
    idx_goal, samples, graph = add_node_to_graph(q_goal, samples, graph, r, obstacles)
    path_idx = dijkstra(graph, idx_start, idx_goal)
    prm_path = [samples[i] for i in path_idx] if path_idx is not None else None

    # Lazy-PRM
    samples_lazy = sample_free(200, bounds, obstacles)
    graph_lazy = build_lazy_prm(samples_lazy, r)
    lazy_path = lazy_prm_query(samples_lazy, graph_lazy, q_start, q_goal, r, obstacles)
      

In a robotics stack (e.g., using ROS and MoveIt), these functions would be replaced by calls to ompl::geometric::PRM configured through Python, but the underlying principles remain the same.

In C++, PRM is often implemented via OMPL and integrated with ROS and MoveIt. Below is a self-contained 2D implementation that mirrors the Python version and uses STL containers; in practice, one would replace these with ompl::base and ompl::geometric abstractions.

#include <vector>
#include <cmath>
#include <random>
#include <limits>
#include <queue>
#include <utility>

struct Point2D {
    double x;
    double y;
};

struct Box {
    double xmin, ymin, xmax, ymax;
};

using Edge = std::pair<int, double>;
using Graph = std::vector<std::vector<Edge> >;

bool inCollision(const Point2D &q, const std::vector<Box> &obstacles) {
    for (const auto &b : obstacles) {
        if (b.xmin <= q.x && q.x <= b.xmax &&
            b.ymin <= q.y && q.y <= b.ymax) {
            return true;
        }
    }
    return false;
}

double euclidean(const Point2D &a, const Point2D &b) {
    double dx = a.x - b.x;
    double dy = a.y - b.y;
    return std::sqrt(dx * dx + dy * dy);
}

std::vector<Point2D> interpolate(const Point2D &a, const Point2D &b, int nSteps) {
    std::vector<Point2D> path;
    path.reserve(static_cast<std::size_t>(nSteps + 1));
    for (int i = 0; i <= nSteps; ++i) {
        double t = static_cast<double>(i) / static_cast<double>(nSteps);
        Point2D q{ (1.0 - t) * a.x + t * b.x,
                   (1.0 - t) * a.y + t * b.y };
        path.push_back(q);
    }
    return path;
}

bool edgeCollisionFree(const Point2D &a,
                       const Point2D &b,
                       const std::vector<Box> &obstacles,
                       int nSteps = 20) {
    auto path = interpolate(a, b, nSteps);
    for (const auto &q : path) {
        if (inCollision(q, obstacles)) {
            return false;
        }
    }
    return true;
}

Graph buildPRM(const std::vector<Point2D> &samples,
               double r,
               const std::vector<Box> &obstacles) {
    int n = static_cast<int>(samples.size());
    Graph graph(n);
    for (int i = 0; i < n; ++i) {
        for (int j = i + 1; j < n; ++j) {
            double d = euclidean(samples[i], samples[j]);
            if (d <= r) {
                if (edgeCollisionFree(samples[i], samples[j], obstacles)) {
                    graph[i].push_back(std::make_pair(j, d));
                    graph[j].push_back(std::make_pair(i, d));
                }
            }
        }
    }
    return graph;
}

std::vector<int> dijkstra(const Graph &graph, int start, int goal) {
    int n = static_cast<int>(graph.size());
    std::vector<double> dist(n, std::numeric_limits<double>::infinity());
    std::vector<int> prev(n, -1);

    using Node = std::pair<double, int>;
    auto cmp = [](const Node &a, const Node &b) { return a.first > b.first; };
    std::priority_queue<Node, std::vector<Node>, decltype(cmp)> pq(cmp);

    dist[start] = 0.0;
    pq.push(std::make_pair(0.0, start));

    while (!pq.empty()) {
        auto [d, u] = pq.top();
        pq.pop();
        if (u == goal) {
            break;
        }
        if (d > dist[u]) {
            continue;
        }
        for (const auto &edge : graph[u]) {
            int v = edge.first;
            double w = edge.second;
            double nd = d + w;
            if (nd < dist[v]) {
                dist[v] = nd;
                prev[v] = u;
                pq.push(std::make_pair(nd, v));
            }
        }
    }

    if (!std::isfinite(dist[goal])) {
        return {};
    }

    std::vector<int> path;
    for (int u = goal; u != -1; u = prev[u]) {
        path.push_back(u);
    }
    std::reverse(path.begin(), path.end());
    return path;
}

// Lazy-PRM variant: build graph with distance-based edges only, then validate on demand.
Graph buildLazyPRM(const std::vector<Point2D> &samples, double r) {
    int n = static_cast<int>(samples.size());
    Graph graph(n);
    for (int i = 0; i < n; ++i) {
        for (int j = i + 1; j < n; ++j) {
            double d = euclidean(samples[i], samples[j]);
            if (d <= r) {
                graph[i].push_back(std::make_pair(j, d));
                graph[j].push_back(std::make_pair(i, d));
            }
        }
    }
    return graph;
}

bool validatePathLazy(const std::vector<int> &path,
                      const std::vector<Point2D> &samples,
                      Graph &graph,
                      const std::vector<Box> &obstacles,
                      int nSteps = 20) {
    for (std::size_t k = 0; k + 1 < path.size(); ++k) {
        int u = path[k];
        int v = path[k + 1];
        if (!edgeCollisionFree(samples[u], samples[v], obstacles, nSteps)) {
            // remove edge u-v
            auto &Nu = graph[u];
            Nu.erase(std::remove_if(Nu.begin(), Nu.end(),
                                    [v](const Edge &e) { return e.first == v; }),
                     Nu.end());
            auto &Nv = graph[v];
            Nv.erase(std::remove_if(Nv.begin(), Nv.end(),
                                    [u](const Edge &e) { return e.first == u; }),
                     Nv.end());
            return false;
        }
    }
    return true;
}
      

To integrate with OMPL, one would instead construct ompl::base::StateSpace and ompl::geometric::SimpleSetup, then select ompl::geometric::PRM or ompl::geometric::LazyPRM as the planner, while reusing the robot kinematics model and collision checking provided by MoveIt.

Java is less common for motion planning than C++ or Python, but similar graph structures can be used in Java-based robotics stacks (for example, via ROS Java, FRC control frameworks, or Java bindings to native planners). Below is a simple 2D PRM implementation:

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

class Point2D {
    double x, y;
    Point2D(double x, double y) { this.x = x; this.y = y; }
}

class Box {
    double xmin, ymin, xmax, ymax;
    Box(double xmin, double ymin, double xmax, double ymax) {
        this.xmin = xmin; this.ymin = ymin;
        this.xmax = xmax; this.ymax = ymax;
    }
}

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

public class PRMPlanner {

    static boolean inCollision(Point2D q, List<Box> obstacles) {
        for (Box b : obstacles) {
            if (b.xmin <= q.x && q.x <= b.xmax
                    && b.ymin <= q.y && q.y <= b.ymax) {
                return true;
            }
        }
        return false;
    }

    static double euclidean(Point2D a, Point2D b) {
        double dx = a.x - b.x;
        double dy = a.y - b.y;
        return Math.sqrt(dx * dx + dy * dy);
    }

    static List<Point2D> interpolate(Point2D a, Point2D b, int nSteps) {
        List<Point2D> pts = new ArrayList<>(nSteps + 1);
        for (int i = 0; i <= nSteps; ++i) {
            double t = (double) i / (double) nSteps;
            double x = (1.0 - t) * a.x + t * b.x;
            double y = (1.0 - t) * a.y + t * b.y;
            pts.add(new Point2D(x, y));
        }
        return pts;
    }

    static boolean edgeCollisionFree(Point2D a,
                                     Point2D b,
                                     List<Box> obstacles,
                                     int nSteps) {
        for (Point2D q : interpolate(a, b, nSteps)) {
            if (inCollision(q, obstacles)) {
                return false;
            }
        }
        return true;
    }

    static List<List<Edge> > buildPRM(List<Point2D> samples,
                                           double r,
                                           List<Box> obstacles) {
        int n = samples.size();
        List<List<Edge> > graph = new ArrayList<>(n);
        for (int i = 0; i < n; ++i) {
            graph.add(new ArrayList<>());
        }
        for (int i = 0; i < n; ++i) {
            for (int j = i + 1; j < n; ++j) {
                double d = euclidean(samples.get(i), samples.get(j));
                if (d <= r) {
                    if (edgeCollisionFree(samples.get(i), samples.get(j),
                                          obstacles, 20)) {
                        graph.get(i).add(new Edge(j, d));
                        graph.get(j).add(new Edge(i, d));
                    }
                }
            }
        }
        return graph;
    }

    static List<Integer> dijkstra(List<List<Edge> > graph,
                                   int start,
                                   int goal) {
        int n = graph.size();
        double[] dist = new double[n];
        int[] prev = new int[n];
        Arrays.fill(dist, Double.POSITIVE_INFINITY);
        Arrays.fill(prev, -1);
        dist[start] = 0.0;

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

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

        if (!Double.isFinite(dist[goal])) {
            return Collections.emptyList();
        }
        List<Integer> path = new ArrayList<>();
        for (int u = goal; u != -1; u = prev[u]) {
            path.add(u);
        }
        Collections.reverse(path);
        return path;
    }

    public static void main(String[] args) {
        // Example usage
        List<Box> obstacles = List.of(new Box(0.3, 0.3, 0.6, 0.7));
        List<Point2D> samples = new ArrayList<>();
        Random rng = new Random(0);
        while (samples.size() < 200) {
            double x = rng.nextDouble();
            double y = rng.nextDouble();
            Point2D q = new Point2D(x, y);
            if (!inCollision(q, obstacles)) {
                samples.add(q);
            }
        }
        double r = 0.15;
        var graph = buildPRM(samples, r, obstacles);
        // Add start and goal, run Dijkstra, etc.
    }
}
      

In Java-centric robotics frameworks, you would typically wrap native C++ planners (e.g., OMPL) via JNI and use Java code only for configuration and system integration, while reusing efficient collision detection primitives from C++ libraries.

MATLAB has a Robotics System Toolbox that includes PRM functionality (mobileRobotPRM for 2D, and integration with rigid body trees for manipulators). Below is a from-scratch 2D implementation; to integrate with Simulink, place the core planning function inside a MATLAB Function block.

function path = prm_plan_2d(q_start, q_goal, bounds, obstacles, n_samples, r)
% PRM-based planner in 2D (educational).
% bounds: [xmin xmax; ymin ymax]
% obstacles: N x 4, each row [xmin ymin xmax ymax]
    if nargin < 6
        r = 0.15;
    end
    if nargin < 5
        n_samples = 200;
    end

    % Sample free configurations
    samples = sample_free(n_samples, bounds, obstacles);

    % Append start and goal
    samples = [samples; q_start(:).'; q_goal(:).'];
    idx_start = size(samples, 1) - 1;
    idx_goal  = size(samples, 1);

    % Build roadmap with collision checking (standard PRM)
    G = build_prm(samples, r, obstacles);

    % Dijkstra search
    path_idx = dijkstra_graph(G, idx_start, idx_goal);
    if isempty(path_idx)
        path = [];
    else
        path = samples(path_idx, :);
    end
end

function samples = sample_free(n_samples, bounds, obstacles)
    xs = rand(n_samples * 2, 1) * (bounds(1,2) - bounds(1,1)) + bounds(1,1);
    ys = rand(n_samples * 2, 1) * (bounds(2,2) - bounds(2,1)) + bounds(2,1);
    samples = zeros(n_samples, 2);
    c = 0;
    for i = 1:numel(xs)
        q = [xs(i), ys(i)];
        if ~in_collision(q, obstacles)
            c = c + 1;
            samples(c, :) = q;
            if c == n_samples
                break;
            end
        end
    end
end

function flag = in_collision(q, obstacles)
    x = q(1); y = q(2);
    flag = false;
    for i = 1:size(obstacles, 1)
        box = obstacles(i, :);
        if box(1) <= x && x <= box(3) && ...
           box(2) <= y && y <= box(4)
            flag = true;
            return;
        end
    end
end

function G = build_prm(samples, r, obstacles)
    n = size(samples, 1);
    G = cell(n, 1);
    for i = 1:n
        G{i} = [];
    end
    for i = 1:n
        for j = i+1:n
            d = euclidean(samples(i,:), samples(j,:));
            if d <= r
                if edge_collision_free(samples(i,:), samples(j,:), obstacles)
                    G{i} = [G{i}; j, d];
                    G{j} = [G{j}; i, d];
                end
            end
        end
    end
end

function d = euclidean(a, b)
    diff = a - b;
    d = sqrt(sum(diff.^2));
end

function ok = edge_collision_free(a, b, obstacles)
    nSteps = 20;
    ok = true;
    for i = 0:nSteps
        t = i / nSteps;
        q = (1 - t) * a + t * b;
        if in_collision(q, obstacles)
            ok = false;
            return;
        end
    end
end

function path = dijkstra_graph(G, s, g)
    n = numel(G);
    dist = inf(n, 1);
    prev = -ones(n, 1);
    visited = false(n, 1);
    dist(s) = 0;
    for k = 1:n
        % find unvisited node with smallest dist
        [~, u] = min(dist + visited * max(dist) * 2);
        if visited(u)
            break;
        end
        visited(u) = true;
        if u == g
            break;
        end
        neighbors = G{u};
        for i = 1:size(neighbors, 1)
            v = neighbors(i, 1);
            w = neighbors(i, 2);
            if ~visited(v)
                nd = dist(u) + w;
                if nd < dist(v)
                    dist(v) = nd;
                    prev(v) = u;
                end
            end
        end
    end
    if ~isfinite(dist(g))
        path = [];
        return;
    end
    % reconstruct
    path_idx = g;
    u = g;
    while prev(u) ~= -1
        u = prev(u);
        path_idx = [u; path_idx]; %#ok<AGROW>
    end
    path = path_idx;
end
      

In Simulink, this planner can be wrapped in a MATLAB Function block that receives obstacle parameters and start/goal configurations and outputs a sequence of waypoints for a low-level controller or trajectory generator.

Mathematica is well suited for prototyping PRM due to its symbolic capabilities and built-in geometry primitives. Below is a basic implementation of PRM in 2D using lists and pure functions:

ClearAll[InCollisionQ, SampleFree, Euclidean, EdgeCollisionFree,
  BuildPRM, DijkstraPath];

InCollisionQ[{x_, y_}, obstacles_List] :=
  Or @@ (With[{b = #},
      b[[1]] <= x && x <= b[[3]] && b[[2]] <= y && y <= b[[4]]
    ] & /@ obstacles);

SampleFree[n_Integer, { {xmin_, xmax_}, {ymin_, ymax_} }, obstacles_List] :=
 Module[{samples = {}, x, y},
  While[Length[samples] < n,
   {x, y} = {RandomReal[{xmin, xmax}], RandomReal[{ymin, ymax}]};
   If[! InCollisionQ[{x, y}, obstacles],
    AppendTo[samples, {x, y}];
   ];
  ];
  samples
 ]

Euclidean[a_List, b_List] := Sqrt[Total[(a - b)^2]];

EdgeCollisionFree[a_List, b_List, obstacles_List, nSteps_: 20] :=
 Module[{pts, q},
  pts = Table[(1 - t) a + t b, {t, 0., 1., 1./nSteps}];
  Not@Or @@ (InCollisionQ[#, obstacles] & /@ pts)
 ]

BuildPRM[samples_List, r_?NumericQ, obstacles_List] :=
 Module[{n = Length[samples], graph},
  graph = Table[{}, {n}];
  Do[
   Do[
    With[{d = Euclidean[samples[[i]], samples[[j]]]},
     If[d <= r && EdgeCollisionFree[samples[[i]], samples[[j]], obstacles],
      graph[[i]] = Append[graph[[i]], {j, d}];
      graph[[j]] = Append[graph[[j]], {i, d}];
     ];
    ],
    {j, i + 1, n}],
   {i, 1, n}];
  graph
 ]

DijkstraPath[graph_List, s_Integer, g_Integer] :=
 Module[{n = Length[graph],
   dist, prev, visited, u, neighbors, nd},
  dist = Table[Infinity, {n}]; prev = Table[-1, {n}];
  visited = Table[False, {n}];
  dist[[s]] = 0.;
  While[True,
   u = First@Ordering[dist + 10.^6 (visited /. {True -> 1, False -> 0}), 1];
   If[visited[[u]] || u == g, Break[]];
   visited[[u]] = True;
   neighbors = graph[[u]];
   Do[
    nd = dist[[u]] + w;
    If[nd < dist[[v]],
     dist[[v]] = nd; prev[[v]] = u;
    ],
    { {v, w}, neighbors}
   ];
  ];
  If[dist[[g]] === Infinity, Return[{}]];
  (* reconstruct path *)
  Module[{path = {g}, cur = g},
   While[prev[[cur]] != -1,
    cur = prev[[cur]];
    path = Prepend[path, cur];
   ];
   path
  ]
 ]

(* Example usage *)
obstacles = { {0.3, 0.3, 0.6, 0.7} };
bounds = { {0., 1.}, {0., 1.} };
samples = SampleFree[200, bounds, obstacles];
r = 0.15;
graph = BuildPRM[samples, r, obstacles];

qStart = {0.1, 0.1};
qGoal = {0.9, 0.9};
(* Append start and goal *)
samplesExt = Join[samples, {qStart, qGoal}];
idxStart = Length[samplesExt] - 1;
idxGoal = Length[samplesExt];

(* Extend graph accordingly, then call DijkstraPath[graph, idxStart, idxGoal] *)
      

For more advanced applications, Mathematica's RegionIntersection and RegionDistance can be used to build higher-fidelity configuration space representations before applying PRM-style sampling.

Problem 1 (Coverage probability of a ball): Let \( B \subset \mathcal{C}_{\text{free}} \) be a measurable subset with \( \mu_{\text{free}}(B) = p > 0 \). Suppose \( n \) configurations are drawn i.i.d. according to \( \mu_{\text{free}} \). Derive the probability that at least one sample lies in \( B \), and show that this probability tends to 1 as \( n \rightarrow \infty \).

Solution:

The probability that a single sample lies outside \( B \) is \( 1 - p \). Since samples are independent, the probability that all \( n \) samples lie outside \( B \) is

\[ \mathbb{P}[\text{no sample in } B] = (1 - p)^n. \]

Therefore the probability of at least one sample in \( B \) is

\[ \mathbb{P}[\exists \text{ sample in } B] = 1 - (1 - p)^n. \]

Since \( 0 < p \le 1 \), we have \( (1 - p)^n \rightarrow 0 \) as \( n \rightarrow \infty \), so the coverage probability tends to 1. This is the basic building block of the probabilistic completeness argument for PRM.

Problem 2 (Expected node degree in PRM): Assume configurations are sampled uniformly in a region of volume \( V \) in \( \mathbb{R}^d \), and edges are added between points whose Euclidean distance is at most \( r \). Approximate the expected degree of a typical node (ignoring boundary effects).

Solution:

Consider a fixed node \( q \). The probability that another point \( q' \) falls within distance \( r \) is approximately the ratio of the ball volume of radius \( r \) to the total volume \( V \):

\[ \mathbb{P}[d(q,q') \le r] \approx \frac{\zeta_d r^d}{V}, \]

where \( \zeta_d \) is the volume of the unit ball in \( \mathbb{R}^d \). There are \( n-1 \) other nodes, so the expected degree is

\[ \mathbb{E}[\deg(q)] \approx (n-1)\,\frac{\zeta_d r^d}{V} \approx n\,\frac{\zeta_d r^d}{V}, \]

justifying the intuition that degree grows linearly with \( n \) for fixed \( r \), and motivating the scaling \( r_n \propto (\log n / n)^{1/d} \) to keep the degree near logarithmic in \( n \).

Problem 3 (Lazy-PRM correctness): Consider a fixed sample set and neighbor structure defining a hypothetical roadmap \( \tilde{G} \) where some edges are actually invalid (in collision). Standard PRM removes invalid edges during construction; Lazy-PRM removes edges only when they appear in a candidate path. Argue that, given infinite time, Lazy-PRM will find the same set of feasible paths as standard PRM (or correctly report infeasibility).

Solution:

Let \( G \) denote the graph obtained by removing all invalid edges from \( \tilde{G} \). Standard PRM returns paths that are simple paths in \( G \). Lazy-PRM starts from \( \tilde{G} \) and iteratively:

• computes a shortest path \( \pi_k \) in the current graph \( \tilde{G}_k \);
• validates edges of \( \pi_k \), and removes any invalid ones.

Each invalid edge is removed at most once. Thus after a finite number of iterations (bounded by the number of invalid edges), the current graph \( \tilde{G}_K \) equals \( G \), and subsequent shortest-path computations are identical to those performed on \( G \). At that point, Lazy-PRM returns exactly the same set of feasible paths as standard PRM. If no path exists in \( G \), both algorithms report infeasibility.

Problem 4 (Robust clearance and local planner step size): Let \( \sigma \) be a path with clearance \( \delta \). Suppose the local planner discretizes the segment between two roadmap nodes into steps of length at most \( h \). Show that if \( h \le \delta \) and the discrete samples are all collision free, then the continuous path segment is collision free.

Solution:

Take two consecutive points \( q_i, q_{i+1} \) on the discretization, with \( d(q_i, q_{i+1}) \le h \). Consider the continuous interpolation \( \gamma(t) = (1-t)\,q_i + t\,q_{i+1} \), \( t \in [0,1] \). For any \( t \), we have

\[ d(\gamma(t), q_i) \le d(q_i, q_{i+1}) \le h \le \delta. \]

Since \( q_i \) is at clearance at least \( \delta \) from obstacles, the closed ball of radius \( \delta \) around \( q_i \) lies entirely in \( \mathcal{C}_{\text{free}} \). Hence every point on \( \gamma \) lies in that ball and is therefore in \( \mathcal{C}_{\text{free}} \). By repeating this argument across all segments, the full discretized path is collision free in the continuous sense.

Problem 5 (Multi-query vs single-query trade-off): Explain why PRM is typically described as a multi-query planner and Lazy-PRM is often used in a single-query mode. Under what circumstances might you still prefer standard PRM over Lazy-PRM?

Solution:

Roadmap construction (sampling and connecting nodes) is independent of specific start/goal pairs, so a single roadmap can be reused for many queries in the same environment, amortizing its cost. This makes PRM, including Lazy-PRM, inherently multi-query. However, Lazy-PRM's advantage is greatest when collision checking is expensive and only a few queries are needed: it performs only as many collision checks as needed to certify the few paths requested. In contrast, standard PRM pays the collision cost upfront for many edges that may never be used. If a large number of queries must be answered on the same roadmap, or if path costs must be very accurately approximated (requiring many validated edges), standard PRM may be preferable because it avoids the repeated graph-search and edge-validation cycles of Lazy-PRM.

In this lesson we formalized PRM as a random geometric graph over configuration space and sketched its probabilistic completeness under robust feasibility assumptions. We studied how connection radius and sampling density influence connectivity and computational cost, and we introduced Lazy-PRM as a variant that postpones collision checks to candidate paths, preserving completeness while often reducing collision checking effort. Finally, we implemented these ideas in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, connecting them to common robotics libraries such as OMPL and MoveIt. Subsequent lessons will extend these ideas to tree-based planners (RRT family) and asymptotically optimal variants.