Consider an \( n \)-DOF manipulator in contact with the environment, e.g., a robot arm pressing on a wall with its end-effector constrained to slide along a line. The contact imposes equality constraints on the configuration \( \mathbf{q} \in \mathbb{R}^n \) of the form \( \boldsymbol{\phi}(\mathbf{q}, t) = \mathbf{0} \). If we apply a general torque command \( \boldsymbol{\tau}_0 \), the resulting acceleration \( \ddot{\mathbf{q}} \) may violate the constraint and cause penetration or loss of contact.

A standard solution is to augment the dynamics with Lagrange multipliers, solving for constraint forces. An equivalent and computationally elegant alternative is to use a projection-based controller:

• Compute an unconstrained control torque \( \boldsymbol{\tau}_0 \) that achieves the desired motion (e.g., computed-torque tracking).
• Compute a projector that selects only those components of \( \boldsymbol{\tau}_0 \) consistent with the constraints.
• Modify the command using this projector, letting the environment (or an explicit constraint term) provide the missing reaction forces.

At the heart of this lesson is the dynamic-consistent projector that respects the robot's inertia matrix, ensuring that projected torques do not create acceleration in the constrained directions.

flowchart TD
  A["Task reference (q_ref, qd_ref, qdd_ref)"] --> B["Unconstrained controller computes tau_0"]
  B --> C["Constraint model: phi(q,t)=0, J_c(q)"]
  C --> D["Build dynamic-consistent projector N_T"]
  D --> E["Projected torque: tau = tau_c + N_T * tau_0"]
  E --> F["Robot motion respecting constraints"]
        

We focus on holonomic equality constraints of the form \( \boldsymbol{\phi}(\mathbf{q}, t) = \mathbf{0} \), where \( \boldsymbol{\phi} : \mathbb{R}^n \times \mathbb{R} \to \mathbb{R}^m \) with \( m \leq n \) the number of independent constraints. Typical examples:

• End-effector must stay on a plane: \( \phi(\mathbf{q}) = \mathbf{n}^\top \mathbf{x}(\mathbf{q}) - d = 0 \).
• Closed kinematic chain loop closure: \( \boldsymbol{\phi}(\mathbf{q}) = \mathbf{x}_A(\mathbf{q}) - \mathbf{x}_B(\mathbf{q}) = \mathbf{0} \).

Differentiating the constraint with respect to time yields the velocity constraint:

\[ \frac{\mathrm{d}}{\mathrm{d}t}\boldsymbol{\phi}(\mathbf{q}, t) = \frac{\partial \boldsymbol{\phi}}{\partial \mathbf{q}} \dot{\mathbf{q}} + \frac{\partial \boldsymbol{\phi}}{\partial t} = \mathbf{J}_c(\mathbf{q}, t)\dot{\mathbf{q}} + \dot{\boldsymbol{\phi}}_t(t) = \mathbf{0}, \]

where \( \mathbf{J}_c(\mathbf{q}, t) \in \mathbb{R}^{m\times n} \) is the constraint Jacobian and \( \dot{\boldsymbol{\phi}}_t(t) \) is an explicit time term (often zero for time-invariant constraints). Differentiating once more gives the acceleration constraint:

\[ \mathbf{J}_c(\mathbf{q}, t)\ddot{\mathbf{q}} + \dot{\mathbf{J}}_c(\mathbf{q}, \dot{\mathbf{q}}, t)\dot{\mathbf{q}} + \ddot{\boldsymbol{\phi}}_t(t) = \mathbf{0}. \]

In control design, we often assume that the system is initialized on the constraint manifold, i.e., \( \boldsymbol{\phi}(\mathbf{q}, t) = \mathbf{0} \) and \( \mathbf{J}_c \dot{\mathbf{q}} + \dot{\boldsymbol{\phi}}_t = \mathbf{0} \) at the start. The controller must then ensure that these equalities remain satisfied for all time.

For an unconstrained manipulator, the joint-space dynamics are (from the standard Euler–Lagrange form):

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) = \boldsymbol{\tau}, \]

where \( \mathbf{M}(\mathbf{q}) \) is the symmetric positive-definite inertia matrix, and \( \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) \) collects Coriolis, centrifugal, and gravity terms.

Under holonomic equality constraints, we add generalized forces associated with Lagrange multipliers \( \boldsymbol{\lambda} \in \mathbb{R}^m \):

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) = \boldsymbol{\tau} + \mathbf{J}_c(\mathbf{q}, t)^\top \boldsymbol{\lambda}, \]

