In earlier lessons, the configuration space (C-space) of a robot was an unconstrained space \( \mathcal{C} \subseteq \mathbb{R}^n \times \mathbb{T}^k \), where each point \( \mathbf{q} \) encodes all joint angles and other generalized coordinates. In many systems, not all configurations are feasible even before considering obstacles:

• Closed kinematic chains (e.g., parallel manipulators, humanoids with both feet fixed) impose loop-closure constraints.
• Task constraints (e.g., an end-effector that must stay on a surface, maintain orientation, or follow a curve) restrict configurations further.

We model these restrictions by a smooth constraint map \( h : \mathcal{C} \rightarrow \mathbb{R}^m \) and define the feasible set

\[ \mathcal{M} \;=\; \{\, \mathbf{q} \in \mathcal{C} \mid h(\mathbf{q}) = \mathbf{0} \,\}. \]

Under mild regularity conditions, \( \mathcal{M} \) is a smooth manifold of dimension \( n - m \), embedded in the ambient C-space. Planning with manifold constraints thus becomes motion planning on a constrained configuration manifold.

flowchart TD
  CSPACE["Ambient C-space C (dim = n)"] --> H["Constraint map h(q) = 0"]
  H --> M["Feasible manifold M (dim = n - m)"]
  M --> SAMPLE["Sample / interpolate in C"]
  SAMPLE --> PROJ["Project onto M using Jacobian-based solver"]
  PROJ --> PATH["Constraint-satisfying path on M"]
        

Let the ambient configuration space be an open subset \( \mathcal{C} \subset \mathbb{R}^n \) for simplicity. A holonomic equality constraint is a smooth function \( h : \mathcal{C} \rightarrow \mathbb{R}^m \) with components \( h(\mathbf{q}) = (h_1(\mathbf{q}), \dots, h_m(\mathbf{q}))^\top \). The constraint Jacobian at \( \mathbf{q} \) is

\[ J_h(\mathbf{q}) \;=\; \frac{\partial h}{\partial \mathbf{q}}(\mathbf{q}) \;\in\; \mathbb{R}^{m \times n}. \]

Intuitively, \( h(\mathbf{q}) = \mathbf{0} \) describes the joint equalities that must hold (e.g., loop closure, task satisfaction). The key regularity assumption is that the Jacobian has full row rank:

\[ \operatorname{rank} J_h(\mathbf{q}_\star) \;=\; m \quad \text{for all } \mathbf{q}_\star \in \mathcal{M}. \]

Under this condition, the Implicit Function Theorem guarantees that near any \( \mathbf{q}_\star \in \mathcal{M} \), the zero set of \( h \) is a smooth manifold. Formally:

Theorem (Embedded Submanifold): Suppose \( h : \mathcal{C} \rightarrow \mathbb{R}^m \) is \(C^1\) and \( \operatorname{rank} J_h(\mathbf{q}_\star) = m \) for some \( \mathbf{q}_\star \in \mathcal{C} \). Then there exists a neighborhood \( U \subset \mathcal{C} \) of \( \mathbf{q}_\star \) such that \( \mathcal{M} \cap U \) is a smooth submanifold of dimension \( n - m \).

Sketch of proof:

1. By permuting coordinates, split \( \mathbf{q} = (\mathbf{u}, \mathbf{v}) \), with \( \mathbf{u} \in \mathbb{R}^{n-m} \) and \( \mathbf{v} \in \mathbb{R}^m \), so that the square submatrix \( \partial h / \partial \mathbf{v} \) at \( \mathbf{q}_\star \) is nonsingular.
2. The Implicit Function Theorem gives a local function \( \phi : \mathbb{R}^{n-m} \rightarrow \mathbb{R}^m \) with \( h(\mathbf{u}, \phi(\mathbf{u})) = \mathbf{0} \) near \( \mathbf{q}_\star \).
3. Thus \( \mathcal{M} \cap U \) is locally parameterized by \( \mathbf{u} \), giving a smooth chart of dimension \( n - m \).

From the planning perspective, this result justifies treating the feasible set as a smooth manifold embedded in C-space, on which we can define curves, velocities, and local coordinates.

