Let \( C \subset \mathbb{R}^n \) denote the configuration space of a robot with \( n \) degrees of freedom (DOFs), \( C_{\text{obs}} \) the obstacle region, and \( C_{\text{free}} \) the free region:

\[ C_{\text{free}} = \{\, q \in C \mid \operatorname{collide}(q) = 0 \,\}, \quad C_{\text{obs}} = C \setminus C_{\text{free}}. \]

Exact analytic descriptions of \( C_{\text{free}} \) are extremely rare beyond low-dimensional systems or simple convex geometries. Practical planners must work with approximations:

\[ \hat{C}_{\text{free}}^- \;\subseteq\; C_{\text{free}} \;\subseteq\; \hat{C}_{\text{free}}^+, \]

where \( \hat{C}_{\text{free}}^- \) is a conservative inner approximation (guaranteed collision-free but possibly incomplete) and \( \hat{C}_{\text{free}}^+ \) is an outer approximation (may include some colliding states but contains all truly free configurations).

Typical approximation schemes used in advanced robotics include:

• Grid / voxel discretizations of \( C \) or of a subset of DOFs.
• Cell decompositions (adaptive or structured) with certified free / blocked labels per cell.
• Sampling-based coverings (finite set of samples plus local neighborhoods).
• Bounding volume hierarchies (BVHs) that approximate the robot and obstacles in workspace and induce conservative predicates on \( C \).

At a systems level, a typical C-space approximation pipeline looks like:

flowchart TD
  A["Robot model + workspace geometry"] --> B["Define configuration space C"]
  B --> C["Choose approximation structure: grid / cells / samples"]
  C --> D["Implement collision or distance queries"]
  D --> E["Build inner and outer approximations of C_free"]
  E --> F["Expose interface: isFree(q), neighbors(q), distanceToObs(q)"]
  F --> G["Use by planners (graph / sampling / optimization)"]
        

The rest of this lesson turns these ideas into mathematically precise notions with implementable algorithms and error guarantees.

To reason rigorously about approximations, we need:

• a metric on configurations, and
• a notion of distance between sets.

Assume \( C \) is endowed with a configuration-space metric \( d_C : C \times C \to \mathbb{R}_{\ge 0} \) introduced in Lesson 4. For sets \( A,B \subseteq C \), the Hausdorff distance is

\[ d_H(A,B) = \max\Big( \sup_{a \in A} \inf_{b \in B} d_C(a,b), \sup_{b \in B} \inf_{a \in A} d_C(a,b) \Big). \]

Definition (set ε-approximation). A set \( \hat{S} \subseteq C \) is an \( \varepsilon \)-approximation of \( S \subseteq C \) if

\[ d_H(\hat{S}, S) \le \varepsilon. \]

In practice we rarely control \( d_H \) exactly, but it is a useful theoretical benchmark.

For path planning, the relevant functional is the clearance of a path \( \gamma : [0,1] \to C \),

\[ \operatorname{clear}(\gamma) = \inf_{t \in [0,1]} d_C\big(\gamma(t), C_{\text{obs}}\big). \]

Intuitively, \( \operatorname{clear}(\gamma) \) is the minimum configuration-space distance from the path to any collision configuration. A path of positive clearance is robust to small perturbations.

The key safety guarantee for inner approximations is:

Theorem (safety of inner approximation, sketch). Suppose we have an inner approximation \( \hat{C}_{\text{free}}^- \subseteq C_{\text{free}} \). Any path \( \gamma \) that lies entirely inside \( \hat{C}_{\text{free}}^- \) is collision-free in the true C-space, i.e. \( \gamma([0,1]) \subseteq \hat{C}_{\text{free}}^- \implies \gamma([0,1]) \subseteq C_{\text{free}} \).

Proof (sketch): By definition of inner approximation, \( q \in \hat{C}_{\text{free}}^- \Rightarrow q \in C_{\text{free}} \). Hence if \( \gamma([0,1]) \subseteq \hat{C}_{\text{free}}^- \), every configuration on the path is free, so the path is collision-free. \(\square\)

