In introductory robotics, configurations are often introduced as joint vectors \( \mathbf{q} \in \mathbb{R}^n \) and the forward kinematics map is derived. In motion planning, we deliberately abstract away these derivations and use configuration space as the domain on which we reason about existence of paths. The central idea is:

Definition 1.1 (Configuration space). Let the set of all physically realizable robot configurations be

\[ \mathcal{Q} = \{ q \mid q \text{ specifies a complete pose of all robot bodies} \}. \]

A configuration space is a pair \( (\mathcal{Q}, \mathcal{T}) \) where \( \mathcal{T} \) is a topology such that the forward kinematics map to workspace is continuous. In almost all robot arms studied in basic kinematics, we can embed \( \mathcal{Q} \) in some \( \mathbb{R}^n \) with additional periodic identifications (e.g. for revolute joints).

For planning, we will treat \( \mathcal{Q} \) as:

\[ \mathcal{Q} \subseteq \mathbb{R}^{n_{\text{pr}}} \times \mathbb{T}^{n_{\text{per}}}, \]

where \( \mathbb{T} \) denotes the circle space corresponding to a periodic joint. Here \( n_{\text{pr}} \) is the number of prismatic coordinates, and \( n_{\text{per}} \) the number of periodic joint coordinates.

Importantly, we consider \( \mathcal{Q} \) before adding obstacles; the obstacle-induced subsets will be treated in Lesson 2.

flowchart TD
  A["Physical robot model"] --> B["Identify configuration variables q"]
  B --> C["Define configuration set Q"]
  C --> D["Equip Q with topology / metric"]
  D --> E["Formulate planning problem purely on Q"]
  E --> F["Later: intersect with constraints and obstacles"]
        

A purely geometric motion planning problem is formulated directly in C-space. We assume the student knows basic definitions of configuration and forward kinematics from standard kinematics and dynamics courses, but we now package these into an abstract planning problem.

Let \( \mathcal{Q} \) be the configuration space and let \( d : \mathcal{Q} \times \mathcal{Q} \to \mathbb{R}_{\ge 0} \) be any metric compatible with the topology (metrics will be studied in Lesson 4).

For this lesson, we assume a given feasible subset \( \mathcal{C}_{\text{feas}} \subseteq \mathcal{Q} \) of configurations that respect internal robot constraints (joint limits, self-collision avoidance, etc.). Obstacles and environment interactions will refine this set to the free space later.

Definition 2.1 (Geometric planning problem in C-space). Given:

• A configuration space \( \mathcal{Q} \) and feasible subset \( \mathcal{C}_{\text{feas}} \subseteq \mathcal{Q} \),
• An initial configuration \( q_{\text{start}} \in \mathcal{C}_{\text{feas}} \),
• A goal region \( \mathcal{G} \subseteq \mathcal{C}_{\text{feas}} \),

the geometric motion planning problem is to decide whether there exists a continuous path

\[ \sigma : [0,1] \to \mathcal{C}_{\text{feas}} \]

such that

\[ \sigma(0) = q_{\text{start}}, \qquad \sigma(1) \in \mathcal{G}. \]

When such a path exists, the problem is called feasible. The planner's job is to either return a representation of some feasible \( \sigma \) or correctly report infeasibility.

Note that this formulation says nothing about dynamics, velocities, or torques; we are strictly in configuration space, not state space. Kinodynamic planning will be introduced in later chapters.

Let the workspace of the robot be a subset \( \mathcal{W} \subseteq \mathbb{R}^3 \) or, more generally, a configuration space for the end-effector or bodies of interest. The (forward) kinematic map

\[ f : \mathcal{Q} \to \mathcal{W}^m \]

maps a configuration to the poses of a finite set of reference frames (end effector, links, etc.). From basic robotics, we know \( f \) is continuous (compositions of polynomial and trigonometric functions).

Proposition 3.1. If \( \sigma : [0,1] \to \mathcal{Q} \) is continuous, then the workspace trajectory \( \gamma : [0,1] \to \mathcal{W}^m \), defined by

\[ \gamma(t) = f(\sigma(t)), \]

is continuous.

Proof. This is a direct application of the fundamental theorem on continuity of compositions: the composition of continuous functions is continuous. The map \( f \) is continuous in \( \mathcal{Q} \) and \( \sigma \) is continuous in \( [0,1] \). Therefore \( \gamma = f \circ \sigma \) is continuous on \( [0,1] \). \(\square\)

