Consider a manipulator with \( n \) joints and a task-space of dimension \( m \) (e.g., planar position \( m=2 \), spatial pose \( m=6 \)). The manipulator is kinematically redundant for the task if \( n > m \). In velocity-level form, the differential kinematics (introduced in the previous lesson on numerical IK) are

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

For a desired task-space velocity \( \dot{\mathbf{x}}_{\text{d}} \), the velocity IK problem is to find joint velocities \( \dot{\mathbf{q}} \) satisfying \( \mathbf{J}(\mathbf{q}) \dot{\mathbf{q}} = \dot{\mathbf{x}}_{\text{d}} \). When \( n > m \) and \( \mathbf{J}(\mathbf{q}) \) has full row rank \( m \), this equation is underdetermined and admits infinitely many solutions. Redundancy resolution chooses one solution (or family of solutions) according to an optimality or behavioral criterion, for example:

• Minimize joint-speed norm \( \|\dot{\mathbf{q}}\| \) (minimum-norm IK).
• Avoid joint limits and self-collisions.
• Shape the posture (e.g., keep elbow away from obstacles).

In this lesson we restrict attention to kinematic redundancy at the velocity level, ignoring dynamics and control; the resulting \( \dot{\mathbf{q}} \) can be integrated to generate joint trajectories.

The most classical redundancy resolution is the minimum Euclidean norm solution of the equality-constrained problem

\[ \min_{\dot{\mathbf{q}}} \frac{1}{2} \, \dot{\mathbf{q}}^{\top} \dot{\mathbf{q}} \quad \text{subject to} \quad \mathbf{J}(\mathbf{q}) \dot{\mathbf{q}} = \dot{\mathbf{x}}_{\text{d}}. \]

Introduce a Lagrange multiplier \( \boldsymbol{\lambda} \in \mathbb{R}^m \) and define the Lagrangian

\[ \mathcal{L}(\dot{\mathbf{q}}, \boldsymbol{\lambda}) = \tfrac{1}{2} \dot{\mathbf{q}}^{\top}\dot{\mathbf{q}} + \boldsymbol{\lambda}^{\top} \left( \mathbf{J} \dot{\mathbf{q}} - \dot{\mathbf{x}}_{\text{d}} \right). \]

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

\[ \frac{\partial \mathcal{L}}{\partial \dot{\mathbf{q}}} = \dot{\mathbf{q}} + \mathbf{J}^{\top} \boldsymbol{\lambda} = \mathbf{0} \quad \Rightarrow \quad \dot{\mathbf{q}} = - \mathbf{J}^{\top} \boldsymbol{\lambda}. \]

Enforcing the constraint \( \mathbf{J} \dot{\mathbf{q}} = \dot{\mathbf{x}}_{\text{d}} \) yields

\[ \mathbf{J} \dot{\mathbf{q}} = - \mathbf{J} \mathbf{J}^{\top} \boldsymbol{\lambda} = \dot{\mathbf{x}}_{\text{d}} \quad \Rightarrow \quad \boldsymbol{\lambda} = - \left( \mathbf{J} \mathbf{J}^{\top} \right)^{-1} \dot{\mathbf{x}}_{\text{d}}, \]

assuming full row rank so that \( \mathbf{J} \mathbf{J}^{\top} \) is invertible. Substituting back gives the minimum-norm joint velocity

