In whole-body robotics (e.g., humanoids, quadrupeds), the robot often maintains multiple rigid contacts (feet on the ground, hands on a rail, etc.). A contact-consistent controller generates motion commands that lie on the contact manifold, i.e., do not induce penetration, separation, or slip (under a rigid/no-slip model).

Let \( \mathbf{q} \in \mathbb{R}^n \) be the generalized coordinate vector (including a floating base if present) and \( \dot{\mathbf{q}} \) its velocity. We model a set of holonomic contact constraints as

\[ \mathbf{c}(\mathbf{q}) = \mathbf{0}, \quad \mathbf{c} : \mathbb{R}^n \rightarrow \mathbb{R}^{n_c}. \]

The corresponding contact Jacobian is \( \mathbf{J}_c(\mathbf{q}) = \frac{\partial \mathbf{c}}{\partial \mathbf{q}} \). Differentiating the constraint gives the velocity and acceleration levels:

\[ \mathbf{J}_c(\mathbf{q}) \, \dot{\mathbf{q}} = \mathbf{0}, \quad \mathbf{J}_c(\mathbf{q}) \, \ddot{\mathbf{q}} + \dot{\mathbf{J}}_c(\mathbf{q},\dot{\mathbf{q}})\, \dot{\mathbf{q}} = \mathbf{0}. \]

A free-space controller might propose some desired acceleration \( \ddot{\mathbf{q}}_{\mathrm{des}} \), ignoring contacts. A contact-consistent controller modifies this to \( \ddot{\mathbf{q}}^{\star} \) that satisfies the acceleration-level constraint while staying as close as possible (in a suitable metric) to \( \ddot{\mathbf{q}}_{\mathrm{des}} \).

At the dynamics level, we recall the standard floating-base dynamics with contacts (already introduced in earlier chapters):

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

where \( \mathbf{M} \) is the mass matrix, \( \mathbf{h} \) contains Coriolis, centrifugal and gravity terms, \( \mathbf{S} \) selects actuated joints, \( \tau \) is the joint torque vector, and \( \boldsymbol{\lambda} \) are Lagrange multipliers (contact forces). Contact-consistent control ensures that the closed-loop system satisfies both dynamics and contact constraints.

When contacts are rigid and non-slipping, the space of feasible accelerations is an affine subspace of \( \mathbb{R}^n \) defined by the acceleration-level constraint

\[ \mathcal{A}_c(\mathbf{q},\dot{\mathbf{q}}) = \left\{ \ddot{\mathbf{q}} \in \mathbb{R}^n \,\bigg|\, \mathbf{J}_c(\mathbf{q}) \ddot{\mathbf{q}} + \dot{\mathbf{J}}_c(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} = \mathbf{0} \right\}. \]

Given a desired (possibly inconsistent) acceleration \( \ddot{\mathbf{q}}_{\mathrm{des}} \) coming from task-space control (Chapter 4) or optimization (Chapter 14, Lesson 2), we pose a weighted least-squares problem:

\[ \begin{aligned} \ddot{\mathbf{q}}^{\star} &= \arg\min_{\ddot{\mathbf{q}}} \; \tfrac{1}{2} (\ddot{\mathbf{q}} - \ddot{\mathbf{q}}_{\mathrm{des}})^\top \mathbf{W} (\ddot{\mathbf{q}} - \ddot{\mathbf{q}}_{\mathrm{des}}) \\ &\text{subject to } \mathbf{J}_c \ddot{\mathbf{q}} + \dot{\mathbf{J}}_c \dot{\mathbf{q}} = \mathbf{0}, \end{aligned} \]

where \( \mathbf{W} \succ \mathbf{0} \) is a symmetric positive-definite weighting matrix. Choosing \( \mathbf{W} = \mathbf{M}(\mathbf{q}) \) leads to dynamically consistent contact projectors.

This is a strictly convex quadratic program with equality constraints, so it admits a unique closed-form solution using the Karush–Kuhn–Tucker (KKT) conditions, as derived next.