This simple observation is key: if we design a continuous path in C-space, it automatically induces a continuous motion of the robot in physical space. Thus C-space is a sound planning domain even if the task is originally specified in workspace.

C-space allows us to restate planning feasibility in purely topological terms.

Definition 4.1 (Path-connectedness). A subset \( X \subseteq \mathcal{Q} \) is path-connected if for all \( q_1, q_2 \in X \) there exists a continuous path \( \sigma : [0,1] \to X \) with \( \sigma(0) = q_1 \) and \( \sigma(1) = q_2 \).

In our planning setting, we are interested in the path-connected components of \( \mathcal{C}_{\text{feas}} \).

Proposition 4.2. Let \( \mathcal{C}_{\text{feas}} \subseteq \mathcal{Q} \) be given. Suppose \( q_{\text{start}} \in \mathcal{C}_{\text{feas}} \) and \( \mathcal{G} \subseteq \mathcal{C}_{\text{feas}} \) is nonempty. Define the path-connected component of \( q_{\text{start}} \) as

\[ \mathcal{P}(q_{\text{start}}) = \{ q \in \mathcal{C}_{\text{feas}} \mid \exists \, \sigma : [0,1] \to \mathcal{C}_{\text{feas}}, \; \sigma(0) = q_{\text{start}}, \; \sigma(1) = q \}. \]

Then a necessary and sufficient condition for existence of a feasible motion is

\[ \mathcal{P}(q_{\text{start}}) \cap \mathcal{G} \neq \emptyset. \]

Proof. (Necessity) If a feasible motion exists, there is \( \sigma \) with \( \sigma(0) = q_{\text{start}} \) and \( \sigma(1) \in \mathcal{G} \). By definition of \( \mathcal{P}(q_{\text{start}}) \), \( \sigma(1) \in \mathcal{P}(q_{\text{start}}) \), so \( \mathcal{P}(q_{\text{start}}) \cap \mathcal{G} \neq \emptyset \).

(Sufficiency) If \( q_{\text{goal}} \in \mathcal{P}(q_{\text{start}}) \cap \mathcal{G} \), then by definition of \( \mathcal{P}(q_{\text{start}}) \) there exists \( \sigma \) with \( \sigma(0) = q_{\text{start}} \) and \( \sigma(1) = q_{\text{goal}} \), and since \( q_{\text{goal}} \in \mathcal{G} \) the same \( \sigma \) certifies feasibility. \(\square\)

The role of a planner is therefore, in principle, to decide connectivity in \( \mathcal{C}_{\text{feas}} \), which is computationally hard in general for high-dimensional systems and complex constraints.

In practice, we rarely manipulate abstract topological spaces directly; instead we work with coordinate vectors \( \mathbf{q} \). For many robots, we can identify \( \mathcal{Q} \) with a subset of a Euclidean space with periodic identifications:

\[ \mathcal{Q} \cong \left\{ \mathbf{q} \in \mathbb{R}^{n_{\text{pr}} + n_{\text{per}}} \mid q_i \in [\ell_i, u_i] \text{ for prismatic joints, and } q_j \in (-\pi,\pi] \text{ for revolute joints} \right\}, \]

with the understanding that revolute coordinates with values differing by integer multiples of \( 2\pi \) represent the same physical configuration.

Redundant manipulators (more DOFs than necessary to realize a task) introduce nonuniqueness in mapping workspace constraints to C-space. From the planning viewpoint, this simply means that the goal set \( \mathcal{G} \) often has positive dimension in \( \mathcal{Q} \), and planners can exploit this internal freedom to improve other criteria (clearance, manipulability, etc.) in later chapters.

Exact reasoning about path-connectedness in high-dimensional C-spaces is difficult. A common strategy is to replace the continuous space with a discrete approximation and then solve a graph search problem.

Let \( \mathcal{Q} \subseteq \mathbb{R}^n \) be bounded and let \( h > 0 \) be a grid spacing. Define the uniform grid

\[ G_h = \left\{ \mathbf{q} \in \mathcal{Q} \,\middle|\, \mathbf{q} = \ell + h \mathbf{k},\; \mathbf{k} \in \mathbb{Z}^n \right\}, \]