For outer approximations, we give completeness-style guarantees under clearance. Let \( \hat{C}_{\text{obs}}^+ \supseteq C_{\text{obs}} \) be an outer approximation to the obstacle region. If a path has clearance > \( \varepsilon \) with respect to \( \hat{C}_{\text{obs}}^+ \), then it is also collision-free w.r.t. the true \( C_{\text{obs}} \) provided the approximation error is smaller than \( \varepsilon \).

These ideas will reappear when we talk about resolution completeness and probabilistic guarantees in later chapters.

A common approach is to discretize a bounded subset of \( C \) into cells. For simplicity, consider an axis-aligned hyper-rectangle \( C_{\text{box}} = [a_1,b_1] \times \dots \times [a_n,b_n] \subseteq C \), and a uniform grid with step sizes \( h = (h_1,\dots,h_n) \). Each grid cell is

\[ V_k = \prod_{i=1}^n [a_i + k_i h_i,\, a_i + (k_i+1) h_i], \quad k_i \in \mathbb{Z}. \]

Let \( q_k \) be a representative point of cell \( V_k \) (typically the center). We call a cell:

• free if we believe every configuration in \( V_k \) is in \( C_{\text{free}} \),
• occupied if we believe every configuration in \( V_k \) is in \( C_{\text{obs}} \),
• mixed / unknown otherwise.

We then define

\[ \hat{C}^{\text{grid}}_{\text{free}}{}^- = \bigcup_{k \in K_{\text{free}}} V_k, \qquad \hat{C}^{\text{grid}}_{\text{free}}{}^+ = C_{\text{box}} \setminus \bigcup_{k \in K_{\text{occ}}} V_k, \]

where \( K_{\text{free}} \) and \( K_{\text{occ}} \) are indices of cells confidently classified as free and occupied.

Conservative criteria. Let \( d(q) \) be a signed distance to obstacles in workspace (positive in free space, negative in collision), pulled back to C-space through forward kinematics, as in Lesson 4. Suppose \( d \) is Lipschitz with constant \( L \) w.r.t. \( d_C \), i.e.

\[ | d(q) - d(q') | \le L \, d_C(q,q') \quad \forall q,q' \in C. \]

For a cell \( V_k \) of radius \( r_k \) (maximal distance from its center \( q_k \) to any point in the cell),

\[ r_k = \sup_{q \in V_k} d_C(q, q_k), \]

we obtain the following sufficient conditions:

• If \( d(q_k) > L r_k \), then \( V_k \subseteq C_{\text{free}} \).
• If \( d(q_k) < -L r_k \), then \( V_k \subseteq C_{\text{obs}} \).

Proof idea. For any \( q \in V_k \), \( |d(q) - d(q_k)| \le L r_k \). If \( d(q_k) > L r_k \), then \( d(q) \ge d(q_k) - L r_k > 0 \), so \( q \in C_{\text{free}} \). The occupied case is similar. \(\square\)

In practice, we may not have exact Lipschitz constants, but we can often bound them using Jacobians of the forward kinematics or conservative workspace bounds. This yields mathematically justified grid-based inner and outer approximations.

Before we study sampling-based planners in later chapters, it is useful to view random or deterministic samples as a way to approximate \( C_{\text{free}} \) itself.

Let \( S = \{ q_1,\dots,q_N \} \subset C \) be a finite sample set. Define the free sample set

\[ S_{\text{free}} = \{\, q_i \in S \mid \operatorname{collide}(q_i) = 0 \,\}. \]

A simple sampling-based approximation thickens these free samples by a radius \( r > 0 \):

\[ \hat{C}^{\text{sample}}_{\text{free}}(S,r) = \bigcup_{q_i \in S_{\text{free}}} B_C(q_i, r), \quad B_C(q,r) = \{ q' \in C \mid d_C(q',q) \le r \}. \]

The dispersion of the samples in a set \( U \subseteq C \) is

