For an \( n \)-DOF manipulator with task-space dimension \( m \) (typically \( m=3 \) for position or \( m=6 \) for full spatial velocity), the differential kinematics at configuration \( \mathbf{q} \in \mathbb{R}^n \) is

\[ \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q}) \dot{\mathbf{q}}, \quad \mathbf{J}(\mathbf{q}) \in \mathbb{R}^{m \times n}. \]

Here \( \dot{\mathbf{q}} \) is the joint velocity vector and \( \dot{\mathbf{x}} \) is the task-space velocity (e.g. translational and/or angular velocity of the end-effector). We assume a Euclidean norm bound on joint speeds,

\[ \|\dot{\mathbf{q}}\|_2 \le 1, \]

which can be interpreted as a limit on the total actuator effort in velocity. Under this constraint, the set of achievable task-space velocities is

\[ \mathcal{E}_v(\mathbf{q}) = \left\{ \dot{\mathbf{x}} \in \mathbb{R}^m \mid \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q}) \dot{\mathbf{q}},\; \|\dot{\mathbf{q}}\|_2 \le 1 \right\}. \]

This set is an ellipsoid in task space, called the velocity manipulability ellipsoid. Its size and shape encode how well the manipulator can move in different directions at that configuration.

flowchart TD
  Q["Joint config q"] --> JAC["Compute Jacobian J(q)"]
  JAC --> MAPV["Velocity map: xdot = J(q) * qdot"]
  MAPV --> ELL["Velocity manipulability ellipsoid in task space"]
  JAC --> PREVIEW["Preview: same J(q) also \nmaps joint effort to task effort \nin statics (later chapters)"]
        

The most widely used kinematic manipulability index is due to Yoshikawa. For a configuration \( \mathbf{q} \) with Jacobian \( \mathbf{J}(\mathbf{q}) \) of full row rank \( m \), the velocity manipulability is defined as

\[ w(\mathbf{q}) = \sqrt{ \det\big( \mathbf{J}(\mathbf{q}) \mathbf{J}(\mathbf{q})^{\mathsf{T}} \big) }. \]

Intuitively, \( w(\mathbf{q}) \) is proportional to the volume of the velocity ellipsoid \( \mathcal{E}_v(\mathbf{q}) \). If \( w(\mathbf{q}) \) is large, the robot can generate large end-effector velocity in many directions; if \( w(\mathbf{q}) \) is small, the robot is close to a kinematic singularity.

To understand its spectral meaning, recall the singular value decomposition (SVD) of the Jacobian:

\[ \mathbf{J}(\mathbf{q}) = \mathbf{U} \boldsymbol{\Sigma} \mathbf{V}^{\mathsf{T}}, \quad \boldsymbol{\Sigma} = \operatorname{diag}(\sigma_1,\dots,\sigma_m), \quad \sigma_1 \ge \dots \ge \sigma_m \ge 0, \]

where \( \mathbf{U} \in \mathbb{R}^{m \times m} \) and \( \mathbf{V} \in \mathbb{R}^{n \times n} \) are orthogonal matrices, and \( \sigma_i \) are the singular values of the Jacobian.

Using the SVD, we can rewrite the manipulability:

\[ \begin{aligned} \mathbf{J}\mathbf{J}^{\mathsf{T}} &= \mathbf{U} \boldsymbol{\Sigma} \mathbf{V}^{\mathsf{T}} \mathbf{V} \boldsymbol{\Sigma}^{\mathsf{T}} \mathbf{U}^{\mathsf{T}} = \mathbf{U} \boldsymbol{\Sigma} \boldsymbol{\Sigma}^{\mathsf{T}} \mathbf{U}^{\mathsf{T}} = \mathbf{U} \operatorname{diag}(\sigma_1^2,\dots,\sigma_m^2) \mathbf{U}^{\mathsf{T}}, \\ \det(\mathbf{J}\mathbf{J}^{\mathsf{T}}) &= \prod_{i=1}^{m} \sigma_i^2, \quad w(\mathbf{q}) = \sqrt{ \prod_{i=1}^{m} \sigma_i^2 } = \prod_{i=1}^{m} \sigma_i. \end{aligned} \]

