Complex robots such as humanoids, legged and dual-arm systems often have redundant DOFs: the number of actuated joints \( n \) exceeds the dimension of any single task. Typical concurrent objectives include:

• Maintaining balance (center-of-mass or support polygon constraints).
• Tracking hand or foot trajectories for manipulation or locomotion.
• Regulating posture and joint configurations (e.g., keeping elbows close to the body).
• Respecting joint limits, self-collision avoidance, and contact constraints.

Whole-body control organizes these objectives as tasks with possibly different dimensions and priorities. A canonical information flow is:

flowchart TD
  T0["High-level goals \n(walk, reach, balance)"]
  T1["Task 1: e.g. \ncenter-of-mass regulation"]
  T2["Task 2: e.g. \nhand trajectory"]
  T3["Task 3: posture / joint \nregularization"]
  M["Kinematic and \ndynamic model \n(q, J_i, constraints)"]
  S["Whole-body solver \n(hierarchical)"]
  CMD["Low-level joint commands \n(q_dot or tau)"]

  T0 --> T1
  T0 --> T2
  T0 --> T3
  T1 --> S
  T2 --> S
  T3 --> S
  M --> S
  S --> CMD
        

Our goal in this lesson is to define the mathematical structure of this solver at the kinematic (velocity) and torque levels:

• How to represent tasks as functions of joint configuration and velocity.
• How to combine tasks using null-space projections.
• How to preserve high-priority tasks while adding lower-priority ones.

Consider an \(n\)-DOF robot with joint configuration \( \mathbf{q} \in \mathbb{R}^n \) and joint velocity \( \dot{\mathbf{q}} \in \mathbb{R}^n \). Previous chapters have already introduced its kinematics and dynamics, so we assume:

• A known forward kinematics map for each task \(i\): \( \mathbf{x}_i = f_i(\mathbf{q}) \).
• Corresponding task Jacobians \( \mathbf{J}_i(\mathbf{q}) \).
• Optionally, constraint Jacobians \( \mathbf{J}_c(\mathbf{q}) \) encoding holonomic or contact constraints.

A task \(i\) of dimension \(m_i\) is specified by:

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

A desired task velocity command is denoted \( \dot{\mathbf{x}}_i^\star \). The instantaneous (kinematic) task control problem for a single task is:

\[ \text{Find } \dot{\mathbf{q}} \in \mathbb{R}^n \text{ such that } \mathbf{J}_i(\mathbf{q})\,\dot{\mathbf{q}} = \dot{\mathbf{x}}_i^\star, \]

which we already encountered in task-space control. Whole-body control generalizes this to multiple tasks:

\[ \text{Tasks } i = 1,\dots,N, \quad \mathbf{J}_i(\mathbf{q})\,\dot{\mathbf{q}} \approx \dot{\mathbf{x}}_i^\star \quad \text{with a priority ordering.} \]

If we stack the first \(k\) tasks row-wise:

\[ \mathbf{J}^{(k)}(\mathbf{q}) = \begin{bmatrix} \mathbf{J}_1(\mathbf{q}) \\ \vdots \\ \mathbf{J}_k(\mathbf{q}) \end{bmatrix} \in \mathbb{R}^{m^{(k)} \times n}, \quad m^{(k)} = \sum_{i=1}^k m_i, \]

then the instantaneous feasibility condition (ignoring inequalities and nonholonomic constraints) is roughly:

\[ \operatorname{rank}\!\big(\mathbf{J}^{(k)}(\mathbf{q})\big) \le n, \quad \text{and typically } m^{(k)} \le n. \]

The null-space dimension of stacked tasks is \( n - \operatorname{rank}(\mathbf{J}^{(k)}) \) and determines how much freedom remains to accommodate further tasks or optimization criteria.

For a single task (dropping subscript \(i\) for clarity), the least-squares solution minimizing task error \( \|\mathbf{J}\dot{\mathbf{q}} - \dot{\mathbf{x}}^\star\|_2 \) is given by the Moore–Penrose pseudoinverse:

\[ \dot{\mathbf{q}}^\star = \mathbf{J}^\#(\mathbf{q}) \,\dot{\mathbf{x}}^\star + \big(\mathbf{I}_n - \mathbf{J}^\#(\mathbf{q}) \mathbf{J}(\mathbf{q})\big)\,\mathbf{z}, \]

where \( \mathbf{J}^\# \) is the pseudoinverse and \( \mathbf{z} \in \mathbb{R}^n \) is an arbitrary vector. The matrix

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

is the null-space projector of \( \mathbf{J} \) (in the least-squares sense). When we set \( \mathbf{z} = \mathbf{0} \), we obtain the minimum-norm solution. When \( n > m \), we can choose \( \mathbf{z} \) to realize secondary objectives without affecting the primary task.

A numerically robust variant is the damped pseudoinverse with regularization parameter \( \lambda > 0 \):

\[ \mathbf{J}^\#_\lambda = \begin{cases} \mathbf{J}^\top\big(\mathbf{J}\mathbf{J}^\top + \lambda^2\mathbf{I}_{m}\big)^{-1}, & m \le n, \\[4pt] \big(\mathbf{J}^\top\mathbf{J} + \lambda^2\mathbf{I}_{n}\big)^{-1}\mathbf{J}^\top, & m \ge n, \end{cases} \]

which avoids numerical issues near singular configurations.

Let us now consider two tasks with different priorities:

• Task 1 (highest priority): \( \mathbf{J}_1 \dot{\mathbf{q}} = \dot{\mathbf{x}}_1^\star \).
• Task 2 (lower priority): \( \mathbf{J}_2 \dot{\mathbf{q}} = \dot{\mathbf{x}}_2^\star \).

First, compute a solution for Task 1 alone:

\[ \dot{\mathbf{q}}_1 = \mathbf{J}_1^\# \dot{\mathbf{x}}_1^\star. \]

Define the null-space projector of Task 1:

\[ \mathbf{N}_1 = \mathbf{I}_n - \mathbf{J}_1^\# \mathbf{J}_1. \]

Any velocity of the form \( \dot{\mathbf{q}} = \dot{\mathbf{q}}_1 + \mathbf{N}_1 \mathbf{w} \), with arbitrary \(\mathbf{w}\), keeps Task 1 perfectly satisfied (under rank conditions). We choose \(\mathbf{w}\) to realize Task 2 within the null space of Task 1:

\[ \dot{\mathbf{q}}_2 = \dot{\mathbf{q}}_1 + \mathbf{N}_1\,\mathbf{J}_2^\#\big(\dot{\mathbf{x}}_2^\star - \mathbf{J}_2 \dot{\mathbf{q}}_1\big). \]

This is the classical two-task hierarchical control law. Let us verify that Task 1 is not disturbed:

\[ \begin{aligned} \mathbf{J}_1 \dot{\mathbf{q}}_2 &= \mathbf{J}_1 \dot{\mathbf{q}}_1 + \mathbf{J}_1 \mathbf{N}_1\,\mathbf{J}_2^\# \big(\dot{\mathbf{x}}_2^\star - \mathbf{J}_2 \dot{\mathbf{q}}_1\big) \\ &= \mathbf{J}_1 \dot{\mathbf{q}}_1 + \big(\mathbf{J}_1 - \mathbf{J}_1 \mathbf{J}_1^\# \mathbf{J}_1\big)\, \mathbf{J}_2^\# \big(\dot{\mathbf{x}}_2^\star - \mathbf{J}_2 \dot{\mathbf{q}}_1\big) \\ &= \mathbf{J}_1 \dot{\mathbf{q}}_1, \end{aligned} \]

using the pseudoinverse property \( \mathbf{J}_1 \mathbf{J}_1^\# \mathbf{J}_1 = \mathbf{J}_1 \). Hence, the additional term lies in the null space of \( \mathbf{J}_1 \) and does not affect Task 1.

Geometrically, we project the Task 2 velocity command into the subspace of joint velocities that preserve Task 1. This idea extends to an arbitrary number of tasks.

For \(N\) tasks with priorities \(1 \) (highest) to \( N \) (lowest), a common recursive scheme is:

\[ \dot{\mathbf{q}}_1 = \mathbf{J}_1^\# \dot{\mathbf{x}}_1^\star, \quad \mathbf{N}_1 = \mathbf{I}_n - \mathbf{J}_1^\# \mathbf{J}_1, \]

\[ \dot{\mathbf{q}}_k = \dot{\mathbf{q}}_{k-1} + \mathbf{N}_{k-1}\,\mathbf{J}_k^\# \big(\dot{\mathbf{x}}_k^\star - \mathbf{J}_k \dot{\mathbf{q}}_{k-1}\big), \quad k = 2,\dots,N, \]

\[ \mathbf{N}_k = \mathbf{N}_{k-1} - \mathbf{N}_{k-1}\,\mathbf{J}_k^\# \mathbf{J}_k \mathbf{N}_{k-1}. \]

Proposition (Task Consistency): Assuming each task is locally feasible and the pseudoinverses are well-defined, the above recursion ensures that for any \(k\), \( \mathbf{J}_i \dot{\mathbf{q}}_k = \dot{\mathbf{x}}_i^\star \) for all \( i \le k \).

Sketch of Proof (Induction over \(k\)):

• For \( k = 1 \), \( \mathbf{J}_1 \dot{\mathbf{q}}_1 = \mathbf{J}_1 \mathbf{J}_1^\# \dot{\mathbf{x}}_1^\star = \dot{\mathbf{x}}_1^\star \) by the pseudoinverse property.
• Assume the statement holds for \(k-1\). For task \(i \le k-1\):

  \[ \mathbf{J}_i \dot{\mathbf{q}}_k = \mathbf{J}_i \dot{\mathbf{q}}_{k-1} + \mathbf{J}_i \mathbf{N}_{k-1}\,\mathbf{J}_k^\# \big(\dot{\mathbf{x}}_k^\star - \mathbf{J}_k \dot{\mathbf{q}}_{k-1}\big). \]

  By construction of \( \mathbf{N}_{k-1} \), we have \( \mathbf{J}_i \mathbf{N}_{k-1} = \mathbf{0} \) for all \( i \le k-1 \), hence the second term vanishes and \( \mathbf{J}_i \dot{\mathbf{q}}_k = \mathbf{J}_i \dot{\mathbf{q}}_{k-1} = \dot{\mathbf{x}}_i^\star \).
• For task \(k\) itself:

  \[ \mathbf{J}_k \dot{\mathbf{q}}_k = \mathbf{J}_k \dot{\mathbf{q}}_{k-1} + \mathbf{J}_k \mathbf{N}_{k-1}\,\mathbf{J}_k^\# \big(\dot{\mathbf{x}}_k^\star - \mathbf{J}_k \dot{\mathbf{q}}_{k-1}\big). \]

  Using \( \mathbf{J}_k \mathbf{N}_{k-1} \mathbf{J}_k^\# \approx \mathbf{J}_k \mathbf{J}_k^\# \approx \mathbf{I}_{m_k} \) (under rank conditions), we obtain \( \mathbf{J}_k \dot{\mathbf{q}}_k = \dot{\mathbf{x}}_k^\star \).

The recursion therefore implements a strict priority hierarchy. Practical implementations often approximate this ideal behavior due to regularization, discretization, and inequality constraints, which will be revisited in later lessons.

flowchart TD
  S["Start with highest-priority task 1"] --> C1["Compute q_dot1 = J1_pinv * xdot1_star"]
  C1 --> N1["Compute N1 = I - J1_pinv * J1"]
  N1 --> K2["For each task k = 2..N"]
  K2 --> SK["Project residual xdot_k_star - Jk * q_dot_(k-1)"]
  SK --> QK["Add correction q_dot_k = q_dot_(k-1) + Nk-1 * Jk_pinv * residual"]
  QK --> UP["Update Nk = Nk-1 - Nk-1 * Jk_pinv * Jk * Nk-1"]
  UP --> K2
        

Previous chapters introduced robot dynamics and operational-space control. We recall the (floating-base or fixed-base) equations of motion in compact form:

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

where \( \mathbf{M} \) is the mass matrix, \( \mathbf{h} \) collects Coriolis, centrifugal and gravity terms, \( \mathbf{S} \) selects actuated joints, and \( \mathbf{J}_c^\top \mathbf{f}_c \) accounts for contact forces.

For a single operational-space task with Jacobian \( \mathbf{J} \), operational-space inertia \( \boldsymbol{\Lambda} \) and task force \( \mathbf{F} \), we may write (schematically)

\[ \boldsymbol{\tau}_\text{task} = \mathbf{J}^\top \mathbf{F}, \quad \mathbf{F} = \boldsymbol{\Lambda}\big(\ddot{\mathbf{x}}^\star + \mathbf{K}_d (\dot{\mathbf{x}}^\star - \dot{\mathbf{x}}) + \mathbf{K}_p (\mathbf{x}^\star - \mathbf{x})\big), \]

using gains \( \mathbf{K}_p, \mathbf{K}_d \) chosen as in previous control chapters. In whole-body control, we generalize this to a hierarchy:

\[ \boldsymbol{\tau} = \boldsymbol{\tau}_1 + \mathbf{N}_1^\top \boldsymbol{\tau}_2 + \mathbf{N}_2^\top \boldsymbol{\tau}_3 + \dots, \]

where \( \boldsymbol{\tau}_k \) is the torque contribution for Task \(k\) and \( \mathbf{N}_k \) are dynamically consistent null-space projectors based on \( \mathbf{M}(\mathbf{q}) \) (rather than the identity metric). This preserves the accelerations of higher-priority tasks when adding lower-priority ones. The detailed construction of dynamically consistent null spaces was already introduced in the operational-space control chapter and will be revisited in later lessons when we compare projection-based and optimization-based schemes.

Near singular configurations or when tasks are nearly conflicting, the stacked Jacobians may become ill-conditioned. Damping and weighting are standard techniques.

For a weighted metric with positive-definite weight \( \mathbf{W} \in \mathbb{R}^{n \times n} \), the weighted pseudoinverse can be defined as:

\[ \mathbf{J}^{\#}_W = \mathbf{W}^{-1}\mathbf{J}^\top \big(\mathbf{J}\mathbf{W}^{-1}\mathbf{J}^\top\big)^{-1}. \]

Choosing \( \mathbf{W} \) diagonal with larger entries for joints we wish to penalize (e.g., close to joint limits) biases the solution away from those joints. Damped weighted pseudoinverses combine this with a regularization \( \lambda \) in the denominator.

These numerical refinements are essential in whole-body control, where many tasks compete over limited actuation, and will remain important when we move to quadratic programming formulations in the next lesson.

Below is a minimal Python implementation of a two-task hierarchical controller at the velocity level. It assumes the availability of task Jacobians and desired task velocities from a model (for example, via pinocchio or another rigid-body library).

import numpy as np

def damped_pinv(J: np.ndarray, lam: float = 1e-4) -> np.ndarray:
    """
    Compute damped Moore-Penrose pseudoinverse of J.
    """
    m, n = J.shape
    if m >= n:
        # column-rank scenario
        return np.linalg.inv(J.T @ J + (lam ** 2) * np.eye(n)) @ J.T
    else:
        # row-rank scenario
        return J.T @ np.linalg.inv(J @ J.T + (lam ** 2) * np.eye(m))

def nullspace_projector(J: np.ndarray, lam: float = 1e-4) -> np.ndarray:
    """
    Compute null-space projector N = I - J^# J using damped pseudoinverse.
    """
    n = J.shape[1]
    J_pinv = damped_pinv(J, lam)
    I = np.eye(n)
    return I - J_pinv @ J

def two_task_velocity_control(J1: np.ndarray,
                              xdot1_star: np.ndarray,
                              J2: np.ndarray,
                              xdot2_star: np.ndarray,
                              lam: float = 1e-4) -> np.ndarray:
    """
    Compute joint velocities for two hierarchical tasks:
    Task 1 has higher priority than Task 2.
    """
    # Task 1 solution
    J1_pinv = damped_pinv(J1, lam)
    qdot1 = J1_pinv @ xdot1_star

    # Null-space projector of Task 1
    N1 = nullspace_projector(J1, lam)

    # Residual for Task 2, respecting Task 1
    residual2 = xdot2_star - J2 @ qdot1
    J2_pinv = damped_pinv(J2 @ N1, lam)  # effective Jacobian in null space
    qdot2_correction = N1 @ J2_pinv @ residual2

    qdot = qdot1 + qdot2_correction
    return qdot

# Example usage with dummy Jacobians
if __name__ == "__main__":
    n_dof = 7
    m1 = 3  # task 1: 3D position
    m2 = 3  # task 2: 3D orientation

    # Random but well-conditioned Jacobians (for illustration)
    rng = np.random.default_rng(0)
    J1 = rng.normal(size=(m1, n_dof))
    J2 = rng.normal(size=(m2, n_dof))

    xdot1_star = np.array([0.1, 0.0, -0.05])
    xdot2_star = np.array([0.0, 0.05, 0.0])

    qdot = two_task_velocity_control(J1, xdot1_star, J2, xdot2_star, lam=1e-3)
    print("Joint velocity command:", qdot)
      

In a realistic implementation, J1 and J2 would be obtained from a robotics library (e.g., pinocchio.computeFrameJacobian for Cartesian frames, plus a posture task Jacobian equal to the identity matrix on selected joints).

The following C++ code uses Eigen to implement a damped pseudoinverse and a two-task hierarchical controller at the velocity level. In practice, this would be integrated with a rigid-body dynamics library such as pinocchio or RBDL.

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

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

MatrixXd dampedPseudoInverse(const MatrixXd& J, double lambda)
{
    int m = J.rows();
    int n = J.cols();
    if (m >= n) {
        MatrixXd A = J.transpose() * J +
                     (lambda * lambda) * MatrixXd::Identity(n, n);
        return A.ldlt().solve(J.transpose());
    } else {
        MatrixXd A = J * J.transpose() +
                     (lambda * lambda) * MatrixXd::Identity(m, m);
        return J.transpose() * A.ldlt().solve(MatrixXd::Identity(m, m));
    }
}

MatrixXd nullspaceProjector(const MatrixXd& J, double lambda)
{
    int n = J.cols();
    MatrixXd J_pinv = dampedPseudoInverse(J, lambda);
    MatrixXd I = MatrixXd::Identity(n, n);
    return I - J_pinv * J;
}

VectorXd twoTaskVelocityControl(const MatrixXd& J1,
                                const VectorXd& xdot1_star,
                                const MatrixXd& J2,
                                const VectorXd& xdot2_star,
                                double lambda)
{
    // Task 1
    MatrixXd J1_pinv = dampedPseudoInverse(J1, lambda);
    VectorXd qdot1 = J1_pinv * xdot1_star;

    // Null space of Task 1
    MatrixXd N1 = nullspaceProjector(J1, lambda);

    // Effective Task 2 in null space
    MatrixXd J2_eff = J2 * N1;
    MatrixXd J2_eff_pinv = dampedPseudoInverse(J2_eff, lambda);

    VectorXd residual2 = xdot2_star - J2 * qdot1;
    VectorXd qdot2_corr = N1 * (J2_eff_pinv * residual2);

    VectorXd qdot = qdot1 + qdot2_corr;
    return qdot;
}

int main()
{
    int n_dof = 7;
    int m1 = 3;
    int m2 = 3;

    MatrixXd J1 = MatrixXd::Random(m1, n_dof);
    MatrixXd J2 = MatrixXd::Random(m2, n_dof);

    VectorXd xdot1_star(m1);
    xdot1_star << 0.1, 0.0, -0.05;

    VectorXd xdot2_star(m2);
    xdot2_star << 0.0, 0.05, 0.0;

    double lambda = 1e-3;
    VectorXd qdot = twoTaskVelocityControl(J1, xdot1_star, J2, xdot2_star, lambda);

    std::cout << "qdot = " << qdot.transpose() << std::endl;
    return 0;
}
      

This structure can be extended to more tasks by iterating the same pattern and updating the null-space projector at each step as described in the mathematical section.

In Java, the EJML library provides dense linear algebra operations that can be used to implement whole-body controllers. Below is a minimal example of two-task hierarchical control using SimpleMatrix.

import org.ejml.simple.SimpleMatrix;

public class HierarchicalWBC {

    public static SimpleMatrix dampedPseudoInverse(SimpleMatrix J, double lambda) {
        int m = J.numRows();
        int n = J.numCols();
        double lam2 = lambda * lambda;

        if (m >= n) {
            SimpleMatrix A = J.transpose().mult(J)
                .plus(SimpleMatrix.identity(n).scale(lam2));
            return A.pseudoInverse().mult(J.transpose());
        } else {
            SimpleMatrix A = J.mult(J.transpose())
                .plus(SimpleMatrix.identity(m).scale(lam2));
            return J.transpose().mult(A.pseudoInverse());
        }
    }

    public static SimpleMatrix nullspaceProjector(SimpleMatrix J, double lambda) {
        int n = J.numCols();
        SimpleMatrix Jpinv = dampedPseudoInverse(J, lambda);
        SimpleMatrix I = SimpleMatrix.identity(n);
        return I.minus(Jpinv.mult(J));
    }

    public static SimpleMatrix twoTaskVelocityControl(SimpleMatrix J1,
                                                      SimpleMatrix xdot1_star,
                                                      SimpleMatrix J2,
                                                      SimpleMatrix xdot2_star,
                                                      double lambda) {
        // Task 1
        SimpleMatrix J1pinv = dampedPseudoInverse(J1, lambda);
        SimpleMatrix qdot1 = J1pinv.mult(xdot1_star);

        // Null space of Task 1
        SimpleMatrix N1 = nullspaceProjector(J1, lambda);

        // Effective Task 2 in null space
        SimpleMatrix J2_eff = J2.mult(N1);
        SimpleMatrix J2_eff_pinv = dampedPseudoInverse(J2_eff, lambda);

        SimpleMatrix residual2 = xdot2_star.minus(J2.mult(qdot1));
        SimpleMatrix qdot2_corr = N1.mult(J2_eff_pinv.mult(residual2));

        return qdot1.plus(qdot2_corr);
    }

    public static void main(String[] args) {
        int n_dof = 7;
        int m1 = 3;
        int m2 = 3;

        SimpleMatrix J1 = SimpleMatrix.random_DDRM(m1, n_dof, -1.0, 1.0, null);
        SimpleMatrix J2 = SimpleMatrix.random_DDRM(m2, n_dof, -1.0, 1.0, null);

        SimpleMatrix xdot1_star = new SimpleMatrix(m1, 1, true, new double[]{0.1, 0.0, -0.05});
        SimpleMatrix xdot2_star = new SimpleMatrix(m2, 1, true, new double[]{0.0, 0.05, 0.0});

        double lambda = 1e-3;
        SimpleMatrix qdot = twoTaskVelocityControl(J1, xdot1_star, J2, xdot2_star, lambda);
        qdot.print();
    }
}
      

This code can be embedded into a real-time Java control loop where J1, J2 and the desired task velocities are updated at each control cycle.

In MATLAB, we can exploit both the core linear algebra routines and the Robotics System Toolbox. At the velocity level, a simple two-task whole-body controller can be implemented as:

function qdot = twoTaskWBC(q, J1, xdot1_star, J2, xdot2_star, lambda)
%TWOTASKWBC Hierarchical two-task velocity control.
%
% q           : current joint configuration (unused here, but typically used
%               to compute J1, J2 from a rigidBodyTree model)
% J1          : Jacobian of high-priority task (m1 x n)
% xdot1_star  : desired task-1 velocity (m1 x 1)
% J2          : Jacobian of second-priority task (m2 x n)
% xdot2_star  : desired task-2 velocity (m2 x 1)
% lambda      : damping coefficient

if nargin < 6
    lambda = 1e-3;
end

J1_pinv = damped_pinv(J1, lambda);
qdot1 = J1_pinv * xdot1_star;

N1 = eye(size(J1, 2)) - J1_pinv * J1;

J2_eff = J2 * N1;
J2_eff_pinv = damped_pinv(J2_eff, lambda);

residual2 = xdot2_star - J2 * qdot1;
qdot2_corr = N1 * (J2_eff_pinv * residual2);

qdot = qdot1 + qdot2_corr;
end

function J_pinv = damped_pinv(J, lambda)
[m, n] = size(J);
if m >= n
    A = J' * J + (lambda^2) * eye(n);
    J_pinv = A \ J';
else
    A = J * J' + (lambda^2) * eye(m);
    J_pinv = J' / A;
end
end
      

In a Simulink environment, this logic can be encapsulated in a MATLAB Function block. The Jacobians J1 and J2 can be computed in a preceding block using a rigidBodyTree model and the block geometricJacobian, and the output qdot can be integrated or fed into a lower-level torque controller implemented in a separate subsystem.

Mathematica provides high-level linear algebra primitives that make hierarchical control concise. The following code implements a two-task velocity controller:

dampedPseudoInverse[J_, lambda_] := Module[{m, n, lam2, A},
  {m, n} = Dimensions[J];
  lam2 = lambda^2;
  If[m >= n,
    A = Transpose[J].J + lam2 IdentityMatrix[n];
    LinearSolve[A, Transpose[J]],
    A = J.Transpose[J] + lam2 IdentityMatrix[m];
    Transpose[J].LinearSolve[A, IdentityMatrix[m]]
  ]
];

nullspaceProjector[J_, lambda_] := Module[{n, Jpinv},
  n = Dimensions[J][[2]];
  Jpinv = dampedPseudoInverse[J, lambda];
  IdentityMatrix[n] - Jpinv.J
];

twoTaskVelocityControl[J1_, xdot1star_, J2_, xdot2star_, lambda_] :=
 Module[{J1pinv, qdot1, N1, J2eff, J2effpinv, residual2, qdot2corr},
  J1pinv = dampedPseudoInverse[J1, lambda];
  qdot1 = J1pinv.xdot1star;
  N1 = nullspaceProjector[J1, lambda];

  J2eff = J2.N1;
  J2effpinv = dampedPseudoInverse[J2eff, lambda];

  residual2 = xdot2star - J2.qdot1;
  qdot2corr = N1.(J2effpinv.residual2);
  qdot1 + qdot2corr
];

(* Example usage with random matrices *)
SeedRandom[0];
nDof = 7; m1 = 3; m2 = 3;
J1 = RandomReal[{-1, 1}, {m1, nDof}];
J2 = RandomReal[{-1, 1}, {m2, nDof}];

xdot1star = {0.1, 0.0, -0.05};
xdot2star = {0.0, 0.05, 0.0};

lambda = 10.^-3;
qdot = twoTaskVelocityControl[J1, xdot1star, J2, xdot2star, lambda]
      

The same pattern can be extended to multiple tasks by looping over the task list and updating the null-space projector at each step.

Problem 1 (Null-Space Dimension for Whole-Body Tasks). Consider a 7-DOF manipulator with tasks: (i) a 3D Cartesian position task for the hand, (ii) a 3D orientation task for the hand, (iii) a 1D scalar posture task (e.g., elbow angle regularization). Assume that each task Jacobian has full row rank at the current configuration. Compute the null-space dimension after each task is imposed hierarchically.

Solution. Let \( n = 7 \). For the first task, \( m_1 = 3 \), so under full row rank \( \operatorname{rank}(\mathbf{J}_1) = 3 \). The null-space dimension is \( n - \operatorname{rank}(\mathbf{J}_1) = 7 - 3 = 4 \).

For the second task, \( m_2 = 3 \) and the effective Jacobian \( \mathbf{J}_2 \mathbf{N}_1 \) has rank at most \(3\). Under generic conditions and sufficient redundancy, we expect \( \operatorname{rank}(\mathbf{J}_2 \mathbf{N}_1) = 3 \). Then the null-space dimension after the second task is \( 4 - 3 = 1 \).

For the third task, \( m_3 = 1 \). Assuming it is independent from the previous ones within the remaining null space, the rank increase is 1, and the final null-space dimension becomes \( 1 - 1 = 0 \). Thus the 7 DOFs are fully utilized by the three tasks.

Problem 2 (Projector Properties). Let \( \mathbf{J} \in \mathbb{R}^{m \times n} \) with pseudoinverse \( \mathbf{J}^\# \) and define \( \mathbf{N} = \mathbf{I}_n - \mathbf{J}^\# \mathbf{J} \). Show that: (a) \( \mathbf{N}^2 = \mathbf{N} \) (idempotence), and (b) \( \mathbf{J}\mathbf{N} = \mathbf{0} \).

Solution.

(a) Compute \( \mathbf{N}^2 \):

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

Using the pseudoinverse property \( \mathbf{J}^\# \mathbf{J} \mathbf{J}^\# = \mathbf{J}^\# \), we get \( \mathbf{J}^\# \mathbf{J} \mathbf{J}^\# \mathbf{J} = \mathbf{J}^\# \mathbf{J} \). Thus

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

(b) Compute \( \mathbf{J}\mathbf{N} \):

\[ \mathbf{J}\mathbf{N} = \mathbf{J}(\mathbf{I}_n - \mathbf{J}^\# \mathbf{J}) = \mathbf{J} - \mathbf{J}\mathbf{J}^\# \mathbf{J} = \mathbf{J} - \mathbf{J} = \mathbf{0}, \]

by the pseudoinverse property \( \mathbf{J}\mathbf{J}^\# \mathbf{J} = \mathbf{J} \). Hence \( \mathbf{N} \) is an idempotent projector onto the (generalized) null space of \( \mathbf{J} \).

Problem 3 (Preservation of High-Priority Task). Consider the two-task control law \( \dot{\mathbf{q}}_2 = \dot{\mathbf{q}}_1 + \mathbf{N}_1 \mathbf{w} \), where \( \dot{\mathbf{q}}_1 = \mathbf{J}_1^\# \dot{\mathbf{x}}_1^\star \) and \( \mathbf{N}_1 = \mathbf{I}_n - \mathbf{J}_1^\# \mathbf{J}_1 \). Show that \( \mathbf{J}_1 \dot{\mathbf{q}}_2 = \dot{\mathbf{x}}_1^\star \) for any choice of \( \mathbf{w} \).

Solution. We compute

\[ \mathbf{J}_1 \dot{\mathbf{q}}_2 = \mathbf{J}_1 \dot{\mathbf{q}}_1 + \mathbf{J}_1 \mathbf{N}_1 \mathbf{w} = \mathbf{J}_1 \mathbf{J}_1^\# \dot{\mathbf{x}}_1^\star + \mathbf{J}_1 (\mathbf{I}_n - \mathbf{J}_1^\# \mathbf{J}_1) \mathbf{w}. \]

The first term equals \( \dot{\mathbf{x}}_1^\star \) because \( \mathbf{J}_1 \mathbf{J}_1^\# \) is the projector onto the row space of \( \mathbf{J}_1 \). The second term simplifies to \( \mathbf{J}_1 \mathbf{w} - \mathbf{J}_1 \mathbf{J}_1^\# \mathbf{J}_1 \mathbf{w} = \mathbf{J}_1 \mathbf{w} - \mathbf{J}_1 \mathbf{w} = \mathbf{0} \), again by the pseudoinverse property. Hence the primary task is exactly preserved.

Problem 4 (Task Conflict Condition). Let \( \mathbf{J}_1 \in \mathbb{R}^{m_1 \times n} \) and \( \mathbf{J}_2 \in \mathbb{R}^{m_2 \times n} \) be the Jacobians of two tasks with desired velocities \( \dot{\mathbf{x}}_1^\star \) and \( \dot{\mathbf{x}}_2^\star \). Using the two-task law with null-space projection, discuss a condition under which Task 2 can be exactly realized without disturbing Task 1.

Solution. In the two-task scheme, Task 2 is realized by choosing a correction \( \dot{\mathbf{q}}_{2,\text{corr}} = \mathbf{N}_1 \mathbf{w} \) such that \( \mathbf{J}_2 \dot{\mathbf{q}}_2 = \dot{\mathbf{x}}_2^\star \). This requires solving

\[ \mathbf{J}_2 \mathbf{N}_1 \mathbf{w} = \dot{\mathbf{x}}_2^\star - \mathbf{J}_2 \dot{\mathbf{q}}_1. \]

A necessary and sufficient condition for the existence of an exact solution is that the right-hand side lies in the range of \( \mathbf{J}_2 \mathbf{N}_1 \), i.e.,

\[ \dot{\mathbf{x}}_2^\star - \mathbf{J}_2 \dot{\mathbf{q}}_1 \in \mathcal{R}(\mathbf{J}_2 \mathbf{N}_1), \]

where \( \mathcal{R}(\cdot) \) denotes the column space. Equivalently, the rank of \( \mathbf{J}_2 \mathbf{N}_1 \) must be high enough to span all directions needed by Task 2 that are not already used to satisfy Task 1. If the rank is too small (for example, because the tasks are fundamentally incompatible or the robot is insufficiently redundant), Task 2 can only be approximately satisfied, and a least-squares solution is used.

Problem 5 (Torque-Level Null-Space Control). Consider a torque command of the form \( \boldsymbol{\tau} = \boldsymbol{\tau}_1 + \mathbf{N}_1^\top \boldsymbol{\tau}_0 \), where \( \boldsymbol{\tau}_1 \) realizes a primary task with Jacobian \( \mathbf{J}_1 \) and \( \mathbf{N}_1 \) is a dynamically consistent null-space projector. Explain why \( \boldsymbol{\tau}_0 \) does not affect the primary task acceleration.

Solution. In the dynamically consistent operational-space framework, the task acceleration for Task 1 depends on the projection of torques through \( \mathbf{J}_1 \mathbf{M}^{-1} \). By definition of the dynamically consistent null-space projector, we have

\[ \mathbf{J}_1 \mathbf{M}^{-1} \mathbf{N}_1^\top = \mathbf{0}. \]

Therefore,

\[ \mathbf{J}_1 \mathbf{M}^{-1} \boldsymbol{\tau} = \mathbf{J}_1 \mathbf{M}^{-1} \boldsymbol{\tau}_1 + \mathbf{J}_1 \mathbf{M}^{-1} \mathbf{N}_1^\top \boldsymbol{\tau}_0 = \mathbf{J}_1 \mathbf{M}^{-1} \boldsymbol{\tau}_1. \]

Thus the additional torque term \( \mathbf{N}_1^\top \boldsymbol{\tau}_0 \) lies in the null space of the mapping from torque to task acceleration and cannot affect the primary task. It can therefore be used safely for lower-priority objectives such as posture regulation or joint limit avoidance.

In this lesson, we formalized whole-body control as the coordination of many DOFs across multiple tasks with different priorities. Starting from the kinematic relationship \( \dot{\mathbf{x}}_i = \mathbf{J}_i \dot{\mathbf{q}} \), we introduced pseudoinverse solutions and null-space projectors that allow lower-priority tasks to be executed within the remaining motion subspace without disturbing higher-priority tasks. We extended this to recursive multi-task hierarchies and outlined how the same principles apply at the torque level using dynamically consistent null spaces. Implementation examples in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica illustrated how these mathematical constructs map to practical code that can be integrated into whole-body control stacks for complex robots.

The next lesson will build on this foundation by casting task hierarchies into explicit quadratic programming formulations, enabling systematic handling of constraints and inequalities in whole-body control.