\[ \delta(S;U) = \sup_{q \in U} \; \inf_{q_i \in S} d_C(q, q_i). \]

If we ensure \( \delta(S; C_{\text{free}}) \le r \), then \( C_{\text{free}} \subseteq \hat{C}^{\text{sample}}_{\text{free}}(S,r) \), i.e. all of the free space lies within radius \( r \) of some free sample. This is an outer approximation; inner approximations can be built using distance bounds as in Section 3.

For i.i.d. uniform samples, classical results in geometric probability show that \( \delta(S; C_{\text{free}}) \to 0 \) almost surely as \( N \to \infty \), with rate roughly \( \delta(S; C_{\text{free}}) = \mathcal{O}\big(N^{-1/n}\big) \) in \( n \)-dimensional spaces. This scaling strongly motivates low-discrepancy sequences and careful design of sampling measures in high-DOF robots.

Later, sampling-based planners will use these approximations together with local connectivity notions to construct roadmaps or trees, but the geometric approximation properties already appear at this stage.

Practical high-DOF robots rely heavily on collision libraries such as FCL (C++), Bullet, or similar tools, which provide fast bounding volume hierarchy (BVH) based distance queries in workspace. Combined with bounds on how much the workspace geometry can move under small changes in configuration, we obtain conservative approximations of \( C_{\text{free}} \).

Let \( X(q) \subset \mathbb{R}^3 \) be the swept volume of the robot at configuration \( q \), and \( O \subset \mathbb{R}^3 \) the union of all workspace obstacles. Define the workspace distance

\[ d_W(q) = \inf_{x \in X(q), \, o \in O} \| x - o \|. \]

Assume we have a bound

\[ | d_W(q) - d_W(q') | \le L_W \, d_C(q,q') \]

for some constant \( L_W \). (This can often be derived from a worst-case bound on link lengths and the robot Jacobian.)

For a grid cell \( V_k \) with representative center \( q_k \) and radius \( r_k \), we re-use the argument of Section 3:

• If \( d_W(q_k) > L_W r_k \), then \( V_k \subseteq C_{\text{free}} \).
• If \( d_W(q_k) < -L_W r_k \) (penetration), then \( V_k \subseteq C_{\text{obs}} \).

This yields certified inner and outer approximations using only BVH distance queries at a finite set of samples \( \{q_k\} \). Much of the sophistication of modern collision libraries is precisely in making \( d_W(q) \) queries fast and robust.

The following flow illustrates a typical conservative classification scheme for grid cells:

flowchart TD
  S["Sample cell center q_k"] --> D["Query workspace distance d_W(q_k)"]
  D --> R["Compute radius r_k in metric on C"]
  R --> C1["Check d_W(q_k) > L_W * r_k ?"]
  C1 -->|yes| F["Mark cell as 'certainly free'"]
  C1 -->|no| C2["Check d_W(q_k) < -L_W * r_k ?"]
  C2 -->|yes| O["Mark cell as \n'certainly colliding'"]
  C2 -->|no| U["Mark cell as \n'unknown / boundary region'"]
        

Unknown cells are typically refined (smaller \( r_k \)) or delegated to more expensive exact collision checks at runtime.

As a concrete example, we approximate the C-space of a planar 2R arm with link lengths \( l_1, l_2 \) operating in a workspace with rectangular obstacles. We build an occupancy grid over \( (\theta_1,\theta_2) \in [-\pi,\pi]^2 \) and classify each cell center as free or colliding via simple segment–rectangle intersection tests.

import numpy as np

# -----------------------------
# Robot and workspace geometry
# -----------------------------
L1, L2 = 1.0, 0.7  # link lengths

# Axis-aligned rectangular obstacles in workspace:
# each obstacle is (xmin, xmax, ymin, ymax)
obstacles = [
    (-0.3, 0.3, 0.5, 0.8),
    (0.6, 1.0, -0.2, 0.1),
]