Consider a kinematic chain whose graph contains a loop (e.g., a four-bar linkage or a 6-DOF arm whose end-effector is rigidly attached to the environment). Let the joint coordinates be \( \mathbf{q} \in \mathbb{R}^n \). For a single loop, we can follow the homogeneous transforms \( T_1(\mathbf{q}), \dots, T_k(\mathbf{q}) \in SE(3) \) around the loop. A closed chain satisfies

\[ T_1(\mathbf{q}) \, T_2(\mathbf{q}) \cdots T_k(\mathbf{q}) \;=\; I_{4\times 4}. \]

Using the Lie group logarithm, we can map this matrix equation to a vector constraint in the Lie algebra \( \mathfrak{se}(3) \cong \mathbb{R}^6 \):

\[ h(\mathbf{q}) \;=\; \operatorname{Log}\!\big( T_1(\mathbf{q}) \cdots T_k(\mathbf{q}) \big) \;\in\; \mathbb{R}^m,\quad m \le 6. \]

Locally, this behaves like \( m \) scalar constraints. If the Jacobian \( J_h(\mathbf{q}) \) has rank \( m \) on the solution set, then the closed-chain configurations form a manifold \( \mathcal{M} \) of dimension \( n - m \). For example:

• A spatial 6-DOF arm with its end-effector welded to the environment typically has \( m = 6 \), giving a \( 0 \)-dimensional discrete set of feasible configurations.
• Two planar 2R arms whose endpoints are connected by a spherical joint in the plane impose 2 position constraints (\( m=2 \)) on 4 joint variables, so \( \dim \mathcal{M} = 2 \).

Differentiating the loop-closure constraint yields a linear velocity-level relation. If \( h(\mathbf{q}(t)) = \mathbf{0} \) for a motion \( \mathbf{q}(t) \), then

\[ \frac{d}{dt} h(\mathbf{q}(t)) \;=\; J_h(\mathbf{q}(t)) \, \dot{\mathbf{q}}(t) \;=\; \mathbf{0}. \]

Thus feasible joint velocities must lie in the kernel of the constraint Jacobian, a fact that we formalize via tangent spaces below.

A common planning scenario constrains the end-effector pose to lie on a given task manifold in workspace. Let the forward kinematics map joint space to task space:

\[ \phi(\mathbf{q}) \in \mathbb{R}^k \quad \text{(e.g., position or pose parameters).} \]

Suppose the end-effector must lie on a smooth task surface \( \mathcal{S} \subset \mathbb{R}^k \), given implicitly by \( g(\mathbf{x}) = \mathbf{0} \). The induced constraint in C-space is

\[ h(\mathbf{q}) \;=\; g(\phi(\mathbf{q})) \;=\; \mathbf{0}. \]

Differentiating yields the constraint Jacobian

\[ J_h(\mathbf{q}) \;=\; \frac{\partial g}{\partial \mathbf{x}}(\phi(\mathbf{q})) \, \frac{\partial \phi}{\partial \mathbf{q}}(\mathbf{q}) \;=\; J_g\big(\phi(\mathbf{q})\big)\, J_\phi(\mathbf{q}). \]

If the robot must follow a time-parameterized task trajectory \( \mathbf{x}_d(t) \in \mathcal{S} \), then the instantaneous kinematic constraint is

\[ \dot{\mathbf{x}}_d(t) \;=\; J_\phi(\mathbf{q}(t)) \, \dot{\mathbf{q}}(t), \quad \text{with } h(\mathbf{q}(t)) = \mathbf{0} \text{ for all } t. \]

This is the usual differential inverse-kinematics equation, further restricted to keep the configuration on the manifold \( \mathcal{M} \).

For a smooth manifold \( \mathcal{M} = \{ \mathbf{q} \mid h(\mathbf{q}) = \mathbf{0} \} \), the tangent space at \( \mathbf{q} \in \mathcal{M} \) can be characterized by curves on the manifold. Let \( \gamma : (-\epsilon, \epsilon) \rightarrow \mathcal{M} \) be a differentiable curve with \( \gamma(0) = \mathbf{q} \). Differentiating the constraint along the curve:

\[ h(\gamma(t)) \equiv \mathbf{0} \;\Rightarrow\; \frac{d}{dt} h(\gamma(t)) \bigg|_{t=0} \;=\; J_h(\mathbf{q}) \, \dot{\gamma}(0) \;=\; \mathbf{0}. \]

Hence any tangent vector \( \mathbf{v} = \dot{\gamma}(0) \) must lie in the null space of \( J_h(\mathbf{q}) \). Conversely, under the rank condition, every vector in the null space is the tangent vector of some curve in \( \mathcal{M} \). We obtain:

\[ T_{\mathbf{q}}\mathcal{M} \;=\; \{\, \mathbf{v} \in \mathbb{R}^n \mid J_h(\mathbf{q}) \, \mathbf{v} = \mathbf{0} \,\}. \]

The dimension of the tangent space is \( n - m \) because \( \dim \ker J_h(\mathbf{q}) = n - \operatorname{rank} J_h(\mathbf{q}) = n - m \) by the rank-nullity theorem.

To project an arbitrary ambient velocity \( \mathbf{u} \in \mathbb{R}^n \) onto the tangent space, we can use an orthogonal projection with respect to the Euclidean metric. If \( J_h(\mathbf{q}) \) has full row rank, then the matrix

\[ \mathbf{P}(\mathbf{q}) \;=\; \mathbf{I}_n \;-\; J_h(\mathbf{q})^\top \big( J_h(\mathbf{q}) \, J_h(\mathbf{q})^\top \big)^{-1} J_h(\mathbf{q}) \]

is an idempotent symmetric projector: \( \mathbf{P}^2(\mathbf{q}) = \mathbf{P}(\mathbf{q}) \), \( \mathbf{P}(\mathbf{q})^\top = \mathbf{P}(\mathbf{q}) \), and \( \operatorname{im} \mathbf{P}(\mathbf{q}) = T_{\mathbf{q}}\mathcal{M} \). Thus a constraint-consistent velocity is

\[ \dot{\mathbf{q}} \;=\; \mathbf{P}(\mathbf{q}) \, \mathbf{u}, \]

where \( \mathbf{u} \) is any reference joint velocity (e.g., from a higher-level planner or controller).

Many planning algorithms operate naturally in the unconstrained C-space (e.g., straight-line interpolation, graph search, or sampling-based planners). To enforce manifold constraints, a standard technique is projection:

1. Work in the ambient C-space to propose a configuration \( \tilde{\mathbf{q}} \).
2. Project \( \tilde{\mathbf{q}} \) onto \( \mathcal{M} \) by solving \( h(\mathbf{q}) = \mathbf{0} \) locally.

A common projection scheme uses Newton or Gauss–Newton iterations. Starting from \( \mathbf{q}_0 = \tilde{\mathbf{q}} \), iterate:

\[ \mathbf{q}_{k+1} \;=\; \mathbf{q}_k \;-\; J_h(\mathbf{q}_k)^\dagger \, h(\mathbf{q}_k), \]

where \( J_h(\mathbf{q}_k)^\dagger \) is the Moore–Penrose pseudoinverse. For small residuals and full-rank Jacobians, this iteration converges quadratically in the neighborhood of the manifold.

A simple projection-based planning strategy between two points on \( \mathcal{M} \) is:

flowchart TD
  S["Start q_start on M"] --> G["Goal q_goal on M"]
  S --> INTERP["Interpolate in C: q_i = (1 - s_i) q_start + s_i q_goal"]
  INTERP --> PROJPTS["For each q_i, run Newton projection h(q) = 0"]
  PROJPTS --> CHECK["Check projection success and constraints"]
  CHECK --> PATH["Connect projected points to form feasible path"]
        

More advanced planners (e.g., constrained RRT-style algorithms) use projection at each extension step, but the underlying mathematics is exactly this local constraint solving on \( \mathcal{M} \).

We implement a simple constraint manifold in several languages: a planar 2R manipulator with a fixed end-effector position \( \mathbf{p}_d = (x_d, y_d) \). The ambient variables are \( \mathbf{q} = (\theta_1, \theta_2) \), and the constraint is

\[ h(\mathbf{q}) \;=\; \phi(\mathbf{q}) - \mathbf{p}_d \;=\; \mathbf{0}, \]