Thus \( w(\mathbf{q}) \) is the product of the nonzero singular values. In particular,

• Singularity detection: if \( \mathbf{J}(\mathbf{q}) \) loses rank, at least one singular value is zero, so \( w(\mathbf{q}) = 0 \).
• Sensitivity to directions: if one singular value is very small, the volume collapses along one direction, indicating poor controllability in that task-space direction.

The velocity manipulability ellipsoid \( \mathcal{E}_v(\mathbf{q}) \) can be written as a quadratic form. For any \( \dot{\mathbf{q}} \) with \( \|\dot{\mathbf{q}}\|_2 \le 1 \), we have \( \dot{\mathbf{x}} = \mathbf{J} \dot{\mathbf{q}} \). The set of all such \( \dot{\mathbf{x}} \) is

\[ \mathcal{E}_v(\mathbf{q}) = \left\{ \dot{\mathbf{x}} \in \mathbb{R}^m \mid \dot{\mathbf{x}}^{\mathsf{T}} \big( \mathbf{J}(\mathbf{q}) \mathbf{J}(\mathbf{q})^{\mathsf{T}} \big)^{-1} \dot{\mathbf{x}} \le 1 \right\}, \quad \text{if }\mathbf{J}(\mathbf{q}) \text{ has full row rank}. \]

The eigen-decomposition of \( \mathbf{J}\mathbf{J}^{\mathsf{T}} \) gives

\[ \mathbf{J}\mathbf{J}^{\mathsf{T}} = \mathbf{U} \operatorname{diag}(\sigma_1^2,\dots,\sigma_m^2) \mathbf{U}^{\mathsf{T}}, \]

so the ellipsoid axes are aligned with the columns of \( \mathbf{U} \), and the semi-axis lengths are the singular values \( \sigma_i \). If the singular values are all equal, the ellipsoid is a sphere, and the configuration is called isotropic.

A useful scalar measure of anisotropy is the ratio \( \sigma_1 / \sigma_m \), which will be further examined in the next lesson in the context of condition numbers and sensitivity.

Although static force transmission is studied in detail in Chapter 9, it is conceptually useful to introduce the dual notion of force manipulability at this stage. In quasi-static conditions, the Jacobian relates joint efforts \( \boldsymbol{\tau} \in \mathbb{R}^n \) and task-space wrench \( \mathbf{f} \in \mathbb{R}^m \) via

\[ \boldsymbol{\tau} = \mathbf{J}(\mathbf{q})^{\mathsf{T}} \mathbf{f}, \]

under assumptions that will be made precise in the statics chapter. Imposing a bound \( \|\boldsymbol{\tau}\|_2 \le 1 \), the set of feasible task-space wrenches is

\[ \mathcal{E}_f(\mathbf{q}) = \left\{ \mathbf{f} \in \mathbb{R}^m \mid \mathbf{f}^{\mathsf{T}} \big( \mathbf{J}(\mathbf{q}) \mathbf{J}(\mathbf{q})^{\mathsf{T}} \big) \mathbf{f} \le 1 \right\}. \]

Note the duality: the same matrix \( \mathbf{J}(\mathbf{q})\mathbf{J}(\mathbf{q})^{\mathsf{T}} \) defines the velocity ellipsoid in inverse form and the force ellipsoid in direct form. Velocity manipulability emphasizes large \( \sigma_i \), while force manipulability emphasizes small task-space forces for given joint efforts, related to \( 1/\sigma_i \).

