Consider an n-DOF manipulator with configuration \( \mathbf{q} \in \mathbb{R}^n \). A task is typically defined as a function of the configuration, \( \mathbf{x}_i = \mathbf{f}_i(\mathbf{q}) \in \mathbb{R}^{m_i} \), for example:

• Task 1: end-effector position and orientation in Cartesian space.
• Task 2: posture optimization (e.g., keep joints near a preferred configuration).
• Task 3: joint limit avoidance.
• Task 4: collision avoidance or distance maximization to obstacles.

For a redundant manipulator (more joints than task coordinates), a single task does not uniquely determine \( \dot{\mathbf{q}} \) or \( \boldsymbol{\tau} \). Task prioritization specifies that:

• High-priority tasks must be satisfied exactly (up to feasibility and numerical error).
• Low-priority tasks may only use motion/torque in the null space of all higher-priority tasks.

Let tasks be ordered \( T_1, T_2, \dots, T_K \) in decreasing priority. A strict hierarchy requires that any modification due to \( T_k \) does not change the output of \( T_j \) for any \( j < k \). At the kinematic level, this is enforced by projecting lower-priority joint velocities into null spaces. At the dynamic level (operational-space control), torques for lower-priority tasks are projected by dynamically consistent projectors.

flowchart TD
  A["Specify tasks x1, x2, ..., xK"] --> B["Assign strict priority order"]
  B --> C["Compute J1 and its pseudoinverse J1_pinv"]
  C --> D["Primary solution dq1 = J1_pinv * dx1"]
  D --> E["Nullspace projector N1 = I - J1_pinv * J1"]
  E --> F["For each lower task i: project into cumulative nullspace"]
  F --> G["Combine dq to form final joint command"]
  G --> H["Convert dq to torque command via low-level joint controller"]
        

For a single task \( \mathbf{x} = \mathbf{f}(\mathbf{q}) \) with Jacobian \( \mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{f}}{\partial \mathbf{q}} \), the kinematic relationship is

\[ \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q}) \, \dot{\mathbf{q}}. \]

Given a desired task-space velocity \( \dot{\mathbf{x}}^\ast \), we want to solve the linear system

\[ \mathbf{J} \dot{\mathbf{q}} = \dot{\mathbf{x}}^\ast. \]

When the robot is redundant (more joints than task coordinates), this system has infinitely many solutions. A standard choice is the minimum-norm solution:

\[ \dot{\mathbf{q}}^\ast = \arg\min_{\dot{\mathbf{q}}} \frac{1}{2} \dot{\mathbf{q}}^{\mathsf{T}} \dot{\mathbf{q}} \quad \text{subject to} \quad \mathbf{J} \dot{\mathbf{q}} = \dot{\mathbf{x}}^\ast . \]

Introducing a Lagrange multiplier \( \boldsymbol{\lambda} \), the Lagrangian is

\[ \mathcal{L}(\dot{\mathbf{q}},\boldsymbol{\lambda}) = \tfrac{1}{2} \dot{\mathbf{q}}^{\mathsf{T}} \dot{\mathbf{q}} + \boldsymbol{\lambda}^{\mathsf{T}} (\mathbf{J}\dot{\mathbf{q}} - \dot{\mathbf{x}}^\ast). \]

Optimality conditions:

\[ \frac{\partial \mathcal{L}}{\partial \dot{\mathbf{q}}} = \dot{\mathbf{q}} + \mathbf{J}^{\mathsf{T}}\boldsymbol{\lambda} = \mathbf{0}, \quad \frac{\partial \mathcal{L}}{\partial \boldsymbol{\lambda}} = \mathbf{J}\dot{\mathbf{q}} - \dot{\mathbf{x}}^\ast = \mathbf{0}. \]

From the first equation, \( \dot{\mathbf{q}} = -\mathbf{J}^{\mathsf{T}}\boldsymbol{\lambda} \). Inserting into the constraint gives

\[ -\mathbf{J}\mathbf{J}^{\mathsf{T}}\boldsymbol{\lambda} = \dot{\mathbf{x}}^\ast \quad \Rightarrow \quad \boldsymbol{\lambda} = -(\mathbf{J}\mathbf{J}^{\mathsf{T}})^{-1}\dot{\mathbf{x}}^\ast. \]

Therefore