where \( \ell \) is a lower corner of a bounding box of \( \mathcal{Q} \). A natural adjacency relation is

\[ \mathbf{q}_1 \sim \mathbf{q}_2 \quad \text{iff} \quad \| \mathbf{q}_1 - \mathbf{q}_2 \|_2 = h. \]

This defines a graph \( \mathcal{G}_h = (V_h, E_h) \) with \( V_h = G_h \cap \mathcal{C}_{\text{feas}} \) and edges \( E_h = \{ (\mathbf{q}_1,\mathbf{q}_2) \mid \mathbf{q}_1 \sim \mathbf{q}_2 \} \).

Proposition 6.1 (Convergence intuition, no proof). Under suitable regularity assumptions on \( \mathcal{C}_{\text{feas}} \), as \( h \to 0 \), connectivity in \( \mathcal{G}_h \) approximates connectivity in \( \mathcal{C}_{\text{feas}} \), in the sense that any two configurations connected by a path in \( \mathcal{C}_{\text{feas}} \) can be joined by a path in \( \mathcal{G}_h \) with vertices converging to configurations on a continuous path.

Thus, search algorithms (treated in the next chapter) operate on a graph that is an abstraction of the underlying C-space.

flowchart TD
  Q["Continuous C-space Q"] --> D["Discretize with grid spacing h"]
  D --> V["Vertices V_h: feasible grid points"]
  V --> E["Edges E_h: local neighbors"]
  E --> G["Graph G_h"]
  G --> P["Run discrete path search on G_h"]
  P --> R["Recover approximate C-space path"]
        

Consider the standard 2R planar manipulator with joint angles \( q_1, q_2 \), both revolute. Ignoring joint limits, the natural configuration space is

\[ \mathcal{Q} = S^1 \times S^1, \]

which topologically is a torus. However, for planning on a computer, a typical representation uses wrapped intervals:

\[ \mathcal{Q}_{\text{rep}} = (-\pi,\pi] \times (-\pi,\pi], \]

with the identification

\[ (q_1, q_2) \sim (q_1 + 2k_1\pi, q_2 + 2k_2\pi), \quad k_1,k_2 \in \mathbb{Z}. \]

Given two configurations \( q^{(0)} = (q_1^{(0)}, q_2^{(0)}) \) and \( q^{(1)} = (q_1^{(1)}, q_2^{(1)}) \), a simple piecewise-linear path in C-space is

\[ \sigma(t) = (1-t) \tilde{q}^{(0)} + t \tilde{q}^{(1)}, \quad t \in [0,1], \]

where \( \tilde{q}^{(0)}, \tilde{q}^{(1)} \) are chosen to respect the shortest angular differences in each coordinate, e.g.

\[ \tilde{q}_i^{(1)} = q_i^{(0)} + \text{wrap}\big(q_i^{(1)} - q_i^{(0)}\big), \]

with \( \text{wrap}(\cdot) \) mapping angular differences to the interval \( (-\pi,\pi] \). This guarantees continuity on the torus while using a vector representation.

We now implement simple C-space abstractions that:

• Represent configurations as vectors.
• Interpolate between configurations to create a path.
• Discretize the path into a finite sequence of configurations.

For this lesson, we leave feasibility checks (obstacles, self-collision) as abstract predicates to be implemented later.

8.1 Python Implementation

import numpy as np

class CSpace:
    """
    Simple box-bounded configuration space with optional periodic coordinates.
    periodic is a boolean mask of same length as bounds.
    """
    def __init__(self, lower_bounds, upper_bounds, periodic=None):
        self.lower = np.array(lower_bounds, dtype=float)
        self.upper = np.array(upper_bounds, dtype=float)
        assert self.lower.shape == self.upper.shape
        self.n = self.lower.size
        if periodic is None:
            periodic = np.zeros(self.n, dtype=bool)
        self.periodic = np.array(periodic, dtype=bool)

    def _wrap(self, q):
        """Wrap periodic coordinates to (-pi, pi]."""
        q = np.array(q, dtype=float)
        for i in range(self.n):
            if self.periodic[i]:
                # Wrap to (-pi, pi]
                q[i] = (q[i] + np.pi) % (2.0 * np.pi) - np.pi
        return q

    def shortest_difference(self, q_from, q_to):
        """Compute shortest coordinate-wise difference on mixed periodic space."""
        q_from = self._wrap(q_from)
        q_to = self._wrap(q_to)
        diff = q_to - q_from
        for i in range(self.n):
            if self.periodic[i]:
                # Choose representation with smallest magnitude
                if diff[i] > np.pi:
                    diff[i] -= 2.0 * np.pi
                elif diff[i] < -np.pi:
                    diff[i] += 2.0 * np.pi
        return diff

    def interpolate(self, q_start, q_goal, num_samples):
        """
        Generate a discrete path from q_start to q_goal using linear interpolation
        in configuration space (respecting periodic coordinates).
        """
        q_start = self._wrap(q_start)
        diff = self.shortest_difference(q_start, q_goal)
        alphas = np.linspace(0.0, 1.0, num_samples)
        path = []
        for a in alphas:
            q = q_start + a * diff
            q = self._wrap(q)
            path.append(q)
        return np.vstack(path)

