Whole-Body Control with Task Priorities

A humanoid has to do many things at the same time: keep its balance, reach with its hand, look at the target, and avoid joint limits. Whole-body control stacks these tasks in priority order. Each lower task can only use the motion left over after the higher tasks are satisfied, so when they conflict, the less important one gives way.

Priority order Elbow height target

Drag the red hand target.

Stacked tasks

Each task $k$ has a Jacobian $J_k$ and a desired velocity $\dot{\mathbf{x}}_k$. Task 1 is solved first. Task 2 is solved only inside the null space of task 1, $N_1 = I - J_1^{+}J_1$, so it cannot disturb task 1, and so on down the stack:

$$\dot{\mathbf{q}} = J_1^{+}\dot{\mathbf{x}}_1 + N_1\,(J_2 N_1)^{+}\big(\dot{\mathbf{x}}_2 - J_2 J_1^{+}\dot{\mathbf{x}}_1\big) + \dots$$

Real whole-body controllers often solve the same idea as a small quadratic program each cycle, which also handles limits and contact forces.

What to notice

With the hand first, drag the target and the hand follows exactly, while the elbow keeps to its height only when it can. Move the target so both cannot be met, and the elbow task gives way. Swap the priorities and now the elbow holds its height no matter what, while the hand falls short. Posture, last in line, only uses what freedom remains, keeping the arm in a natural pose.