Engineering Guides · Module 14

Whole-body control: tasks, priorities and the null space

A robot with more joints than a task needs can do a second task for free, as long as it moves only in the directions the first task ignores.

Intermediate
Module 14: Motion Control & Actuator Integration
Written for ROS 2 Jazzy where ROS 2 is used
Sources checked 2026-11-06

Assumptions: Numbers in the examples are illustrative and the code is executed by the site’s tests. Facts taken from a cited source are marked; other statements are general engineering knowledge and must be confirmed on the datasheet of the part you use.

1. Engineering problem

A humanoid has dozens of joints and several simultaneous goals: keep balance, move a hand, look at a target, avoid joint limits. Whole-body control resolves them by giving the tasks priorities, so that a lower-priority task never disturbs a higher-priority one.

2. When is this needed?

Understanding how humanoid and mobile-manipulator controllers combine goals.
Choosing a control library.
Debugging a task that fights another.

3. Selection and design decisions

Tools (README descriptions, read 2026-11-06): TSID is a C++ library for optimization-based inverse-dynamics control built on the Pinocchio rigid-body dynamics library, with Python examples for manipulators, humanoids and quadrupeds. The idea at its simplest, for velocity-level kinematics: the primary task solves J₁·q̇ = ẋ₁ with the pseudo-inverse q̇₁ = J₁⁺ẋ₁; a secondary task is added in the null space of the first, q̇ = J₁⁺ẋ₁ + (I − J₁⁺J₁)·q̇₂, so it cannot change the primary task’s motion. Real whole-body controllers solve this as a constrained optimisation (torques, contact forces, limits) at every control step.

Overview
Concept	Meaning
Task	A goal in some space: end-effector pose, centre-of-mass position, posture
Jacobian J	Maps joint velocities to task velocities
Pseudo-inverse J⁺	Least-norm joint velocity that achieves a task velocity
Null space (I − J⁺J)	Joint motions that do not change the task
Priority	A lower task acts only in the null space of the higher ones
Constraints	Joint limits, torque limits, contact forces, friction cones

4. Advantages and disadvantages

Strict priorities protect the important task and can starve the others.
Weighted tasks blend goals smoothly and allow conflicts to degrade all of them.
Kinematic (velocity) control is simple; inverse dynamics handles contacts and torque limits.

5. Requirements and constraints

The robot has redundancy relative to the primary task (more joints than task dimensions).
Singularities and joint limits are handled (damping, constraints).
The control loop meets its deadline with the chosen solver.

6. Architecture

tasks (priority 1 … n) ─▶ stack: primary solution + null-space projections ─▶ joint velocities or torques ─▶ ros2_control

7. Implementation

List the tasks and their priorities.
Compute the Jacobians and the pseudo-inverse solutions.
Project lower-priority tasks into the null spaces (below).
Add joint, torque and contact constraints.
Verify that the primary task error does not change when a secondary task is added.

import numpy as np
L = np.array([0.5, 0.4, 0.3])
def fk(q):
    a = np.cumsum(q); return np.array([np.sum(L * np.cos(a)), np.sum(L * np.sin(a))])
def jac(q):
    a = np.cumsum(q); J = np.zeros((2, 3))
    for i in range(3):
        J[0, i] = -np.sum(L[i:] * np.sin(a[i:])); J[1, i] = np.sum(L[i:] * np.cos(a[i:]))
    return J
q0 = np.array([0.2, 0.9, 1.4]); q = q0.copy(); q_rest = np.array([0.0, 1.2, 0.0]); target = np.array([0.6, 0.6])
for _ in range(400):
    J = jac(q); Jp = np.linalg.pinv(J)
    dq1 = Jp @ (target - fk(q))                                   # task 1: end-effector position
    N = np.eye(3) - Jp @ J                                          # null space of task 1
    dq2 = N @ (1.0 * (q_rest - q))                                  # task 2: stay near the rest posture, only inside the null space
    q = q + 0.5 * dq1 + 0.2 * dq2
print("end-effector error (m):", round(float(np.linalg.norm(target - fk(q))), 5))
print("distance of the posture from rest with the secondary task:", round(float(np.linalg.norm(q - q_rest)), 3))
q2 = q0.copy()
for _ in range(400):
    J = jac(q2); q2 = q2 + 0.5 * np.linalg.pinv(J) @ (target - fk(q2))
print("distance from rest without it:", round(float(np.linalg.norm(q2 - q_rest)), 3), "; the primary task is solved equally well in both")

Printed (checked by the site’s tests):

end-effector error (m): 0.0
distance of the posture from rest with the secondary task: 0.406
distance from rest without it: 1.386 ; the primary task is solved equally well in both

Reading it: a three-joint planar arm must put its tip at (0.6, 0.6) m; since three joints exceed the two task dimensions, a secondary task of staying near a rest posture is added in the null space. Both runs reach the target (tip error 0.0), but the posture ends at 0.406 from the rest posture with the secondary task against 1.386 without it. The arm and numbers are illustrative.

8. Real examples

Mobile manipulator: the hand follows a path (priority 1) while the base posture and elbow stay away from limits (priority 2).

9. Failure scenarios

A secondary task written outside the null space that fights the primary one.
Pseudo-inverse near a singularity, producing huge joint velocities.
Ignoring torque limits at the inverse-dynamics level.

10. Troubleshooting

Check that the primary task error is unchanged when the secondary task is switched on.
Look at the singular values of the Jacobian.
Check the solver’s time per step.

11. Validation

The example prints the lines shown (executed by the site tests).
The primary task error stays within tolerance with all lower-priority tasks active.

13. Further references

TSID README: stack-of-tasks/tsid (a C++ library for optimization-based inverse-dynamics control built on Pinocchio), read 2026-11-06.
Null-space projection is standard robotics mathematics (see robotics textbooks); the specific formulations of whole-body controllers differ by library and were not read in depth here.