\[ \dot{\mathbf{q}}_{\star} = \mathbf{J}^{\top} \left( \mathbf{J} \mathbf{J}^{\top} \right)^{-1} \dot{\mathbf{x}}_{\text{d}} \equiv \mathbf{J}^{\#} \dot{\mathbf{x}}_{\text{d}}, \]

where \( \mathbf{J}^{\#} \) denotes the Moore–Penrose pseudoinverse for the full-row-rank case. If the task is not exactly reachable, the same expression solves the least-squares problem \( \min_{\dot{\mathbf{q}}} \|\mathbf{J}\dot{\mathbf{q}} - \dot{\mathbf{x}}_{\text{d}}\|^2 \) with minimum joint-speed norm.

flowchart TD
  XD["Desired xdot_d"] --> JMAT["Compute J(q)"]
  JMAT --> PINV["Compute J_pinv = J' (J J')^-1"]
  PINV --> QDOT0["Primary solution qdot_0 = J_pinv * xdot_d"]
  QDOT0 --> INT["Integrate qdot_0 to update q"]
        

Redundancy manifests as a nontrivial null-space of the Jacobian:

\[ \mathcal{N}(\mathbf{J}) = \left\{ \mathbf{z} \in \mathbb{R}^n \,\middle|\, \mathbf{J} \mathbf{z} = \mathbf{0} \right\}. \]

Any velocity \( \mathbf{z} \in \mathcal{N}(\mathbf{J}) \) leaves the task velocity unchanged, because \( \mathbf{J}\mathbf{z} = \mathbf{0} \). The general solution of \( \mathbf{J} \dot{\mathbf{q}} = \dot{\mathbf{x}}_{\text{d}} \) can be written as

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

where \( \mathbf{I}_n \) is the \( n \times n \) identity matrix. The matrix \( \mathbf{N}(\mathbf{q}) = \mathbf{I}_n - \mathbf{J}^{\#}\mathbf{J} \) is the null-space projector.

Property 1: null-space.

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

because \( \mathbf{J}\mathbf{J}^{\#} \) is the identity on the row space of \( \mathbf{J} \) (one of the Penrose conditions). Thus every vector of the form \( \mathbf{N}\mathbf{z} \) lies in the null-space.

Property 2: idempotence.

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

Since \( \mathbf{J}^{\#}\mathbf{J}\mathbf{J}^{\#} = \mathbf{J}^{\#} \), we obtain

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

so \( \mathbf{N} \) is indeed a projector (idempotent). Consequently, adding any term \( \mathbf{N}\mathbf{z} \) to the minimum-norm solution preserves the task velocity while exploring posture changes.

Redundancy allows us to optimize a secondary objective while still tracking the primary task. Let \( H(\mathbf{q}) \) be a smooth scalar objective (e.g., a cost measuring distance to joint limits). The gradient-projection method chooses a null-space velocity in the direction of the negative gradient:

\[ \dot{\mathbf{q}} = \mathbf{J}^{\#} \dot{\mathbf{x}}_{\text{d}} - \alpha \, \mathbf{N}(\mathbf{q}) \, \nabla_{\mathbf{q}} H(\mathbf{q}), \quad \alpha > 0. \]

The time derivative of the objective along the motion is

\[ \frac{\mathrm{d}H}{\mathrm{d}t} = \nabla_{\mathbf{q}} H(\mathbf{q})^{\top} \dot{\mathbf{q}} = \nabla_{\mathbf{q}} H^{\top} \mathbf{J}^{\#} \dot{\mathbf{x}}_{\text{d}} - \alpha \nabla_{\mathbf{q}} H^{\top} \mathbf{N} \nabla_{\mathbf{q}} H. \]

The second term is nonpositive, because \( \mathbf{N} \) is symmetric positive semidefinite on the relevant subspace: \( \nabla_{\mathbf{q}} H^{\top} \mathbf{N} \nabla_{\mathbf{q}} H \ge 0 \). Therefore, for sufficiently small \( \alpha \), the secondary objective decreases while the primary task is preserved.

A common choice for joint-limit avoidance is the quadratic cost

\[ H(\mathbf{q}) = \frac{1}{2} \sum_{i=1}^{n} w_i \left( \frac{q_i - q_{i,\mathrm{mid}}}{q_{i,\mathrm{max}} - q_{i,\mathrm{min}}} \right)^{2}, \]

where \( q_{i,\mathrm{min}} \) and \( q_{i,\mathrm{max}} \) are joint limits, \( q_{i,\mathrm{mid}} \) is their midpoint, and \( w_i > 0 \) are weights.

flowchart TD
  START["Desired xdot_d and posture objective H(q)"] --> JSTEP["Compute J(q) and J_pinv"]
  JSTEP --> PRIMARY["Primary qdot_0 = J_pinv * xdot_d"]
  START --> GRAD["Compute grad H(q)"]
  GRAD --> PROJ["Compute N(q) = I - J_pinv * J"]
  PROJ --> NSVEL["Null-space term qdot_H = -alpha * N * gradH"]
  PRIMARY --> SUM["Total qdot = qdot_0 + qdot_H"]
  NSVEL --> SUM
        

The Euclidean norm in joint space is often not the most meaningful metric: some joints may be closer to limits or more costly to move. Let \( \mathbf{W} \in \mathbb{R}^{n \times n} \) be a symmetric positive-definite weight matrix. Consider the optimization

\[ \min_{\dot{\mathbf{q}}} \frac{1}{2} \dot{\mathbf{q}}^{\top} \mathbf{W} \dot{\mathbf{q}} \quad \text{subject to} \quad \mathbf{J} \dot{\mathbf{q}} = \dot{\mathbf{x}}_{\text{d}}. \]

The Lagrangian is

\[ \mathcal{L}(\dot{\mathbf{q}}, \boldsymbol{\lambda}) = \tfrac{1}{2} \dot{\mathbf{q}}^{\top} \mathbf{W} \dot{\mathbf{q}} + \boldsymbol{\lambda}^{\top} \left( \mathbf{J} \dot{\mathbf{q}} - \dot{\mathbf{x}}_{\text{d}} \right). \]

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

\[ \frac{\partial \mathcal{L}}{\partial \dot{\mathbf{q}}} = \mathbf{W} \dot{\mathbf{q}} + \mathbf{J}^{\top} \boldsymbol{\lambda} = \mathbf{0} \quad \Rightarrow \quad \dot{\mathbf{q}} = - \mathbf{W}^{-1} \mathbf{J}^{\top} \boldsymbol{\lambda}. \]

Enforcing the constraint:

\[ \mathbf{J} \dot{\mathbf{q}} = - \mathbf{J} \mathbf{W}^{-1} \mathbf{J}^{\top} \boldsymbol{\lambda} = \dot{\mathbf{x}}_{\text{d}} \quad \Rightarrow \quad \boldsymbol{\lambda} = - \left( \mathbf{J} \mathbf{W}^{-1} \mathbf{J}^{\top} \right)^{-1} \dot{\mathbf{x}}_{\text{d}}. \]

Thus,

\[ \dot{\mathbf{q}}_{\star} = \mathbf{W}^{-1} \mathbf{J}^{\top} \left( \mathbf{J} \mathbf{W}^{-1} \mathbf{J}^{\top} \right)^{-1} \dot{\mathbf{x}}_{\text{d}} \equiv \mathbf{J}^{\#}_{\mathbf{W}} \dot{\mathbf{x}}_{\text{d}}, \]

where \( \mathbf{J}^{\#}_{\mathbf{W}} \) is the weighted pseudoinverse. Large weights in \( \mathbf{W} \) suppress motion of the corresponding joints. The null-space projector is now \( \mathbf{N}_{\mathbf{W}} = \mathbf{I}_n - \mathbf{J}^{\#}_{\mathbf{W}}\mathbf{J} \), and the gradient-projection method can be generalized by replacing \( \mathbf{J}^{\#}, \mathbf{N} \) with their weighted counterparts.

When multiple tasks are present (e.g., end-effector tracking as task 1, posture shaping as task 2), one can treat them hierarchically. Let \( \mathbf{J}_1 \) and \( \mathbf{J}_2 \) be the Jacobians of tasks 1 and 2, respectively. A standard two-level construction is

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

where \( \mathbf{N}_1 = \mathbf{I}_n - \mathbf{J}_1^{\#}\mathbf{J}_1 \) projects the secondary task into the null-space of the primary task. This guarantees that the primary task is not affected by the secondary one (at least locally and away from singularities). In this course we focus mainly on the single-task case with null-space optimization; more advanced hierarchical schemes can be studied after mastering Jacobian properties in the next chapter.

We now implement a simple redundancy-resolution step for a planar 3-DOF arm tracking a 2D end-effector velocity. We use numpy for linear algebra and a damped pseudoinverse to handle near-singular configurations.

import numpy as np

def planar3_fk(q, link_lengths):
    """
    Simple planar 3R forward kinematics returning end-effector position.
    q: array-like, shape (3,)
    link_lengths: array-like, shape (3,)
    """
    q1, q2, q3 = q
    l1, l2, l3 = link_lengths
    c1 = np.cos(q1); s1 = np.sin(q1)
    c12 = np.cos(q1 + q2); s12 = np.sin(q1 + q2)
    c123 = np.cos(q1 + q2 + q3); s123 = np.sin(q1 + q2 + q3)

    x = l1 * c1 + l2 * c12 + l3 * c123
    y = l1 * s1 + l2 * s12 + l3 * s123
    return np.array([x, y])

def planar3_jacobian(q, link_lengths):
    """
    Jacobian J(q) mapping joint velocities to end-effector linear velocity.
    Shape: (2, 3).
    """
    q1, q2, q3 = q
    l1, l2, l3 = link_lengths
    s1 = np.sin(q1); c1 = np.cos(q1)
    s12 = np.sin(q1 + q2); c12 = np.cos(q1 + q2)
    s123 = np.sin(q1 + q2 + q3); c123 = np.cos(q1 + q2 + q3)

    # Partial derivatives of x, y with respect to q1, q2, q3
    # x = l1 c1 + l2 c12 + l3 c123
    # y = l1 s1 + l2 s12 + l3 s123
    J = np.zeros((2, 3))
    J[0, 0] = -l1 * s1 - l2 * s12 - l3 * s123
    J[1, 0] =  l1 * c1 + l2 * c12 + l3 * c123
    J[0, 1] = -l2 * s12 - l3 * s123
    J[1, 1] =  l2 * c12 + l3 * c123
    J[0, 2] = -l3 * s123
    J[1, 2] =  l3 * c123
    return J

def damped_pseudoinverse(J, lam=1e-3):
    """
    Damped pseudoinverse: J^T (J J^T + lam^2 I)^(-1).
    Suitable for m <= n and full (or nearly full) row rank.
    """
    m, n = J.shape
    JJt = J @ J.T
    return J.T @ np.linalg.inv(JJt + (lam ** 2) * np.eye(m))

def joint_limit_cost(q, q_min, q_max, w=None):
    """
    Quadratic joint-limit cost H(q).
    """
    q_mid = 0.5 * (q_min + q_max)
    span = q_max - q_min
    if w is None:
        w = np.ones_like(q)
    normed = (q - q_mid) / span
    return 0.5 * np.sum(w * normed * normed)

def joint_limit_grad(q, q_min, q_max, w=None):
    """
    Gradient of the joint-limit cost with respect to q.
    """
    q_mid = 0.5 * (q_min + q_max)
    span = q_max - q_min
    if w is None:
        w = np.ones_like(q)
    normed = (q - q_mid) / span
    return w * normed / (span)

def redundancy_resolution_step(q, xdot_des, link_lengths,
                               q_min, q_max,
                               alpha=0.1, lam=1e-3):
    """
    One Euler integration step of redundancy-resolved IK.

    q: current joint configuration, shape (3,)
    xdot_des: desired end-effector velocity (2,)
    """
    J = planar3_jacobian(q, link_lengths)
    J_pinv = damped_pseudoinverse(J, lam=lam)

    # Primary task
    qdot_0 = J_pinv @ xdot_des

    # Null-space projector
    N = np.eye(J.shape[1]) - J_pinv @ J

    # Secondary objective: joint-limit avoidance
    gradH = joint_limit_grad(q, q_min, q_max)
    qdot_H = -alpha * (N @ gradH)

    qdot = qdot_0 + qdot_H
    return qdot

# Example usage (single time step)
if __name__ == "__main__":
    link_lengths = np.array([0.4, 0.3, 0.2])
    q = np.array([0.0, 0.3, -0.2])
    q_min = np.array([-np.pi, -np.pi, -np.pi])
    q_max = np.array([ np.pi,  np.pi,  np.pi])

    # Track a small velocity in x direction
    xdot_des = np.array([0.05, 0.0])

    qdot = redundancy_resolution_step(q, xdot_des, link_lengths,
                                      q_min, q_max,
                                      alpha=0.2, lam=1e-3)
    q_next = q + 0.01 * qdot  # simple Euler step
    print("qdot:", qdot)
    print("q_next:", q_next)
      

This code demonstrates how null-space motion (driven by the joint-limit cost) can coexist with the primary tracking of the end-effector velocity. For longer trajectories, the step would be executed in a loop with updated task velocities and joint states.

In C++, the Eigen library is standard for dense linear algebra. The snippet below illustrates a generic damped pseudoinverse and a redundancy-resolution step using the same planar 3R example.

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

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

MatrixXd dampedPseudoinverse(const MatrixXd& J, double lambda)
{
    const int m = J.rows();
    MatrixXd JJt = J * J.transpose();
    MatrixXd I = MatrixXd::Identity(m, m);
    MatrixXd inv = (JJt + (lambda * lambda) * I).inverse();
    return J.transpose() * inv;  // J^T (J J^T + lambda^2 I)^(-1)
}

MatrixXd planar3Jacobian(const VectorXd& q,
                         const VectorXd& link_lengths)
{
    double q1 = q(0), q2 = q(1), q3 = q(2);
    double l1 = link_lengths(0), l2 = link_lengths(1), l3 = link_lengths(2);

    double s1 = std::sin(q1), c1 = std::cos(q1);
    double s12 = std::sin(q1 + q2), c12 = std::cos(q1 + q2);
    double s123 = std::sin(q1 + q2 + q3), c123 = std::cos(q1 + q2 + q3);

    MatrixXd J(2, 3);
    J(0, 0) = -l1 * s1 - l2 * s12 - l3 * s123;
    J(1, 0) =  l1 * c1 + l2 * c12 + l3 * c123;
    J(0, 1) = -l2 * s12 - l3 * s123;
    J(1, 1) =  l2 * c12 + l3 * c123;
    J(0, 2) = -l3 * s123;
    J(1, 2) =  l3 * c123;
    return J;
}

VectorXd jointLimitGradient(const VectorXd& q,
                            const VectorXd& q_min,
                            const VectorXd& q_max)
{
    VectorXd q_mid = 0.5 * (q_min + q_max);
    VectorXd span = q_max - q_min;
    VectorXd normed = (q - q_mid).cwiseQuotient(span);
    // Unit weights for simplicity
    return normed.cwiseQuotient(span);
}

VectorXd redundancyResolutionStep(const VectorXd& q,
                                  const VectorXd& xdot_des,
                                  const VectorXd& link_lengths,
                                  const VectorXd& q_min,
                                  const VectorXd& q_max,
                                  double alpha,
                                  double lambda)
{
    MatrixXd J = planar3Jacobian(q, link_lengths);
    MatrixXd J_pinv = dampedPseudoinverse(J, lambda);

    VectorXd qdot_0 = J_pinv * xdot_des;

    MatrixXd I = MatrixXd::Identity(J.cols(), J.cols());
    MatrixXd N = I - J_pinv * J;

    VectorXd gradH = jointLimitGradient(q, q_min, q_max);
    VectorXd qdot_H = -alpha * (N * gradH);

    VectorXd qdot = qdot_0 + qdot_H;
    return qdot;
}

int main()
{
    VectorXd link_lengths(3);
    link_lengths << 0.4, 0.3, 0.2;

    VectorXd q(3);
    q << 0.0, 0.3, -0.2;

    VectorXd q_min(3), q_max(3);
    q_min << -M_PI, -M_PI, -M_PI;
    q_max <<  M_PI,  M_PI,  M_PI;

    VectorXd xdot_des(2);
    xdot_des << 0.05, 0.0;

    double alpha = 0.2;
    double lambda = 1e-3;

    VectorXd qdot = redundancyResolutionStep(q, xdot_des,
                                             link_lengths,
                                             q_min, q_max,
                                             alpha, lambda);
    VectorXd q_next = q + 0.01 * qdot;

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

The structure mirrors the Python implementation. In a full robotics library, such functions would be methods attached to a manipulator model class with precomputed link parameters.

In Java, the EJML library (Efficient Java Matrix Library) is widely used for robotics-related linear algebra. Below is a minimal example of a damped pseudoinverse and redundancy-resolution step.

import org.ejml.simple.SimpleMatrix;

public class RedundantIK {

    public static SimpleMatrix dampedPseudoinverse(SimpleMatrix J, double lambda) {
        int m = J.numRows();
        SimpleMatrix JJt = J.mult(J.transpose());
        SimpleMatrix I = SimpleMatrix.identity(m);
        SimpleMatrix inv = JJt.plus(I.scale(lambda * lambda)).invert();
        return J.transpose().mult(inv);
    }

    public static SimpleMatrix planar3Jacobian(double[] q, double[] L) {
        double q1 = q[0], q2 = q[1], q3 = q[2];
        double l1 = L[0], l2 = L[1], l3 = L[2];

        double s1 = Math.sin(q1), c1 = Math.cos(q1);
        double s12 = Math.sin(q1 + q2), c12 = Math.cos(q1 + q2);
        double s123 = Math.sin(q1 + q2 + q3), c123 = Math.cos(q1 + q2 + q3);

        SimpleMatrix J = new SimpleMatrix(2, 3);
        J.set(0, 0, -l1 * s1 - l2 * s12 - l3 * s123);
        J.set(1, 0,  l1 * c1 + l2 * c12 + l3 * c123);
        J.set(0, 1, -l2 * s12 - l3 * s123);
        J.set(1, 1,  l2 * c12 + l3 * c123);
        J.set(0, 2, -l3 * s123);
        J.set(1, 2,  l3 * c123);
        return J;
    }

    public static SimpleMatrix jointLimitGradient(double[] q,
                                                  double[] qMin,
                                                  double[] qMax) {
        int n = q.length;
        SimpleMatrix grad = new SimpleMatrix(n, 1);
        for (int i = 0; i < n; ++i) {
            double qMid = 0.5 * (qMin[i] + qMax[i]);
            double span = qMax[i] - qMin[i];
            double normed = (q[i] - qMid) / span;
            grad.set(i, 0, normed / span);
        }
        return grad;
    }

    public static SimpleMatrix redundancyResolutionStep(double[] q,
                                                        double[] xdotDes,
                                                        double[] L,
                                                        double[] qMin,
                                                        double[] qMax,
                                                        double alpha,
                                                        double lambda) {
        SimpleMatrix J = planar3Jacobian(q, L);
        SimpleMatrix Jpinv = dampedPseudoinverse(J, lambda);

        SimpleMatrix xdot = new SimpleMatrix(2, 1, true, xdotDes);
        SimpleMatrix qdot0 = Jpinv.mult(xdot);

        SimpleMatrix I = SimpleMatrix.identity(J.numCols());
        SimpleMatrix N = I.minus(Jpinv.mult(J));

        SimpleMatrix gradH = jointLimitGradient(q, qMin, qMax);
        SimpleMatrix qdotH = N.mult(gradH).scale(-alpha);

        return qdot0.plus(qdotH);
    }

    public static void main(String[] args) {
        double[] L = {0.4, 0.3, 0.2};
        double[] q = {0.0, 0.3, -0.2};
        double[] qMin = {-Math.PI, -Math.PI, -Math.PI};
        double[] qMax = { Math.PI,  Math.PI,  Math.PI};
        double[] xdotDes = {0.05, 0.0};

        SimpleMatrix qdot = redundancyResolutionStep(q, xdotDes,
                                                     L, qMin, qMax,
                                                     0.2, 1e-3);

        System.out.println("qdot = ");
        qdot.print();
    }
}
      

The overall structure remains identical across languages: compute \( \mathbf{J} \), construct a (possibly damped) pseudoinverse, form the null-space projector, and add a gradient-projection term for secondary objectives.

10.1 MATLAB / Simulink

MATLAB provides convenient functions pinv and null. A velocity-level redundancy resolution step can be coded as:

function qdot = redundancy_step(q, xdot_des, L, q_min, q_max, alpha, lambda)
% q: 3x1, xdot_des: 2x1, L: 3x1 link lengths
% q_min, q_max: 3x1 joint limits

J = planar3_jacobian(q, L);

% Damped pseudoinverse: J' * inv(J*J' + lambda^2 I)
m = size(J, 1);
J_pinv = J' * ((J*J' + (lambda^2)*eye(m)) \ eye(m));

qdot0 = J_pinv * xdot_des;
N = eye(size(J, 2)) - J_pinv * J;

% Joint-limit gradient
q_mid = 0.5 * (q_min + q_max);
span = (q_max - q_min);
normed = (q - q_mid) ./ span;
gradH = normed ./ span;

qdotH = -alpha * (N * gradH);

qdot = qdot0 + qdotH;
end

function J = planar3_jacobian(q, L)
q1 = q(1); q2 = q(2); q3 = q(3);
l1 = L(1); l2 = L(2); l3 = L(3);

s1 = sin(q1); c1 = cos(q1);
s12 = sin(q1 + q2); c12 = cos(q1 + q2);
s123 = sin(q1 + q2 + q3); c123 = cos(q1 + q2 + q3);

J = zeros(2, 3);
J(1,1) = -l1*s1 - l2*s12 - l3*s123;
J(2,1) =  l1*c1 + l2*c12 + l3*c123;
J(1,2) = -l2*s12 - l3*s123;
J(2,2) =  l2*c12 + l3*c123;
J(1,3) = -l3*s123;
J(2,3) =  l3*c123;
end
      

In Simulink, this function can be called from a MATLAB Function block within a loop that integrates \( \dot{\mathbf{q}} \).

10.2 Wolfram Mathematica

In Mathematica, redundancy resolution can be expressed compactly using PseudoInverse and NullSpace:

planar3Jacobian[q_, L_] := Module[
  {q1, q2, q3, l1, l2, l3, s1, c1, s12, c12, s123, c123},
  {q1, q2, q3} = q;
  {l1, l2, l3} = L;
  s1 = Sin[q1]; c1 = Cos[q1];
  s12 = Sin[q1 + q2]; c12 = Cos[q1 + q2];
  s123 = Sin[q1 + q2 + q3]; c123 = Cos[q1 + q2 + q3];
  {
    {-l1 s1 - l2 s12 - l3 s123, -l2 s12 - l3 s123, -l3 s123},
    { l1 c1 + l2 c12 + l3 c123,  l2 c12 + l3 c123,  l3 c123}
  }
];

jointLimitGrad[q_, qmin_, qmax_] := Module[
  {qmid, span, normed},
  qmid = 0.5 (qmin + qmax);
  span = qmax - qmin;
  normed = (q - qmid)/span;
  normed/span
];

redundancyStep[q_, xdotDes_, L_, qmin_, qmax_, alpha_, lambda_] := Module[
  {J, m, Jpinv, I, N, gradH, qdot0, qdotH},
  J = planar3Jacobian[q, L];
  m = Length[J];
  Jpinv = Transpose[J].Inverse[J.Transpose[J] + (lambda^2) IdentityMatrix[m]];
  qdot0 = Jpinv.xdotDes;
  I = IdentityMatrix[Length[q]];
  N = I - Jpinv.J;
  gradH = jointLimitGrad[q, qmin, qmax];
  qdotH = -alpha N.gradH;
  qdot0 + qdotH
];

(* Example call *)
L = {0.4, 0.3, 0.2};
q = {0., 0.3, -0.2};
qmin = {-Pi, -Pi, -Pi};
qmax = { Pi,  Pi,  Pi};
xdotDes = {0.05, 0.0};

qdot = redundancyStep[q, xdotDes, L, qmin, qmax, 0.2, 10.^-3];
      

Symbolic capabilities in Mathematica also allow analytic manipulation of Jacobians and null-spaces for simple manipulators, which can be used to verify the algebra derived earlier in the lesson.

In this lesson we studied redundancy resolution for inverse kinematics. Starting from the velocity relation \( \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}} \), we derived the minimum-norm pseudoinverse solution and showed that the general solution includes a null-space component that does not affect the primary task. We introduced the null-space projector \( \mathbf{N} = \mathbf{I}_n - \mathbf{J}^{\#}\mathbf{J} \) and used it to project gradients of secondary objectives, yielding the classical gradient-projection method. Weighted pseudoinverses allow joint-dependent metrics, and the same algebra underlies hierarchical task-priority schemes.

Implementation examples in Python, C++, Java, MATLAB/Simulink, and Mathematica demonstrated how the theory translates to code. These redundancy-resolution techniques will interact closely with Jacobian properties and manipulability measures, which we investigate in detail in the next chapter on differential kinematics and singularities.