# Example usage: 2R planar arm with both joints periodic
cspace = CSpace(lower_bounds=[-np.pi, -np.pi],
                upper_bounds=[ np.pi,  np.pi],
                periodic=[True, True])

q_start = [0.0, 0.0]
q_goal  = [np.pi, -np.pi]  # same as [-pi, -pi] physically
path = cspace.interpolate(q_start, q_goal, num_samples=5)
print(path)
      

8.2 C++ Implementation

#include <vector>
#include <cmath>
#include <cassert>

class CSpace {
public:
    CSpace(const std::vector<double>& lower_bounds,
           const std::vector<double>& upper_bounds,
           const std::vector<bool>& periodic)
        : lower_(lower_bounds),
          upper_(upper_bounds),
          periodic_(periodic) {
        assert(lower_.size() == upper_.size());
        assert(lower_.size() == periodic_.size());
        n_ = static_cast<std::size_t>(lower_.size());
    }

    std::vector<double> wrap(const std::vector<double>& q) const {
        std::vector<double> res = q;
        for (std::size_t i = 0; i < n_; ++i) {
            if (periodic_[i]) {
                double x = res[i];
                // wrap to (-pi, pi]
                x = std::fmod(x + M_PI, 2.0 * M_PI);
                if (x < 0.0) x += 2.0 * M_PI;
                res[i] = x - M_PI;
            }
        }
        return res;
    }

    std::vector<double> shortestDifference(const std::vector<double>& q_from,
                                             const std::vector<double>& q_to) const {
        std::vector<double> a = wrap(q_from);
        std::vector<double> b = wrap(q_to);
        std::vector<double> diff(n_);
        for (std::size_t i = 0; i < n_; ++i) {
            double d = b[i] - a[i];
            if (periodic_[i]) {
                if (d > M_PI) {
                    d -= 2.0 * M_PI;
                } else if (d < -M_PI) {
                    d += 2.0 * M_PI;
                }
            }
            diff[i] = d;
        }
        return diff;
    }

    std::vector<std::vector<double> >
    interpolate(const std::vector<double>& q_start,
                const std::vector<double>& q_goal,
                std::size_t num_samples) const {
        std::vector<double> qs = wrap(q_start);
        std::vector<double> diff = shortestDifference(qs, q_goal);
        std::vector<std::vector<double> > path;
        path.reserve(num_samples);
        for (std::size_t k = 0; k < num_samples; ++k) {
            double alpha = 0.0;
            if (num_samples > 1) {
                alpha = static_cast<double>(k) /
                        static_cast<double>(num_samples - 1);
            }
            std::vector<double> q(n_);
            for (std::size_t i = 0; i < n_; ++i) {
                q[i] = qs[i] + alpha * diff[i];
            }
            q = wrap(q);
            path.push_back(q);
        }
        return path;
    }

private:
    std::vector<double> lower_, upper_;
    std::vector<bool> periodic_;
    std::size_t n_;
};

// Example usage (2R planar arm):
// CSpace cspace({-M_PI, -M_PI}, {M_PI, M_PI}, {true, true});
// auto path = cspace.interpolate({0.0, 0.0}, {M_PI, -M_PI}, 5);
      

8.3 Java Implementation

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

public class CSpace {
    private final double[] lower;
    private final double[] upper;
    private final boolean[] periodic;
    private final int n;