where the forward kinematics for link lengths \( \ell_1, \ell_2 \) is

\[ \phi(\mathbf{q}) \;=\; \begin{bmatrix} \ell_1 \cos\theta_1 + \ell_2 \cos(\theta_1 + \theta_2) \\ \ell_1 \sin\theta_1 + \ell_2 \sin(\theta_1 + \theta_2) \end{bmatrix}. \]

The Jacobian of the task map is

\[ J_\phi(\mathbf{q}) \;=\; \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}. \]

The projection update \( \mathbf{q}_{k+1} = \mathbf{q}_k - J_\phi(\mathbf{q}_k)^\dagger h(\mathbf{q}_k) \) can be implemented using standard numerical linear algebra in each language.

7.1 Python (NumPy / SciPy, OMPL context)

In Python, we can use numpy for linear algebra. In larger projects, OMPL's pyompl bindings and the Robotics Toolbox for Python provide higher-level support for constrained planning, but here we implement projection from scratch.

import numpy as np

L1, L2 = 1.0, 1.0

def fk_planar_2R(q, l1=L1, l2=L2):
    theta1, theta2 = q
    x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
    y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
    return np.array([x, y])

def jacobian_fk(q, l1=L1, l2=L2):
    theta1, theta2 = q
    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]
    ])
    return J

def constraint(q, p_des):
    return fk_planar_2R(q) - p_des

def project_onto_task_manifold(q_init, p_des, max_iters=20, tol=1e-10):
    q = np.array(q_init, dtype=float)
    for k in range(max_iters):
        h_q = constraint(q, p_des)
        J = jacobian_fk(q)
        # Pseudoinverse of 2x2 Jacobian using SVD
        U, S, Vt = np.linalg.svd(J)
        J_pinv = Vt.T @ np.diag(1.0 / S) @ U.T
        q = q - J_pinv @ h_q
        if np.linalg.norm(h_q) < tol:
            break
    return q

if __name__ == "__main__":
    p_des = np.array([1.0, 1.0])
    q_guess = np.array([0.1, 0.2])
    q_proj = project_onto_task_manifold(q_guess, p_des)
    print("Projected configuration:", q_proj)
    print("End-effector position:", fk_planar_2R(q_proj))
      

7.2 C++ (Eigen, OMPL / KDL Context)

In C++, libraries such as Eigen provide linear algebra, while OMPL and KDL (Kinematics and Dynamics Library) offer robotics-specific primitives. The following snippet uses Eigen to implement the same projection:

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

static const double L1 = 1.0;
static const double L2 = 1.0;

Eigen::Vector2d fk_planar_2R(const Eigen::Vector2d& q) {
    double theta1 = q(0);
    double theta2 = q(1);
    double x = L1 * std::cos(theta1) + L2 * std::cos(theta1 + theta2);
    double y = L1 * std::sin(theta1) + L2 * std::sin(theta1 + theta2);
    return Eigen::Vector2d(x, y);
}

Eigen::Matrix2d jacobian_fk(const Eigen::Vector2d& q) {
    double theta1 = q(0);
    double theta2 = q(1);
    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;
}

Eigen::Vector2d projectOntoTaskManifold(
    const Eigen::Vector2d& q_init,
    const Eigen::Vector2d& p_des,
    int max_iters = 20,
    double tol = 1e-10)
{
    Eigen::Vector2d q = q_init;
    for (int k = 0; k < max_iters; ++k) {
        Eigen::Vector2d h = fk_planar_2R(q) - p_des;
        Eigen::Matrix2d J = jacobian_fk(q);
        // For a full-rank 2x2, pseudoinverse reduces to this expression:
        Eigen::Matrix2d J_pinv = J.transpose() * (J * J.transpose()).inverse();
        q -= J_pinv * h;
        if (h.norm() < tol) {
            break;
        }
    }
    return q;
}
      

7.3 Java (Arrays and Simple Linear Algebra, ROSJava Context)

Java-based robotics often relies on bindings to C++ libraries via ROSJava and pure Java linear algebra libraries such as EJML or Apache Commons Math. Below is a minimal implementation using 2x2 matrix inversion coded by hand:

public class Planar2RProjection {

    static double L1 = 1.0;
    static double L2 = 1.0;