def fk_2r(theta1, theta2):
    """
    Forward kinematics for a planar 2R arm.
    Returns positions of joints and end-effector in workspace.
    """
    x1 = L1 * np.cos(theta1)
    y1 = L1 * np.sin(theta1)
    x2 = x1 + L2 * np.cos(theta1 + theta2)
    y2 = y1 + L2 * np.sin(theta1 + theta2)
    return np.array([[0.0, 0.0],
                     [x1, y1],
                     [x2, y2]])

def seg_intersects_aabb(p, q, aabb):
    """
    Check whether segment pq intersects an axis-aligned bounding box (AABB).
    Using Liang-Barsky style clipping for robustness.
    """
    xmin, xmax, ymin, ymax = aabb

    dx = q[0] - p[0]
    dy = q[1] - p[1]

    t0, t1 = 0.0, 1.0

    def clip(p, q, r0, r1):
        if q == 0.0:
            return (True, r0, r1) if p <= r1 and p >= r0 else (False, r0, r1)
        t_min = (r0 - p) / q
        t_max = (r1 - p) / q
        if t_min > t_max:
            t_min, t_max = t_max, t_min
        return True, t_min, t_max

    ok, t_min_x, t_max_x = clip(p[0], dx, xmin, xmax)
    if not ok:
        return False
    t0 = max(t0, t_min_x)
    t1 = min(t1, t_max_x)
    if t0 > t1:
        return False

    ok, t_min_y, t_max_y = clip(p[1], dy, ymin, ymax)
    if not ok:
        return False
    t0 = max(t0, t_min_y)
    t1 = min(t1, t_max_y)
    if t0 > t1:
        return False

    return True

def is_collision(theta1, theta2):
    pts = fk_2r(theta1, theta2)
    segs = [(pts[0], pts[1]), (pts[1], pts[2])]
    for obs in obstacles:
        for p, q in segs:
            if seg_intersects_aabb(p, q, obs):
                return True
    return False

# -----------------------------
# C-space grid
# -----------------------------
N = 200  # resolution per joint
theta1_vals = np.linspace(-np.pi, np.pi, N)
theta2_vals = np.linspace(-np.pi, np.pi, N)

# occupancy: 0 = free, 1 = colliding
grid = np.zeros((N, N), dtype=np.uint8)

for i, t1 in enumerate(theta1_vals):
    for j, t2 in enumerate(theta2_vals):
        grid[i, j] = 1 if is_collision(t1, t2) else 0

# Optional: visualize with matplotlib (not shown here in HTML)
# import matplotlib.pyplot as plt
# plt.imshow(grid.T, origin="lower", extent=[-np.pi, np.pi, -np.pi, np.pi])
# plt.xlabel("theta1"), plt.ylabel("theta2")
# plt.show()

# Note:
# In a larger system, you might integrate this grid with OMPL (Python bindings)
# by creating a StateValidityChecker that queries 'grid' instead of calling
# full geometric collision checks every time.
      

This example uses only basic trigonometry and axis-aligned geometry, but the same structure scales to higher DOFs with more sophisticated collision backends.

In C++, common robotics stacks (e.g., OMPL and MoveIt) rely on FCL for collision and distance queries. Here we sketch a minimal configuration-space approximation layer that:

• wraps a configuration vector \( q \) into a robot pose,
• queries FCL for distance to obstacles, and
• classifies grid cells as in Section 5.

#include <vector>
#include <array>
#include <cmath>
#include <fcl/fcl.h>

// Simple alias for a configuration (n-DOF)
using Config = std::vector<double>;

struct CSpaceCell {
    Config center;
    double radius;     // radius in configuration metric
    double dist_ws;    // workspace distance d_W(center)
    enum class Label { Unknown, Free, Colliding } label;
};

class CSpaceApproximator {
public:
    CSpaceApproximator(double Lw,
                       const std::vector<std::shared_ptr<fcl::CollisionObjectd> >& robot_links,
                       const std::vector<std::shared_ptr<fcl::CollisionObjectd> >& obstacles)
        : Lw_(Lw), robot_links_(robot_links), obstacles_(obstacles) {}