    public CSpace(double[] lowerBounds, double[] upperBounds, boolean[] periodicFlags) {
        if (lowerBounds.length != upperBounds.length ||
            lowerBounds.length != periodicFlags.length) {
            throw new IllegalArgumentException("Mismatched array lengths");
        }
        this.lower = lowerBounds.clone();
        this.upper = upperBounds.clone();
        this.periodic = periodicFlags.clone();
        this.n = lower.length;
    }

    private double[] wrap(double[] q) {
        double[] res = q.clone();
        for (int i = 0; i < n; ++i) {
            if (periodic[i]) {
                double x = res[i];
                // wrap to (-pi, pi]
                x = (x + Math.PI) % (2.0 * Math.PI);
                if (x < 0.0) {
                    x += 2.0 * Math.PI;
                }
                res[i] = x - Math.PI;
            }
        }
        return res;
    }

    private double[] shortestDifference(double[] qFrom, double[] qTo) {
        double[] a = wrap(qFrom);
        double[] b = wrap(qTo);
        double[] diff = new double[n];
        for (int i = 0; i < n; ++i) {
            double d = b[i] - a[i];
            if (periodic[i]) {
                if (d > Math.PI) {
                    d -= 2.0 * Math.PI;
                } else if (d < -Math.PI) {
                    d += 2.0 * Math.PI;
                }
            }
            diff[i] = d;
        }
        return diff;
    }

    public List<double[]> interpolate(double[] qStart,
                                        double[] qGoal,
                                        int numSamples) {
        double[] qs = wrap(qStart);
        double[] diff = shortestDifference(qs, qGoal);
        List<double[]> path = new ArrayList<>(numSamples);
        for (int k = 0; k < numSamples; ++k) {
            double alpha = (numSamples > 1)
                    ? (double) k / (double) (numSamples - 1)
                    : 0.0;
            double[] q = new double[n];
            for (int i = 0; i < n; ++i) {
                q[i] = qs[i] + alpha * diff[i];
            }
            q = wrap(q);
            path.add(q);
        }
        return path;
    }

    // Example usage in main:
    // public static void main(String[] args) {
    //     CSpace cspace = new CSpace(
    //         new double[]{-Math.PI, -Math.PI},
    //         new double[]{ Math.PI,  Math.PI},
    //         new boolean[]{true, true}
    //     );
    //     List<double[]> path = cspace.interpolate(
    //         new double[]{0.0, 0.0},
    //         new double[]{Math.PI, -Math.PI},
    //         5
    //     );
    // }
}
      

8.4 MATLAB / Simulink Implementation

function path = cspace_interpolate(q_start, q_goal, periodic, num_samples)
%CSpace interpolation with optional periodic coordinates.
% q_start, q_goal: 1-by-n
% periodic: 1-by-n logical indicating periodic coordinates
% num_samples: scalar number of samples along the path

q_start = wrap_angles(q_start, periodic);
diff = shortest_difference(q_start, q_goal, periodic);
alphas = linspace(0.0, 1.0, num_samples);
n = numel(q_start);
path = zeros(num_samples, n);
for k = 1:num_samples
    a = alphas(k);
    q = q_start + a * diff;
    q = wrap_angles(q, periodic);
    path(k, :) = q;
end
end

function q = wrap_angles(q, periodic)
% Wrap periodic coordinates to (-pi, pi]
for i = 1:numel(q)
    if periodic(i)
        x = q(i);
        x = mod(x + pi, 2.0 * pi);
        if x < 0.0
            x = x + 2.0 * pi;
        end
        q(i) = x - pi;
    end
end
end

function diff = shortest_difference(q_from, q_to, periodic)
q_from = wrap_angles(q_from, periodic);
q_to   = wrap_angles(q_to, periodic);
diff   = q_to - q_from;
for i = 1:numel(diff)
    if periodic(i)
        if diff(i) > pi
            diff(i) = diff(i) - 2.0 * pi;
        elseif diff(i) < -pi
            diff(i) = diff(i) + 2.0 * pi;
        end
    end
end
end

% Example usage (2R arm):
% periodic = [true, true];
% q_start = [0, 0];
% q_goal  = [pi, -pi];
% path = cspace_interpolate(q_start, q_goal, periodic, 5);

% Simulink remark:
% A Simulink implementation would represent q_start, q_goal, and periodic as inputs,
% implement wrap_angles and shortest_difference as MATLAB Function blocks, and
% generate a sequence of configurations by iterating alpha in a For Iterator Subsystem.
      