A desirable property of manipulability measures is invariance under coordinate changes that preserve geometry. Let \( \tilde{\mathbf{x}} = \mathbf{Q} \mathbf{x} \) be a new task-space coordinate with \( \mathbf{Q} \in \mathbb{R}^{m \times m} \) orthogonal (i.e. \( \mathbf{Q}^{\mathsf{T}}\mathbf{Q} = \mathbf{I}_m \)). Then the new Jacobian is

\[ \tilde{\mathbf{J}}(\mathbf{q}) = \mathbf{Q} \mathbf{J}(\mathbf{q}). \]

The corresponding manipulability satisfies

\[ \begin{aligned} \det\big( \tilde{\mathbf{J}}\tilde{\mathbf{J}}^{\mathsf{T}} \big) &= \det\big( \mathbf{Q} \mathbf{J}\mathbf{J}^{\mathsf{T}} \mathbf{Q}^{\mathsf{T}} \big) = \det(\mathbf{Q})^2 \det\big( \mathbf{J}\mathbf{J}^{\mathsf{T}} \big) = \det\big( \mathbf{J}\mathbf{J}^{\mathsf{T}} \big), \\ w_{\tilde{\mathbf{x}}}(\mathbf{q}) &= \sqrt{ \det\big( \tilde{\mathbf{J}}\tilde{\mathbf{J}}^{\mathsf{T}} \big) } = \sqrt{ \det\big( \mathbf{J}\mathbf{J}^{\mathsf{T}} \big) } = w(\mathbf{q}), \end{aligned} \]

since \( \det(\mathbf{Q}) = \pm 1 \). Thus the manipulability is invariant under rigid rotations of the task frame. A similar argument holds for orthogonal re-parameterizations in joint space.

Because \( w(\mathbf{q}) \) has units depending on the choice of task variables (e.g. meters vs. radians), it is common to use a normalized index. One simple choice is

\[ \bar{w}(\mathbf{q}) = \frac{ \big( \prod_{i=1}^{m} \sigma_i(\mathbf{q}) \big)^{1/m} }{ \frac{1}{m} \sum_{i=1}^{m} \sigma_i(\mathbf{q}) }, \]

which equals one for perfectly isotropic configurations and is less than one otherwise. This ratio is dimensionless and emphasizes shape rather than absolute size of the velocity ellipsoid.

Consider a planar 2R arm with link lengths \( \ell_1, \ell_2 > 0 \) and joint angles \( \theta_1, \theta_2 \). The end-effector position is

\[ \begin{aligned} x(\theta_1,\theta_2) &= \ell_1 \cos\theta_1 + \ell_2 \cos(\theta_1 + \theta_2), \\ y(\theta_1,\theta_2) &= \ell_1 \sin\theta_1 + \ell_2 \sin(\theta_1 + \theta_2). \end{aligned} \]

Differentiating w.r.t. time gives the Jacobian \( \dot{\mathbf{x}} = \mathbf{J}(\theta_1,\theta_2)\dot{\mathbf{q}} \) with \( \dot{\mathbf{x}} = [\dot{x},\dot{y}]^{\mathsf{T}} \), \( \dot{\mathbf{q}} = [\dot{\theta}_1,\dot{\theta}_2]^{\mathsf{T}} \):

\[ \mathbf{J}(\theta_1,\theta_2) = \begin{bmatrix} -\ell_1 \sin\theta_1 - \ell_2 \sin(\theta_1 + \theta_2) & -\ell_2 \sin(\theta_1 + \theta_2) \\ \ell_1 \cos\theta_1 + \ell_2 \cos(\theta_1 + \theta_2) & \ell_2 \cos(\theta_1 + \theta_2) \end{bmatrix}. \]

For a square \( 2 \times 2 \) Jacobian, we have \( \det(\mathbf{J}\mathbf{J}^{\mathsf{T}}) = \det(\mathbf{J})^2 \), so

\[ w(\theta_1,\theta_2) = \sqrt{\det(\mathbf{J}\mathbf{J}^{\mathsf{T}})} = |\det(\mathbf{J})|. \]