    void classifyCell(CSpaceCell& cell) {
        // 1. Place robot according to cell.center
        forwardKinematicsToFCL(cell.center);

        // 2. Query minimal distance across all link-obstacle pairs
        double dmin = queryWorkspaceDistance();

        cell.dist_ws = dmin;

        if (dmin >  Lw_ * cell.radius) {
            cell.label = CSpaceCell::Label::Free;
        } else if (dmin < -Lw_ * cell.radius) {
            cell.label = CSpaceCell::Label::Colliding;
        } else {
            cell.label = CSpaceCell::Label::Unknown;
        }
    }

private:
    double Lw_;
    std::vector<std::shared_ptr<fcl::CollisionObjectd> > robot_links_;
    std::vector<std::shared_ptr<fcl::CollisionObjectd> > obstacles_;

    void forwardKinematicsToFCL(const Config& q) {
        // Update the pose of each robot_links_[i] based on the joint configuration q.
        // This is a standard kinematic update using your favorite math library (Eigen, etc.).
        // ...
    }

    double queryWorkspaceDistance() {
        fcl::DistanceRequestd req;
        fcl::DistanceResultd res;
        double dmin = std::numeric_limits<double>::infinity();

        for (const auto& link : robot_links_) {
            for (const auto& obs : obstacles_) {
                res.clear();
                fcl::distance(link.get(), obs.get(), req, res);
                dmin = std::min(dmin, res.min_distance);
            }
        }
        return dmin;
    }
};
      

This code isolates C-space approximation logic from the underlying collision engine. Higher-level planners see only an API that returns labels and approximate distances on a finite set of cells.

For mobile platforms or low-DOF subsystems, it is common to approximate \( C_{\text{free}} \) as a 2D occupancy grid in workspace for a disk-shaped robot. Here the configuration is \( q = (x,y) \), and we precompute a boolean grid using Minkowski sums (robot radius + obstacles).

public class OccupancyGrid2D {
    private final int width, height;
    private final double resolution;     // meters per cell
    private final boolean[][] occ;       // true = occupied

    // Obstacles as axis-aligned boxes
    public static class Box {
        public final double xmin, xmax, ymin, ymax;
        public Box(double xmin, double xmax, double ymin, double ymax) {
            this.xmin = xmin; this.xmax = xmax;
            this.ymin = ymin; this.ymax = ymax;
        }
    }

    private final java.util.List<Box> obstacles;
    private final double robotRadius;

    public OccupancyGrid2D(int width, int height, double resolution,
                           java.util.List<Box> obstacles,
                           double robotRadius) {
        this.width = width;
        this.height = height;
        this.resolution = resolution;
        this.obstacles = obstacles;
        this.robotRadius = robotRadius;
        this.occ = new boolean[width][height];
        precompute();
    }

    private void precompute() {
        for (int ix = 0; ix < width; ++ix) {
            for (int iy = 0; iy < height; ++iy) {
                double x = (ix + 0.5) * resolution;
                double y = (iy + 0.5) * resolution;
                occ[ix][iy] = isColliding(x, y);
            }
        }
    }

    private boolean isColliding(double x, double y) {
        for (Box b : obstacles) {
            double cx = Math.max(b.xmin, Math.min(x, b.xmax));
            double cy = Math.max(b.ymin, Math.min(y, b.ymax));
            double dx = x - cx;
            double dy = y - cy;
            if (dx * dx + dy * dy <= robotRadius * robotRadius) {
                return true;
            }
        }
        return false;
    }

    public boolean isFreeCell(int ix, int iy) {
        if (ix < 0 || ix >= width || iy < 0 || iy >= height) {
            return false; // treat outside as occupied
        }
        return !occ[ix][iy];
    }
}
      

This grid is a practical outer approximation: any configuration marked occupied is truly in collision (under inflated obstacles), while some configurations marked free may be slightly conservative depending on how inflation is chosen. Java robotics stacks (e.g., ROSJava-based systems) commonly use similar occupancy grids as a basis for higher-level planning.

