[WBC 04] - Task Priority: Nullspace Projection vs Weighted QP vs HQP

A comparison of three common ways to handle task priority in Whole-Body Control: nullspace projection, weighted QP, and hierarchical QP.

WBC04. Task Priority: Nullspace Projection vs Weighted QP vs HQP

In the previous post, we looked at how an SE(3) task becomes an acceleration-level constraint.

A pose target is not sent directly to the WBC solver. The task computes pose error, velocity error, desired task acceleration, Jacobian, and drift. Then it gives the solver a relation like:

J_task(q) qddot = a_des - drift

That works when we look at one task.

But whole-body control is not about one task.

A humanoid may need to control several objectives at the same time:

center of mass
base orientation
left foot contact
right foot contact
hand pose
posture
joint limits
torque limits
contact forces

All of these tasks ultimately depend on the same robot motion.

They share the same generalized acceleration:

qddot

So the next problem is unavoidable:

What happens when tasks conflict?

If the hand wants to move forward, but doing so would disturb balance, which task should win?

If a posture task wants to straighten the knee, but the foot contact constraint requires the knee to stay bent, which objective should be preserved?

This is where task priority becomes central.

There are several ways to represent priority in WBC. In this post, we compare three common approaches:

1. Weighted QP
2. Nullspace projection
3. Hierarchical QP

These methods are often presented as mathematical alternatives, but they also lead to different software architectures.

The key question is not simply:

Which one is better?

The better question is:

What kind of priority guarantee do we need, and what constraints must the controller handle?

---

1. The task conflict problem

Suppose we have two acceleration-level tasks:

J1 qddot = a1
J2 qddot = a2

If there exists a qddot that satisfies both equations, life is easy.

But in whole-body control, that is often not the case.

The robot has finite degrees of freedom. Contacts restrict motion. Torque limits restrict feasible acceleration. Friction constraints restrict contact forces. Some tasks may simply be incompatible at a given instant.

For example:

Task 1:
  keep the center of mass stable

Task 2:
  move the hand to a far target

Task 3:
  keep posture close to nominal

If the hand target is too far, the robot may not be able to reach it without moving the CoM or violating a contact constraint.

So WBC needs a rule for resolving conflicts.

There are two broad philosophies:

Soft priority:
  important tasks get larger weights, but all tasks are optimized together.

Strict priority:
  lower-priority tasks are only allowed to use motion that does not disturb higher-priority tasks.

Weighted QP belongs to the first category.

Nullspace projection and HQP are closer to the second category.

---

2. Weighted QP: priority as cost weight

The most straightforward approach is to put all task errors into one optimization problem.

For example:

minimize over qddot

  w1 || J1 qddot - a1 ||²
+ w2 || J2 qddot - a2 ||²
+ w3 || J3 qddot - a3 ||²

Here, task priority is represented by weights.

If task 1 is more important than task 2, we choose:

w1 >> w2

In a full inverse-dynamics WBC formulation, this objective may be combined with dynamics, torque limits, friction cones, and contact force constraints.

The software structure is relatively simple:

solver.addCost(com_task, weight_com);
solver.addCost(hand_task, weight_hand);
solver.addCost(posture_task, weight_posture);

solver.addConstraint(dynamics);
solver.addConstraint(contact_constraints);
solver.addConstraint(torque_limits);

solution = solver.solve();

This is one reason weighted QP is attractive.

Everything goes into one QP.

Tasks are costs. Physical limits are constraints. The solver returns the best compromise.

Advantages

Weighted QP is easy to formulate and implement compared to strict hierarchy methods.

It also handles inequality constraints naturally. Torque limits, friction cones, unilateral contact constraints, and joint limits can be added as constraints in the same optimization problem.

tau_min <= tau <= tau_max
friction_cone(lambda) >= 0
joint_limit_lower <= q <= joint_limit_upper

This makes weighted QP practical for real robot systems.

It is also flexible. If a task should matter more, increase its weight. If a posture objective should be soft, give it a lower weight.

Limitations

The main limitation is that weights do not create strict priority.

A large weight means:

This task is expensive to violate.

It does not mean:

This task can never be violated by lower-priority tasks.

If all tasks are in the same objective, then the optimizer can still trade errors between them.

For example, a lower-priority hand task can slightly disturb a higher-priority balance task if the cost tradeoff allows it.