A direct computation of \( \det(\mathbf{J}) \) yields

\[ \det(\mathbf{J}(\theta_1,\theta_2)) = \ell_1 \ell_2 \sin\theta_2, \]

hence the manipulability index is

\[ w(\theta_1,\theta_2) = \ell_1 \ell_2 |\sin\theta_2|. \]

This has several important consequences:

• Singular configurations: \( w = 0 \) if and only if \( \sin\theta_2 = 0 \), that is \( \theta_2 = 0 \) or \( \theta_2 = \pi \) (mod \( 2\pi \)), when the links are fully stretched or folded, and the end-effector cannot move in some Cartesian direction.
• Independence from \( \theta_1 \): the manipulability does not depend on the absolute base orientation \( \theta_1 \), only on the relative angle \( \theta_2 \). Rotating the whole arm rigidly does not change the velocity ellipsoid volume.
• Design trade-offs: increasing \( \ell_1 \) or \( \ell_2 \) scales the manipulability linearly. However, mechanical constraints and inertia must also be considered in dynamics.

In software, manipulability is typically evaluated via the singular values of the Jacobian. For an \( m \times n \) Jacobian with \( m \le n \):

1. Compute the Jacobian \( \mathbf{J}(\mathbf{q}) \) at the current \( \mathbf{q} \).
2. Compute its SVD to obtain singular values \( \sigma_1,\dots,\sigma_m \).
3. Discard singular values below a numerical tolerance (to avoid underflow), and compute the product of the remaining ones.
4. Optionally normalize or compute additional indices (e.g. anisotropy ratio \( \sigma_1 / \sigma_m \)).

flowchart TD
  A["Robot model + config q"] --> B["Compute Jacobian J(q)"]
  B --> C["SVD: singular values sigma_i"]
  C --> D["Compute w = product of nonzero sigma_i"]
  D --> E{"Is w below \nchosen threshold?"}
  E -->|yes| SING["Mark configuration as near singular"]
  E -->|no| REG["Configuration is well manipulable"]
        

Numerically, one must be careful with:

• Scaling of task coordinates: mixing translational and rotational velocities in the same Jacobian (e.g. units of meters and radians) can distort manipulability unless appropriate weighting is used.
• Redundant manipulators: when \( n > m \), there are at most \( m \) nonzero singular values; the remaining reflect redundancy and do not directly affect \( w(\mathbf{q}) \).
• Ill-conditioning: near singularities, some \( \sigma_i \) approach zero, and numerical noise can dominate. Thresholds (e.g. \( \sigma_i \le 10^{-6} \)) are used to decide rank.

This section illustrates how to compute Yoshikawa manipulability in several languages commonly used in robotics. In all cases, the input is a Jacobian matrix \( \mathbf{J}(\mathbf{q}) \), which can be obtained from previous forward and differential kinematics routines.

8.1 Python (NumPy and Robotics Toolbox for Python)

import numpy as np

def yoshikawa_manipulability(J, tol=1e-6):
    """
    Compute Yoshikawa's velocity manipulability index
    w(q) = product of nonzero singular values of J(q).
    Parameters
    ----------
    J : (m, n) ndarray
        Jacobian matrix at configuration q.
    tol : float
        Threshold below which singular values are treated as zero.
    """
    # Singular value decomposition
    U, s, Vt = np.linalg.svd(J, full_matrices=False)
    # Keep only significant singular values
    s_nonzero = s[s > tol]
    if s_nonzero.size == 0:
        return 0.0
    return float(np.prod(s_nonzero))


def jacobian_2r(theta1, theta2, l1, l2):
    """
    Planar 2R Jacobian for end-effector position (x, y).
    """
    s1 = np.sin(theta1)
    c1 = np.cos(theta1)
    s12 = np.sin(theta1 + theta2)
    c12 = np.cos(theta1 + theta2)

    J = np.array([
        [-l1 * s1 - l2 * s12, -l2 * s12],
        [ l1 * c1 + l2 * c12,  l2 * c12]
    ], dtype=float)
    return J