MATLAB's Robotics System Toolbox provides rigidBodyTree models and collision objects with checkCollision. We can exploit this to approximate the C-space of a manipulator using a joint grid.

% Define a simple 2R planar robot in rigidBodyTree
robot = rigidBodyTree('DataFormat','row','MaxNumBodies',3);
L1 = 1.0; L2 = 0.7;

body1 = rigidBody('link1');
joint1 = rigidBodyJoint('joint1','revolute');
setFixedTransform(joint1,trvec2tform([0 0 0]));
joint1.JointAxis = [0 0 1];
body1.Joint = joint1;
addBody(robot,body1,'base');

body2 = rigidBody('link2');
joint2 = rigidBodyJoint('joint2','revolute');
setFixedTransform(joint2,trvec2tform([L1 0 0]));
joint2.JointAxis = [0 0 1];
body2.Joint = joint2;
addBody(robot,body2,'link1');

ee = rigidBody('tool');
setFixedTransform(ee.Joint,trvec2tform([L2 0 0]));
addBody(robot,ee,'link2');

% Workspace obstacle as a box
env = {collisionBox(0.4, 0.2, 0.5)}; % [X Y Z] dimensions
Tobs = trvec2tform([0.6 0.0 0.25]);  % pose in workspace
env{1}.Pose = Tobs;

% Joint grid approximation
N = 100;
theta1_vals = linspace(-pi, pi, N);
theta2_vals = linspace(-pi, pi, N);
occ = false(N, N);

for i = 1:N
    for j = 1:N
        q = [theta1_vals(i) theta2_vals(j)];
        isColliding = checkCollision(robot, q, env, ...
                                     'IgnoreSelfCollision','on');
        occ(i,j) = isColliding;
    end
end

% At this point, occ(i,j) is a discrete approximation of C_obs
% that can be exported or used in planners.
      

In Simulink, the same logic can be placed in a MATLAB Function block:

function coll = cspaceCollision(theta1, theta2)
%#codegen
persistent robot env
if isempty(robot)
    % Initialize persistent robot and environment (similar to script above)
    % ...
end
q = [theta1 theta2];
coll = checkCollision(robot, q, env, 'IgnoreSelfCollision','on');
end
      

The block outputs a Boolean collision flag that can be used to drive higher-level supervisory logic or state machines in Simulink.

Mathematica is well-suited to representing C-space regions as implicit sets and visualizing 2D projections. For a planar 2R arm, we can approximate the collision region by sampling over \( (\theta_1,\theta_2) \) and using region constructs:

(* Link lengths *)
l1 = 1.0; l2 = 0.7;

fk2R[{th1_, th2_}] := Module[
  {x1, y1, x2, y2},
  x1 = l1*Cos[th1];  y1 = l1*Sin[th1];
  x2 = x1 + l2*Cos[th1 + th2]; 
  y2 = y1 + l2*Sin[th1 + th2];
  { {0, 0}, {x1, y1}, {x2, y2} }
];

(* Axis-aligned rectangular obstacle *)
obs = Rectangle[{-0.3, 0.5}, {0.3, 0.8}];

segmentIntersectsRectangle[{p_, q_}, rect_] := 
 Module[{poly = DiscretizeRegion[rect]},
  RegionIntersection[Line[{p, q}], poly] =!= EmptyRegion[2]
 ];

collisionQ[{th1_, th2_}] := Module[
  {pts, seg1, seg2},
  pts = fk2R[{th1, th2}];
  seg1 = {pts[[1]], pts[[2]]};
  seg2 = {pts[[2]], pts[[3]]};
  Or[
    segmentIntersectsRectangle[seg1, obs],
    segmentIntersectsRectangle[seg2, obs]
  ]
];

(* Sampled approximation of C_obs *)
samples = 
  Flatten[
    Table[{th1, th2}, {th1, -Pi, Pi, Pi/100.}, {th2, -Pi, Pi, Pi/100.}],
    1
  ];