Form the Lagrangian with multiplier \( \boldsymbol{\nu} \in \mathbb{R}^{n_c} \):

\[ \mathcal{L}(\ddot{\mathbf{q}},\boldsymbol{\nu}) = \tfrac{1}{2}(\ddot{\mathbf{q}} - \ddot{\mathbf{q}}_{\mathrm{des}})^\top \mathbf{W}(\ddot{\mathbf{q}} - \ddot{\mathbf{q}}_{\mathrm{des}}) + \boldsymbol{\nu}^\top (\mathbf{J}_c \ddot{\mathbf{q}} + \dot{\mathbf{J}}_c \dot{\mathbf{q}}). \]

Stationarity with respect to \( \ddot{\mathbf{q}} \) gives

\[ \frac{\partial \mathcal{L}}{\partial \ddot{\mathbf{q}}} = \mathbf{W}(\ddot{\mathbf{q}} - \ddot{\mathbf{q}}_{\mathrm{des}}) + \mathbf{J}_c^\top \boldsymbol{\nu} = \mathbf{0}. \]

Solving for \( \ddot{\mathbf{q}} \) yields

\[ \ddot{\mathbf{q}} = \ddot{\mathbf{q}}_{\mathrm{des}} - \mathbf{W}^{-1} \mathbf{J}_c^\top \boldsymbol{\nu}. \]

Enforcing the constraint \( \mathbf{J}_c \ddot{\mathbf{q}} + \dot{\mathbf{J}}_c \dot{\mathbf{q}} = \mathbf{0} \) gives

\[ \mathbf{J}_c \ddot{\mathbf{q}}_{\mathrm{des}} - \mathbf{J}_c \mathbf{W}^{-1}\mathbf{J}_c^\top \boldsymbol{\nu} + \dot{\mathbf{J}}_c \dot{\mathbf{q}} = \mathbf{0}, \]

so

\[ \boldsymbol{\nu} = \left(\mathbf{J}_c \mathbf{W}^{-1}\mathbf{J}_c^\top\right)^{-1} \left(\mathbf{J}_c \ddot{\mathbf{q}}_{\mathrm{des}} + \dot{\mathbf{J}}_c \dot{\mathbf{q}}\right). \]

Substituting back, the optimal acceleration is

\[ \ddot{\mathbf{q}}^{\star} = \ddot{\mathbf{q}}_{\mathrm{des}} - \mathbf{W}^{-1}\mathbf{J}_c^\top \left(\mathbf{J}_c \mathbf{W}^{-1}\mathbf{J}_c^\top\right)^{-1} \left(\mathbf{J}_c \ddot{\mathbf{q}}_{\mathrm{des}} + \dot{\mathbf{J}}_c \dot{\mathbf{q}}\right). \]

Define the weighted pseudo-inverse of the contact Jacobian

