Nullspace Concepts, Robotics Applications, Implementation

May 17, 2025 | Tech Robotics

Preface

Nullspace is an important concept in Linear algebra and is widely used in robotics. This post discusses its concepts, applications in the control of redundant robots, and C++ implementation.

For reference, see Wikipedia@Kernel , Dantopa@ComputeNullspace , Wikipedia@PseudoInverse , and 2016@Strang .

Concepts

For a system of linear equations $$ Ax = b \label{eq:axb} $$ with $A\in \mathbb{R}^{m \times n}$, $x\in \mathbb{R}^n$, $b\in \mathbb{R}^m$. Its solution set is $$ x = \{ v + w : Av = b \wedge w \in \mathcal{N}(A) \} $$ where $v, w, \in \mathbb{R}^n$ and $\mathcal{N}(A)$ is the nullspace of $A$.

Intuition

If you have a concrete solution $x_1 \in \mathbb{R}^n$ for \eqref{eq:axb} and $\mathcal{N}(A)$, then, for any $x_2\in\mathbb{R}^n$ $$ x = x_1 + \mathcal{N}(A)x_2 $$ is also a solution, namely, $$ Ax_1 = b \label{eq:Ax1b} $$ and $$ A\Big[ x_1 + \mathcal{N}(A)x_2 \Big] = b \label{eq:Ax1x2b} $$ The above indicates that, any $x_2$ projected by $\mathcal{N}(A)$ can be added to $x_1$ without violating $Ax=b$, that is, $$ \mathcal{N}(A)x_2 \in \mathcal{N}(A) , \,\, \forall x_2\in \mathbb{R}^n $$ $$ A \, \mathcal{N}(A) = 0 $$

Computation

One can compute the nullspace of $A$ by via Singular Value Decomposition, specifically, $$ A = U \Sigma V^T \\ = \begin{bmatrix} u_1 & \cdots & u_{\rho} & u_{\rho +1} & \cdots & u_m \end{bmatrix} \begin{bmatrix} S_{\rho\times\rho} & 0 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} v^T_1 \\ \vdots \\ v^T_{\rho} \\ v^T_{\rho +1} \\ \vdots \\ v^T_n \end{bmatrix} $$ then $$\mathcal{N}(A) = \text{span} \{ v_{\rho +1}, \cdots, v_n \}$$

Another way to compute the nullspace is by $$\mathcal{N}(A) = I - A^{\dagger} A$$ where $A^{\dagger}$ is the pseudo-inverse of $A$ and $I\in\mathbb{R}^{n\times n}$ is an identity matrix.

Implementation

Using the numerical example given in Wikipedia@Kernel , that is $$ A = \begin{bmatrix} 2 & 3 & 5 \\ -4 & 2 & 3 \end{bmatrix} $$ and its nullspace $$ \mathcal{N}(A) = \begin{bmatrix} -1 \\ -26 \\ 16 \end{bmatrix} $$

Assuming $$ b = \begin{bmatrix} 1 \\ 2 \end{bmatrix} $$ below implementation

  • computes $\mathcal{N}(A)$ and a concrete solution $x_1 = A^{\dagger}b$, then
  • verifies that $$ \begin{split} Ax_1 &= A\Big[ x_1 + \mathcal{N}(A) x_2 \Big] \\ &= A x_1 + \cancelto{\mathbf{0}}{A \, \mathcal{N}(A)} \, x_2 \\ &= A x_1 = A A^{\dagger} b = b \end{split} $$

Applications in Redundant Robot Control

In robotics, at the end-effector and joint space kinematic level, the following equations hold $$ \dot{x} = J\dot{q} \label{eq:kinematics} $$ $$ \dot{\theta} = I\dot{q} $$ where $J$ is the robot end-effector Jacobian, mapping joint velocity to end-effector Cartesian velocity; $I$ is the robot joint space Jacobian, mapping joint velocity to joint velocity. One typically has control of the joint velocity $\dot{q}$.

To achieve a Cartesian space end-effector motion control task, one can design a control law $$ \dot{q}_1 = J^{\dagger} u_1 $$ $$ u_1 = K_p (\mathcal{X}_d - \mathcal{X}) + \dot{\mathcal{X}}_d $$

To achieve a joint space motion control task, one can design a control law $$ \dot{q}_2 = I u_2 $$ $$ u_2 = k_p(q_d - q) + \dot{q}_d $$

When the robot has redundant-DOF, one can design a control law such that: (1) the robot primarily tries to achieve the Cartesian space end-effector motion control task, and (2) without impacting the primary task, it also tries to achieve the joint space motion control task as much as possible.


Using the concept of nullspace, such a prioritized control law and the Cartesian system dynamics can be written as $$ \dot{q} = \dot{q}_1 + \mathcal{N}(J) \dot{q}_2 \label{eq:control} $$ for \eqref{eq:kinematics} and \eqref{eq:control} are formulated exactly as \eqref{eq:Ax1b} and \eqref{eq:Ax1x2b}.

Physically, with such a control law combining prioritized control velocities, the Cartesian space system dynamics is $$ \begin{split} \dot{x} = J\dot{q} &= J \Big[ \dot{q}_1 + \mathcal{N}(J) \dot{q}_2 \Big] \\ &= J\dot{q}_1 + J\mathcal{N}(J)\dot{q}_2 \\ &= J\dot{q}_1 \end{split} $$ where the only effective term in the combined motion control law is $\dot{q}_1$, which is exactly the control law that achieves the Cartesian motion control task, since the rest of the velocity disappears due to the multiplication of the end-effector Jacobian and its nullspace projector.

And the Joint space system dynamics is $$ \begin{split} \dot{\theta} = I \dot{q} &= I \Big[ \dot{q}_1 + \mathcal{N}(J) \dot{q}_2 \Big] \\ &= \dot{q}_1 + \mathcal{N}(J) \dot{q}_2 \end{split} $$ where the exact joint velocity $\dot{q}_2$ driving the robot to desired joint position is projected by the nullspace of the robot end-effector Jacobian, and affected by the joint velocity driving Cartesian task error to zero; however, it (1) will still somehow capable of driving the robot toward the joint space motion task depending on the instantaneous properties of the combined control law, and (2) it will not affect the Cartesian motion control task guaranteed by nullspace.

Extension

Note that control law in \eqref{eq:control}, there are ways to compute $\mathcal{N}(J)$, to guarantee that tasks are strictly prioritized, i.e., low priority tasks won't affect high priority ones, one has to compute the dynamically-consistent generalized Jacobian inverse $\bar{J}$.

That is $$ \mathcal{N}(A) = I - \bar{J} J $$ with $$ \bar{J} = M^{-1} J^T (J M^{-1} J^T)^{-1} $$ as oppose to the regular pseudo-inverse $$ J^{\dagger} = J^T (J J^T)^{-1} $$

In practice, one also has to consider singularity issues when computing the inverse term in the generalized inverse computation. For further discussion, please refer to Robot Singularity Avoidance, Analysis and Comparison and Analytical Whole Body Control, Prioritized Recursive Nullspace.