    public static double[] fk(double[] q) {
        double theta1 = q[0];
        double theta2 = q[1];
        double x = L1 * Math.cos(theta1) + L2 * Math.cos(theta1 + theta2);
        double y = L1 * Math.sin(theta1) + L2 * Math.sin(theta1 + theta2);
        return new double[]{x, y};
    }

    public static double[][] jacobian(double[] q) {
        double theta1 = q[0];
        double theta2 = q[1];
        double s1  = Math.sin(theta1);
        double c1  = Math.cos(theta1);
        double s12 = Math.sin(theta1 + theta2);
        double c12 = Math.cos(theta1 + theta2);

        double[][] J = new double[2][2];
        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;
    }

    private static double[][] inv2x2(double[][] A) {
        double a = A[0][0];
        double b = A[0][1];
        double c = A[1][0];
        double d = A[1][1];
        double det = a * d - b * c;
        double invDet = 1.0 / det;
        return new double[][]{
            {  d * invDet, -b * invDet},
            { -c * invDet,  a * invDet}
        };
    }

    public static double[] project(double[] qInit, double[] pDes,
                                   int maxIters, double tol) {
        double[] q = qInit.clone();
        for (int k = 0; k < maxIters; ++k) {
            double[] h = fk(q);
            h[0] -= pDes[0];
            h[1] -= pDes[1];

            double[][] J = jacobian(q);

            // J_pinv = J^T (J J^T)^{-1} for full-rank 2x2 J
            double[][] JJt = new double[2][2];
            // JJt = J * J^T
            for (int i = 0; i < 2; ++i) {
                for (int j = 0; j < 2; ++j) {
                    JJt[i][j] = J[i][0] * J[j][0] + J[i][1] * J[j][1];
                }
            }
            double[][] JJtInv = inv2x2(JJt);

            double[][] Jt = new double[2][2];
            Jt[0][0] = J[0][0]; Jt[0][1] = J[1][0];
            Jt[1][0] = J[0][1]; Jt[1][1] = J[1][1];

            double[][] Jpinv = new double[2][2];
            // Jpinv = J^T * JJtInv
            for (int i = 0; i < 2; ++i) {
                for (int j = 0; j < 2; ++j) {
                    Jpinv[i][j] = Jt[i][0] * JJtInv[0][j] + Jt[i][1] * JJtInv[1][j];
                }
            }

            // q = q - J_pinv * h
            double dq0 = Jpinv[0][0] * h[0] + Jpinv[0][1] * h[1];
            double dq1 = Jpinv[1][0] * h[0] + Jpinv[1][1] * h[1];
            q[0] -= dq0;
            q[1] -= dq1;

            double normH = Math.sqrt(h[0] * h[0] + h[1] * h[1]);
            if (normH < tol) {
                break;
            }
        }
        return q;
    }
}
      

7.4 MATLAB / Simulink (Robotics System Toolbox)

MATLAB has a Robotics System Toolbox and the Robotics Toolbox for MATLAB, which provide ready-made kinematics and Jacobian functions. Below is a basic Newton projector:

function q_proj = projectPlanar2R(q_init, p_des, max_iters, tol)
    if nargin < 3, max_iters = 20; end
    if nargin < 4, tol = 1e-10; end
    L1 = 1.0; L2 = 1.0;
    q = q_init(:);
    for k = 1:max_iters
        h = fk_planar_2R(q, L1, L2) - p_des(:);
        J = jacobian_planar_2R(q, L1, L2);
        J_pinv = pinv(J);  % 2x2 pseudoinverse
        q = q - J_pinv * h;
        if norm(h) < tol
            break;
        end
    end
    q_proj = q;
end

function p = fk_planar_2R(q, L1, L2)
    th1 = q(1); th2 = q(2);
    x = L1 * cos(th1) + L2 * cos(th1 + th2);
    y = L1 * sin(th1) + L2 * sin(th1 + th2);
    p = [x; y];
end

function J = jacobian_planar_2R(q, L1, L2)
    th1 = q(1); th2 = q(2);
    s1  = sin(th1);
    c1  = cos(th1);
    s12 = sin(th1 + th2);
    c12 = cos(th1 + th2);
    J = [ -L1 * s1 - L2 * s12, -L2 * s12;
           L1 * c1 + L2 * c12,  L2 * c12 ];
