Consider a manipulator with \( n \) joints and a task-space of dimension \( m \) (e.g., end-effector position/orientation). The task kinematics are

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

The manipulator is kinematically redundant for the task if \( n > m \) and the Jacobian has full row rank \( \operatorname{rank}(J) = m \). Then the linear system for a desired task-space velocity \( \dot{\mathbf{x}}_{\mathrm{d}} \) has infinitely many joint velocities \( \dot{\mathbf{q}} \) satisfying \( \dot{\mathbf{x}}_{\mathrm{d}} = J(\mathbf{q}) \dot{\mathbf{q}} \).

The set of such joint velocities is an affine subspace

\[ \left\{ \dot{\mathbf{q}} \in \mathbb{R}^n \,\middle|\, J(\mathbf{q}) \dot{\mathbf{q}} = \dot{\mathbf{x}}_{\mathrm{d}} \right\} = \dot{\mathbf{q}}_p + \mathcal{N}(J(\mathbf{q})), \]

where \( \dot{\mathbf{q}}_p \) is a particular solution and \( \mathcal{N}(J) \) is the null space \( \{ \mathbf{z} \in \mathbb{R}^n \mid J(\mathbf{q}) \mathbf{z} = \mathbf{0} \} \). Null-space control exploits this freedom: the primary task is realized by the particular solution, while additional objectives are pursued using motion in the null space, which by definition does not change the instantaneous task-space velocity.

flowchart TD
  A["Desired task velocity xdot_d"] --> B["Compute Jacobian J(q)"]
  B --> C["Compute pseudoinverse J_sharp"]
  C --> D["Primary joint vel: qdot_task = J_sharp * xdot_d"]
  D --> E["Compute null projector N = I - J_sharp * J"]
  E --> F["Choose secondary objective gradient: grad_h(q)"]
  F --> G["Null motion: qdot_null = -k_h * grad_h(q)"]
  G --> H["Total qdot = qdot_task + N * qdot_null"]
  H --> I["Integrate qdot to update q"]
        

At torque level (operational-space dynamics), a similar structure will appear: primary task torques plus null-space torques that do not affect end-effector accelerations. We will make this precise after introducing pseudoinverses and null-space projectors.

For a full-row-rank Jacobian \( J(\mathbf{q}) \in \mathbb{R}^{m \times n} \) with \( m \leq n \), the Moore–Penrose pseudoinverse is