8.5 Wolfram Mathematica Implementation

wrapAngles[q_List, periodic_List] := Module[{res = q, n = Length[q]},
  Do[
    If[periodic[[i]],
      (* wrap to (-Pi, Pi] *)
      res[[i]] = Mod[res[[i]] + Pi, 2 Pi];
      If[res[[i]] < 0, res[[i]] = res[[i]] + 2 Pi];
      res[[i]] = res[[i]] - Pi;
    ],
    {i, 1, n}
  ];
  res
]

shortestDifference[qFrom_List, qTo_List, periodic_List] := Module[
  {a, b, diff, n},
  a = wrapAngles[qFrom, periodic];
  b = wrapAngles[qTo, periodic];
  n = Length[a];
  diff = b - a;
  Do[
    If[periodic[[i]],
      If[diff[[i]] > Pi, diff[[i]] = diff[[i]] - 2 Pi];
      If[diff[[i]] < -Pi, diff[[i]] = diff[[i]] + 2 Pi];
    ],
    {i, 1, n}
  ];
  diff
]

cspaceInterpolate[qStart_List, qGoal_List, periodic_List, numSamples_Integer] :=
 Module[{qs, diff, alphas, path},
  qs = wrapAngles[qStart, periodic];
  diff = shortestDifference[qs, qGoal, periodic];
  alphas = N@Subdivide[0.0, 1.0, numSamples - 1];
  path = Table[
    wrapAngles[qs + alpha diff, periodic],
    {alpha, alphas}
  ];
  path
]

(* Example usage:
   periodic = {True, True};
   qStart = {0.0, 0.0};
   qGoal  = {Pi, -Pi};
   path   = cspaceInterpolate[qStart, qGoal, periodic, 5];
*)
      

Problem 1 (C-space vs workspace continuity). Let \( \mathcal{Q} \) be a configuration space and \( f : \mathcal{Q} \to \mathcal{W}^m \) the continuous forward kinematics map. Show that if two configurations \( q_a, q_b \in \mathcal{Q} \) are connected by a continuous path \( \sigma : [0,1] \to \mathcal{Q} \), then the corresponding workspace trajectories of all reference frames are continuous, and no instantaneous "teleportation" of robot links occurs.

Solution. For each link or frame index \( i \), let \( f_i : \mathcal{Q} \to \mathcal{W} \) be the projection of \( f \) to the pose of that frame. Since \( f \) is continuous, so is each \( f_i \). Define \( \gamma_i(t) = f_i(\sigma(t)) \). By composition of continuous functions, \( \gamma_i \) is continuous on \( [0,1] \). Therefore the pose of each frame changes continuously over time, which excludes any discontinuous jump. So planning in C-space guarantees physically realizable continuous motions in workspace.

Problem 2 (Path-connectedness and feasibility). Let \( \mathcal{C}_{\text{feas}} \), \( q_{\text{start}} \), and \( \mathcal{G} \) be as in Section 4. Prove that if \( \mathcal{C}_{\text{feas}} \) is path-connected and \( \mathcal{G} \cap \mathcal{C}_{\text{feas}} \neq \emptyset \), then every \( q_{\text{start}} \in \mathcal{C}_{\text{feas}} \) admits a feasible path to some goal configuration.

Solution. Let \( q_{\text{goal}} \in \mathcal{G} \cap \mathcal{C}_{\text{feas}} \). Since \( \mathcal{C}_{\text{feas}} \) is path-connected, there exists a continuous path \( \sigma : [0,1] \to \mathcal{C}_{\text{feas}} \) with \( \sigma(0) = q_{\text{start}} \) and \( \sigma(1) = q_{\text{goal}} \). Since \( q_{\text{goal}} \in \mathcal{G} \), this \( \sigma \) is a feasible solution to the planning problem. As \( q_{\text{start}} \) was arbitrary, this holds for all start configurations in \( \mathcal{C}_{\text{feas}} \).