end
      

In Simulink, one can wrap projectPlanar2R inside a MATLAB Function block that takes a candidate configuration and desired task position as inputs, outputting a constraint-satisfying configuration at each step.

7.5 Wolfram Mathematica

Mathematica provides symbolic and numeric solvers that can directly handle the constraints. The following code uses FindRoot to project a configuration:

L1 = 1.0; L2 = 1.0;
pDes = {1.0, 1.0};

fk[{th1_, th2_}] := {
  L1*Cos[th1] + L2*Cos[th1 + th2],
  L1*Sin[th1] + L2*Sin[th1 + th2]
};

projectPlanar2R[qInit_List] := Module[
  {th1, th2, sol},
  sol = FindRoot[
    fk[{th1, th2}] == pDes,
    { {th1, qInit[[1]]}, {th2, qInit[[2]]} }
  ];
  {th1, th2} /. sol
]

qProj = projectPlanar2R[{0.1, 0.2}];
N[{qProj, fk[qProj]}]
      

For more complex manifolds, one can represent \( \mathcal{M} \) as an implicit region and use RegionPlot, ParametricRegion, or Geodesic-related tools to study its geometry.

Problem 1 (Constraint sphere as manifold): Let \( \mathcal{C} = \mathbb{R}^3 \) with constraint \( h(\mathbf{q}) = \|\mathbf{q}\|^2 - 1 = 0 \). Show that \( \mathcal{M} = \{\mathbf{q} \mid h(\mathbf{q}) = 0\} \) is a 2-dimensional manifold and compute \( T_{\mathbf{q}}\mathcal{M} \) at \( \mathbf{q}_0 = (0,0,1)^\top \).

Solution:

The constraint is \( h(\mathbf{q}) = q_1^2 + q_2^2 + q_3^2 - 1 \). Its gradient is

\[ J_h(\mathbf{q}) \;=\; \nabla h(\mathbf{q})^\top \;=\; 2 \begin{bmatrix} q_1 & q_2 & q_3 \end{bmatrix}. \]

For any point with \( \|\mathbf{q}\| = 1 \), the gradient is nonzero, so \( \operatorname{rank} J_h(\mathbf{q}) = 1 \). By the submanifold theorem, the zero set is a smooth manifold of dimension \( 3 - 1 = 2 \), the unit sphere. At \( \mathbf{q}_0 = (0,0,1)^\top \) we have

\[ J_h(\mathbf{q}_0) \;=\; 2\begin{bmatrix} 0 & 0 & 1 \end{bmatrix}. \]

The tangent space is the kernel:

\[ T_{\mathbf{q}_0}\mathcal{M} \;=\; \{ \mathbf{v} \in \mathbb{R}^3 \mid J_h(\mathbf{q}_0)\mathbf{v} = 0 \} \;=\; \{ (v_1, v_2, v_3)^\top \mid v_3 = 0 \}. \]

Thus \( T_{\mathbf{q}_0}\mathcal{M} \) is the plane spanned by \( e_1, e_2 \) (the tangent plane of the sphere at the north pole).

Problem 2 (Closed-chain DOF count): Consider a planar four-bar linkage: four rigid links connected in a loop by four revolute joints, with one link fixed to the ground. Using a configuration-space viewpoint, argue that the closed chain generically has one degree of freedom.

Solution:

Each revolute joint contributes one scalar coordinate, so we start with \( n = 4 \) joint variables \( \theta_1, \dots, \theta_4 \). However, the planar position and orientation of the loop relative to the world are not all independent because one link is fixed as ground.

One approach is to embed the mechanism into the plane as four link frames with 3 planar DOFs each (translation and rotation), giving \( 4 \times 3 = 12 \) variables. Each revolute joint enforces coincidence of two points (2 constraints) and equality of orientations (1 constraint), for 3 scalar constraints per joint. With 4 joints, that is \( 4 \times 3 = 12 \) constraints. Subtracting global planar rigid motion (3 DOFs) gives:

\[ \dim \mathcal{M} \;=\; 12 - 12 - 3 + 3 \;=\; 1. \]