if __name__ == "__main__":
    l1 = 1.0
    l2 = 0.7
    theta1 = 0.5
    theta2 = 1.0

    J = jacobian_2r(theta1, theta2, l1, l2)
    w = yoshikawa_manipulability(J)
    print("J =\n", J)
    print("Yoshikawa manipulability w =", w)

    # If using Robotics Toolbox for Python (Peter Corke)
    # from roboticstoolbox import DHRobot, RevoluteDH
    # Define robot, compute J0 = robot.jacob0(q) and pass to yoshikawa_manipulability
      

8.2 C++ with Eigen (and KDL Mention)

In C++, the Eigen library provides efficient SVD. Libraries such as Orocos KDL or ROS kinematics plugins can be used to obtain the Jacobian, which is then passed to the manipulability routine.

#include <Eigen/Dense>
#include <iostream>
#include <cmath>

double yoshikawaManipulability(const Eigen::MatrixXd& J, double tol = 1e-6)
{
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::VectorXd s = svd.singularValues();

    double w = 1.0;
    int rank = 0;
    for (int i = 0; i < s.size(); ++i)
    {
        if (s(i) > tol)
        {
            w *= s(i);
            ++rank;
        }
    }
    if (rank == 0)
    {
        return 0.0;
    }
    return w;
}

Eigen::Matrix2d jacobian2R(double theta1, double theta2, double l1, double l2)
{
    double s1 = std::sin(theta1);
    double c1 = std::cos(theta1);
    double s12 = std::sin(theta1 + theta2);
    double c12 = std::cos(theta1 + theta2);

    Eigen::Matrix2d J;
    J(0, 0) = -l1 * s1 - l2 * s12;
    J(0, 1) = -l2 * s12;
    J(1, 0) =  l1 * c1 + l2 * c12;
    J(1, 1) =  l2 * c12;
    return J;
}

int main()
{
    double l1 = 1.0;
    double l2 = 0.7;
    double theta1 = 0.5;
    double theta2 = 1.0;

    Eigen::Matrix2d J = jacobian2R(theta1, theta2, l1, l2);
    double w = yoshikawaManipulability(J);

    std::cout << "J =\n" << J << std::endl;
    std::cout << "Yoshikawa manipulability w = " << w << std::endl;

    // In a ROS + KDL workflow, you would obtain J from a KDL::ChainJntToJacSolver
    // and then convert to Eigen::MatrixXd before calling yoshikawaManipulability.
    return 0;
}
      

8.3 Java with EJML

In Java, EJML provides linear algebra routines. The Jacobian can be generated from a custom kinematics library or imported from a robotics framework, then passed to the SVD-based function below.

import org.ejml.simple.SimpleMatrix;
import org.ejml.simple.SimpleSVD;

public class ManipulabilityDemo {

    public static double yoshikawaManipulability(SimpleMatrix J, double tol) {
        SimpleSVD<SimpleMatrix> svd = J.svd();
        double[] s = svd.getSingularValues();

        double w = 1.0;
        int rank = 0;
        for (int i = 0; i < s.length; ++i) {
            if (s[i] > tol) {
                w *= s[i];
                ++rank;
            }
        }
        if (rank == 0) {
            return 0.0;
        }
        return w;
    }

    public static SimpleMatrix jacobian2R(double theta1, double theta2, double l1, double l2) {
        double s1 = Math.sin(theta1);
        double c1 = Math.cos(theta1);
        double s12 = Math.sin(theta1 + theta2);
        double c12 = Math.cos(theta1 + theta2);

        // Row-major initialization
        return new SimpleMatrix(2, 2, true,
                -l1 * s1 - l2 * s12, -l2 * s12,
                 l1 * c1 + l2 * c12,  l2 * c12);
    }