together with the acceleration constraint:

\[ \mathbf{J}_c(\mathbf{q}, t)\ddot{\mathbf{q}} + \dot{\mathbf{J}}_c(\mathbf{q}, \dot{\mathbf{q}}, t)\dot{\mathbf{q}} + \ddot{\boldsymbol{\phi}}_t(t) = \mathbf{0}. \]

Let us define the vector \( \mathbf{b}_c(\mathbf{q}, \dot{\mathbf{q}}, t) = \dot{\mathbf{J}}_c\dot{\mathbf{q}} + \ddot{\boldsymbol{\phi}}_t \), so the constraint is compactly \( \mathbf{J}_c \ddot{\mathbf{q}} + \mathbf{b}_c = \mathbf{0} \). Substituting \( \ddot{\mathbf{q}} = \mathbf{M}^{-1}(\boldsymbol{\tau} + \mathbf{J}_c^\top \boldsymbol{\lambda} - \mathbf{h}) \) into the constraint leads to

\[ \mathbf{J}_c \mathbf{M}^{-1}(\boldsymbol{\tau} + \mathbf{J}_c^\top \boldsymbol{\lambda} - \mathbf{h}) + \mathbf{b}_c = \mathbf{0}. \]

Solving for the multipliers gives

\[ \boldsymbol{\lambda} = -\left(\mathbf{J}_c \mathbf{M}^{-1} \mathbf{J}_c^\top\right)^{-1} \left( \mathbf{J}_c \mathbf{M}^{-1}(\boldsymbol{\tau} - \mathbf{h}) + \mathbf{b}_c \right), \]

assuming \( \mathbf{J}_c \) has full row rank and \( \mathbf{J}_c \mathbf{M}^{-1} \mathbf{J}_c^\top \) is invertible. This Lagrange-multiplier formulation is equivalent to a projection formulation that we derive next.

The quantity \( \mathbf{J}_c \mathbf{M}^{-1} \mathbf{J}_c^\top \) is an operational-space inertia associated with the constrained directions. Define

\[ \boldsymbol{\Lambda}_c(\mathbf{q}) \triangleq \left(\mathbf{J}_c(\mathbf{q}) \mathbf{M}(\mathbf{q})^{-1} \mathbf{J}_c(\mathbf{q})^\top\right)^{-1}. \]

Using this, we can define the dynamic-consistent pseudoinverse of the constraint Jacobian:

\[ \mathbf{J}_c^{\#}(\mathbf{q}) \triangleq \mathbf{M}(\mathbf{q})^{-1} \mathbf{J}_c(\mathbf{q})^\top \boldsymbol{\Lambda}_c(\mathbf{q}), \]

and the corresponding null-space projector:

\[ \mathbf{N}(\mathbf{q}) \triangleq \mathbf{I}_n - \mathbf{J}_c^{\#}(\mathbf{q}) \mathbf{J}_c(\mathbf{q}). \]

Proposition. The matrix \( \mathbf{N}(\mathbf{q}) \) is a projector onto the dynamically consistent null space of \( \mathbf{J}_c \), i.e., \( \mathbf{J}_c \mathbf{N} = \mathbf{0} \) and \( \mathbf{N}^2 = \mathbf{N} \).

Proof sketch.

1. Null-space property. Under the assumption that \( \mathbf{J}_c \) has full row rank, one can verify that \( \mathbf{J}_c \mathbf{J}_c^{\#} = \mathbf{I}_m \). Then \( \mathbf{J}_c \mathbf{N} = \mathbf{J}_c - \mathbf{J}_c \mathbf{J}_c^{\#} \mathbf{J}_c = \mathbf{J}_c - \mathbf{I}_m \mathbf{J}_c = \mathbf{0} \).
2. Idempotence. \( \mathbf{N}^2 = (\mathbf{I}_n - \mathbf{J}_c^{\#}\mathbf{J}_c)^2 = \mathbf{I}_n - 2\mathbf{J}_c^{\#}\mathbf{J}_c + \mathbf{J}_c^{\#}\mathbf{J}_c\mathbf{J}_c^{\#}\mathbf{J}_c \). Because \( \mathbf{J}_c\mathbf{J}_c^{\#} = \mathbf{I}_m \) and \( \mathbf{J}_c^{\#}\mathbf{J}_c\mathbf{J}_c^{\#} = \mathbf{J}_c^{\#} \), this simplifies to \( \mathbf{N}^2 = \mathbf{I}_n - \mathbf{J}_c^{\#}\mathbf{J}_c = \mathbf{N} \).

In joint acceleration space, any vector of the form \( \ddot{\mathbf{q}} = \ddot{\mathbf{q}}_p + \mathbf{N} \mathbf{v} \), where \( \ddot{\mathbf{q}}_p \) is a particular solution and \( \mathbf{v} \) is arbitrary, automatically satisfies the acceleration constraint \( \mathbf{J}_c \ddot{\mathbf{q}} + \mathbf{b}_c = \mathbf{0} \), provided that \( \ddot{\mathbf{q}}_p \) satisfies \( \mathbf{J}_c \ddot{\mathbf{q}}_p + \mathbf{b}_c = \mathbf{0} \).

In torque space, we will use the transpose \( \mathbf{N}^\top(\mathbf{q}) \) to project torques so that they belong to the dynamically consistent null space of the constraints.

Suppose we have an unconstrained torque \( \boldsymbol{\tau}_0 \) (e.g., from computed-torque control) that would achieve the desired trajectory in free space. Under constraints, we seek a modified torque \( \boldsymbol{\tau} \) that:

• generates accelerations that respect the constraint;
• is as close as possible to \( \boldsymbol{\tau}_0 \) in a suitable metric.

Write the modified torque as \( \boldsymbol{\tau} = \boldsymbol{\tau}_0 + \mathbf{J}_c^\top \boldsymbol{\lambda} \). Plugging this into the constrained dynamics and enforcing the acceleration constraint yields

\[ \boldsymbol{\lambda} = -\boldsymbol{\Lambda}_c \left( \mathbf{J}_c \mathbf{M}^{-1}(\boldsymbol{\tau}_0 - \mathbf{h}) + \mathbf{b}_c \right), \]

and thus

\[ \boldsymbol{\tau} = \boldsymbol{\tau}_0 - \mathbf{J}_c^\top \boldsymbol{\Lambda}_c \left( \mathbf{J}_c \mathbf{M}^{-1}(\boldsymbol{\tau}_0 - \mathbf{h}) + \mathbf{b}_c \right). \]

This can be interpreted as a projection of the unconstrained torque:

\[ \boldsymbol{\tau} = \underbrace{ \left( \mathbf{I}_n - \mathbf{J}_c^\top \boldsymbol{\Lambda}_c \mathbf{J}_c \mathbf{M}^{-1} \right) }_{\mathbf{N}^\top(\mathbf{q})} \boldsymbol{\tau}_0 - \mathbf{J}_c^\top \boldsymbol{\Lambda}_c \left( \mathbf{b}_c - \mathbf{J}_c \mathbf{M}^{-1}\mathbf{h} \right). \]

Key property. For any torque of the form \( \boldsymbol{\tau}_N = \mathbf{N}^\top \boldsymbol{\tau}_0 \), the induced acceleration satisfies

\[ \mathbf{J}_c \mathbf{M}^{-1} \boldsymbol{\tau}_N = \mathbf{J}_c \mathbf{M}^{-1} \left( \mathbf{I}_n - \mathbf{J}_c^\top \boldsymbol{\Lambda}_c \mathbf{J}_c \mathbf{M}^{-1} \right) \boldsymbol{\tau}_0 = \mathbf{0}, \]

because \( \mathbf{J}_c \mathbf{M}^{-1} \mathbf{J}_c^\top \boldsymbol{\Lambda}_c = \mathbf{I}_m \). Thus \( \mathbf{N}^\top \) is exactly the projector that annihilates accelerations along the constrained directions.

Given the robot state \( (\mathbf{q}, \dot{\mathbf{q}}) \) and a desired reference trajectory, one control cycle (e.g., a servo step) for equality-constrained control proceeds as follows:

1. Compute the inertia matrix \( \mathbf{M}(\mathbf{q}) \) and non-linear term \( \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) \) using your robotics dynamics library (e.g., Pinocchio, RBDL, Robotics System Toolbox).
2. Evaluate the constraint Jacobian \( \mathbf{J}_c(\mathbf{q}) \) and the bias term \( \mathbf{b}_c(\mathbf{q}, \dot{\mathbf{q}}, t) \).
3. Compute an unconstrained torque \( \boldsymbol{\tau}_0 \) (e.g., via joint-space PD or computed-torque control).
4. Form \( \boldsymbol{\Lambda}_c = (\mathbf{J}_c\mathbf{M}^{-1}\mathbf{J}_c^\top)^{-1} \) and the projector \( \mathbf{N}^\top = \mathbf{I}_n - \mathbf{J}_c^\top \boldsymbol{\Lambda}_c \mathbf{J}_c\mathbf{M}^{-1} \).
5. Optionally compute a particular constraint torque term ensuring any desired constraint acceleration (often zero) and add it.
6. Send the projected torque \( \boldsymbol{\tau} \) to the actuators.

flowchart TD
  S["State (q, qd)"] --> DYN["Compute M(q), h(q,qd)"]
  DYN --> JC["Compute constraint J_c(q) and b_c"]
  JC --> CTRL["Unconstrained control law gives tau_0"]
  CTRL --> PROJ["Build Lambda_c and projector N_T"]
  PROJ --> TAU["Tau = projected torque"]
  TAU --> ROB["Apply to robot under constraints"]
        

In Python, we can rely on numpy for linear algebra and robotics libraries such as pinocchio or roboticstoolbox-python to obtain \( \mathbf{M} \), \( \mathbf{h} \) and Jacobians. The snippet below abstracts away the details of those calls and focuses on the projection logic.

import numpy as np

def compute_projected_torque(M, h, Jc, bc, tau0, eps=1e-9):
    """
    Equality-constrained torque projection in Python (NumPy).

    Parameters
    ----------
    M : (n, n) array
        Joint-space inertia matrix.
    h : (n,) array
        Nonlinear term h(q, qd).
    Jc : (m, n) array
        Constraint Jacobian.
    bc : (m,) array
        Constraint bias term b_c(q, qd, t).
    tau0 : (n,) array
        Unconstrained torque command.
    eps : float
        Damping for numerical robustness.

    Returns
    -------
    tau : (n,) array
        Constraint-consistent torque.
    lam : (m,) array
        Lagrange multipliers (generalized constraint forces).
    """
    # Solve M x = y using a linear solver for numerical stability
    Minv_tau0_minus_h = np.linalg.solve(M, tau0 - h)
    JMJT = Jc @ np.linalg.solve(M, Jc.T)

    # (J_c M^{-1} J_c^T)^(-1) with small Tikhonov regularization
    JMJT_reg = JMJT + eps * np.eye(JMJT.shape[0])
    Lambda_c = np.linalg.inv(JMJT_reg)

    # Lagrange multipliers enforcing the acceleration constraint
    lam = -Lambda_c @ (Jc @ Minv_tau0_minus_h + bc)

    # Final constrained torque
    tau = tau0 + Jc.T @ lam
    return tau, lam

# Example usage: (Assuming get_M_h_Jc_bc is provided by a robotics library)
def controller_step(q, qd, q_ref, qd_ref, qdd_ref):
    # User-provided functions from dynamics library (e.g., pinocchio)
    M, h = get_M_h(q, qd)
    Jc, bc = get_constraint_jacobian_and_bias(q, qd)

    # Unconstrained computed-torque PD controller
    Kp = np.diag([100.0, 80.0, 60.0])
    Kd = np.diag([20.0,  16.0, 12.0])
    e  = q_ref - q
    ed = qd_ref - qd
    v  = qdd_ref + Kd @ ed + Kp @ e        # desired joint acceleration
    tau0 = M @ v + h                       # inverse-dynamics torque

    tau, lam = compute_projected_torque(M, h, Jc, bc, tau0)
    return tau
      

In a real codebase, get_M_h and get_constraint_jacobian_and_bias would call into a library such as pinocchio (C++/Python) or roboticstoolbox, which computes the dynamics and Jacobians from the robot model.

In C++, it is common to use Eigen for linear algebra and a dynamics engine such as RBDL or Pinocchio. The following snippet sketches the projection step given matrices and vectors obtained from such libraries.

#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;

struct ConstrainedTorqueResult {
    VectorXd tau;
    VectorXd lambda;
};

ConstrainedTorqueResult computeProjectedTorque(
    const MatrixXd& M,
    const VectorXd& h,
    const MatrixXd& Jc,
    const VectorXd& bc,
    const VectorXd& tau0,
    double eps = 1e-9)
{
    const int n = M.rows();
    const int m = Jc.rows();

    // Solve M x = (tau0 - h)
    VectorXd Minv_tau0_minus_h = M.ldlt().solve(tau0 - h);

    // Compute Jc * M^{-1} * Jc^T
    MatrixXd Minv_JcT = M.ldlt().solve(Jc.transpose());      // (n,m)
    MatrixXd JMJT = Jc * Minv_JcT;                           // (m,m)

    // Regularize and invert
    MatrixXd JMJT_reg = JMJT + eps * MatrixXd::Identity(m, m);
    MatrixXd Lambda_c = JMJT_reg.inverse();

    // Lagrange multipliers
    VectorXd lambda = -Lambda_c * (Jc * Minv_tau0_minus_h + bc);

    // Final torque
    VectorXd tau = tau0 + Jc.transpose() * lambda;

    return {tau, lambda};
}

// Example usage inside control loop (using Pinocchio or RBDL to get M, h, Jc, bc)
void controllerStep(const VectorXd& q,
                    const VectorXd& qd,
                    const VectorXd& q_ref,
                    const VectorXd& qd_ref,
                    const VectorXd& qdd_ref,
                    VectorXd& tau_out)
{
    MatrixXd M;
    VectorXd h;
    // e.g. pinocchio::crba(model, data, q); pinocchio::nonLinearEffects(...);
    getMassMatrixAndBias(q, qd, M, h);

    MatrixXd Jc;
    VectorXd bc;
    getConstraintJacobianAndBias(q, qd, Jc, bc);

    // Unconstrained PD + feedforward
    MatrixXd Kp = MatrixXd::Identity(q.size(), q.size()) * 100.0;
    MatrixXd Kd = MatrixXd::Identity(q.size(), q.size()) * 20.0;

    VectorXd e  = q_ref  - q;
    VectorXd ed = qd_ref - qd;
    VectorXd v  = qdd_ref + Kd * ed + Kp * e;
    VectorXd tau0 = M * v + h;

    auto res = computeProjectedTorque(M, h, Jc, bc, tau0);
    tau_out = res.tau;
}
      

Here getMassMatrixAndBias and getConstraintJacobianAndBias are thin wrappers around, e.g., RBDL or Pinocchio procedures that compute \( \mathbf{M}, \mathbf{h}, \mathbf{J}_c, \mathbf{b}_c \) from the robot model.

For Java, a common choice for linear algebra is EJML. Robotics-specific toolboxes are less standardized; one typically wraps a C++ engine (via JNI) or implements dynamics directly for simpler robots. The following fragment shows the projection formula using EJML.

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

public class EqualityConstrainedControl {

    public static class Result {
        public DMatrixRMaj tau;
        public DMatrixRMaj lambda;
    }

    public static Result computeProjectedTorque(
            DMatrixRMaj M,
            DMatrixRMaj h,
            DMatrixRMaj Jc,
            DMatrixRMaj bc,
            DMatrixRMaj tau0,
            double eps)
    {
        int n = M.getNumRows();
        int m = Jc.getNumRows();

        // Minv*(tau0 - h) via solve
        DMatrixRMaj tau0_minus_h = new DMatrixRMaj(n, 1);
        CommonOps_DDRM.subtract(tau0, h, tau0_minus_h);

        DMatrixRMaj Minv_tau0_minus_h = new DMatrixRMaj(n, 1);
        CommonOps_DDRM.solve(M, tau0_minus_h, Minv_tau0_minus_h);

        // Minv*Jc^T
        DMatrixRMaj JcT = new DMatrixRMaj(n, m);
        CommonOps_DDRM.transpose(Jc, JcT);

        DMatrixRMaj Minv_JcT = new DMatrixRMaj(n, m);
        CommonOps_DDRM.solve(M, JcT, Minv_JcT);

        // JMJT = Jc * Minv * Jc^T
        DMatrixRMaj JMJT = new DMatrixRMaj(m, m);
        CommonOps_DDRM.mult(Jc, Minv_JcT, JMJT);

        // Regularize and invert JMJT
        for (int i = 0; i < m; ++i) {
            double v = JMJT.get(i, i);
            JMJT.set(i, i, v + eps);
        }
        DMatrixRMaj Lambda_c = new DMatrixRMaj(m, m);
        CommonOps_DDRM.invert(JMJT, Lambda_c);

        // lambda = -Lambda_c * (Jc * Minv_tau0_minus_h + bc)
        DMatrixRMaj tmp = new DMatrixRMaj(m, 1);
        CommonOps_DDRM.mult(Jc, Minv_tau0_minus_h, tmp);
        CommonOps_DDRM.addEquals(tmp, 1.0, bc);

        DMatrixRMaj lambda = new DMatrixRMaj(m, 1);
        CommonOps_DDRM.mult(Lambda_c, tmp, lambda);
        CommonOps_DDRM.scale(-1.0, lambda);

        // tau = tau0 + Jc^T * lambda
        DMatrixRMaj tau = tau0.copy();
        DMatrixRMaj JcT_lambda = new DMatrixRMaj(n, 1);
        CommonOps_DDRM.mult(JcT, lambda, JcT_lambda);
        CommonOps_DDRM.addEquals(tau, 1.0, JcT_lambda);

        Result res = new Result();
        res.tau = tau;
        res.lambda = lambda;
        return res;
    }
}
      

This Java implementation mirrors the algebra of the Python and C++ versions. To get \( \mathbf{M}, \mathbf{h}, \mathbf{J}_c, \mathbf{b}_c \), one could either call into a C++ robotics library via JNI or implement the Lagrangian dynamics for a lower-DOF arm directly in Java.

MATLAB, together with the Robotics System Toolbox, is a natural environment for implementing projection-based controllers. The function below assumes that helper functions compute the inertia, bias, and constraint Jacobian/bias given the current state.

function [tau, lambda] = projectedTorque(q, qd, q_ref, qd_ref, qdd_ref)
% Equality-constrained torque projection for a manipulator.
%
% q, qd      : current joint position and velocity (n-by-1)
% q_ref, ... : reference trajectory
%
% Dependencies:
%   getMassMatrixAndBias   - returns M(q), h(q, qd)
%   getConstraintJacobian  - returns Jc(q), bc(q, qd, t)

    % Dynamics
    [M, h] = getMassMatrixAndBias(q, qd);
    [Jc, bc] = getConstraintJacobian(q, qd);

    n = length(q);
    m = size(Jc, 1);

    % Unconstrained computed-torque PD
    Kp = diag(100 * ones(n, 1));
    Kd = diag(20  * ones(n, 1));

    e  = q_ref  - q;
    ed = qd_ref - qd;
    v  = qdd_ref + Kd * ed + Kp * e;

    tau0 = M * v + h;

    % Projection
    Minv_tau0_minus_h = M \ (tau0 - h);
    Minv_JcT = M \ Jc';

    JMJT = Jc * Minv_JcT;
    eps = 1e-9;
    Lambda_c = inv(JMJT + eps * eye(m));

    lambda = -Lambda_c * (Jc * Minv_tau0_minus_h + bc);
    tau = tau0 + Jc' * lambda;
end
      

In Simulink, one can encapsulate the above code in a MATLAB Function block. The block takes \( \mathbf{q}, \dot{\mathbf{q}} \) and the reference trajectory as inputs and outputs the torque \( \boldsymbol{\tau} \). Dynamics and Jacobian computations can be provided by Simscape Multibody or the Robotics System Toolbox blocks.

Wolfram Mathematica can be used to verify algebraic identities and derive projectors symbolically. The example below defines symbolic matrices and shows that the projector annihilates constrained directions:

(* Dimensions: M is n x n, Jc is m x n *)
Clear[M, Jc, tau0, h, bc];
n = 4; m = 2;

M  = Array[M, {n, n}];
Jc = Array[Jc, {m, n}];
tau0 = Array[t0, {n, 1}];
h    = Array[h,  {n, 1}];
bc   = Array[bc, {m, 1}];

Minv = Inverse[M];
JMJT = Jc . Minv . Transpose[Jc];
LambdaC = Inverse[JMJT];

(* Dynamic-consistent projector in torque space *)
NT = IdentityMatrix[n] - Transpose[Jc].LambdaC.Jc.Minv;

(* Verify the constraint property Jc Minv NT == 0 *)
Simplify[Jc . Minv . NT]
      

Mathematica will return a symbolic zero matrix (under suitable assumptions), confirming that \( \mathbf{J}_c \mathbf{M}^{-1} \mathbf{N}^\top = \mathbf{0} \) and thus that torques in the range of \( \mathbf{N}^\top \) do not induce constraint-violating accelerations.

In this lesson we formalized equality constraints in robot motion as holonomic constraints \( \boldsymbol{\phi}(\mathbf{q}, t) = \mathbf{0} \) with associated velocity and acceleration constraints. We incorporated them into the joint-space dynamics via Lagrange multipliers and then derived an equivalent projection-based controller using the dynamic-consistent pseudoinverse of the constraint Jacobian.

The resulting projector \( \mathbf{N}^\top \) allows us to decompose control torques into constraint-consistent components and reaction forces, guaranteeing that the induced accelerations do not violate equality constraints. We implemented this structure in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, preparing the ground for inequality-constrained control and contact transitions in the next lessons.