Problem 3 (Grid discretization error bound, 1D case). Consider a 1D configuration space \( \mathcal{Q} = [a,b] \) with \( \mathcal{C}_{\text{feas}} = \mathcal{Q} \). Let \( h > 0 \) be a grid spacing and \( G_h = \{ a + kh \mid k \in \mathbb{Z}, a \le a+kh \le b \} \). Suppose \( q_{\text{start}}, q_{\text{goal}} \in \mathcal{Q} \) with \( q_{\text{start}} < q_{\text{goal}} \). Show that if \( h \le q_{\text{goal}} - q_{\text{start}} \), then there exists a finite sequence of grid points from \( G_h \) that connects neighborhoods of \( q_{\text{start}} \) and \( q_{\text{goal}} \), and quantify the maximum distance between the continuous straight-line path and the piecewise-constant grid path.

Solution. Let \( k_{\text{start}} = \left\lceil (q_{\text{start}} - a)/h \right\rceil \) and \( k_{\text{goal}} = \left\lfloor (q_{\text{goal}} - a)/h \right\rfloor \). Then \( q_s' = a + k_{\text{start}} h \ge q_{\text{start}} \) and \( q_g' = a + k_{\text{goal}} h \le q_{\text{goal}} \), with both in \( G_h \). Since \( h \le q_{\text{goal}} - q_{\text{start}} \), we have \( k_{\text{goal}} \ge k_{\text{start}} \), so we can enumerate \( q_i = a + (k_{\text{start}} + i) h \) for \( i = 0,\dots,k_{\text{goal}} - k_{\text{start}} \). This yields a sequence of grid points from \( q_s' \) to \( q_g' \). The continuous straight line is \( \sigma(t) = (1-t) q_{\text{start}} + t q_{\text{goal}} \). At any parameter where we follow the piecewise-constant path at a grid point \( q_i \), the distance to the nearest point on the line segment is at most \( h \), because the spacing between consecutive grid points is \( h \) and the line is monotone in 1D. Thus the maximum deviation of the grid path from the ideal straight-line path is bounded by \( h \).

Problem 4 (2R planar arm C-space identification). For the 2R planar arm with joint angles \( q_1, q_2 \) both revolute without limits, justify the claim that \( \mathcal{Q} \cong S^1 \times S^1 \). Then argue why \( \mathcal{Q} \) is path-connected.

Solution. Each angle coordinate \( q_i \) can be changed by any integer multiple of \( 2\pi \) without altering the physical configuration. Therefore the set of distinct physical configurations for one joint is \( \mathbb{R} / (2\pi \mathbb{Z}) \), which is homeomorphic to the circle \( S^1 \). Since joints are independent, the configuration space is the Cartesian product \( S^1 \times S^1 \). The circle \( S^1 \) is path-connected, and the Cartesian product of two path-connected spaces is path-connected. Hence \( \mathcal{Q} \) is path-connected, so any two configurations can be joined by a continuous path in C-space.

Problem 5 (Metric compatibility and interpolation). Let \( d(\cdot,\cdot) \) be the metric on \( \mathcal{Q}_{\text{rep}} \subseteq \mathbb{R}^n \) given by standard Euclidean distance, with angular wrap-around handled by the shortest-angle rule (as in Section 7). Show that the interpolation \( \sigma(t) \) defined by

\[ \sigma(t) = q_{\text{start}} + t \, \Delta q_{\text{short}}, \quad 0 \le t \le 1, \]

where \( \Delta q_{\text{short}} \) is the shortest difference vector, is a constant-speed path with respect to the induced length measure from \( d \).

Solution. By construction, for any \( s,t \in [0,1] \),

\[ d(\sigma(s), \sigma(t)) = \big\| \sigma(t) - \sigma(s) \big\|_2 = \big\| (t-s) \Delta q_{\text{short}} \big\|_2 = |t-s| \, \big\| \Delta q_{\text{short}} \big\|_2. \]

Thus the distance traveled between parameters \( s \) and \( t \) is proportional to \( |t-s| \), with proportionality constant \( \|\Delta q_{\text{short}}\|_2 \). Therefore the instantaneous speed (metric derivative) is constant and equal to this norm, independent of \( t \), so the path is constant-speed under \( d \).

In this lesson, we abstracted configuration space as the fundamental domain for motion planning, independent of concrete kinematic derivations. We formulated planning as the search for a continuous curve in a feasible subset of C-space, interpreted feasibility via path-connectedness, and introduced basic discretizations that connect continuous C-space to graph-based search. We also developed language-agnostic implementations of C-space interpolation that respect periodic coordinates. Subsequent lessons will refine the feasible set to account for obstacles and topological complexity, and will introduce metrics and advanced manifold structures to better model high-DOF robots.