\[ \dot{\mathbf{q}}^\ast = \mathbf{J}^{\mathsf{T}}(\mathbf{J}\mathbf{J}^{\mathsf{T}})^{-1} \dot{\mathbf{x}}^\ast \equiv \mathbf{J}^{\#} \dot{\mathbf{x}}^\ast, \]

where \( \mathbf{J}^{\#} \) is the (right) Moore–Penrose pseudoinverse of \( \mathbf{J} \) under the assumption that \( \mathbf{J} \) has full row rank.

The general solution of the constraint is

\[ \dot{\mathbf{q}} = \dot{\mathbf{q}}^\ast + \left( \mathbf{I} - \mathbf{J}^{\#} \mathbf{J} \right) \mathbf{z}, \quad \mathbf{z} \in \mathbb{R}^n. \]

Define the null-space projector \( \mathbf{N} = \mathbf{I} - \mathbf{J}^{\#} \mathbf{J} \). It satisfies \( \mathbf{J}\mathbf{N} = \mathbf{0} \), so any motion \( \dot{\mathbf{q}}_{\mathrm{null}} = \mathbf{N}\mathbf{z} \) is invisible to the task (does not change \( \dot{\mathbf{x}} \)).

Let there be \( K \) tasks \( \mathbf{x}_i = \mathbf{f}_i(\mathbf{q}) \) with Jacobians \( \mathbf{J}_i(\mathbf{q}) \) and desired task velocities \( \dot{\mathbf{x}}_i^\ast \). We assume a strict priority order \( T_1 \succ T_2 \succ \dots \succ T_K \).

3.1 Two-task hierarchy

For two tasks, the task-priority method constructs joint velocities as

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

and then

\[ \dot{\mathbf{q}}_2 = \dot{\mathbf{q}}_1 + \mathbf{N}_1 \mathbf{J}_2^{\#} \left( \dot{\mathbf{x}}_2^\ast - \mathbf{J}_2 \dot{\mathbf{q}}_1 \right), \quad \mathbf{N}_1 = \mathbf{I} - \mathbf{J}_1^{\#}\mathbf{J}_1. \]

Proposition (priority preservation). Under the above construction, assuming exact arithmetic and full-row-rank Jacobians, the higher-priority task is unaffected:

\[ \mathbf{J}_1 \dot{\mathbf{q}}_2 = \dot{\mathbf{x}}_1^\ast. \]

Proof.

\[ \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^{\#} (\dot{\mathbf{x}}_2^\ast - \mathbf{J}_2 \dot{\mathbf{q}}_1) \\ &= \mathbf{J}_1 \dot{\mathbf{q}}_1 + \underbrace{\mathbf{J}_1 \mathbf{N}_1}_{=\mathbf{0}} \mathbf{J}_2^{\#}(\dot{\mathbf{x}}_2^\ast - \mathbf{J}_2 \dot{\mathbf{q}}_1) \\ &= \mathbf{J}_1 \dot{\mathbf{q}}_1 = \mathbf{J}_1 \mathbf{J}_1^{\#} \dot{\mathbf{x}}_1^\ast = \dot{\mathbf{x}}_1^\ast, \end{aligned} \]

where we used \( \mathbf{J}_1 \mathbf{N}_1 = \mathbf{0} \) and \( \mathbf{J}_1 \mathbf{J}_1^{\#} = \mathbf{I}_{m_1} \). Hence the second task operates purely in the null space of the first task.

3.2 Recursive stack of tasks

For more than two tasks, define recursively for \( k \ge 2 \)

\[ \begin{aligned} \dot{\mathbf{q}}_1 &= \mathbf{J}_1^{\#} \dot{\mathbf{x}}_1^\ast, \\ \mathbf{N}_{[1]} &= \mathbf{I} - \mathbf{J}_1^{\#} \mathbf{J}_1, \\ \dot{\mathbf{q}}_k &= \dot{\mathbf{q}}_{k-1} + \mathbf{N}_{[k-1]} \mathbf{J}_k^{\#} \left( \dot{\mathbf{x}}_k^\ast - \mathbf{J}_k \dot{\mathbf{q}}_{k-1} \right), \\ \mathbf{N}_{[k]} &= \mathbf{N}_{[k-1]} - \mathbf{N}_{[k-1]} \mathbf{J}_k^{\#} \mathbf{J}_k \mathbf{N}_{[k-1]} . \end{aligned} \]

Here \( \mathbf{N}_{[k]} \) is the projector onto the cumulative null space of tasks \( 1,\dots,k \). Inductively, one can prove that for any \( j < k \), \( \mathbf{J}_j \dot{\mathbf{q}}_k = \dot{\mathbf{x}}_j^\ast \), so all higher-priority tasks remain unaffected by the inclusion of lower-priority ones (as long as the combined tasks are feasible).

Recall the robot dynamics (students have seen this in robotics dynamics):

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

For a single operational-space task with Jacobian \( \mathbf{J}_1 \), the task dynamics (Khatib's operational-space formulation) are

\[ \boldsymbol{\Lambda}_1(\mathbf{q}) \ddot{\mathbf{x}}_1 + \boldsymbol{\mu}_1(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{p}_1(\mathbf{q}) = \mathbf{F}_1, \]

where

\[ \boldsymbol{\Lambda}_1 = \left( \mathbf{J}_1 \mathbf{M}^{-1} \mathbf{J}_1^{\mathsf{T}} \right)^{-1}, \quad \mathbf{J}_1^{\#_M} = \mathbf{M}^{-1} \mathbf{J}_1^{\mathsf{T}} \boldsymbol{\Lambda}_1 \]

is the dynamically consistent pseudoinverse. The torque realizing a desired task acceleration \( \ddot{\mathbf{x}}_1^\ast \) is often written as

\[ \boldsymbol{\tau}_1 = \mathbf{J}_1^{\mathsf{T}} \mathbf{F}_1 + \mathbf{N}_{1,M}^{\mathsf{T}} \boldsymbol{\tau}_0, \quad \mathbf{N}_{1,M} = \mathbf{I} - \mathbf{J}_1^{\#_M} \mathbf{J}_1, \]

where \( \boldsymbol{\tau}_0 \) is an arbitrary torque in the null space (for posture, joint regularization, etc.).

4.1 Two-task torque hierarchy

For two tasks in operational space, with task-level forces \( \mathbf{F}_1, \mathbf{F}_2 \), a common hierarchical torque decomposition is

\[ \begin{aligned} \boldsymbol{\tau}_1 &= \mathbf{J}_1^{\mathsf{T}} \mathbf{F}_1, \\ \boldsymbol{\tau}_2 &= \mathbf{J}_2^{\mathsf{T}} \mathbf{F}_2, \\ \boldsymbol{\tau} &= \boldsymbol{\tau}_1 + \mathbf{N}_{1,M}^{\mathsf{T}} \boldsymbol{\tau}_2 . \end{aligned} \]

Because \( \mathbf{J}_1 \mathbf{N}_{1,M} = \mathbf{0} \), the second task produces no acceleration in task 1, so the higher-priority task is protected at the dynamic level as well. Generalization to more tasks uses cumulative dynamically consistent projectors analogous to the kinematic case.

flowchart TD
  S["Desired operational accelerations ddx1*, ddx2*, ..."] --> F1["Compute task forces F1, F2, ..."]
  F1 --> T1["tau1 = J1^T * F1"]
  T1 --> N1["Compute dynamic nullspace projector N1_M"]
  N1 --> T2["Project tau2 into nullspace: tau2_null = N1_M^T * tau2"]
  T2 --> SUM["Total torque tau = tau1 + tau2_null + ..."]
  SUM --> JOINT["Send tau to joint torque controllers"]
        

Near kinematic singularities, the Jacobian \( \mathbf{J} \) becomes ill-conditioned and the pseudoinverse \( \mathbf{J}^{\#} \) can produce excessively large joint velocities. A standard regularization is the damped least-squares pseudoinverse:

\[ \mathbf{J}_\lambda^{\#} = \mathbf{J}^{\mathsf{T}} \left( \mathbf{J} \mathbf{J}^{\mathsf{T}} + \lambda^2 \mathbf{I} \right)^{-1}, \quad \lambda > 0. \]

This solves

\[ \dot{\mathbf{q}}^\ast = \arg\min_{\dot{\mathbf{q}}} \left\| \mathbf{J}\dot{\mathbf{q}} - \dot{\mathbf{x}}^\ast \right\|^2 + \lambda^2 \left\| \dot{\mathbf{q}} \right\|^2, \]

trading exact satisfaction of the task for bounded joint velocities. When damped pseudoinverses are used in hierarchies, the projectors \( \mathbf{N} = \mathbf{I} - \mathbf{J}_\lambda^{\#}\mathbf{J} \) are no longer exact null-space projectors; higher-priority tasks are only approximately preserved, leading to priority leakage. This trade-off between strict priority and numerical robustness is important in practical robot control.

The following Python code (using numpy) implements a two-task hierarchy at the velocity level. Task 1 is typically the end-effector task; task 2 can encode posture or joint-limit avoidance.

import numpy as np

def pseudoinverse(J, damping=0.0):
    """
    Right pseudoinverse J# = J^T (J J^T + λ^2 I)^(-1)
    for full-row-rank Jacobian J (m x n, m <= n).
    """
    J = np.asarray(J, dtype=float)
    m, n = J.shape
    if damping > 0.0:
        JJt = J @ J.T + (damping ** 2) * np.eye(m)
    else:
        JJt = J @ J.T
    return J.T @ np.linalg.inv(JJt)

def two_task_hierarchy(J1, dx1, J2, dx2, damping1=0.0, damping2=0.0):
    """
    Compute joint velocity command dq implementing:
      - Task 1 with highest priority
      - Task 2 projected into nullspace of Task 1.

    J1: m1 x n, primary task Jacobian
    dx1: m1 vector, desired primary task velocity
    J2: m2 x n, secondary task Jacobian
    dx2: m2 vector, desired secondary task velocity
    """
    J1 = np.asarray(J1, dtype=float)
    J2 = np.asarray(J2, dtype=float)
    dx1 = np.asarray(dx1, dtype=float).reshape(-1)
    dx2 = np.asarray(dx2, dtype=float).reshape(-1)

    n = J1.shape[1]
    I = np.eye(n)

    # Primary task
    J1_pinv = pseudoinverse(J1, damping=damping1)
    dq1 = J1_pinv @ dx1
    N1 = I - J1_pinv @ J1  # nullspace projector of task 1

    # Secondary task projected in nullspace of primary
    # Use effective Jacobian in the nullspace
    J2_bar = J2 @ N1
    J2_bar_pinv = pseudoinverse(J2_bar, damping=damping2)
    dx2_tilde = dx2 - J2 @ dq1
    dq2_null = J2_bar_pinv @ dx2_tilde

    dq = dq1 + N1 @ dq2_null
    return dq

# Example usage: a 7-DOF arm with Cartesian task (3D position)
if __name__ == "__main__":
    # Random demo Jacobians (for illustration)
    np.random.seed(0)
    n = 7
    J1 = np.random.randn(3, n)   # primary: Cartesian position
    J2 = np.eye(n)               # secondary: joint posture (identity)

    dx1 = np.array([0.1, -0.05, 0.0])   # desired end-effector velocity
    q_pref = np.zeros(n)                # preferred posture
    q_current = np.array([0.2, -0.3, 0.1, 0.0, 0.2, -0.1, 0.1])

    # Simple posture task: try to move back toward q_pref
    # approximated as dx2 = k_posture * (q_pref - q_current)
    k_posture = 0.5
    dx2 = k_posture * (q_pref - q_current)

    dq_cmd = two_task_hierarchy(J1, dx1, J2, dx2,
                                damping1=1e-3, damping2=1e-3)
    print("Joint velocity command:", dq_cmd)
      

This velocity command can be integrated and wrapped in a low-level joint-space PD controller (Chapter 2) to yield a complete kinematic task hierarchy controller.

In C++, Eigen is a convenient library for matrix computations. The code below computes the two-task kinematic hierarchy as in the Python example.

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

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

MatrixXd pseudoinverse(const MatrixXd& J, double damping = 0.0)
{
    // Right pseudoinverse: J# = J^T (J J^T + λ^2 I)^(-1)
    const int m = J.rows();
    const int n = J.cols();
    MatrixXd JJt = J * J.transpose();
    if (damping > 0.0) {
        JJt += (damping * damping) * MatrixXd::Identity(m, m);
    }
    MatrixXd JJt_inv = JJt.inverse();
    return J.transpose() * JJt_inv;
}

VectorXd twoTaskHierarchy(const MatrixXd& J1,
                          const VectorXd& dx1,
                          const MatrixXd& J2,
                          const VectorXd& dx2,
                          double damping1 = 0.0,
                          double damping2 = 0.0)
{
    const int n = J1.cols();
    MatrixXd I = MatrixXd::Identity(n, n);

    MatrixXd J1_pinv = pseudoinverse(J1, damping1);
    VectorXd dq1 = J1_pinv * dx1;
    MatrixXd N1 = I - J1_pinv * J1;

    MatrixXd J2_bar = J2 * N1;
    MatrixXd J2_bar_pinv = pseudoinverse(J2_bar, damping2);
    VectorXd dx2_tilde = dx2 - J2 * dq1;
    VectorXd dq2_null = J2_bar_pinv * dx2_tilde;

    VectorXd dq = dq1 + N1 * dq2_null;
    return dq;
}

int main()
{
    const int n = 7;
    MatrixXd J1(3, n);
    MatrixXd J2(n, n);
    J1.setRandom();
    J2.setIdentity();

    VectorXd dx1(3);
    dx1 << 0.1, -0.05, 0.0;

    VectorXd q_pref = VectorXd::Zero(n);
    VectorXd q_current(n);
    q_current << 0.2, -0.3, 0.1, 0.0, 0.2, -0.1, 0.1;

    double k_posture = 0.5;
    VectorXd dx2 = k_posture * (q_pref - q_current);

    VectorXd dq = twoTaskHierarchy(J1, dx1, J2, dx2, 1e-3, 1e-3);
    std::cout << "dq command:\n" << dq.transpose() << std::endl;
    return 0;
}
      

In a real controller, dx1 and dx2 are computed from tracking errors, and dq is converted to joint torques via a joint-space tracking law.

Java does not have a built-in matrix library; we can either use libraries such as Apache Commons Math or implement minimal linear algebra. Below is a simple example using plain arrays and a naive pseudoinverse based on \( \mathbf{J}^{\#} = \mathbf{J}^{\mathsf{T}}(\mathbf{J}\mathbf{J}^{\mathsf{T}})^{-1} \). In practice, one should use a robust library (SVD-based pseudoinverse).

public class TwoTaskHierarchy {

    // Multiply A (m x n) by B (n x p)
    static double[][] matMul(double[][] A, double[][] B) {
        int m = A.length;
        int n = A[0].length;
        int p = B[0].length;
        double[][] C = new double[m][p];
        for (int i = 0; i < m; ++i) {
            for (int k = 0; k < n; ++k) {
                for (int j = 0; j < p; ++j) {
                    C[i][j] += A[i][k] * B[k][j];
                }
            }
        }
        return C;
    }

    // Transpose of A
    static double[][] transpose(double[][] A) {
        int m = A.length;
        int n = A[0].length;
        double[][] At = new double[n][m];
        for (int i = 0; i < m; ++i) {
            for (int j = 0; j < n; ++j) {
                At[j][i] = A[i][j];
            }
        }
        return At;
    }

    // Very naive matrix inverse for small matrices using Gauss-Jordan
    static double[][] invert(double[][] A) {
        int n = A.length;
        double[][] B = new double[n][2*n];
        for (int i = 0; i < n; ++i) {
            for (int j = 0; j < n; ++j) {
                B[i][j] = A[i][j];
            }
            B[i][n + i] = 1.0;
        }
        for (int i = 0; i < n; ++i) {
            double pivot = B[i][i];
            for (int j = 0; j < 2*n; ++j) {
                B[i][j] /= pivot;
            }
            for (int k = 0; k < n; ++k) {
                if (k == i) continue;
                double factor = B[k][i];
                for (int j = 0; j < 2*n; ++j) {
                    B[k][j] -= factor * B[i][j];
                }
            }
        }
        double[][] Inv = new double[n][n];
        for (int i = 0; i < n; ++i) {
            for (int j = 0; j < n; ++j) {
                Inv[i][j] = B[i][n + j];
            }
        }
        return Inv;
    }

    static double[][] pseudoinverse(double[][] J) {
        // J# = J^T (J J^T)^(-1)
        double[][] Jt = transpose(J);
        double[][] JJt = matMul(J, Jt);
        double[][] JJtInv = invert(JJt);
        return matMul(Jt, JJtInv);
    }

    static double[] twoTaskHierarchy(double[][] J1, double[] dx1,
                                     double[][] J2, double[] dx2) {
        int n = J1[0].length;
        double[][] I = new double[n][n];
        for (int i = 0; i < n; ++i) I[i][i] = 1.0;

        double[][] J1_pinv = pseudoinverse(J1);
        double[] dq1 = matVecMul(J1_pinv, dx1);
        double[][] N1 = matSub(I, matMul(J1_pinv, J1));

        double[][] J2_bar = matMul(J2, N1);
        double[][] J2_bar_pinv = pseudoinverse(J2_bar);
        double[] J2dq1 = matVecMul(J2, dq1);
        double[] dx2_tilde = vecSub(dx2, J2dq1);
        double[] dq2_null = matVecMul(J2_bar_pinv, dx2_tilde);

        double[] dq = vecAdd(dq1, matVecMul(N1, dq2_null));
        return dq;
    }

    // Helper functions matVecMul, vecAdd, vecSub are omitted for brevity
    // but follow straightforward double-loop implementations.
}
      

This structure mirrors the Python and C++ implementations and can be embedded into a Java real-time control framework.

In MATLAB, the pseudoinverse is directly available as pinv, and the controller can be placed in a MATLAB Function block in Simulink.

function dq = two_task_hierarchy(J1, dx1, J2, dx2, lambda1, lambda2)
% TWO_TASK_HIERARCHY  Two-level kinematic task hierarchy.
%   J1, dx1 : primary task Jacobian and desired velocity
%   J2, dx2 : secondary task Jacobian and desired velocity
%   lambda1, lambda2 : damping factors for pseudoinverse

if nargin < 5
    lambda1 = 0.0;
    lambda2 = 0.0;
end

[m1, n] = size(J1);
I = eye(n);

if lambda1 > 0
    J1_pinv = J1' / (J1 * J1' + (lambda1^2) * eye(m1));
else
    J1_pinv = pinv(J1);
end

dq1 = J1_pinv * dx1(:);
N1 = I - J1_pinv * J1;

J2_bar = J2 * N1;
[m2, ~] = size(J2_bar);
if lambda2 > 0
    J2_bar_pinv = J2_bar' / (J2_bar * J2_bar' + (lambda2^2) * eye(m2));
else
    J2_bar_pinv = pinv(J2_bar);
end

dx2_tilde = dx2(:) - J2 * dq1;
dq2_null = J2_bar_pinv * dx2_tilde;

dq = dq1 + N1 * dq2_null;
      

In Simulink, this function can be called from a MATLAB Function block whose inputs are J1, dx1, J2, dx2 from upstream blocks (Jacobian computation and task generation), and whose output dq is fed to a joint-level controller or an integrator to compute \( \mathbf{q}(t) \).

Mathematica provides PseudoInverse and symbolic manipulation, which is useful for analyzing properties of task hierarchies symbolically or numerically.

(* Two-task hierarchy in Mathematica *)

twoTaskHierarchy[J1_, dx1_, J2_, dx2_, lambda1_:0., lambda2_:0.] :=
 Module[{n, Iden, J1pinv, dq1, N1, J2bar, J2barPinv, dx2tilde, dq2null},
  n = Dimensions[J1][[2]];
  Iden = IdentityMatrix[n];

  (* Damped pseudoinverse via Tikhonov regularization *)
  J1pinv =
   If[lambda1 > 0,
    Transpose[J1].Inverse[J1.Transpose[J1] + lambda1^2 IdentityMatrix[Dimensions[J1][[1]]]],
    PseudoInverse[J1]
   ];

  dq1 = J1pinv . dx1;
  N1 = Iden - J1pinv . J1;

  J2bar = J2 . N1;
  J2barPinv =
   If[lambda2 > 0,
    Transpose[J2bar].Inverse[J2bar.Transpose[J2bar] + lambda2^2 IdentityMatrix[Dimensions[J2bar][[1]]]],
    PseudoInverse[J2bar]
   ];

  dx2tilde = dx2 - J2 . dq1;
  dq2null = J2barPinv . dx2tilde;

  dq1 + N1 . dq2null
 ]

(* Example numeric call *)
J1 = RandomReal[{-1, 1}, {3, 7}];
J2 = IdentityMatrix[7];
dx1 = {0.1, -0.05, 0.0};
qPref = ConstantArray[0., 7];
qCurrent = {0.2, -0.3, 0.1, 0.0, 0.2, -0.1, 0.1};
kPosture = 0.5;
dx2 = kPosture (qPref - qCurrent);

dqCmd = twoTaskHierarchy[J1, dx1, J2, dx2, 0.001, 0.001]
      

With symbolic J1, one can also verify properties such as \( \mathbf{J}_1 \dot{\mathbf{q}}_2 = \dot{\mathbf{x}}_1^\ast \) exactly.

In this lesson we developed a rigorous framework for hierarchical task control of redundant robots. Starting from the pseudoinverse solution of a single task, we defined null-space projectors and used them to construct stacks of tasks that preserve higher-priority objectives. We then extended the concept to the torque level via dynamically consistent pseudoinverses in operational space, ensuring that lower-priority torques do not disturb higher-priority task accelerations. Finally, we discussed damped pseudoinverses, their impact on numerical robustness and priority leakage, and provided multi-language implementations that can be integrated with earlier joint-space and task-space control architectures.