At the core of motion planning and manipulation is a metric space: \( (\mathcal{X}, d) \), where \( \mathcal{X} \) is typically a configuration space, state space, or space of trajectories, and \( d \) is a metric. Formally, a metric is a function

\[ d : \mathcal{X}\times\mathcal{X} \rightarrow \mathbb{R}_{\ge 0} \]

such that for all \( x,y,z \in \mathcal{X} \):

\[ \begin{aligned} &\text{(M1) Non\text{-}negativity:}&\quad d(x,y) \ge 0,\\ &\text{(M2) Identity of indiscernibles:}&\quad d(x,y)=0 \Leftrightarrow x=y,\\ &\text{(M3) Symmetry:}&\quad d(x,y)=d(y,x),\\ &\text{(M4) Triangle inequality:}&\quad d(x,z)\le d(x,y)+d(y,z). \end{aligned} \]

In robotics, metrics play two roles:

• Geometry of the search space: nearest-neighbor queries in PRM/RRT, heuristic costs in discrete planners, local optimization steps.
• Quality measures for solutions and algorithms: path length, clearance, smoothness, energy, success rates, sample efficiency, etc.

A general planner output is a set of feasible trajectories \( \{\sigma_i\} \) in some space \( \mathcal{X} \). Metrics turn this set into ordered solutions via a cost functional \( J(\sigma) \) built from \( d \) and other terms.

flowchart TD
  A["Problem spec: space X, obstacles, dynamics"] --> B["Planner / policy (PRM, RRT, TrajOpt, RL)"]
  B --> C["Candidate paths / trajectories sigma_i"]
  C --> D["Geometric metrics: \npath_length, clearance"]
  C --> E["Dynamic metrics: \ntime, energy, smoothness"]
  C --> F["Task metrics: \nsuccess rate, accuracy, safety"]
  D --> G["Aggregate scores / Pareto front"]
  E --> G
  F --> G
  G --> H["Benchmark tables, plots, and comparisons"]
        

The rest of this lesson builds these pieces formally and shows how to compute them in practice.

Let the configuration space be \( \mathcal{C}\subset\mathbb{R}^n \) (or more generally a smooth manifold, as in earlier chapters). A common choice of metric is a weighted Euclidean metric.

\[ d_W(q_1,q_2) \;=\; \sqrt{(q_2-q_1)^{\top} W (q_2-q_1)}, \quad W \in \mathbb{R}^{n\times n} \text{ symmetric positive definite.} \]

The diagonal entries of \( W \) encode how "expensive" motion in each joint is, e.g. heavy base motions vs. light wrist rotations.

2.1 Metrics with Revolute Joints (Manifold Structure)

For a revolute joint angle \( \theta \in \mathbb{S}^1 \), a natural distance is

\[ \delta(\theta_1,\theta_2) \;=\; \operatorname{atan2}\!\bigl( \sin(\theta_2-\theta_1), \cos(\theta_2-\theta_1) \bigr), \quad d_{\text{ang}}(\theta_1,\theta_2)=|\delta(\theta_1,\theta_2)|. \]

This chooses the shorter arc on the circle. For a configuration \( q=(p,\theta) \) with prismatic/joint coordinates \( p\in\mathbb{R}^m \) and revolute angles \( \theta\in \mathbb{S}^k \), we can combine prismatic and angular distances:

\[ d(q_1,q_2)^2 \;=\; \sum_{i=1}^{m} w_i (p_{2,i}-p_{1,i})^2 + \sum_{j=1}^{k} w_{m+j}\,\delta(\theta_{1,j},\theta_{2,j})^2. \]

2.2 Proof that Weighted Euclidean Distance is a Metric

Assume \( W \) is symmetric positive definite. Then there exists an invertible matrix \( L \) (e.g. Cholesky factor) such that \( W=L^{\top}L \). For \( \Delta=q_2-q_1 \),

\[ d_W(q_1,q_2)^2 = \Delta^{\top}W\Delta = \Delta^{\top}L^{\top}L\Delta = \Vert L\Delta\Vert_2^2. \]

Define \( \Vert x\Vert_W := \Vert Lx\Vert_2 \). This is a norm because:

• \( \Vert x\Vert_W \ge 0 \) and \( \Vert x\Vert_W=0 \) implies \( Lx=0 \), hence \( x=0 \) (since \( L \) invertible).
• \( \Vert x\Vert_W=\Vert -x\Vert_W \) (symmetry of Euclidean norm).
• Triangle inequality: \( \Vert x+z\Vert_W = \Vert Lx+Lz\Vert_2 \le \Vert Lx\Vert_2+\Vert Lz\Vert_2 = \Vert x\Vert_W+\Vert z\Vert_W. \)

Therefore \( d_W(q_1,q_2)=\Vert q_2-q_1\Vert_W \) satisfies (M1)–(M4) and is a valid metric.

2.3 State-Space Metrics

For kinodynamic planning, states are \( x=(q,\dot{q}) \in \mathcal{X}\subset\mathbb{R}^{2n} \). A typical metric is

\[ d_{\text{state}}(x_1,x_2)^2 = \Vert q_2-q_1\Vert_{W_q}^2 + \Vert \dot{q}_2-\dot{q}_1\Vert_{W_{\dot{q}}}^2, \]

where \( W_q, W_{\dot{q}} \) are positive definite weight matrices. This metric is crucial for sampling-based kinodynamic planners and for defining "closeness" of states in PRM-like graphs in state space.

For manipulation, the robot's end-effector pose or contact wrench is often more meaningful than raw joint angles. Let \( f:\mathcal{C}\rightarrow\mathcal{Y} \) map configuration to task space (e.g. \( \mathcal{Y} = SE(3) \) pose space).

3.1 Task-Space Induced Metrics

A simple task-space distance between two configurations is

\[ d_{\text{task}}(q_1,q_2) = \Vert f(q_2)-f(q_1)\Vert_{W_{\text{task}}}, \]

where \( \Vert\cdot\Vert_{W_{\text{task}}} \) is a weighted norm in position/orientation coordinates (e.g. meters for position, radians for orientation).

Locally, if \( J(q)=\partial f/\partial q \) is the manipulator Jacobian, then for small \( \delta q \),

\[ f(q+\delta q)-f(q) \approx J(q)\,\delta q, \quad d_{\text{task}}(q,q+\delta q)^2 \approx \delta q^{\top} M(q)\,\delta q \]

with the configuration-space metric tensor

\[ M(q) = J(q)^{\top} W_{\text{task}} J(q). \]

This is a Riemannian metric on \( \mathcal{C} \) that biases motion according to task-space sensitivity.

3.2 Velocity Manipulability

For an \( m\times n \) Jacobian \( J(q) \) (\( m \) task dimensions, \( n \) joints), the Yoshikawa manipulability is

\[ w(q) = \sqrt{\det\bigl(J(q)J(q)^{\top}\bigr)}. \]

If \( \sigma_1,\dots,\sigma_m \) are the singular values of \( J(q) \), then \( w(q)=\prod_{i=1}^{m}\sigma_i \). Large manipulability implies a large "velocity ellipsoid" in task space: velocities \( \dot{y}=J(q)\dot{q} \) that can be produced with unit-norm joint velocities.

For a planar 2-link arm, one can show (Problem 2) that \( w(q) = |l_1 l_2 \sin(q_2)| \). Singularities (zero manipulability) occur when the arm is fully stretched or folded (\( q_2=0 \) or \( q_2=\pi \)), where the end-effector loses a degree of freedom.

3.3 Grasp Quality Metrics (Connection)

Earlier we studied grasp wrench space and force-closure. Many grasp quality metrics have the form

\[ Q_{\text{grasp}}(G) = \Phi\bigl(\mathcal{W}(G)\bigr), \]

where \( G \) is the grasp map, \( \mathcal{W}(G) \) the resulting wrench space, and \( \Phi \) some function of its shape (e.g. minimum distance of the origin to the boundary, isotropy index). These are task-space metrics defined on spaces of possible wrenches and directly comparable across different hands or grasps.

A path is a continuous curve \( \sigma:[0,1]\rightarrow\mathcal{C} \). Given a configuration-space metric \( \Vert\cdot\Vert_M \) with SPD matrix \( M \), the arc length of \( \sigma \) is

\[ L(\sigma) = \int_0^1 \bigl\Vert \dot{\sigma}(t)\bigr\Vert_M \, dt = \int_0^1 \sqrt{ \dot{\sigma}(t)^{\top} M\,\dot{\sigma}(t) }\,dt. \]