This is not always bad. In some systems, a soft compromise is exactly what we want.

But it is not the same as hierarchy.

Weighted QP also introduces tuning problems.

Weights depend on units, task dimensions, numerical scaling, and solver conditioning.

A position task measured in meters, an orientation task measured in radians, a force task measured in newtons, and a momentum task do not naturally live on the same scale.

So a weight is not just a priority number.

It is also a numerical scaling parameter.

This can make tuning difficult.

Summary

Weighted QP represents priority through the objective function.

Priority = cost weight

It is simple, flexible, and constraint-friendly.

But priority is soft.

---

3. Nullspace projection: priority as motion subspace

Nullspace projection takes a different view.

Instead of putting all tasks into one cost, it solves tasks in priority order.

First, solve the high-priority task.

Then, solve the lower-priority task only in the null space of the higher-priority task.

For a first task:

J1 qddot = a1

a simple solution is:

qddot1 = J1# a1

where J1# is some generalized inverse.

The nullspace projector is:

N1 = I - J1# J1

Any motion inside N1 does not affect task 1.

So a second task can be added as:

qddot = qddot1 + N1 z

where z is chosen to reduce task 2 error.

A common form is:

qddot2 =
qddot1
+ N1 (J2 N1)# (a2 - J2 qddot1)

The exact expression depends on the chosen inverse and dynamics consistency, but the idea is the same:

Task 2 is allowed to use only the motion directions that do not disturb task 1.

For multiple tasks, the process continues recursively.

In software, nullspace projection often looks like a sequential algorithm:

qddot = solveFirstTask(task1);

N = computeNullspace(task1);

qddot += solveInNullspace(task2, N);

N = updateNullspace(N, task2);

qddot += solveInNullspace(task3, N);

Advantages

The main advantage is strict task hierarchy.

If implemented correctly, a lower-priority task cannot change the solution of a higher-priority task.

This is very attractive for WBC.

For example:

contact constraint
→ center of mass
→ hand task
→ posture

The hand task should not break contact. The posture task should not disturb the hand task. Redundancy is used only after higher-priority objectives are satisfied.

Nullspace projection also has a clear physical interpretation.

It says:

Use the remaining motion freedom after satisfying more important objectives.

This matches the classical operational-space control view of task and posture decomposition.

It can also be computationally efficient, especially when the task dimensions are moderate and the structure is exploited well.

Limitations

The main limitation is inequality constraints.

Many real robot constraints are inequalities:

normal force >= 0
friction cone satisfied
tau_min <= tau <= tau_max
joint position within limits
contact wrench inside support region

Nullspace projection handles equality tasks naturally. It is less natural for inequalities.

This becomes a serious issue for dynamic legged robots.

Contact is not simply:

foot acceleration = 0

It also requires feasible contact force.

The foot cannot pull the ground. Tangential force must remain inside the friction cone. Reaction forces must stay within feasible bounds.

If the controller treats contact only as an equality constraint, it may produce a mathematically consistent acceleration but a physically infeasible contact force.

That is why pure nullspace projection can be difficult to use as the full solution for modern humanoid WBC.

Another limitation is numerical robustness. Nullspace projectors depend on ranks, singular values, and generalized inverses. When a task becomes near-singular, the projector can become sensitive.

Summary

Nullspace projection represents priority through subspace restriction.

Priority = allowed motion subspace

It gives strict hierarchy and a clean interpretation.

But inequality constraints are difficult to incorporate directly.

---

4. Hierarchical QP: priority as optimization hierarchy

Hierarchical Quadratic Programming, or HQP, tries to combine two ideas:

strict priority
+
optimization with constraints

Instead of solving one weighted QP, HQP solves a sequence of optimization problems.

At the first level, it solves the highest-priority task:

Level 1:
  minimize || J1 qddot - a1 ||²
  subject to constraints

At the second level, it solves the next task while preserving the optimal result of level 1:

Level 2:
  minimize || J2 qddot - a2 ||²
  subject to
    level 1 optimality preserved
    constraints

Then level 3 does the same for the next task.

Level 3:
  minimize || J3 qddot - a3 ||²
  subject to
    level 1 and 2 optimality preserved
    constraints

In software, the structure may look like:

level1.addTask(contact_task);
level1.addConstraint(dynamics);
level1.addConstraint(torque_limits);
sol1 = level1.solve();