collPts = Select[samples, collisionQ[#] &];

ListDensityPlot[
  collPts,
  PlotRange -> { {-Pi, Pi}, {-Pi, Pi} },
  ColorFunction -> "Avocado",
  PlotLegends -> None,
  AxesLabel -> {"theta1", "theta2"}
]
      

This visualization is a sampled approximation to \( C_{\text{obs}} \) in the \( (\theta_1,\theta_2) \)-plane. Higher-dimensional C-spaces can be explored via slices and projections using similar techniques.

Problem 1 (Inner vs outer approximations and safety). Let \( C_{\text{free}} \subseteq C \) and approximate sets \( \hat{C}_{\text{free}}^- \subseteq \hat{C}_{\text{free}}^+ \subseteq C \) satisfy \( \hat{C}_{\text{free}}^- \subseteq C_{\text{free}} \subseteq \hat{C}_{\text{free}}^+ \). Show that any planner that restricts itself to paths lying entirely in \( \hat{C}_{\text{free}}^- \) is safe (never returns colliding paths), but may be incomplete even when a path exists in \( C_{\text{free}} \).

Solution: Safety follows from the definition of inner approximation: if \( q \in \hat{C}_{\text{free}}^- \), then \( q \in C_{\text{free}} \). Therefore any path \( \gamma \) with \( \gamma([0,1]) \subseteq \hat{C}_{\text{free}}^- \) lies entirely in \( C_{\text{free}} \) and is collision-free. Incompleteness arises because it may happen that \( C_{\text{free}} \setminus \hat{C}_{\text{free}}^- \neq \emptyset \). If all collision-free paths between two configurations pass through \( C_{\text{free}} \setminus \hat{C}_{\text{free}}^- \), then no path contained in \( \hat{C}_{\text{free}}^- \) exists, so the planner will fail even though a true path exists in \( C_{\text{free}} \).

Problem 2 (Resolution and clearance for a 1D example). Consider a 1D C-space \( C = [0,1] \) with a single obstacle interval \( C_{\text{obs}} = [a,b] \subset (0,1) \). A path is just a continuous function \( \gamma : [0,1] \to [0,1] \). Suppose a particular path \( \gamma \) satisfies \( \operatorname{clear}(\gamma) = \delta > 0 \), where the metric is the usual absolute value. Let a uniform grid with spacing \( h \) be given: grid points \( q_k = kh \), cells \( V_k = [kh, (k+1)h] \). Show that if \( h < \delta \), then there exists a sequence of grid points \( q_{k_0}, q_{k_1}, \dots, q_{k_m} \) that connects the start and goal and lies entirely in \( C_{\text{free}} \).

Solution: The clearance condition implies that the image of \( \gamma \) lies in \( [0,1] \setminus (a',b') \) where \( a' = a - \delta \), \( b' = b + \delta \) (after clipping to \( [0,1] \)). Since \( h < \delta \), every grid point is at distance strictly less than \( \delta \) from some point on the path image. By sampling the path at times \( t_i = i/m \) for sufficiently large \( m \) and snapping \( \gamma(t_i) \) to the nearest grid point, we obtain a sequence of free grid points (no snapped point can fall inside \( [a,b] \) because that would contradict the clearance). The sequence can be ordered such that successive points are neighbors with step size \( \le h \), thus forming a discrete free path.

Problem 3 (Lipschitz bound for a simple planar arm). Consider the 2R planar arm with link lengths \( l_1, l_2 \) and configuration \( q = (\theta_1,\theta_2) \). Let the workspace distance \( d_W(q) \) be defined as in Section 5, with \( d_C \) the Euclidean metric on angles. Show that a crude Lipschitz bound is \( L_W \le l_1 + l_2 \).

Solution: The end-effector position map \( f(q) = (x(q),y(q)) \) is \( x = l_1\cos\theta_1 + l_2\cos(\theta_1 + \theta_2) \), \( y = l_1\sin\theta_1 + l_2\sin(\theta_1 + \theta_2) \). The Jacobian \( J(q) = \partial f / \partial q \) has columns bounded in norm by \( l_1 + l_2 \), since rotating either joint by a small angle moves any point on the arm by at most \( l_1 + l_2 \). Therefore \( \| f(q) - f(q') \| \le (l_1 + l_2) \| q - q' \| \). Workspace distance \( d_W(q) \) is a 1-Lipschitz function of the robot geometry in workspace, so composing with \( f \) yields \( | d_W(q) - d_W(q') | \le (l_1 + l_2) \| q - q' \| \), hence \( L_W \le l_1 + l_2 \).

Problem 4 (Probability of missing a small free region in 1D). Let \( C = [0,1] \) and \( C_{\text{free}} = [0,1] \setminus [a,b] \) with \( 0 < a < b < 1 \). You draw \( N \) i.i.d. samples \( q_i \) uniformly from \( C_{\text{free}} \). For a small free interval \( I = [c,d] \subseteq C_{\text{free}} \) of length \( \ell = d-c \), compute the probability that no sample falls in \( I \), and show that this tends to zero as \( N \to \infty \).

Solution: The sampling distribution is uniform over \( C_{\text{free}} \), so the probability that a single sample falls in \( I \) is \( p = \ell / |C_{\text{free}}| \), where \( |C_{\text{free}}| = 1 - (b-a) \) is the Lebesgue measure of \( C_{\text{free}} \). The probability that it does not fall in \( I \) is \( 1 - p \). For \( N \) independent samples, the probability that none fall in \( I \) is \( (1-p)^N \). Since \( 0 < p \le 1 \), we have \( \lim_{N \to \infty} (1-p)^N = 0 \), so the event of missing \( I \) vanishes in the limit.

Problem 5 (Adaptive refinement flow). Consider a grid-based C-space approximation with Lipschitz constant \( L_W \) and a threshold \( \tau > 0 \). Cells with \( |d_W(q_k)| \le \tau \) are marked as uncertain, while others are classified as in Section 5. Describe an adaptive refinement strategy that repeatedly subdivides uncertain cells until either the cell radius \( r_k \) becomes smaller than a target resolution \( r_{\min} \) or the cell is reclassified as free or colliding. Sketch this refinement as a flow.

Solution: A natural strategy is:

• Initialize with a coarse grid over \( C_{\text{box}} \).
• For each cell, compute \( d_W(q_k) \) and classify: free, colliding, or uncertain (if \( |d_W(q_k)| \le \tau \)).
• For uncertain cells with \( r_k > r_{\min} \), subdivide into smaller cells, recompute \( r_k \) and reclassify children cells.
• Stop when no uncertain cell has radius above \( r_{\min} \).

The following flow diagram summarizes the logic:

flowchart TD
  A["Start with coarse grid"] --> B["For each cell: compute d_W(q_k)"]
  B --> C["Classify: free / colliding / uncertain"]
  C --> D["Collect uncertain cells with radius > r_min"]
  D -->|none| E["Stop (approximation done)"]
  D -->|some| F["Subdivide each uncertain cell into children"]
  F --> B
        

Under mild regularity of \( d_W \), this process converges to an approximation whose boundary error is bounded by a constant times \( r_{\min} \).

In this lesson, we moved from the abstract notion of configuration spaces to concrete, implementable approximations suitable for planning:

• We defined inner and outer approximations \( \hat{C}_{\text{free}}^- \) and \( \hat{C}_{\text{free}}^+ \), and related them to path clearance and safety.
• We reviewed grid and cell decomposition methods and showed how Lipschitz bounds on distance functions yield conservative classification rules.
• We interpreted samples as geometric coverings of \( C_{\text{free}} \) and connected dispersion and probability of coverage to approximation quality.
• We demonstrated how collision libraries and robotics toolboxes (FCL, OMPL, MATLAB Robotics System Toolbox, etc.) implement these theoretical ideas in practice.

Subsequent chapters will build on these approximations to design graph-search and sampling-based planners that inherit safety and (probabilistic or resolution) completeness properties from the underlying C-space approximations.