Equivalently, from the joint-space viewpoint, writing the loop-closure equations in terms of joint angles yields two independent scalar constraints (for the endpoint position) on the four joint variables, and one global angle can be absorbed into the ground link. The remaining joint manifold has generically a single degree of freedom, consistent with classical mobility formulas.

Problem 3 (Tangent space equals null space): Let \( \mathcal{M} = \{ \mathbf{q} \mid h(\mathbf{q}) = \mathbf{0} \} \) with \( h \) and \( J_h \) as above and \( \operatorname{rank} J_h(\mathbf{q}) = m \). Prove that \( T_{\mathbf{q}}\mathcal{M} = \ker J_h(\mathbf{q}) \).

Solution:

(Subset) Let \( \mathbf{v} \in T_{\mathbf{q}}\mathcal{M} \). Then there exists a differentiable curve \( \gamma(t) \in \mathcal{M} \) such that \( \gamma(0) = \mathbf{q} \) and \( \dot{\gamma}(0) = \mathbf{v} \). Since \( h(\gamma(t)) \equiv \mathbf{0} \), differentiating at \( t = 0 \) gives

\[ 0 \;=\; \frac{d}{dt} h(\gamma(t)) \bigg|_{t=0} \;=\; J_h(\mathbf{q}) \, \dot{\gamma}(0) \;=\; J_h(\mathbf{q}) \, \mathbf{v}, \]

so \( \mathbf{v} \in \ker J_h(\mathbf{q}) \), and \( T_{\mathbf{q}}\mathcal{M} \subseteq \ker J_h(\mathbf{q}) \).

(Reverse inclusion) Conversely, if \( \mathbf{v} \in \ker J_h(\mathbf{q}) \), the implicit function theorem guarantees that locally, \( \mathcal{M} \) is parameterized by \( \mathbf{u} \in \mathbb{R}^{n-m} \), and any direction in \( \ker J_h(\mathbf{q}) \) can be realized by a curve in \( \mathcal{M} \) through \( \mathbf{q} \). Thus every null-space vector is a tangent vector, and \( T_{\mathbf{q}}\mathcal{M} = \ker J_h(\mathbf{q}) \).

Problem 4 (Single Newton step for planar 2R): For the planar 2R manipulator with end-effector position constraint \( \phi(\mathbf{q}) = \mathbf{p}_d \), derive the explicit Newton step

\[ \mathbf{q}_{k+1} \;=\; \mathbf{q}_k \;-\; J_\phi(\mathbf{q}_k)^\dagger \big( \phi(\mathbf{q}_k) - \mathbf{p}_d \big). \]

assuming that \( J_\phi(\mathbf{q}_k) \) is invertible (no singularities).

Solution:

For a square nonsingular Jacobian \( J_\phi(\mathbf{q}_k) \in \mathbb{R}^{2 \times 2} \), the pseudoinverse equals the matrix inverse: \( J_\phi(\mathbf{q}_k)^\dagger = J_\phi(\mathbf{q}_k)^{-1} \). Writing the standard Newton step for the vector equation \( \phi(\mathbf{q}) - \mathbf{p}_d = \mathbf{0} \) gives

\[ \mathbf{q}_{k+1} \;=\; \mathbf{q}_k \;-\; J_\phi(\mathbf{q}_k)^{-1} \big(\phi(\mathbf{q}_k) - \mathbf{p}_d\big). \]

This is exactly the stated iteration with \( J_\phi(\mathbf{q}_k)^\dagger \) replaced by \( J_\phi(\mathbf{q}_k)^{-1} \). When \( J_\phi \) is not square or near singularities, one must use the full pseudoinverse (e.g., via SVD) to obtain a least-squares step.

In this lesson we elevated C-space from an unconstrained ambient space to a constrained configuration manifold framework. Holonomic equality constraints define smooth submanifolds when the Jacobian has full row rank, and closed kinematic chains as well as task-space requirements fit naturally into this model. We showed that tangent spaces coincide with the null space of the constraint Jacobian, enabling projection of velocities and configurations onto the manifold. Finally, we implemented Newton-style projection for a planar 2R manipulator in several programming languages, a building block for constrained planners in later chapters.