\[ \mathbf{J}_c^{\#\mathbf{W}} \;=\; \mathbf{W}^{-1}\mathbf{J}_c^\top \left(\mathbf{J}_c \mathbf{W}^{-1}\mathbf{J}_c^\top\right)^{-1}, \]

and the associated contact projector

\[ \mathbf{N}_c^{\mathbf{W}} = \mathbf{I} - \mathbf{J}_c^{\#\mathbf{W}}\mathbf{J}_c. \]

Then we can write the solution compactly as

\[ \ddot{\mathbf{q}}^{\star} = \mathbf{N}_c^{\mathbf{W}} \, \ddot{\mathbf{q}}_{\mathrm{des}} - \mathbf{J}_c^{\#\mathbf{W}} \, \dot{\mathbf{J}}_c \dot{\mathbf{q}}. \]

With \( \mathbf{W} = \mathbf{M}(\mathbf{q}) \) we obtain the standard dynamically consistent pseudo-inverse

\[ \mathbf{J}_c^{\#\mathbf{M}} = \mathbf{M}^{-1}\mathbf{J}_c^\top \left(\mathbf{J}_c \mathbf{M}^{-1}\mathbf{J}_c^\top\right)^{-1}, \quad \mathbf{N}_c = \mathbf{I} - \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c, \]

which is central to whole-body operational-space formulations.

We now show that \( \mathbf{N}_c \) is a projector onto the null-space of \( \mathbf{J}_c \) with respect to the metric \( \mathbf{M} \).

4.1 Null-space property

Take any vector \( \mathbf{x} \). From the definition,

\[ \mathbf{J}_c \mathbf{N}_c \mathbf{x} = \mathbf{J}_c (\mathbf{I} - \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c)\mathbf{x} = \mathbf{J}_c \mathbf{x} - \mathbf{J}_c \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c \mathbf{x}. \]

By properties of the weighted pseudo-inverse \( \mathbf{J}_c^{\#\mathbf{M}} \) (it yields the minimum-\( \mathbf{M} \)-norm solution), the product \( \mathbf{P}_c = \mathbf{J}_c \mathbf{J}_c^{\#\mathbf{M}} \) is a projector onto the range of \( \mathbf{J}_c \), hence \( \mathbf{J}_c \mathbf{J}_c^{\#\mathbf{M}} \mathbf{J}_c = \mathbf{J}_c \). Therefore,

\[ \mathbf{J}_c \mathbf{N}_c \mathbf{x} = \mathbf{J}_c \mathbf{x} - \mathbf{J}_c \mathbf{x} = \mathbf{0}, \]

which shows that the image of \( \mathbf{N}_c \) lies in the null-space of \( \mathbf{J}_c \).

4.2 Idempotence

We check that \( \mathbf{N}_c^2 = \mathbf{N}_c \):

\[ \mathbf{N}_c^2 = (\mathbf{I} - \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c)^2 = \mathbf{I} - 2\mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c + \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c. \]

Using again \( \mathbf{J}_c \mathbf{J}_c^{\#\mathbf{M}} \mathbf{J}_c = \mathbf{J}_c \), we obtain

\[ \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c = \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c, \]

hence

\[ \mathbf{N}_c^2 = \mathbf{I} - 2\mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c + \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c = \mathbf{I} - \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c = \mathbf{N}_c. \]

4.3 Dynamic consistency

A key property is that \( \mathbf{N}_c \) is \( \mathbf{M} \)-orthogonal:

\[ \mathbf{N}_c^\top \mathbf{M} = \mathbf{M}\mathbf{N}_c. \]

This implies that projection by \( \mathbf{N}_c \) preserves kinetic energy contributions of motions within the contact manifold and leads to dynamically consistent task decoupling (as in operational-space control). The proof uses the symmetry of \( \mathbf{M} \) and the weighted least-squares characterization of \( \mathbf{J}_c^{\#\mathbf{M}} \), and is standard in the literature on operational-space dynamics.

In whole-body control, we typically obtain a preliminary acceleration \( \ddot{\mathbf{q}}_{\mathrm{task}} \) from task tracking, then apply \( \mathbf{N}_c \) and the inhomogeneous term to make it contact-consistent:

\[ \ddot{\mathbf{q}}^{\star} = \mathbf{N}_c \ddot{\mathbf{q}}_{\mathrm{task}} - \mathbf{J}_c^{\#\mathbf{M}} \dot{\mathbf{J}}_c \dot{\mathbf{q}}. \]

The following diagram summarizes how contact-consistent control fits into a typical whole-body controller: tasks propose accelerations, contacts define constraints, and the projector enforces consistency before torques are computed (e.g., via QP or inverse dynamics).

flowchart TD
  T["Task accelerations qdd_task from stack"] --> U["Unconstrained qdd_des"]
  C["Contact model (Jc, Jc_dot, qdot)"] --> P["Compute dyn-consistent Jc_pinv and Nc"]
  U --> P
  P --> Q["Contact-consistent qdd_star"]
  Q --> ID["Inverse dynamics / QP to compute tau, lambda"]
  C --> ID
        

In the next sections we show how to implement the contact projection numerically with standard robotics and linear-algebra libraries.

In Python, we can use numpy for linear algebra and obtain the dynamics quantities from libraries such as pinocchio, pybullet, or ROS-control-based stacks. Below is a minimal example for computing the dynamically consistent pseudo-inverse and contact-consistent acceleration:

import numpy as np

def dyn_consistent_pinv(M, Jc):
    """
    Compute dynamically consistent pseudo-inverse of Jc:
        Jc_dyn_pinv = M^{-1} Jc^T (Jc M^{-1} Jc^T)^{-1}
    M  : (n,n) positive-definite mass matrix
    Jc : (nc,n) contact Jacobian
    """
    Minv = np.linalg.inv(M)
    # Lambda_c = (Jc M^{-1} Jc^T)^{-1} (operational contact inertia)
    Lambda_c = np.linalg.inv(Jc @ Minv @ Jc.T)
    Jc_dyn_pinv = Minv @ Jc.T @ Lambda_c
    return Jc_dyn_pinv

def contact_projector(M, Jc):
    """
    Nc = I - Jc_dyn_pinv Jc
    """
    n = M.shape[0]
    Jc_dyn_pinv = dyn_consistent_pinv(M, Jc)
    Nc = np.eye(n) - Jc_dyn_pinv @ Jc
    return Nc, Jc_dyn_pinv

def contact_consistent_acc(M, Jc, Jc_dot_qdot, qdd_des):
    """
    Compute contact-consistent acceleration:
        qdd_star = Nc qdd_des - Jc_dyn_pinv Jc_dot_qdot
    Inputs:
      M            : (n,n) mass matrix at current q
      Jc           : (nc,n) contact Jacobian
      Jc_dot_qdot  : (nc,) = Jc_dot(q,qdot) @ qdot (known from dynamics)
      qdd_des      : (n,) unconstrained desired acceleration
    """
    Nc, Jc_dyn_pinv = contact_projector(M, Jc)
    return Nc @ qdd_des - Jc_dyn_pinv @ Jc_dot_qdot

# Example usage (with dummy numbers; in practice, query M, Jc, Jc_dot_qdot from a robotics lib)
n = 6
nc = 3
M = np.eye(n)  # replace with pinocchio.crba(model, data, q)
Jc = np.random.randn(nc, n)  # replace with contact Jacobian from forward kinematics
Jc_dot_qdot = np.zeros(nc)   # replace with pinocchio.computeJdotQdot(...)
qdd_des = np.zeros(n)

qdd_star = contact_consistent_acc(M, Jc, Jc_dot_qdot, qdd_des)
print("Contact-consistent qdd_star:", qdd_star)
      

In a full whole-body controller, qdd_des would come from a prioritized task stack (Chapter 14, Lesson 2), and the resulting qdd_star would be fed to inverse dynamics (e.g. pinocchio.rnea) or a QP that solves for \( \tau \) and \( \boldsymbol{\lambda} \).

In C++, whole-body controllers commonly rely on Eigen for linear algebra and on dynamics libraries such as RBDL, Pinocchio, or Drake. The following snippet illustrates the projector computation with Eigen:

#include <Eigen/Dense>

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

MatrixXd dynConsistentPinv(const MatrixXd& M, const MatrixXd& Jc)
{
    // Jc_dyn_pinv = M^{-1} Jc^T (Jc M^{-1} Jc^T)^{-1}
    MatrixXd Minv = M.inverse();
    MatrixXd temp = Jc * Minv * Jc.transpose();
    MatrixXd Lambda_c = temp.inverse();
    MatrixXd Jc_dyn_pinv = Minv * Jc.transpose() * Lambda_c;
    return Jc_dyn_pinv;
}

void contactProjector(const MatrixXd& M,
                      const MatrixXd& Jc,
                      MatrixXd& Nc,
                      MatrixXd& Jc_dyn_pinv)
{
    int n = static_cast<int>(M.rows());
    Jc_dyn_pinv = dynConsistentPinv(M, Jc);
    Nc = MatrixXd::Identity(n, n) - Jc_dyn_pinv * Jc;
}

VectorXd contactConsistentAcc(const MatrixXd& M,
                              const MatrixXd& Jc,
                              const VectorXd& Jc_dot_qdot,
                              const VectorXd& qdd_des)
{
    MatrixXd Nc, Jc_dyn_pinv;
    contactProjector(M, Jc, Nc, Jc_dyn_pinv);
    // qdd_star = Nc qdd_des - Jc_dyn_pinv Jc_dot_qdot
    return Nc * qdd_des - Jc_dyn_pinv * Jc_dot_qdot;
}
      

In a robotics application, M, Jc, and Jc_dot_qdot would be obtained from Pinocchio (computeMassMatrix, getFrameJacobian, computeJdotQdot) or RBDL (CompositeRigidBodyAlgorithm, CalcPointJacobian, CalcPointJacobianDot), and the resulting qdd_star would be integrated inside a whole-body QP or hierarchical controller.

For Java, a convenient numerical library is EJML. Below we implement the same projector using dense matrices:

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

public class ContactConsistency {

    public static DMatrixRMaj dynConsistentPinv(DMatrixRMaj M, DMatrixRMaj Jc) {
        int n = M.getNumRows();
        int nc = Jc.getNumRows();

        DMatrixRMaj Minv = new DMatrixRMaj(n, n);
        CommonOps_DDRM.invert(M, Minv);

        DMatrixRMaj JcT = new DMatrixRMaj(n, nc);
        CommonOps_DDRM.transpose(Jc, JcT);

        DMatrixRMaj temp = new DMatrixRMaj(nc, nc);
        // temp = Jc * Minv * Jc^T
        DMatrixRMaj JcMinv = new DMatrixRMaj(nc, n);
        CommonOps_DDRM.mult(Jc, Minv, JcMinv);
        CommonOps_DDRM.mult(JcMinv, JcT, temp);

        DMatrixRMaj Lambda_c = new DMatrixRMaj(nc, nc);
        CommonOps_DDRM.invert(temp, Lambda_c);

        DMatrixRMaj Jc_dyn_pinv = new DMatrixRMaj(n, nc);
        // Jc_dyn_pinv = Minv * Jc^T * Lambda_c
        DMatrixRMaj temp2 = new DMatrixRMaj(n, nc);
        CommonOps_DDRM.mult(Minv, JcT, temp2);
        CommonOps_DDRM.mult(temp2, Lambda_c, Jc_dyn_pinv);
        return Jc_dyn_pinv;
    }

    public static DMatrixRMaj contactProjector(DMatrixRMaj M, DMatrixRMaj Jc,
                                               DMatrixRMaj Jc_dyn_pinv) {
        int n = M.getNumRows();
        Jc_dyn_pinv.set(dynConsistentPinv(M, Jc));

        DMatrixRMaj Nc = CommonOps_DDRM.identity(n);
        DMatrixRMaj temp = new DMatrixRMaj(n, n);
        CommonOps_DDRM.mult(Jc_dyn_pinv, Jc, temp);
        CommonOps_DDRM.subtractEquals(Nc, temp); // Nc = I - Jc_dyn_pinv * Jc
        return Nc;
    }

    public static DMatrixRMaj contactConsistentAcc(DMatrixRMaj M,
                                                   DMatrixRMaj Jc,
                                                   DMatrixRMaj Jc_dot_qdot,
                                                   DMatrixRMaj qdd_des) {
        int n = M.getNumRows();
        DMatrixRMaj Jc_dyn_pinv = new DMatrixRMaj(n, Jc.getNumRows());
        DMatrixRMaj Nc = contactProjector(M, Jc, Jc_dyn_pinv);

        DMatrixRMaj qdd_star = new DMatrixRMaj(n, 1);
        // qdd_star = Nc qdd_des - Jc_dyn_pinv Jc_dot_qdot
        DMatrixRMaj temp = new DMatrixRMaj(n, 1);
        CommonOps_DDRM.mult(Nc, qdd_des, qdd_star);
        CommonOps_DDRM.mult(Jc_dyn_pinv, Jc_dot_qdot, temp);
        CommonOps_DDRM.subtractEquals(qdd_star, temp);
        return qdd_star;
    }
}
      

Robotics frameworks in Java can call this routine once the robot model provides M and Jc (e.g., via bindings to a C++ dynamics engine).

MATLAB’s matrix syntax makes the projector definition concise. Assuming access to the mass matrix and contact Jacobian from a Robotics System Toolbox model, we can write:

function qdd_star = contactConsistentAcc(M, Jc, Jc_dot_qdot, qdd_des)
%CONTACTCONSISTENTACC Compute contact-consistent acceleration
%   M            : (n,n) mass matrix
%   Jc           : (nc,n) contact Jacobian
%   Jc_dot_qdot  : (nc,1) = Jc_dot(q,qdot) * qdot
%   qdd_des      : (n,1) unconstrained acceleration

Minv = inv(M);
Lambda_c = inv(Jc * Minv * Jc.');      % operational contact inertia
Jc_dyn_pinv = Minv * Jc.' * Lambda_c;  % M-consistent pseudo-inverse
Nc = eye(size(M,1)) - Jc_dyn_pinv * Jc;

qdd_star = Nc * qdd_des - Jc_dyn_pinv * Jc_dot_qdot;
end
      

In Simulink, one can encapsulate this computation in a MATLAB Function block. The block inputs are M, Jc, Jc_dot_qdot, and qdd_des, and its output qdd_star is fed into an inverse-dynamics block or into a whole-body QP block. The controller block diagram mirrors the conceptual pipeline shown earlier.

In Wolfram Mathematica, we can express the same projector using Inverse and standard matrix operations:

JcDynPseudoInverse[M_, Jc_] := Module[{Minv, Lambda, pinv},
  Minv   = Inverse[M];
  Lambda = Inverse[Jc.Minv.Transpose[Jc]];
  pinv   = Minv.Transpose[Jc].Lambda;
  pinv
];

ContactProjector[M_, Jc_] := Module[{JcPinv, n, Nc},
  n      = Length[M];
  JcPinv = JcDynPseudoInverse[M, Jc];
  Nc     = IdentityMatrix[n] - JcPinv.Jc;
  {Nc, JcPinv}
];

ContactConsistentAcc[M_, Jc_, JcDotQdot_, qddDes_] :=
 Module[{Nc, JcPinv},
  {Nc, JcPinv} = ContactProjector[M, Jc];
  (* qddStar = Nc qddDes - JcPinv JcDotQdot *)
  Nc.qddDes - JcPinv.JcDotQdot
];

(* Example with symbolic M and Jc (for small systems) *)
(* M  = { {m1, 0}, {0, m2} };
   Jc = { {j11, j12} };
   Simplify[ContactConsistentAcc[M, Jc, {0}, {qdd1Des, qdd2Des}]]; *)
      

For low-dimensional systems, Mathematica can symbolically verify properties like \( \mathbf{N}_c^2 = \mathbf{N}_c \) and \( \mathbf{J}_c \mathbf{N}_c = \mathbf{0} \) by expressing them as algebraic identities.

Problem 1 (KKT Derivation of Contact-Consistent Acceleration). Starting from the optimization problem in Section 2 with \( \mathbf{W} = \mathbf{M}(\mathbf{q}) \), derive the closed-form expression \( \ddot{\mathbf{q}}^{\star} = \mathbf{N}_c \ddot{\mathbf{q}}_{\mathrm{des}} - \mathbf{J}_c^{\#\mathbf{M}} \dot{\mathbf{J}}_c \dot{\mathbf{q}} \).

Solution. The derivation is identical to Section 3 with \( \mathbf{W} = \mathbf{M} \). Writing the KKT system explicitly:

\[ \begin{bmatrix} \mathbf{M} & \mathbf{J}_c^\top \\ \mathbf{J}_c & \mathbf{0} \end{bmatrix} \begin{bmatrix} \ddot{\mathbf{q}} \\ \boldsymbol{\nu} \end{bmatrix} = \begin{bmatrix} \mathbf{M} \ddot{\mathbf{q}}_{\mathrm{des}} \\ -\dot{\mathbf{J}}_c \dot{\mathbf{q}} \end{bmatrix}. \]

Solving the first block row for \( \ddot{\mathbf{q}} \) and substituting into the second yields the formula for \( \boldsymbol{\nu} \), and thus for \( \ddot{\mathbf{q}}^{\star} \), which matches the expression using \( \mathbf{J}_c^{\#\mathbf{M}} \) and \( \mathbf{N}_c \).

Problem 2 (Projector Properties). Prove that \( \mathbf{N}_c = \mathbf{I} - \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c \) satisfies \( \mathbf{N}_c^2 = \mathbf{N}_c \) and \( \mathbf{J}_c \mathbf{N}_c = \mathbf{0} \).

Solution. Using the pseudo-inverse property \( \mathbf{J}_c \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c = \mathbf{J}_c \), we compute

\[ \mathbf{J}_c \mathbf{N}_c = \mathbf{J}_c (\mathbf{I} - \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c) = \mathbf{J}_c - \mathbf{J}_c \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c = \mathbf{0}. \]

For idempotence,

\[ \mathbf{N}_c^2 = (\mathbf{I} - \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c)^2 = \mathbf{I} - 2\mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c + \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c. \]

Since \( \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c = \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c \), the last two terms cancel to leave \( \mathbf{N}_c^2 = \mathbf{N}_c \). Thus \( \mathbf{N}_c \) is a projector whose image is the null-space of \( \mathbf{J}_c \).

Problem 3 (2-DOF Example). Consider a planar 2-DOF manipulator whose end-effector is rigidly fixed to the world (one contact point with a scalar constraint on the end-effector’s vertical position). Assume \( \mathbf{M} = \mathrm{diag}(m_1, m_2) \) and \( \mathbf{J}_c = [\alpha \;\; \beta] \). Compute \( \mathbf{J}_c^{\#\mathbf{M}} \) and \( \mathbf{N}_c \).

Solution. We have

\[ \mathbf{M}^{-1} = \begin{bmatrix} 1/m_1 & 0 \\ 0 & 1/m_2 \end{bmatrix}, \quad \mathbf{J}_c = \begin{bmatrix} \alpha & \beta \end{bmatrix}. \]

Then

\[ \mathbf{J}_c \mathbf{M}^{-1}\mathbf{J}_c^\top = \alpha^2/m_1 + \beta^2/m_2. \]

Hence

\[ \mathbf{J}_c^{\#\mathbf{M}} = \mathbf{M}^{-1}\mathbf{J}_c^\top \left(\mathbf{J}_c \mathbf{M}^{-1}\mathbf{J}_c^\top\right)^{-1} = \frac{1}{\alpha^2/m_1 + \beta^2/m_2} \begin{bmatrix} \alpha/m_1 \\ \beta/m_2 \end{bmatrix}. \]

The projector is

\[ \mathbf{N}_c = \mathbf{I}_2 - \mathbf{J}_c^{\#\mathbf{M}}\mathbf{J}_c = \mathbf{I}_2 - \frac{1}{\alpha^2/m_1 + \beta^2/m_2} \begin{bmatrix} \alpha/m_1 \\ \beta/m_2 \end{bmatrix} \begin{bmatrix} \alpha & \beta \end{bmatrix}. \]

Any vector \( \ddot{\mathbf{q}}_{\mathrm{des}} \) is mapped by \( \mathbf{N}_c \) into an acceleration orthogonal (in the metric \( \mathbf{M} \)) to the contact constraint direction.

Problem 4 (Equivalence Between Projection and Constrained QP). Consider the constrained problem with \( \mathbf{W} = \mathbf{M} \). Show that the solution of the constrained QP can be obtained by first projecting onto the null-space of \( \mathbf{J}_c \) and then adding a particular solution that enforces the inhomogeneous term \( \dot{\mathbf{J}}_c \dot{\mathbf{q}} \).

Solution. Any solution of the constraint \( \mathbf{J}_c \ddot{\mathbf{q}} + \dot{\mathbf{J}}_c \dot{\mathbf{q}} = \mathbf{0} \) can be written as

\[ \ddot{\mathbf{q}} = \ddot{\mathbf{q}}_p + \mathbf{N}_c \mathbf{z}, \]

where \( \ddot{\mathbf{q}}_p \) is any particular solution (e.g. \( -\mathbf{J}_c^{\#\mathbf{M}} \dot{\mathbf{J}}_c \dot{\mathbf{q}} \)) and \( \mathbf{z} \) is arbitrary. Inserting in the cost functional, the term involving \( \ddot{\mathbf{q}}_p \) is fixed, and the optimal \( \mathbf{z} \) is obtained by minimizing

\[ \tfrac{1}{2} \left(\mathbf{N}_c \mathbf{z} + \ddot{\mathbf{q}}_p - \ddot{\mathbf{q}}_{\mathrm{des}}\right)^\top \mathbf{M} \left(\mathbf{N}_c \mathbf{z} + \ddot{\mathbf{q}}_p - \ddot{\mathbf{q}}_{\mathrm{des}}\right). \]

Since \( \mathbf{N}_c \) is an \( \mathbf{M} \)-orthogonal projector, the minimum is reached at \( \mathbf{z}^{\star} = \ddot{\mathbf{q}}_{\mathrm{des}} - \ddot{\mathbf{q}}_p \), leading exactly to \( \ddot{\mathbf{q}}^{\star} = \mathbf{N}_c \ddot{\mathbf{q}}_{\mathrm{des}} + (\mathbf{I}-\mathbf{N}_c)\ddot{\mathbf{q}}_p \). Choosing \( \ddot{\mathbf{q}}_p = -\mathbf{J}_c^{\#\mathbf{M}} \dot{\mathbf{J}}_c \dot{\mathbf{q}} \) yields the formula of Section 3.

Problem 5 (Contact Mode Transitions Conceptual Flow). Sketch how a controller should update \( \mathbf{J}_c \) and the projector when a new contact is added or removed (e.g., a foot hits or leaves the ground).

Solution (conceptual flow). The following diagram outlines the high-level logic:

flowchart TD
  S["Start of control cycle"] --> D["Detect contact set from sensors/estimator"]
  D --> U["Has contact set changed?"]
  U -->|"yes"| R["Rebuild Jc, Jc_dot, recompute projector Nc"]
  U -->|"no"| K["Keep current Jc and Nc"]
  R --> C["Update whole-body QP and constraint tasks"]
  K --> C
  C --> N["Compute qdd_star, tau, lambda for new cycle"]
        

Mathematically, a change in contact set changes the dimension and structure of \( \mathbf{J}_c \) and thus of \( \mathbf{N}_c \). The controller must update these at each mode transition to remain contact-consistent.

This lesson introduced contact-consistent control as a way to ensure that whole-body motion commands respect rigid contact constraints. We formulated holonomic contacts at acceleration level, constructed dynamically consistent pseudo-inverses and projectors, and proved that the resulting projector maps arbitrary accelerations into the contact-consistent subspace. We then showed how to integrate this construction into whole-body control pipelines and how to implement it with standard numerical libraries in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. In subsequent lessons, this projector will be combined with task hierarchies and optimal controllers to trade off performance and constraint satisfaction in multi-contact scenarios.