level2.addTask(com_task);
level2.addConstraint(preserve(level1));
sol2 = level2.solve();

level3.addTask(hand_task);
level3.addConstraint(preserve(level2));
sol3 = level3.solve();

There are different ways to implement the preservation of previous optimality, often involving slack variables, equality constraints on previous optimal costs, or nullspace formulations.

The important idea is:

Lower-priority tasks are optimized only after higher-priority tasks have already claimed their solution quality.

Advantages

HQP gives a principled way to express strict priority.

Unlike weighted QP, priority is not just a large number.

A lower-priority task cannot improve itself by sacrificing a higher-priority task beyond the allowed relaxation.

HQP can also include inequality constraints.

This is a major advantage over pure nullspace projection.

Torque limits, friction cones, contact force bounds, and joint limits can be represented as optimization constraints.

So HQP is conceptually very suitable for humanoid WBC:

strict task hierarchy
+
physical inequality constraints
+
optimization-based feasibility handling

Limitations

The main limitation is complexity.

HQP is more difficult to implement than a single weighted QP.

It may require multiple QP solves or a more sophisticated solver structure. It also needs careful treatment of infeasibility, slack variables, numerical tolerances, and rank conditions.

This matters for real-time control.

Humanoid WBC may need to run at hundreds of Hz or around 1 kHz. If each control tick requires several optimization problems, computation time becomes a serious design constraint.

HQP also increases software complexity.

The controller must manage:

priority levels
task grouping
slack variables
constraint inheritance
optimality preservation
solver warm-starting
numerical tolerances
fallback behavior

This is not impossible. Many humanoid systems use HQP-style methods successfully.

But it is not a small abstraction.

Summary

HQP represents priority through optimization levels.

Priority = optimization hierarchy

It can provide strict hierarchy and handle inequalities.

But it is more complex and can be computationally expensive.

---

5. Why inequality constraints change the story

The difference between these methods becomes much sharper once contact constraints enter the problem.

For a fixed-base manipulator doing only kinematic task tracking, nullspace projection can be very clean.

But humanoids are not only tracking motions.

They must satisfy contact and force feasibility.

For a standing robot, contact constraints include at least:

no slip
no pulling on the ground
contact wrench inside feasible region
normal force within reasonable bounds
torque limits respected

These are not just nice-to-have conditions.

They define whether the motion is physically possible.

For example, suppose the controller computes a base or CoM acceleration that requires a contact force outside the friction cone.

The task acceleration may look valid in kinematics.

But the robot cannot realize it without slipping.

This is where weighted QP and HQP have an advantage: inequality constraints fit naturally inside optimization.

Nullspace projection can enforce motion constraints elegantly, but force inequalities require additional machinery.

So the real distinction is not just mathematical style.

It is about what the controller must guarantee.

If the main issue is task redundancy:
  nullspace projection is attractive.

If the main issue is physical feasibility under inequalities:
  QP-based methods become attractive.

If both strict hierarchy and inequalities matter:
  HQP becomes attractive.

This is why there is no universally best formulation.

The right method depends on the robot, the task, the required control rate, and the kind of constraints that must be enforced.

---

6. Software architecture differences

These formulations lead to different code structures.

Weighted QP architecture

In a weighted QP, tasks are usually added as costs:

solver.addCost(com_task, weight_com);
solver.addCost(hand_task, weight_hand);
solver.addCost(posture_task, weight_posture);

solver.addConstraint(dynamics);
solver.addConstraint(friction_cone);
solver.addConstraint(torque_limit);

solution = solver.solve();

The solver owns the tradeoff.

The task object provides:

A matrix
target vector
weight

The priority semantics are mostly encoded in weights.

This is easy to implement, but harder to reason about when tasks conflict.

---

Nullspace projection architecture

In nullspace projection, task execution is sequential:

qddot = solveTask(task1);
N = computeNullspace(task1);

qddot += solveTaskInNullspace(task2, N);
N = updateNullspace(N, task2);

qddot += solveTaskInNullspace(task3, N);

The controller owns the hierarchy.

The task object provides:

Jacobian
desired acceleration
priority order

The solver may be lightweight, but the nullspace logic becomes central.

This is clean for strict equality-task hierarchy, but contact inequalities require additional handling.

---

HQP architecture

In HQP, tasks are grouped by priority levels:

hqp.level(0).addTask(contact_or_balance);
hqp.level(0).addConstraint(dynamics);
hqp.level(0).addConstraint(torque_limits);