    public static void main(String[] args) {
        double l1 = 1.0;
        double l2 = 0.7;
        double theta1 = 0.5;
        double theta2 = 1.0;

        SimpleMatrix J = jacobian2R(theta1, theta2, l1, l2);
        double w = yoshikawaManipulability(J, 1e-6);

        System.out.println("J =");
        J.print();
        System.out.println("Yoshikawa manipulability w = " + w);
    }
}
      

8.4 MATLAB/Simulink (Robotics System Toolbox)

In MATLAB, the Robotics System Toolbox provides the rigidBodyTree class, which can compute Jacobians. We then apply SVD to obtain manipulability. The same function can be wrapped inside a Simulink MATLAB Function block.

function w = yoshikawaManipulability(J, tol)
%YOSHIKAWAMANIPULABILITY Yoshikawa velocity manipulability index.
%   w = yoshikawaManipulability(J, tol) returns the product of the
%   nonzero singular values of J. If tol is omitted, a default tolerance
%   is used.

    if nargin < 2
        tol = 1e-6;
    end

    s = svd(J);
    s = s(s > tol);

    if isempty(s)
        w = 0.0;
    else
        w = prod(s);
    end
end


function demo_planar2R()
    l1 = 1.0;
    l2 = 0.7;
    theta1 = 0.5;
    theta2 = 1.0;

    J = jacobian2R(theta1, theta2, l1, l2);
    w = yoshikawaManipulability(J);

    fprintf("J =\n");
    disp(J);
    fprintf("Yoshikawa manipulability w = %f\n", w);
end


function J = jacobian2R(theta1, theta2, l1, l2)
    s1 = sin(theta1);
    c1 = cos(theta1);
    s12 = sin(theta1 + theta2);
    c12 = cos(theta1 + theta2);

    J = [ -l1 * s1 - l2 * s12, -l2 * s12;
           l1 * c1 + l2 * c12,  l2 * c12 ];
end
      

In a Robotics System Toolbox workflow, you can create a rigidBodyTree model, call geometricJacobian to get J at configuration q, and then call yoshikawaManipulability(J). Embedding this call in a Simulink MATLAB Function block yields a real-time manipulability monitor.

8.5 Wolfram Mathematica

(* Yoshikawa manipulability for a Jacobian J *)
YoshikawaManipulability[J_?MatrixQ, tol_: 10.^(-6)] := Module[{s},
  s = SingularValueList[J];
  s = Select[s, # > tol &];
  If[Length[s] == 0, 0.0, Times @@ s]
];

(* Planar 2R Jacobian *)
Jacobian2R[theta1_, theta2_, l1_, l2_] := Module[
  {s1, c1, s12, c12},
  s1 = Sin[theta1]; c1 = Cos[theta1];
  s12 = Sin[theta1 + theta2]; c12 = Cos[theta1 + theta2];
  {
    {-l1*s1 - l2*s12, -l2*s12},
    { l1*c1 + l2*c12,  l2*c12}
  }
];

(* Example usage *)
l1 = 1.0; l2 = 0.7;
theta1 = 0.5; theta2 = 1.0;
J = Jacobian2R[theta1, theta2, l1, l2];
w = YoshikawaManipulability[J]
      

In this lesson we formalized manipulability as a quantitative measure of how effectively a manipulator can generate task-space velocities (and, in preview, forces) at a given configuration. Starting from the Jacobian, we defined Yoshikawa's velocity manipulability index, interpreted it as the volume of the velocity ellipsoid, and expressed it in terms of the Jacobian singular values. We studied invariance properties, isotropy, and normalization, and analyzed a classic planar 2R example, explicitly linking singularities to loss of manipulability. Finally, we provided concrete implementations in Python, C++, Java, MATLAB/Simulink, and Mathematica, preparing for the next lesson on condition numbers and sensitivity of manipulator kinematics.