Let \( \mathcal{C} \) denote the configuration space of an \(n\)-DOF robot, with configurations \( \mathbf{q} \in \mathcal{C} \). A metric is a function \( d : \mathcal{C} \times \mathcal{C} \rightarrow \mathbb{R}_{\ge 0} \) satisfying:

\[ \begin{aligned} &\text{(M1) } d(\mathbf{q},\mathbf{q}') \ge 0,\quad d(\mathbf{q},\mathbf{q}') = 0 \iff \mathbf{q} = \mathbf{q}',\\ &\text{(M2) } d(\mathbf{q},\mathbf{q}') = d(\mathbf{q}',\mathbf{q}),\\ &\text{(M3) } d(\mathbf{q},\mathbf{q}'') \le d(\mathbf{q},\mathbf{q}') + d(\mathbf{q}',\mathbf{q}'') \quad \text{(triangle inequality).} \end{aligned} \]

In motion planning, metrics are used to:

• Measure path length in \( \mathcal{C} \) (for optimality and smoothing).
• Define neighborhoods (e.g., nearest neighbors in future sampling-based planners).
• Encode robot-dependent structure (e.g., joint limits, workspace sensitivity, dynamics).

For high-DOF manipulators, naive Euclidean distance between joint vectors can be misleading: a small motion of a distal joint can move the end-effector more than a large proximal motion; angular wrap-around must be handled; and dynamics can make some directions “heavier” than others. We therefore need carefully designed metrics.

flowchart TD
  A["Robot model (links, joints)"] --> B["Configuration space C (dimension n)"]
  B --> C["Choose joint-level distances (R, S1, SE(3), etc.)"]
  C --> D["Combine into product metric on C"]
  D --> E["Refine with weights (limits, scaling)"]
  E --> F["Optionally use Jacobian / mass matrix"]
  F --> G["Final metric d(q1,q2)"]
  G --> H["Used in planning, nearest neighbor, path length"]
        

When \( \mathcal{C} \subseteq \mathbb{R}^n \) (e.g., prismatic or joint-angle vectors without wrap-around), a common class of metrics is induced by norms. Let \( \|\cdot\| \) be a norm on \( \mathbb{R}^n \). Define

\[ d(\mathbf{q}_1,\mathbf{q}_2) \;=\; \|\mathbf{q}_1 - \mathbf{q}_2\|. \]

Then \( d \) is a metric, because the norm axioms directly imply (M1)–(M3). Typical choices:

• Euclidean (ℓ₂) metric:

  \[ d_2(\mathbf{q}_1,\mathbf{q}_2) = \big\|\mathbf{q}_1 - \mathbf{q}_2\big\|_2 = \sqrt{\sum_{i=1}^n (q_{1,i}-q_{2,i})^2}. \]

• Weighted Euclidean metric with symmetric positive definite (SPD) matrix \( W \in \mathbb{R}^{n\times n} \):

  \[ d_W(\mathbf{q}_1,\mathbf{q}_2) = \sqrt{(\mathbf{q}_1-\mathbf{q}_2)^\top W\,(\mathbf{q}_1-\mathbf{q}_2)}. \]

• ℓ₁ and ℓ_∞ metrics:

  \[ d_1(\mathbf{q}_1,\mathbf{q}_2) = \sum_{i=1}^n |q_{1,i}-q_{2,i}|,\quad d_\infty(\mathbf{q}_1,\mathbf{q}_2) = \max_{1\le i\le n}|q_{1,i}-q_{2,i}|. \]

In finite dimensions, all norms are equivalent: there exist constants \( \alpha,\beta > 0 \) such that

\[ \alpha \,\|\mathbf{v}\|_a \le \|\mathbf{v}\|_b \le \beta \,\|\mathbf{v}\|_a \quad \forall\,\mathbf{v}\in\mathbb{R}^n. \]

Consequently, any two norm-induced metrics on \( \mathbb{R}^n \) define the same open sets and convergence concept. However, the numerical values of distances differ and strongly affect algorithmic performance in high-DOF planning.

A typical manipulator has a mix of revolute and prismatic joints. For joint \(i\), let:

• Revolute: configuration variable \( q_i \in S^1 \), usually represented by an angle \( \theta_i \in \mathbb{R} \) modulo \( 2\pi \).
• Prismatic: configuration variable \( q_i \in \mathbb{R} \), usually bounded by limits.

For an angle pair \( \theta_1, \theta_2 \), the geodesic distance on the circle is the length of the shortest arc:

\[ d_{S^1}(\theta_1,\theta_2) = \min\big(|\Delta|,\; 2\pi - |\Delta|\big), \quad \Delta = \theta_2 - \theta_1. \]

For a prismatic joint, we typically use the absolute difference:

\[ d_{\mathbb{R}}(q_1,q_2) = |q_1-q_2|. \]

For an \(n\)-DOF arm with joint types \( \text{type}_i \in \{\text{revolute},\text{prismatic}\} \) and positive joint weights \( w_i > 0 \), a standard product metric is

\[ d(\mathbf{q}_1,\mathbf{q}_2) = \sqrt{\sum_{i=1}^n w_i^2\,\delta_i(\mathbf{q}_1,\mathbf{q}_2)^2}, \]

where

\[ \delta_i(\mathbf{q}_1,\mathbf{q}_2) = \begin{cases} d_{S^1}(q_{1,i},q_{2,i}) & \text{if joint } i \text{ is revolute} \\ |q_{1,i} - q_{2,i}| & \text{if joint } i \text{ is prismatic.} \end{cases} \]

The weights \( w_i \) can encode:

• Different joint ranges (e.g., normalize by joint span).
• Preferred usage (penalize certain joints more heavily).
• Velocity limits (scale coordinates by max velocity).

This product metric is the workhorse for practical implementations and will be used in the multi-language code section below.

Configuration space \( \mathcal{C} \) is often a smooth manifold (e.g., products of circles and intervals). A Riemannian metric is a smoothly varying inner product on each tangent space \( T_{\mathbf{q}}\mathcal{C} \):

\[ g_{\mathbf{q}}(\mathbf{v},\mathbf{w}) = \mathbf{v}^\top G(\mathbf{q})\,\mathbf{w}, \quad \mathbf{v},\mathbf{w} \in T_{\mathbf{q}}\mathcal{C}, \]

where \( G(\mathbf{q}) \) is SPD. Local distances are defined by

\[ \|\mathbf{v}\|_{\mathbf{q}} = \sqrt{g_{\mathbf{q}}(\mathbf{v},\mathbf{v})} = \sqrt{\mathbf{v}^\top G(\mathbf{q})\mathbf{v}}, \]

and global distances are geodesic lengths (minimal path integrals).

4.1 Task-Space Metrics via the Jacobian

Consider the forward kinematics \( \mathbf{x} = f(\mathbf{q}) \in \mathbb{R}^m \) (task space, usually end-effector pose parameters). Let \( \mathbf{J}(\mathbf{q}) \in \mathbb{R}^{m\times n} \) be the Jacobian, so that for small motions

\[ \Delta \mathbf{x} \approx \mathbf{J}(\mathbf{q})\,\Delta \mathbf{q}. \]

Suppose task space carries a simple metric \( \|\Delta \mathbf{x}\|_W^2 = \Delta \mathbf{x}^\top W \Delta \mathbf{x} \) with SPD \( W \in \mathbb{R}^{m\times m} \). Then pulling this back to configuration space gives

\[ \|\Delta \mathbf{q}\|_{\mathbf{q}}^2 = \Delta \mathbf{q}^\top \mathbf{J}(\mathbf{q})^\top W\, \mathbf{J}(\mathbf{q})\,\Delta \mathbf{q}. \]

Thus the Riemannian metric tensor is

\[ G(\mathbf{q}) = \mathbf{J}(\mathbf{q})^\top W\,\mathbf{J}(\mathbf{q}). \]

This metric measures configuration changes by their induced motion in task space, possibly with anisotropic weighting (e.g., prioritize translational motion over orientation).

4.2 Dynamics-Aware Metrics via the Mass Matrix

For a rigid robot, kinetic energy is

\[ T(\mathbf{q},\dot{\mathbf{q}}) = \tfrac{1}{2} \,\dot{\mathbf{q}}^\top M(\mathbf{q})\,\dot{\mathbf{q}}, \]

where \( M(\mathbf{q}) \) is the joint-space mass matrix, symmetric and SPD. From the viewpoint of Riemannian geometry, \( M(\mathbf{q}) \) defines a metric

\[ g_{\mathbf{q}}(\mathbf{v},\mathbf{w}) = \mathbf{v}^\top M(\mathbf{q})\,\mathbf{w}, \]

meaning that directions with high inertia are “longer” in this metric. Distances induced by \( M \) correlate with dynamic effort and are therefore well suited to cost functionals that penalize kinetic energy or torque.

In practice, one often uses a regularized approximation:

\[ G(\mathbf{q}) = M(\mathbf{q}) + \lambda I, \quad \lambda > 0, \]

which ensures robustness near singular configurations and yields a well-conditioned metric for numerical algorithms.

We now outline a generic procedure for computing a configuration distance for an \(n\)-DOF robot with revolute and prismatic joints and per-joint weights \( w_i > 0 \). The algorithm is simple yet robust and forms the basis for the code implementations.

flowchart TD
  S["Input: q1[1..n], q2[1..n], \njoint_types[1..n], \nweights[1..n]"] --> L1["sum_sq = 0"]
  L1 --> L2["for i in 1..n"]
  L2 --> C1{"joint_types[i] == \n'revolute' ?"}
  C1 -->|yes| R1["delta = q2[i] - q1[i]
delta = wrap_to_pi(delta)"]
  C1 -->|no| P1["delta = q2[i] - q1[i]"]
  R1 --> A1["sum_sq += (weights[i]*delta)^2"]
  P1 --> A1
  A1 --> L2
  L2 --> L3["d = sqrt(sum_sq)"]
  L3 --> OUT["Return d"]
        

Here, wrap_to_pi maps any real number \( \Delta \) to the interval \((-\pi,\pi]\) by subtracting or adding multiples of \( 2\pi \), implementing the circle geodesic. This ensures correct distances across the \( 2\pi \) wrap-around.

We implement the mixed-joint metric in five environments. The focus is on a pure configuration-space distance; later in the course, this function will be plugged into planners and trajectory optimization routines.

6.1 Python Implementation (with NumPy)

import numpy as np

REVOLUTE = 0
PRISMATIC = 1

def wrap_to_pi(angle):
    """Map any angle (rad) to (-pi, pi]."""
    # Using modular arithmetic with numpy
    return (angle + np.pi) % (2.0 * np.pi) - np.pi

def config_distance(q1, q2, joint_types, weights=None):
    """
    Compute weighted configuration distance between q1 and q2.

    Parameters
    ----------
    q1, q2 : array-like, shape (n,)
        Joint configurations.
    joint_types : array-like of ints (REVOLUTE or PRISMATIC)
    weights : array-like of shape (n,), optional
        Positive weights. If None, all ones.
    """
    q1 = np.asarray(q1, dtype=float)
    q2 = np.asarray(q2, dtype=float)
    joint_types = np.asarray(joint_types, dtype=int)

    assert q1.shape == q2.shape
    n = q1.size
    assert joint_types.size == n

    if weights is None:
        weights = np.ones(n, dtype=float)
    else:
        weights = np.asarray(weights, dtype=float)
        assert weights.size == n

    deltas = np.empty(n, dtype=float)
    for i in range(n):
        if joint_types[i] == REVOLUTE:
            delta = q2[i] - q1[i]
            delta = wrap_to_pi(delta)
        else:  # PRISMATIC
            delta = q2[i] - q1[i]
        deltas[i] = weights[i] * delta

    return np.linalg.norm(deltas, ord=2)

if __name__ == "__main__":
    # Example: 7-DOF arm: R-R-P-R-R-R-P
    q1 = [0.0, 0.5, 0.1, -1.0, 0.3, 0.2, 0.05]
    q2 = [np.pi - 0.1, 0.4, 0.15, -0.8, 0.0, 0.0, 0.10]
    joint_types = [REVOLUTE, REVOLUTE, PRISMATIC,
                   REVOLUTE, REVOLUTE, REVOLUTE, PRISMATIC]
    # Example weights: normalize revolute joints by pi, prismatic by 0.5 m
    weights = [1.0 / np.pi, 1.0 / np.pi, 1.0 / 0.5,
               1.0 / np.pi, 1.0 / np.pi, 1.0 / np.pi, 1.0 / 0.5]

    d = config_distance(q1, q2, joint_types, weights)
    print("Configuration distance:", d)
      

In Python-based robotics stacks (e.g., OMPL Python bindings, future planning labs), this function can be passed as the state-space distance callback.

6.2 C++ Implementation (Eigen-style, header-only)

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

enum JointType { REVOLUTE = 0, PRISMATIC = 1 };

inline double wrapToPi(double angle)
{
    // Map angle to (-pi, pi]
    const double twoPi = 2.0 * M_PI;
    angle = std::fmod(angle + M_PI, twoPi);
    if (angle < 0.0)
        angle += twoPi;
    return angle - M_PI;
}

double configDistance(const std::vector<double>& q1,
                      const std::vector<double>& q2,
                      const std::vector<JointType>& jointTypes,
                      const std::vector<double>& weights)
{
    const std::size_t n = q1.size();
    assert(q2.size() == n);
    assert(jointTypes.size() == n);
    assert(weights.size() == n);

    double sumSq = 0.0;
    for (std::size_t i = 0; i < n; ++i)
    {
        double delta = q2[i] - q1[i];
        if (jointTypes[i] == REVOLUTE)
        {
            delta = wrapToPi(delta);
        }
        const double wdelta = weights[i] * delta;
        sumSq += wdelta * wdelta;
    }
    return std::sqrt(sumSq);
}

// Example usage in a planning library:
// double distance(const State* s1, const State* s2) {
//     // Extract q1, q2 from state representation and call configDistance(...)
// }
      

This function can be plugged into C++ planning frameworks (e.g., OMPL or custom planners) as the state metric. Using std::vector<double> keeps the code flexible; in practice, Eigen::VectorXd or fixed-size vectors may be more efficient.

6.3 Java Implementation

public class ConfigMetric {

    public enum JointType { REVOLUTE, PRISMATIC }

    private static double wrapToPi(double angle) {
        double twoPi = 2.0 * Math.PI;
        angle = (angle + Math.PI) % twoPi;
        if (angle < 0.0) {
            angle += twoPi;
        }
        return angle - Math.PI;
    }

    public static double distance(double[] q1,
                                  double[] q2,
                                  JointType[] jointTypes,
                                  double[] weights) {

        int n = q1.length;
        if (q2.length != n || jointTypes.length != n || weights.length != n) {
            throw new IllegalArgumentException("Dimension mismatch");
        }

        double sumSq = 0.0;
        for (int i = 0; i < n; ++i) {
            double delta = q2[i] - q1[i];
            if (jointTypes[i] == JointType.REVOLUTE) {
                delta = wrapToPi(delta);
            }
            double wdelta = weights[i] * delta;
            sumSq += wdelta * wdelta;
        }
        return Math.sqrt(sumSq);
    }

    public static void main(String[] args) {
        double[] q1 = {0.0, 0.5, 0.1};
        double[] q2 = {Math.PI - 0.2, 0.6, 0.05};
        JointType[] jointTypes = {
            JointType.REVOLUTE,
            JointType.REVOLUTE,
            JointType.PRISMATIC
        };
        double[] weights = {1.0 / Math.PI, 1.0 / Math.PI, 2.0};

        double d = distance(q1, q2, jointTypes, weights);
        System.out.println("Configuration distance = " + d);
    }
}
      

In Java-based robotics middleware or simulation environments, this static method can be attached to the configuration space representation.

6.4 MATLAB / Simulink Implementation

function d = configDistance(q1, q2, jointTypes, weights)
%CONFIGDISTANCE Weighted configuration-space distance for mixed joints.
%   q1, q2       : column vectors (n-by-1) or row vectors (1-by-n)
%   jointTypes   : cell array of strings: 'R' (revolute) or 'P' (prismatic)
%   weights      : numeric vector of length n

    q1 = q1(:);
    q2 = q2(:);
    n  = numel(q1);

    assert(numel(q2) == n, 'Dimension mismatch q2');
    assert(numel(jointTypes) == n, 'Dimension mismatch jointTypes');
    assert(numel(weights) == n, 'Dimension mismatch weights');

    deltas = zeros(n,1);
    for i = 1:n
        delta = q2(i) - q1(i);
        if jointTypes{i} == 'R'
            % Wrap to (-pi, pi]
            delta = wrapToPi(delta);
        end
        deltas(i) = weights(i) * delta;
    end

    d = norm(deltas, 2);
end

function ang = wrapToPi(ang)
%WRAPTOPI Wrap angle in radians to (-pi, pi]
    ang = mod(ang + pi, 2*pi);
    if ang < 0
        ang = ang + 2*pi;
    end
    ang = ang - pi;
end

% Example usage:
% q1 = [0; 0.5; 0.1];
% q2 = [pi-0.2; 0.4; 0.15];
% jointTypes = {'R','R','P'};
% weights = [1/pi; 1/pi; 2];
% d = configDistance(q1, q2, jointTypes, weights);
      

Simulink integration: place a MATLAB Function block inside a Simulink model and paste configDistance and wrapToPi into the function body. Provide inputs q1, q2 as vectors and feed the output d into downstream blocks (e.g., a cost computation or planning subsystem).

6.5 Wolfram Mathematica Implementation

Clear[wrapToPi, configDistance];

wrapToPi[angle_] := Module[{twoPi = 2.0*Pi, a = angle},
  a = Mod[a + Pi, twoPi];
  If[a < 0.0, a = a + twoPi];
  a - Pi
];

(* jointTypes: list of "R" or "P" *)
configDistance[q1_List, q2_List, jointTypes_List, weights_List] := Module[
  {n, deltas},
  n = Length[q1];
  If[Length[q2] != n || Length[jointTypes] != n || Length[weights] != n,
    Return[$Failed];
  ];
  deltas = Table[
    Module[{delta = q2[[i]] - q1[[i]]},
      If[jointTypes[[i]] === "R",
        delta = wrapToPi[delta];
      ];
      weights[[i]]*delta
    ],
    {i, 1, n}
  ];
  Norm[deltas, 2]
];

(* Example *)
q1 = {0.0, 0.5, 0.1};
q2 = {Pi - 0.2, 0.4, 0.15};
jointTypes = {"R", "R", "P"};
weights = {1.0/Pi, 1.0/Pi, 2.0};

d = configDistance[q1, q2, jointTypes, weights]
      

Mathematica’s symbolic capabilities can also be used (outside the scope of this lesson) to analytically study geodesics induced by Jacobian- or mass-based metrics.

7.1 Weighted Euclidean Metric is a Metric

Let \( W \in \mathbb{R}^{n\times n} \) be SPD. Define

\[ d_W(\mathbf{q}_1,\mathbf{q}_2) = \sqrt{(\mathbf{q}_1-\mathbf{q}_2)^\top W\,(\mathbf{q}_1-\mathbf{q}_2)}. \]

Define the norm \( \|\mathbf{v}\|_W = \sqrt{\mathbf{v}^\top W \mathbf{v}} \). Since \( W \) is SPD, this is an inner-product norm induced by \( \langle \mathbf{v},\mathbf{w} \rangle_W = \mathbf{v}^\top W \mathbf{w} \). Thus \( \|\cdot\|_W \) satisfies all norm axioms, and \( d_W \) is a metric by Section 2.

Moreover, if \( \lambda_{\min},\lambda_{\max} \) are the minimal and maximal eigenvalues of \( W \), then for any \( \mathbf{v} \),

\[ \lambda_{\min} \|\mathbf{v}\|_2^2 \le \mathbf{v}^\top W \mathbf{v} \le \lambda_{\max} \|\mathbf{v}\|_2^2, \]

so

\[ \sqrt{\lambda_{\min}} \,\|\mathbf{v}\|_2 \le \|\mathbf{v}\|_W \le \sqrt{\lambda_{\max}} \,\|\mathbf{v}\|_2, \]

proving norm equivalence. Thus all SPD-weighted metrics on \( \mathbb{R}^n \) are topologically equivalent to the Euclidean metric.

7.2 Circle Metric \( d_{S^1} \) is a Metric

Recall

\[ d_{S^1}(\theta_1,\theta_2) = \min\big(|\theta_2-\theta_1|,\; 2\pi - |\theta_2-\theta_1|\big). \]

Non-negativity and symmetry are obvious. Identity of indiscernibles holds because \( d_{S^1}(\theta_1,\theta_2) = 0 \) implies \( |\theta_2-\theta_1| \) is a multiple of \( 2\pi \), so the two angles represent the same point on the circle.

For the triangle inequality, note that each angle corresponds to a point on the unit circle and \( d_{S^1} \) is proportional to the shortest arc length between them. Shortest path lengths on any metric graph (here the circle) satisfy the triangle inequality: concatenating two arcs between \( \theta_1\rightarrow\theta_2 \) and \( \theta_2\rightarrow\theta_3 \) cannot be shorter than the shortest arc between \( \theta_1 \) and \( \theta_3 \).

Problem 1 (SPD-Weighted Metric and Norm Equivalence): Let \( W \in \mathbb{R}^{n\times n} \) be SPD and \( d_W(\mathbf{q}_1,\mathbf{q}_2) \) as in Section 7.1. Show that \( d_W \) is a metric and that there exist constants \( 0 < \alpha \le \beta \) such that \[ \alpha\, d_2(\mathbf{q}_1,\mathbf{q}_2) \le d_W(\mathbf{q}_1,\mathbf{q}_2) \le \beta\, d_2(\mathbf{q}_1,\mathbf{q}_2) \] for all \( \mathbf{q}_1,\mathbf{q}_2 \).

Solution: The first part is as in Section 7.1: \( \|\cdot\|_W \) is a norm induced by the inner product \( \langle \mathbf{v},\mathbf{w} \rangle_W \), so \( d_W \) is a metric. For equivalence, take \( \mathbf{v} = \mathbf{q}_1 - \mathbf{q}_2 \). Using the eigenvalue bounds,

\[ \lambda_{\min}\|\mathbf{v}\|_2^2 \le \mathbf{v}^\top W \mathbf{v} \le \lambda_{\max}\|\mathbf{v}\|_2^2, \]

and taking square roots,

\[ \sqrt{\lambda_{\min}} \,\|\mathbf{v}\|_2 \le d_W(\mathbf{q}_1,\mathbf{q}_2) \le \sqrt{\lambda_{\max}} \,\|\mathbf{v}\|_2. \]

Thus choose \( \alpha = \sqrt{\lambda_{\min}} \), \( \beta = \sqrt{\lambda_{\max}} \).

Problem 2 (Product Metric for \(S^1\times\mathbb{R}\)): Consider a planar 2-DOF robot with one revolute joint \( q_1 \in S^1 \) and one prismatic joint \( q_2 \in \mathbb{R} \). Define

\[ d((q_1,q_2),(q_1',q_2')) = \sqrt{w_1^2 d_{S^1}(q_1,q_1')^2 + w_2^2 (q_2-q_2')^2}, \]

with \( w_1,w_2 > 0 \). Show that \( d \) is a metric on \( S^1\times\mathbb{R} \).

Solution: Let \( \delta_1 = d_{S^1}(q_1,q_1') \), \( \delta_2 = |q_2-q_2'| \). Define \( \mathbf{\delta} = (w_1\delta_1,w_2\delta_2)^\top\in\mathbb{R}^2 \). Then \( d((q_1,q_2),(q_1',q_2')) = \|\mathbf{\delta}\|_2 \). Since \( d_{S^1} \) and the absolute value are metrics on their respective spaces, and the product of metric spaces endowed with the Euclidean norm of component distances is a metric, \( d \) satisfies (M1)–(M3). Explicitly:

• Non-negativity and symmetry follow from those of \( d_{S^1} \) and \( |\cdot| \).
• \( d((q_1,q_2),(q_1',q_2')) = 0 \) implies \( \delta_1=\delta_2=0 \), so \( q_1,q_1' \) represent the same angle and \( q_2=q_2' \).
• The triangle inequality follows from the coordinate-wise triangle inequalities and the Euclidean norm’s triangle inequality: \[ \|\mathbf{\delta}^{(13)}\|_2 \le \|\mathbf{\delta}^{(12)} + \mathbf{\delta}^{(23)}\|_2 \le \|\mathbf{\delta}^{(12)}\|_2 + \|\mathbf{\delta}^{(23)}\|_2. \]

Problem 3 (Jacobian-Based Local Metric): Let \( \mathbf{x} = f(\mathbf{q}) \in \mathbb{R}^m \) be task-space coordinates and \( \mathbf{J}(\mathbf{q}) = \partial f/\partial \mathbf{q} \). Assume task space has Euclidean metric \( \|\Delta \mathbf{x}\|_2 \). Show that the pullback of this metric to configuration space is given (to first order) by \( G(\mathbf{q}) = \mathbf{J}(\mathbf{q})^\top \mathbf{J}(\mathbf{q}) \).

Solution: For small \( \Delta \mathbf{q} \), \( \Delta \mathbf{x} \approx \mathbf{J}(\mathbf{q})\,\Delta \mathbf{q} \). The squared task-space length is

\[ \|\Delta \mathbf{x}\|_2^2 \approx \|\mathbf{J}(\mathbf{q})\,\Delta \mathbf{q}\|_2^2 = \Delta \mathbf{q}^\top \mathbf{J}(\mathbf{q})^\top \mathbf{J}(\mathbf{q})\,\Delta \mathbf{q}. \]

Hence the local inner product is \( g_{\mathbf{q}}(\mathbf{v},\mathbf{w}) = \mathbf{v}^\top \mathbf{J}(\mathbf{q})^\top\mathbf{J}(\mathbf{q})\mathbf{w} \), giving \( G(\mathbf{q}) = \mathbf{J}(\mathbf{q})^\top \mathbf{J}(\mathbf{q}) \).

Problem 4 (Angular Wrap-Around Edge Case): Suppose a single revolute joint with \( q_1 = -\pi + \epsilon \) and \( q_2 = \pi - \epsilon \) for small \( \epsilon > 0 \).

1. Compute the naive Euclidean distance \( |q_2 - q_1| \).
2. Compute \( d_{S^1}(q_1,q_2) \).
3. Explain why using \( d_{S^1} \) is crucial in planning.

Solution:

1. Naive distance: \[ |q_2 - q_1| = |(\pi-\epsilon) - (-\pi+\epsilon)| = |2\pi - 2\epsilon| \approx 2\pi. \]
2. Geodesic distance: \[ d_{S^1}(q_1,q_2) = \min\big(|2\pi-2\epsilon|, 2\pi - |2\pi-2\epsilon|\big) = \min(2\pi-2\epsilon, 2\epsilon) = 2\epsilon. \]
3. In configuration space, these two poses are almost identical; using naive distance would make them appear far apart, causing nearest-neighbor searches and path smoothing to fail badly. Using \( d_{S^1} \) correctly captures the topology of \( S^1 \).

In this lesson we formalized metrics and distance functions on high-DOF configuration spaces. We studied norm-induced metrics on \( \mathbb{R}^n \), product metrics for mixed revolute/prismatic joints with proper angular wrap-around, and Riemannian metrics derived from the Jacobian and mass matrix. We proved that SPD-weighted Euclidean metrics are true metrics and are equivalent to the standard Euclidean metric, and that circle distances \( d_{S^1} \) satisfy the metric axioms. Finally, we implemented a practical distance function in Python, C++, Java, MATLAB/Simulink, and Mathematica, which will be reused in subsequent planning and trajectory-optimization chapters.