hqp.level(1).addTask(com_task);
hqp.level(1).preserveHigherPriority();

hqp.level(2).addTask(hand_task);
hqp.level(2).preserveHigherPriority();

solution = hqp.solve();

The hierarchy is represented explicitly in the optimization structure.

The task object provides:

Jacobian
desired acceleration
priority level
optional slack behavior

The solver must manage priority preservation and constraints across levels.

This is powerful, but the software stack becomes more involved.

---

7. Comparison summary

Method	Priority type	Inequality constraints	Main advantage	Main limitation
Weighted QP	Soft priority through weights	Natural	Simple and flexible	No strict priority guarantee
Nullspace Projection	Strict priority through motion subspaces	Difficult	Clear hierarchy and efficient execution	Hard to handle friction cones, torque limits, unilateral contacts directly
HQP	Strict priority through optimization levels	Natural	Strict hierarchy with constraints	More complex and potentially expensive

A shorter way to remember it:

Weighted QP:
  easy to formulate, hard to guarantee priority

Nullspace Projection:
  easy to interpret hierarchy, hard to handle inequalities

HQP:
  expressive and principled, harder to implement in real time

---

8. Which one should a WBC codebase support?

For a software architecture project, the answer should not be hard-coded too early.

Different controllers may need different priority mechanisms.

A manipulation controller with soft objectives may be fine with a weighted QP.

A classical operational-space controller may benefit from nullspace projection.

A humanoid controller with strict contact, torque, and task hierarchy may need HQP or a related prioritized optimization method.

So the architecture should separate:

Task representation
Priority representation
Constraint representation
Solver backend
Command adapter

A Task should not assume whether it will be used inside a weighted QP, nullspace projection controller, or HQP solver.

At minimum, a task should expose:

Jacobian
desired acceleration
task dimension
mask
weight
priority level
frame convention

Then the solver backend can decide how to use it.

For example:

TaskSE3Equality hand_task;
TaskCoM com_task;
TaskPosture posture_task;

solver.addTask(com_task);
solver.addTask(hand_task);
solver.addTask(posture_task);

The same task objects could be interpreted differently:

Weighted QP:
  use task.weight()

Nullspace projection:
  use task.priority()

HQP:
  group tasks by priority level

This is the software architecture point.

The task should describe the control objective.

The solver should decide how priority is enforced.

---

9. Summary

Task priority is one of the central problems in WBC.

Once multiple tasks share the same robot body, conflicts are inevitable. The controller must decide which objectives are preserved and which objectives are allowed to degrade.

Weighted QP, nullspace projection, and HQP answer this question differently.

Weighted QP puts all tasks into one optimization and uses weights to shape the tradeoff.

priority = cost weight

Nullspace projection solves tasks sequentially and projects lower-priority tasks into the nullspace of higher-priority tasks.

priority = allowed motion subspace

HQP solves a sequence of optimization problems that preserve higher-priority optimality while optimizing lower-priority objectives.

priority = optimization hierarchy

None of these is universally best.

Weighted QP is simple and handles inequalities naturally, but it does not provide strict hierarchy.

Nullspace projection gives a clean strict hierarchy, but inequality constraints are difficult to incorporate directly.

HQP can handle both hierarchy and inequalities, but it increases implementation and computational complexity.

For real WBC software, the important decision is not only mathematical.

It is architectural.

A good WBC stack should separate task definition from priority enforcement. A task should define what it wants. The solver backend should decide how that task is combined with other tasks and constraints.

In the next post, we will look at the Task abstraction more directly: what a task object should own, what it should expose, and how it should stay independent of the solver backend.

References

• Y. Nakamura, H. Hanafusa, and T. Yoshikawa, “Task-Priority Based Redundancy Control of Robot Manipulators,” The International Journal of Robotics Research, vol. 6, no. 2, pp. 3–15, 1987.

• A. Escande, N. Mansard, and P.-B. Wieber, “Hierarchical Quadratic Programming: Fast Online Humanoid-Robot Motion Generation,” The International Journal of Robotics Research, vol. 33, no. 7, pp. 1006–1028, 2014.

• D. Kim, J. Lee, J. Ahn, O. Campbell, H. Hwang, and L. Sentis, “Computationally-Robust and Efficient Prioritized Whole-Body Controller with Contact Constraints,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018.