In practice, planners work with discrete sequences \( q_0,\dots,q_N \). A consistent Riemann sum approximation is

\[ \widehat{L}(\sigma) = \sum_{k=0}^{N-1} d_M(q_k,q_{k+1}), \quad d_M(q_k,q_{k+1}) = \bigl\Vert q_{k+1}-q_k\bigr\Vert_M. \]

4.1 Clearance Metrics

Let \( \mathcal{C}_{\text{obs}} \) be the obstacle region. The clearance of a configuration is

\[ \text{clr}(q) = \inf_{q'\in\mathcal{C}_{\text{obs}}} d(q,q'). \]

For a path \( \sigma \), we often use the minimum clearance

\[ \text{clr}_{\min}(\sigma) = \inf_{t\in[0,1]} \text{clr}\bigl(\sigma(t)\bigr) \approx \min_{k} \text{clr}(q_k), \]

or an integrated clearance for robustness. Planners balance path length against clearance by defining a composite cost, e.g.

\[ J_{\text{geo}}(\sigma) = L(\sigma) + \lambda_{\text{clr}}\, \Psi\bigl(\text{clr}_{\min}(\sigma)\bigr), \]

where \( \Psi \) is decreasing in clearance (e.g. \( \Psi(c)=1/(c+\epsilon) \) ).

4.2 Dynamic and Smoothness Metrics

When trajectories are time-parameterized \( q:[0,T]\rightarrow\mathbb{R}^n \), we may penalize velocities, accelerations, or jerks. A common smoothness cost is

\[ J_{\text{smooth}}(q) = \int_0^T \Vert \ddot{q}(t)\Vert_{W_{\ddot{q}}}^2 \,dt. \]

Energy-like metrics arise from dynamics:

\[ J_{\text{energy}}(q) = \int_0^T \Vert \tau(t)\Vert_{W_{\tau}}^2\,dt, \]

where \( \tau(t) \) are joint torques obtained from inverse dynamics. In trajectory optimization, the overall cost is typically

\[ J(q) = \alpha L(q) + \beta J_{\text{smooth}}(q) + \gamma J_{\text{energy}}(q) + \text{penalties for collisions, joint limits, etc.} \]

The choice of \( \alpha,\beta,\gamma \) dramatically affects motion characteristics and should reflect task priorities (speed vs. comfort, safety margin, energy consumption).

Beyond single-path quality, we need metrics comparing algorithms (planners, controllers, policies). Let runs be indexed by \( i=1,\dots,N \).

5.1 Success and Failure Statistics

Let \( S_i \) be the indicator of success in run \( i \). The empirical success rate is

\[ \hat{p}_{\text{succ}} = \frac{1}{N}\sum_{i=1}^N S_i. \]

If runs are independent with true success probability \( p_{\text{succ}} \), then \( \mathbb{E}[\hat{p}_{\text{succ}}]=p_{\text{succ}} \) (unbiasedness). Confidence intervals can be constructed using binomial theory.

5.2 Cost and Time Distributions

Let \( J_i \) be the cost functional (e.g. path length) for run \( i \), and \( T_i \) the planning time. Benchmark reports usually tabulate

• \( \bar{J} = \frac{1}{N}\sum_i J_i \) (mean cost),
• \( \bar{T} = \frac{1}{N}\sum_i T_i \) (mean planning time),
• quantiles (medians, 90th percentiles) to capture variability and outliers.

For asymptotically optimal planners (e.g. RRT\(^*\), PRM\(^*\)), one often studies how the cost decreases as a function of the number of samples or planning time.

5.3 Anytime Performance and Empirical Convergence

In an anytime setting, a planner returns a sequence of improving solutions with costs \( J^{(k)} \) at iteration \( k \). A typical metric is the normalized suboptimality

\[ \epsilon^{(k)} = \frac{J^{(k)}-J^{\star}}{J^{\star}}, \]

where \( J^{\star} \) is a reference cost (e.g. best cost over all planners or a known lower bound). Empirical curves \( \epsilon^{(k)} \) vs. time summarize convergence speed.

Many sampling-based planner guarantees (probabilistic completeness, asymptotic optimality) depend on the topology of the space, not on a specific choice of metric, as long as the metric is "reasonable".