\[ J^{\#}(\mathbf{q}) = J(\mathbf{q})^{\top} \left(J(\mathbf{q}) J(\mathbf{q})^{\top}\right)^{-1} \in \mathbb{R}^{n \times m}. \]

It is the minimum-norm right-inverse of \( J \) in the sense that the solution \( \dot{\mathbf{q}} = J^{\#} \dot{\mathbf{x}}_{\mathrm{d}} \) minimizes \( \|\dot{\mathbf{q}}\|_2 \) among all joint velocities that realize the desired task velocity.

Key identity (minimum-norm solution). The general solution of \( J \dot{\mathbf{q}} = \dot{\mathbf{x}}_{\mathrm{d}} \) is

\[ \dot{\mathbf{q}} = J^{\#} \dot{\mathbf{x}}_{\mathrm{d}} + \left(I_n - J^{\#} J\right)\mathbf{z}, \quad \mathbf{z} \in \mathbb{R}^{n}, \]

where \( N = I_n - J^{\#}J \) is the orthogonal projector onto the null space \( \mathcal{N}(J) \). We verify the two crucial properties:

\[ J N = J \left(I_n - J^{\#} J\right) = J - J J^{\#} J = J - J = \mathbf{0}, \]

using the Moore–Penrose condition \( J J^{\#} J = J \), and

\[ N^2 = \left(I_n - J^{\#} J\right)\left(I_n - J^{\#} J\right) = I_n - 2 J^{\#} J + J^{\#} J J^{\#} J = I_n - J^{\#} J = N, \]

so \( N \) is indeed a projector. Thus any vector in the range of \( N \) lies in \( \mathcal{N}(J) \) and does not change the task velocity.

For singular configurations, one often uses the damped pseudoinverse

\[ J^{\#}_{\lambda} = J^{\top} \left(J J^{\top} + \lambda^2 I_m \right)^{-1}, \quad \lambda > 0, \]

which regularizes small singular values of \( J \) and avoids numerically large joint velocities near singularities. The corresponding projector \( N_{\lambda} = I_n - J^{\#}_{\lambda} J \) is no longer an exact projector onto \( \mathcal{N}(J) \), but approximates it while improving robustness.

Let the primary control goal be end-effector tracking: track a desired trajectory \( \mathbf{x}_{\mathrm{d}}(t) \). A standard task-space PD control law (from previous lessons) prescribes a desired task velocity

\[ \dot{\mathbf{x}}_{\mathrm{d}} = \dot{\mathbf{x}}_{\mathrm{ref}} + K_p \left(\mathbf{x}_{\mathrm{d}} - \mathbf{x}\right) + K_d \left(\dot{\mathbf{x}}_{\mathrm{d,ff}} - \dot{\mathbf{x}}\right), \]

where \( K_p, K_d \in \mathbb{R}^{m \times m} \) are positive-definite gains and \( \dot{\mathbf{x}}_{\mathrm{ref}}, \dot{\mathbf{x}}_{\mathrm{d,ff}} \) denote reference or feedforward velocities (they can be zero for simple tracking).

The joint velocity command with redundancy resolution is then

\[ \dot{\mathbf{q}} = J^{\#} \dot{\mathbf{x}}_{\mathrm{d}} + \left(I_n - J^{\#} J\right) \dot{\mathbf{q}}_{\mathrm{null}}. \]

To encode a secondary objective (e.g., posture, joint-limit avoidance, manipulability), we introduce a smooth scalar cost function \( h(\mathbf{q}) \) and choose

\[ \dot{\mathbf{q}}_{\mathrm{null}} = -k_h \nabla_{\mathbf{q}} h(\mathbf{q}), \quad k_h > 0. \]

Then the overall velocity-level controller becomes

\[ \dot{\mathbf{q}} = J^{\#} \dot{\mathbf{x}}_{\mathrm{d}} - k_h \left(I_n - J^{\#} J\right) \nabla_{\mathbf{q}} h(\mathbf{q}). \]

Task invariance. Because \( J(I_n - J^{\#}J) = \mathbf{0} \), the task velocity induced by \( \dot{\mathbf{q}} \) is

\[ \dot{\mathbf{x}} = J \dot{\mathbf{q}} = J J^{\#} \dot{\mathbf{x}}_{\mathrm{d}} - k_h J\left(I_n - J^{\#}J\right)\nabla_{\mathbf{q}} h = J J^{\#} \dot{\mathbf{x}}_{\mathrm{d}}. \]

Under full-row-rank conditions, \( J J^{\#} = I_m \), so \( \dot{\mathbf{x}} = \dot{\mathbf{x}}_{\mathrm{d}} \). The null-space term does not affect the instantaneous task-space behavior.

Secondary objective descent. The time derivative of the cost along the motion is

\[ \dot{h}(\mathbf{q}) = \nabla_{\mathbf{q}} h(\mathbf{q})^{\top} \dot{\mathbf{q}} = \nabla_{\mathbf{q}} h^{\top} J^{\#} \dot{\mathbf{x}}_{\mathrm{d}} - k_h \nabla_{\mathbf{q}} h^{\top} \left(I_n - J^{\#} J\right) \nabla_{\mathbf{q}} h. \]

If \( \nabla_{\mathbf{q}} h \) lies predominantly in the null space of \( J \), the first term is small and the second term is non-positive:

\[ \nabla_{\mathbf{q}} h^{\top} \left(I_n - J^{\#} J\right) \nabla_{\mathbf{q}} h \ge 0, \]

so \( \dot{h} \lesssim 0 \). More rigorous Lyapunov convergence proofs can be constructed for specific choices of \( h(\mathbf{q}) \) and gain ranges.

In the operational-space control framework (Lesson 2), joint dynamics are

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

with positive-definite inertia matrix \( M(\mathbf{q}) \in \mathbb{R}^{n \times n} \) and joint torques \( \boldsymbol{\tau} \). The task-space acceleration obeys

\[ \ddot{\mathbf{x}} = J \ddot{\mathbf{q}} + \dot{J} \dot{\mathbf{q}}. \]

We define the dynamically consistent generalized inverse

\[ J^{\#}_M(\mathbf{q}) = M(\mathbf{q})^{-1} J(\mathbf{q})^{\top} \left( J(\mathbf{q}) M(\mathbf{q})^{-1} J(\mathbf{q})^{\top} \right)^{-1}, \]

which minimizes joint torques and accelerations with respect to the kinetic energy metric induced by \( M(\mathbf{q}) \). The associated null-space projector is

\[ N_M(\mathbf{q}) = I_n - J^{\#}_M(\mathbf{q}) J(\mathbf{q}). \]

Property (dynamic decoupling). One can show that

\[ J(\mathbf{q}) M(\mathbf{q})^{-1} N_M(\mathbf{q})^{\top} = \mathbf{0}, \]

i.e., joint torques of the form \( \boldsymbol{\tau}_0 \in \operatorname{Range}(N_M^{\top}) \) do not induce accelerations in the task space.

A standard torque-level redundancy-resolving operational-space controller is then

\[ \boldsymbol{\tau} = J(\mathbf{q})^{\top} \mathbf{F}^{\ast} + N_M(\mathbf{q})^{\top} \boldsymbol{\tau}_0, \]

where \( \mathbf{F}^{\ast} \) is the operational-space control wrench obtained in Lesson 2 (e.g., including task-space inertia \( \Lambda \), Coriolis/centrifugal, gravity terms), and \( \boldsymbol{\tau}_0 \) is a joint-space torque realizing secondary objectives (posture, joint-limit avoidance, balance for a base, etc.). Because of the decoupling property, the secondary torque does not interfere with the primary end-effector dynamics.

flowchart TD
  A["Desired task wrench F_star"] --> B["Primary torque tau_task = J^T * F_star"]
  B --> C["Compute dynamic J_sharp_M and N_M = I - J_sharp_M * J"]
  C --> D["Secondary objective: tau_0 (e.g. posture PD)"]
  D --> E["Project: tau_null = N_M^T * tau_0"]
  E --> F["Total torque tau = tau_task + tau_null"]
  F --> G["Robot dynamics M(q) qdd + C(q,qd) qd + g(q) = tau"]
        

We implement basic kinematic null-space redundancy resolution and, assuming we have access to the inertia matrix \( M(\mathbf{q}) \), also the dynamically consistent projector. The code uses numpy and is structured to plug into a control loop that receives the current state and outputs joint velocity or torque commands.

import numpy as np

def pseudoinverse(J, damping=0.0):
    """
    Compute Moore-Penrose or damped pseudoinverse of J (m x n, m <= n).
    """
    m, n = J.shape
    if damping <= 0.0:
        # Standard right pseudoinverse: J^T (J J^T)^{-1}
        JJt = J @ J.T
        return J.T @ np.linalg.inv(JJt)
    else:
        # Damped pseudoinverse: J^T (J J^T + lambda^2 I)^{-1}
        JJt = J @ J.T
        return J.T @ np.linalg.inv(JJt + (damping ** 2) * np.eye(m))

def null_projector(J, damping=0.0):
    J_sharp = pseudoinverse(J, damping)
    n = J.shape[1]
    return np.eye(n) - J_sharp @ J

def velocity_level_control(q, x, xdot, x_d, xdot_d_ff, Kp, Kd,
                           secondary_grad, k_h=1.0, damping=0.0):
    """
    q: (n,) joint positions
    x: (m,) current task position
    xdot: (m,) current task velocity
    x_d: (m,) desired task position
    xdot_d_ff: (m,) desired feedforward task velocity
    Kp, Kd: (m x m) gains
    secondary_grad: function grad_h(q) returning (n,)
    """
    # User-provided Jacobian J(q) (m x n)
    J = jacobian(q)  # assumed available from kinematics library

    # Task-space velocity command (PD in task space)
    x_error = x_d - x
    xdot_error = xdot_d_ff - xdot
    xdot_d = xdot_d_ff + Kp @ x_error + Kd @ xdot_error

    # Kinematic redundancy resolution
    J_sharp = pseudoinverse(J, damping)
    N = np.eye(J.shape[1]) - J_sharp @ J

    qdot_task = J_sharp @ xdot_d
    grad_h = secondary_grad(q)

    qdot_null = -k_h * grad_h
    qdot = qdot_task + N @ qdot_null
    return qdot

def dynamic_pseudoinverse(J, M):
    """
    Dynamically consistent generalized inverse:
    J_sharp_M = M^{-1} J^T (J M^{-1} J^T)^{-1}
    """
    Minv = np.linalg.inv(M)
    JM = J @ Minv @ J.T
    return Minv @ J.T @ np.linalg.inv(JM)

def dynamic_null_projector(J, M):
    J_sharp_M = dynamic_pseudoinverse(J, M)
    n = J.shape[1]
    return np.eye(n) - J_sharp_M @ J

def torque_level_control(q, qdot, x, xdot,
                         x_d, xdot_d, xddot_d,
                         Kp_task, Kd_task,
                         tau0_func):
    """
    Basic torque-level operational-space controller with null-space torque.

    tau = J^T F_star + N_M^T tau_0
    """
    # Robot model access (from a dynamics library)
    M = inertia_matrix(q)             # (n x n)
    C = coriolis_matrix(q, qdot)      # (n x n) or (n,) depending on convention
    g = gravity_vector(q)             # (n,)

    J = jacobian(q)                   # (m x n)
    Jdot = jacobian_dot(q, qdot)      # (m x n)

    # Operational-space mass matrix: Lambda = (J M^{-1} J^T)^{-1}
    Minv = np.linalg.inv(M)
    Lambda_inv = J @ Minv @ J.T
    Lambda = np.linalg.inv(Lambda_inv)

    # Task-space dynamics terms (simplified):
    # mu = Lambda * (J M^{-1} C qdot - Jdot qdot), p = Lambda * J M^{-1} g
    mu = Lambda @ (J @ Minv @ (C @ qdot) - Jdot @ qdot)
    p = Lambda @ (J @ Minv @ g)

    # Task-space PD+feedforward wrench
    x_error = x_d - x
    xdot_error = xdot_d - xdot
    F_star = Lambda @ (xddot_d + Kp_task @ x_error + Kd_task @ xdot_error) + mu + p

    # Primary torque
    tau_task = J.T @ F_star

    # Null-space projector and secondary torque
    N_M = dynamic_null_projector(J, M)
    tau_0 = tau0_func(q, qdot)  # e.g. posture regulator torque
    tau_null = N_M.T @ tau_0

    tau = tau_task + tau_null
    # Final torque to apply at the actuators
    return tau
      

In practice, functions such as jacobian, inertia_matrix, and gravity_vector are provided by robotics libraries like pinocchio, pyrobot, RBDL (via Python bindings), or custom model code generated from a URDF.

In C++, linear algebra for redundancy control is typically handled with Eigen. Below is a minimal example of kinematic redundancy resolution, assuming Jacobian and gradients are provided.

#include <Eigen/Dense>

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

MatrixXd pseudoinverse(const MatrixXd& J, double damping = 0.0) {
    int m = J.rows();
    int n = J.cols();
    if (damping <= 0.0) {
        MatrixXd JJt = J * J.transpose();
        return J.transpose() * JJt.inverse();
    } else {
        MatrixXd JJt = J * J.transpose();
        MatrixXd I = MatrixXd::Identity(m, m);
        return J.transpose() * (JJt + damping * damping * I).inverse();
    }
}

MatrixXd nullProjector(const MatrixXd& J, double damping = 0.0) {
    MatrixXd Jsharp = pseudoinverse(J, damping);
    int n = J.cols();
    MatrixXd I = MatrixXd::Identity(n, n);
    return I - Jsharp * J;
}

VectorXd velocityLevelControl(
    const VectorXd& q,
    const VectorXd& x,
    const VectorXd& xdot,
    const VectorXd& x_d,
    const VectorXd& xdot_d_ff,
    const MatrixXd& Kp,
    const MatrixXd& Kd,
    const VectorXd& grad_h,
    double k_h,
    const MatrixXd& J,
    double damping = 0.0)
{
    // Task-space velocity command
    VectorXd x_error = x_d - x;
    VectorXd xdot_error = xdot_d_ff - xdot;
    VectorXd xdot_d = xdot_d_ff + Kp * x_error + Kd * xdot_error;

    MatrixXd Jsharp = pseudoinverse(J, damping);
    MatrixXd N = nullProjector(J, damping);

    VectorXd qdot_task = Jsharp * xdot_d;
    VectorXd qdot_null = -k_h * grad_h;

    VectorXd qdot = qdot_task + N * qdot_null;
    return qdot;
}
      

In a full system, J and grad_h are computed from the robot model and the chosen secondary objective function, respectively. This code can be called at each control cycle to produce joint velocity commands.

In Java, the EJML library provides dense linear algebra. The following snippet outlines a kinematic redundancy controller using EJML’s DMatrixRMaj type.

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.decomposition.chol.CholeskyDecompositionInner_DDRM;

public class RedundancyController {

    public static DMatrixRMaj pseudoinverse(DMatrixRMaj J, double damping) {
        int m = J.numRows;
        int n = J.numCols;
        DMatrixRMaj JJt = new DMatrixRMaj(m, m);
        CommonOps_DDRM.mult(J, J, JJt, true, false); // JJt = J * J^T

        DMatrixRMaj A = JJt.copy();
        if (damping > 0.0) {
            for (int i = 0; i < m; ++i) {
                A.set(i, i, A.get(i, i) + damping * damping);
            }
        }

        // Invert A via Cholesky (since JJt is SPD if full rank and damped)
        CholeskyDecompositionInner_DDRM chol = new CholeskyDecompositionInner_DDRM(true);
        if (!chol.decompose(A)) {
            throw new RuntimeException("Cholesky failed (Jacobian not full row rank?)");
        }
        DMatrixRMaj Ainv = new DMatrixRMaj(m, m);
        chol.invert(Ainv);

        DMatrixRMaj Jt = new DMatrixRMaj(n, m);
        CommonOps_DDRM.transpose(J, Jt);

        DMatrixRMaj Jsharp = new DMatrixRMaj(n, m);
        CommonOps_DDRM.mult(Jt, Ainv, Jsharp);
        return Jsharp;
    }

    public static DMatrixRMaj nullProjector(DMatrixRMaj J, double damping) {
        DMatrixRMaj Jsharp = pseudoinverse(J, damping);
        int n = J.numCols;
        DMatrixRMaj I = CommonOps_DDRM.identity(n);
        DMatrixRMaj N = I.copy();

        DMatrixRMaj JJ = new DMatrixRMaj(n, n);
        CommonOps_DDRM.mult(Jsharp, J, JJ);
        CommonOps_DDRM.subtractEquals(N, JJ); // N = I - Jsharp * J
        return N;
    }
}
      

This Java implementation mirrors the C++ and Python versions and can be integrated into a real-time control component running on a JVM-based robot controller.

In MATLAB, redundancy resolution can be implemented using built-in matrix operations. This function computes velocity-level joint commands for a redundant manipulator, and can be called directly from a MATLAB Function block in Simulink.

function qdot = redundancy_control(q, x, xdot, x_d, xdot_d_ff, Kp, Kd, k_h)
%REDUNDANCY_CONTROL Velocity-level null-space redundancy control.
%
% q, x, xdot, x_d, xdot_d_ff: column vectors
% Kp, Kd: task-space gain matrices
% k_h: null-space gradient descent gain

    % User-defined Jacobian and secondary gradient
    J = jacobian_robot(q);        % m x n
    grad_h = secondary_gradient(q); % n x 1

    % Task-space velocity command
    x_error = x_d - x;
    xdot_error = xdot_d_ff - xdot;
    xdot_d = xdot_d_ff + Kp * x_error + Kd * xdot_error;

    % Moore-Penrose pseudoinverse
    JJt = J * J.';
    Jsharp = J.' / JJt;  % J^T * (J J^T)^{-1}

    % Null-space projector
    [m, n] = size(J);
    N = eye(n) - Jsharp * J;

    % Joint velocities
    qdot_task = Jsharp * xdot_d;
    qdot_null = -k_h * grad_h;

    qdot = qdot_task + N * qdot_null;
end
      

In Simulink, this function can be wrapped in a block that receives the current state and reference and outputs qdot. A separate integrator block can integrate qdot to update the joint positions \( \mathbf{q} \).

Wolfram Mathematica is convenient for symbolic verification of null-space properties. The following example constructs a symbolic Jacobian and checks that the projector \( N = I_n - J^{\#} J \) satisfies \( J N = 0 \).

(* Example: 2D task (x,y), 3-joint planar arm with lengths l1,l2,l3 *)

ClearAll["Global`*"];

(* Joint variables and link lengths *)
q1 =.; q2 =.; q3 =.;
l1 =.; l2 =.; l3 =.;

(* Forward kinematics (planar) *)
x[q1_, q2_, q3_] := l1 Cos[q1] + l2 Cos[q1 + q2] + l3 Cos[q1 + q2 + q3];
y[q1_, q2_, q3_] := l1 Sin[q1] + l2 Sin[q1 + q2] + l3 Sin[q1 + q2 + q3];

(* Jacobian: J = d(x,y)/d(q1,q2,q3) *)
Jmat = D[{x[q1, q2, q3], y[q1, q2, q3]}, {{q1, q2, q3}}] // Simplify;

(* Moore-Penrose pseudoinverse with symbolic assumption of full row rank *)
Jsharp = Transpose[Jmat].Inverse[Jmat.Transpose[Jmat]] // Simplify;

n = 3;
Imat = IdentityMatrix[n];
Nmat = Imat - Jsharp.Jmat // Simplify;

(* Check J N = 0 *)
Simplify[Jmat.Nmat]
      

The result of Simplify[Jmat.Nmat] is the zero matrix, confirming symbolically that the null-space projector annihilates task-space motion for this example.

In this lesson we have:

• Defined kinematic redundancy as the case \( n > m \) with full-row-rank Jacobian and interpreted the solution space of \( \dot{\mathbf{x}} = J \dot{\mathbf{q}} \) as an affine subspace composed of a particular solution plus the Jacobian null space.
• Introduced the Moore–Penrose pseudoinverse and showed that \( \dot{\mathbf{q}} = J^{\#} \dot{\mathbf{x}}_{\mathrm{d}} \) is the minimum-norm solution, with \( N = I_n - J^{\#} J \) projecting onto \( \mathcal{N}(J) \).
• Constructed velocity-level redundancy controllers that embed secondary objectives (e.g., posture, joint limits, manipulability) via gradient descent in the null space, without altering the primary end-effector behavior.
• Derived the dynamically consistent generalized inverse and null-space projector, and showed that null-space torques \( N_M^{\top}\boldsymbol{\tau}_0 \) leave task-space accelerations unaffected in the operational-space control framework.
• Provided practical implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica that realize redundancy handling and verify the theoretical properties symbolically or numerically.

In the next lesson we will generalize these ideas to task hierarchies, where multiple tasks with strict priority levels are stacked and enforced via nested null-space projectors or optimization-based formulations.