6.1 Equivalence of Weighted Euclidean Metrics

Let \( W_1,W_2 \) be two SPD matrices on \( \mathbb{R}^n \) and \( d_{W_i} \) their induced metrics. There exist constants \( c_1,c_2 > 0 \) such that

\[ c_1\,d_{W_1}(q_1,q_2) \;\le\; d_{W_2}(q_1,q_2) \;\le\; c_2\,d_{W_1}(q_1,q_2) \quad\text{for all } q_1,q_2. \]

Sketch of proof. Let \( \lambda_{\min}(W) \), \( \lambda_{\max}(W) \) be the extreme eigenvalues. For any \( \Delta \in \mathbb{R}^n \),

\[ \lambda_{\min}(W)\Vert\Delta\Vert_2^2 \le \Delta^{\top}W\Delta \le \lambda_{\max}(W)\Vert\Delta\Vert_2^2. \]

Applying this to \( W_1 \) and \( W_2 \) and combining bounds yields the inequalities with appropriate constants. Thus the metrics are equivalent: they induce the same convergent sequences and open-set topology.

6.2 Reparameterization Invariance of Path Length

Let \( \sigma:[0,1]\rightarrow\mathcal{C} \) be a path and \( \phi:[0,1]\rightarrow[0,1] \) a smooth, strictly increasing function with \( \phi(0)=0,\phi(1)=1 \). Define \( \tilde{\sigma}=\sigma\circ\phi \). Then

\[ L(\tilde{\sigma}) = \int_0^1 \bigl\Vert \tfrac{d}{dt}\sigma(\phi(t))\bigr\Vert_M\,dt = \int_0^1 \bigl\Vert \dot{\sigma}(\phi(t))\,\phi'(t)\bigr\Vert_M\,dt. \]

Since the norm is homogeneous and \( \phi'(t)>0 \),

\[ L(\tilde{\sigma}) = \int_0^1 \bigl\Vert \dot{\sigma}(\phi(t))\bigr\Vert_M\,\phi'(t)\,dt. \]

With substitution \( u=\phi(t) \), \( du=\phi'(t)\,dt \), we obtain \( L(\tilde{\sigma}) = \int_0^1 \Vert \dot{\sigma}(u)\Vert_M\,du = L(\sigma) \). Thus length depends only on the geometric curve, not on how fast it is traversed.

In Python, numerical work is usually done with numpy. In motion-planning systems, Python bindings to OMPL or MoveIt can accept custom distance functions very similar to the one below.

import numpy as np

def wrap_angle(angle: float) -> float:
    """Wrap angle to [-pi, pi]."""
    return (angle + np.pi) % (2.0 * np.pi) - np.pi


class ConfigMetric:
    """
    Weighted configuration-space metric with support for revolute joints.

    weights: 1D array of positive weights.
    revolute: boolean mask, True for revolute joints.
    """
    def __init__(self, weights, revolute):
        self.W = np.diag(np.asarray(weights, dtype=float))
        self.revolute = np.asarray(revolute, dtype=bool)

    def distance(self, q1, q2) -> float:
        q1 = np.asarray(q1, dtype=float)
        q2 = np.asarray(q2, dtype=float)
        diff = q2 - q1
        # Wrap differences for revolute joints
        if self.revolute.any():
            diff[self.revolute] = (diff[self.revolute] + np.pi) % (2.0 * np.pi) - np.pi
        return float(np.sqrt(diff.T @ self.W @ diff))

    def path_length(self, path) -> float:
        """
        path: iterable of configurations (list or array of shape (N, n)).
        """
        path = np.asarray(path, dtype=float)
        total = 0.0
        for i in range(len(path) - 1):
            total += self.distance(path[i], path[i + 1])
        return float(total)


def path_clearance(path, dist_to_obstacles) -> float:
    """
    path: iterable of configurations.
    dist_to_obstacles: callable q -> clearance(q).
    """
    return float(min(dist_to_obstacles(q) for q in path))


def composite_cost(path, metric: ConfigMetric, dist_to_obstacles,
                   alpha: float = 1.0, beta: float = 1.0) -> float:
    """
    Example geometric cost:
        J = alpha * L(path) + beta / (clr_min(path) + eps)
    """
    L = metric.path_length(path)
    clr_min = path_clearance(path, dist_to_obstacles)
    eps = 1e-6
    return float(alpha * L + beta / (clr_min + eps))
      

In OMPL's Python bindings, an analogous function can be set via SpaceInformation.setStateValidityChecker and RealVectorStateSpace.setDistanceFunction, reusing the logic of ConfigMetric.distance.

In C++, planners such as OMPL and MoveIt use C++ classes to represent state spaces and distance functions. The following function implements a weighted configuration-space distance suitable for use as a custom distance function in such frameworks.

#include <vector>
#include <cmath>
#include <cstddef>

double wrapAngle(double angle)
{
    const double pi = 3.141592653589793;
    double a = std::fmod(angle + pi, 2.0 * pi);
    if (a < 0.0) {
        a += 2.0 * pi;
    }
    return a - pi;
}

// weights.size() == q1.size() == q2.size()
// revolute[i] is true if joint i is revolute.
double configDistance(const std::vector<double> &q1,
                      const std::vector<double> &q2,
                      const std::vector<double> &weights,
                      const std::vector<bool> &revolute)
{
    const std::size_t n = q1.size();
    double sum = 0.0;

    for (std::size_t i = 0; i < n; ++i) {
        double diff = q2[i] - q1[i];
        if (revolute[i]) {
            diff = wrapAngle(diff);
        }
        sum += weights[i] * diff * diff;
    }
    return std::sqrt(sum);
}
      

In OMPL, one would typically wrap this logic in a functor or lambda and pass it to ompl::base::RealVectorStateSpace::setDistanceFunction, or embed it inside a custom StateSpace subclass. The key observation is that the mathematical metric defined earlier corresponds one-to-one with such an implementation.

Java-based robotics stacks (e.g. ROSJava, certain industrial controllers) can use similar metric implementations. Below is a simple class that computes a weighted configuration-space distance.

public final class ConfigMetric {
    private final double[] weights;
    private final boolean[] revolute;

    public ConfigMetric(double[] weights, boolean[] revolute) {
        if (weights.length != revolute.length) {
            throw new IllegalArgumentException("Dimension mismatch");
        }
        this.weights = weights.clone();
        this.revolute = revolute.clone();
    }

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

    public double distance(double[] q1, double[] q2) {
        if (q1.length != q2.length || q1.length != weights.length) {
            throw new IllegalArgumentException("Dimension mismatch");
        }
        double sum = 0.0;
        for (int i = 0; i < q1.length; ++i) {
            double diff = q2[i] - q1[i];
            if (revolute[i]) {
                diff = wrapAngle(diff);
            }
            sum += weights[i] * diff * diff;
        }
        return Math.sqrt(sum);
    }
}
      

In a Java motion-planning library, this metric can be used to evaluate path lengths, nearest neighbors in a roadmap, or cost terms in optimization-based planners built atop numerical libraries such as EJML or Apache Commons Math.

MATLAB's Robotics System Toolbox provides kinematics and dynamics, while Simulink can simulate controllers and log joint trajectories. The function below computes path length and a jerk-based smoothness metric from joint position logs.

function metrics = evaluateTrajectory(q, t)
% Evaluate path length and jerk cost for a joint trajectory.
% q: N-by-n matrix of joint positions.
% t: N-by-1 vector of time stamps (strictly increasing).

dt = diff(t);           % (N-1)-by-1
dq = diff(q) ./ dt;     % (N-1)-by-n
ddq = diff(dq) ./ dt(1:end-1);      % (N-2)-by-n
dddq = diff(ddq) ./ dt(1:end-2);    % (N-3)-by-n

% Discrete path length in configuration space (unit weights).
L = 0.0;
for k = 1:(size(q, 1) - 1)
    diffq = q(k + 1, :) - q(k, :);
    L = L + norm(diffq, 2);
end

% Jerk-based smoothness cost: integral of squared jerk norm.
t_mid = t(3:end-1);           % time grid for jerk samples
jerk_sq = sum(dddq.^2, 2);    % squared jerk norm at each sample
smoothness = trapz(t_mid, jerk_sq);

metrics.pathLength = L;
metrics.jerkCost   = smoothness;
end
      

In a Simulink model using the Robotics System Toolbox, one typically logs the joint positions as a timeseries (e.g. via To Workspace blocks). After simulation, the arrays q and t extracted from the logs can be passed into evaluateTrajectory to obtain quantitative metrics for the trajectory.

Mathematica excels at symbolic derivations. The following code derives manipulability for a planar 2-link arm and defines a general configuration-space path length for a discrete sequence of configurations.

(* Planar 2-link arm kinematics *)
l1 = 1.0;
l2 = 1.0;

J[q1_, q2_] := {
  {-l1*Sin[q1] - l2*Sin[q1 + q2], -l2*Sin[q1 + q2]},
  { l1*Cos[q1] + l2*Cos[q1 + q2],  l2*Cos[q1 + q2]}
};

(* Yoshikawa manipulability: w(q) = sqrt(det(J J^T)) *)
manipulability[q1_, q2_] :=
  Sqrt[Det[J[q1, q2].Transpose[J[q1, q2]]]];

(* Weighted configuration-space distance *)
configDistance[q1_List, q2_List, w_List] :=
  Sqrt[(q2 - q1).DiagonalMatrix[w].(q2 - q1)];

(* Discrete path length under the above metric *)
pathLength[path_List, w_List] :=
  Total[
    Table[
      configDistance[path[[i]], path[[i + 1]], w],
      {i, 1, Length[path] - 1}
    ]
  ];

(* Example: evaluate manipulability at configuration (q1, q2) = (Pi/4, Pi/4) *)
Simplify[manipulability[Pi/4, Pi/4]]
      

Symbolic expressions such as manipulability[q1, q2] can be analyzed to find singularities and level sets, providing insight into how the metric behaves over the configuration space.

Problem 1 (Weighted Euclidean Metric): Let \( W\in\mathbb{R}^{n\times n} \) be symmetric positive definite. Define \( d_W(q_1,q_2)=\sqrt{(q_2-q_1)^{\top}W(q_2-q_1)} \). Prove that \( d_W \) is a metric on \( \mathbb{R}^n \).

Solution: We already sketched the argument in Section 2.2. Because \( W \) is SPD, there exists an invertible \( L \) with \( W=L^{\top}L \). Define \( \Vert x\Vert_W=\Vert Lx\Vert_2 \). This satisfies norm axioms, hence \( d_W(q_1,q_2)=\Vert q_2-q_1\Vert_W \) satisfies non-negativity, identity of indiscernibles, symmetry, and triangle inequality. Thus \( d_W \) is a metric.

Problem 2 (Manipulability of a Planar 2-Link Arm): Consider a planar 2-link manipulator with link lengths \( l_1,l_2 \), joint angles \( q_1,q_2 \), and Jacobian \( J(q) \) as in Section 11. Show that the Yoshikawa manipulability simplifies to \( w(q)=|l_1 l_2 \sin(q_2)| \) and identify the singular configurations.

Solution: For a 2-by-2 Jacobian \( J \), we have \( \det(JJ^{\top})=\det(J)\det(J^{\top})=\det(J)^2 \), so \( w(q)=\sqrt{\det(JJ^{\top})}=|\det(J)| \). Direct computation of the determinant of

\[ J(q_1,q_2) = \begin{bmatrix} -l_1\sin q_1 - l_2\sin(q_1+q_2) & -l_2\sin(q_1+q_2)\\ l_1\cos q_1 + l_2\cos(q_1+q_2) & l_2\cos(q_1+q_2) \end{bmatrix} \]

yields \( \det(J)=l_1 l_2 \sin(q_2) \). Thus \( w(q)=|l_1 l_2 \sin(q_2)| \). Singularities (zero manipulability) occur when \( \sin(q_2)=0 \), i.e. \( q_2=0,\pi \), corresponding to fully stretched or folded arm configurations where the end-effector loses one motion direction.

Problem 3 (Reparameterization Invariance of Length): Let \( \sigma:[0,1]\rightarrow\mathcal{C} \) be a differentiable path and \( \phi:[0,1]\rightarrow[0,1] \) smooth, strictly increasing, with \( \phi(0)=0,\phi(1)=1 \). Show that \( L(\sigma\circ\phi)=L(\sigma) \) under a configuration-space metric \( \Vert\cdot\Vert_M \).

Solution: This is the detailed version of Section 6.2. We compute

\[ L(\sigma\circ\phi) = \int_0^1 \bigl\Vert\tfrac{d}{dt}\sigma(\phi(t))\bigr\Vert_M\,dt = \int_0^1 \bigl\Vert \dot{\sigma}(\phi(t))\phi'(t)\bigr\Vert_M\,dt. \]

The norm is positively homogeneous, so \( \Vert \dot{\sigma}(\phi(t))\phi'(t)\Vert_M =\phi'(t)\Vert\dot{\sigma}(\phi(t))\Vert_M \) since \( \phi'(t)>0 \). With substitution \( u=\phi(t) \), we get \( L(\sigma\circ\phi)=\int_0^1\Vert\dot{\sigma}(u)\Vert_M\,du=L(\sigma) \).

Problem 4 (Equivalence of Two Weighted Metrics): Let \( W_1,W_2 \) be SPD matrices and \( d_{W_1},d_{W_2} \) their induced metrics. Prove there exist constants \( c_1,c_2>0 \) such that \( c_1 d_{W_1}(q_1,q_2)\le d_{W_2}(q_1,q_2)\le c_2 d_{W_1}(q_1,q_2) \) for all \( q_1,q_2 \).

Solution: Let \( \Delta=q_2-q_1 \). Using eigenvalue bounds for SPD matrices,

\[ \lambda_{\min}(W_i)\Vert\Delta\Vert_2^2 \le \Delta^{\top}W_i\Delta \le \lambda_{\max}(W_i)\Vert\Delta\Vert_2^2. \]

Thus \( \sqrt{\lambda_{\min}(W_i)}\Vert\Delta\Vert_2 \le d_{W_i}(q_1,q_2) \le \sqrt{\lambda_{\max}(W_i)}\Vert\Delta\Vert_2 \). Eliminating \( \Vert\Delta\Vert_2 \) between the bounds for \( i=1,2 \) gives the desired inequality, with \( c_1=\sqrt{\lambda_{\min}(W_2)/\lambda_{\max}(W_1)} \), \( c_2=\sqrt{\lambda_{\max}(W_2)/\lambda_{\min}(W_1)} \).

Problem 5 (Path Refinement Cannot Shorten Length): Let \( d \) be any metric on \( \mathcal{C} \). Consider a discrete path \( q_0,\dots,q_N \) with length \( L=\sum_{k=0}^{N-1} d(q_k,q_{k+1}) \). Show that inserting an extra point \( \tilde{q} \) between \( q_i \) and \( q_{i+1} \) cannot decrease the path length, i.e. \( d(q_i,\tilde{q})+d(\tilde{q},q_{i+1})\ge d(q_i,q_{i+1}) \).

Solution: The triangle inequality applied to the triple \( (q_i,\tilde{q},q_{i+1}) \) yields \( d(q_i,q_{i+1}) \le d(q_i,\tilde{q})+d(\tilde{q},q_{i+1}) \). The rest of the path is unchanged, so the total length of the refined path is at least \( L \). This justifies that path refinement (adding more waypoints along straight segments) cannot produce a shorter path under a valid metric.

Conceptual flow for designing metrics:

flowchart TD
  S["Start"] --> X["Choose space: config, state, task, trajectory"]
  X --> G["Define geometric metric d on the space"]
  X --> D["Define dynamic / task costs (time, energy, success)"]
  G --> C["Combine into scalar cost J_total"]
  D --> C
  C --> E["Use J_total in planner / controller and benchmark"]
        

In this lesson we formalized metrics at several levels: configuration- and state-space distances, task-space and manipulability metrics, path-length and clearance functionals, dynamic smoothness and energy costs, and benchmark-level statistics (success rates, time and cost distributions, anytime convergence). We proved basic properties such as the metric nature of weighted Euclidean distances, equivalence of weighted norms, and reparameterization invariance of path length, and analyzed manipulability for a simple planar manipulator. Practical implementations in Python, C++, Java, MATLAB/Simulink, and Mathematica showed how these mathematical objects become concrete tools in evaluation and planning pipelines.

These metric constructions will be used throughout the remaining chapter: to design benchmark tasks and datasets, to define stress-test conditions, and to make experimental evaluations reproducible and comparable across algorithms and platforms.