[WBC 04] - Combining Tasks into a Whole-Body Controller

The hand, center-of-mass, and posture tasks can each compute a valid acceleration request and still disagree about how the robot should move. They all depend on the same generalized acceleration $a=\dot v$:

WBC 03 built a TaskRequest(J, b) from a reference and model data. This post combines those requests. The task computes what it wants; the controller decides which requests may compromise and which requirements must hold. Null-space projection first shows how to preserve a primary output. Weighted QP adds physical constraints and trades tracking errors; HQP combines those constraints with explicit task levels.

Reach with the hand while keeping support

Talos starts in the half-sitting pose from WBC 01, with zero velocity and both feet fixed. A right-hand target 0.30 m forward with $K_p=100$ s$^{-2}$ requests $b_h=[30,0,0]^T$ m/s². The CoM reference and joint posture reference remain at their current values, so $b_{\rm com}=0$ and $b_{\rm posture}=0$.

The controller must reconcile the forward hand request with the stationary CoM and feet. The Talos examples below compare one offline control update at this state.

Null-space projection preserves the primary output

Null-space projection (NP) restricts a lower task to motion directions that leave the achieved primary output unchanged.

See the remaining motion on a small robot

A three-link planar arm has three joint velocities but only two endpoint position outputs. Away from singularities, it can change posture while keeping that endpoint fixed. The primary task is $J(q)\dot q=0$; the secondary request bends the third joint toward a posture reference.

The direct request $z$ moves the endpoint because $Jz\ne0$. Projecting it gives $\dot q=(I-J^\dagger J)z$, so $J\dot q=0$. Several joints now move together to cancel their effects on the endpoint. The endpoint orientation is free; only its position is held.

The 3D view transforms a whole lattice in joint-velocity coordinates $(\dot q_1,\dot q_2,\dot q_3)$, not just one arrow. First impose height alone:

A rank-one $J_y\in\mathbb R^{1\times3}$ removes one motion direction. Every sample maps into the two-dimensional plane $\ker J_y$. Adding the independent x-position requirement gives $J=[J_x;J_y]$ with rank two. Applying $N=I-J^\dagger J$ then collapses that plane onto the line $\ker J$. Since $\ker J\subset\ker J_y$, this second projection preserves the height requirement.

The faint cube marks the original 3D sample region. The green plane and line are allowed command spaces, not Cartesian surfaces. The height-only step explains the geometry; the robot trajectory holds both endpoint coordinates. Interpolated frames illustrate the transformation; the exact projector relation holds at each completed stage.

The second task asks for a posture velocity $z$. Its request can change, but its contribution to the first task is always zero after projection:

Here the primary target is stationary, so $b_1=0$ and $\dot q_{\rm primary}=0$. The two task panels show the changing posture request and its allowed component alongside the unchanged primary output. The allowed joint-3 velocity need not equal the requested one: the lower task gives up the component that would disturb the higher task.

This example uses velocity-level kinematics to show the null-space idea. The Jacobian and projector are recomputed during integration. Keeping only the initial projector would preserve the task at that initial configuration, not throughout a finite motion.

In software, the task supplies its Jacobian and target. The NP backend computes the remaining motion space and solves the lower task through its projected Jacobian. The task itself does not decide its priority.

Separate physical feasibility from tracking error

The arm example preserves an endpoint using joint velocities, without checking torque or contact-force limits. For the Talos acceleration tasks, the controller must also find compatible joint torques and ground contact forces. Its decision vector can include all three:

For the generic formulation, $\lambda$ stacks contact wrenches in the same frames and order as the rows of $J_c$. A motion task becomes a residual over this larger vector:

Dynamics and maintained stationary contacts supply hard equalities:

$S$ selects actuated velocity coordinates. Torque bounds and admissible contact wrenches further restrict the feasible set. A contact model can impose unilateral force, friction, and center-of-pressure limits. A QP uses linear inequalities, including a friction-pyramid approximation to the circular Coulomb cone.

In the computed comparison, the solver uses $y=[a;\tau;f]\in\mathbb R^{94}$ and substitutes $\lambda=Ff$. Each sole has four support points at $(\pm0.10,\pm0.065,0)$ m. The common feasible set $\mathcal F$ imposes:

• All 38 rigid-body dynamics rows and 12 stationary-contact acceleration rows.
• Unscaled URDF torque bounds and actuated acceleration bounds $|Sa|\le10$ rad/s².
• At each support point, $f_z\ge0$, $|f_x|\le0.3f_z$, and $|f_y|\le0.3f_z$ in sole-local axes.
• Total normal force between 5 and 1000 N per foot.

These friction-pyramid inequalities are a model approximation. Joint position prediction, collisions, contact changes, and future feasibility are outside this single-state comparison. Because $v=0$, the acceleration drift terms vanish at this snapshot.

Tracking a hand reference can remain a soft cost. Keeping a contact stationary is a hard requirement for this chosen contact mode. A CoM request may be hard, soft, or prioritized depending on the controller design. Its name alone does not determine how it enters the solver.

Joint limits also need a constraint on the decision variables. At a fixed state, $q$ is already known; writing $q_{\min}\le q\le q_{\max}$ does not constrain the current acceleration solve. One approach bounds predicted joint position and velocity using $a$ and the control period. These bounds require a chosen prediction model and do not by themselves guarantee safety for all future motion.

Weighted QP trades task errors within physical limits

A weighted controller minimizes tracking residuals inside the feasible set:

$E,e$ collect hard equalities; $G,g$ collect linear inequality bounds. The solver can trade one soft task’s error against another’s. Increasing the CoM weight makes CoM error more expensive, but does not give it strict priority over the hand task.

The same expression takes the standard QP form:

Each task contributes a matrix and a vector to the shared problem. Because $H\succeq0$, the cost is convex. The affine constraints define a convex feasible set. Any local minimum is global, although a feasible minimizer need not be unique or even exist for every convex QP. See Boyd and Vandenberghe, §4.4.

The Talos weighted objective is

For this example, translation residuals use a 1 m/s² scale and the posture residual uses a 10 rad/s² scale. These scales and weights are illustrative controller settings.

The plot shows the cost on a two-dimensional slice of the actual 94-variable problem. Its coordinates $u,v$ combine two directions $d_u,d_v$ from a static-reference solution $y_s$:

Every point in this plane satisfies the hard equalities. The polygon adds the inequalities; the contours connect points with the same tracking cost. The weighted solution lies at $(u,v)=(1,0)$. The solver searches all 94 variables, while the plot shows this one slice.

For implementation, each task contributes $w_iA_i^TA_i$ to $H$ and $-w_iA_i^Tb_i$ to $c$. The assembler adds those contributions and passes one cost and the hard constraints to the QP backend.

The weighted result requests 17.946 m/s² of forward hand acceleration and leaves a 0.083 m/s² CoM acceleration norm. A torque bound is active. The hand’s full three-component acceleration residual is 12.506 m/s².

Gains, weights, and priority levels act at different stages:

Weights also depend on scaling. A metre of position error and a radian of orientation error are different quantities. Choose residual scales before interpreting weight ratios; increasing weights across many orders of magnitude can worsen numerical conditioning.

HQP preserves task results across optimization levels

Weighted QP allows soft tasks to compromise. Hierarchical quadratic programming (HQP) preserves higher-level results through successive optimization levels. Each level improves its own objective while preserving higher-level results and the common hard constraints. It can handle both equality and inequality objectives; see Escande, Mansard, and Wieber (2014).

For the equality least-squares tasks used here, a simplified two-level construction is

$\mathcal F$ is the common feasible set. Preserving $A_1y_1$ preserves the optimal first-level residual, including when it is nonzero. Fixing the entire vector $y=y_1$ would remove the remaining freedom needed by the second task. Hierarchical inequality objectives need their own slack and active-constraint treatment; the equality formula above does not cover that case.

This preserves the same quantity as NP: the achieved primary output. NP restricts the correction to a null space; HQP constrains the next solution to keep $A_1y$ unchanged.

For this Talos snapshot, the three levels minimize CoM acceleration, hand acceleration error, and scaled posture acceleration, respectively. All levels retain the same $\mathcal F$ used by the weighted QP.

The posture level preserves all three achieved CoM and hand outputs. It cannot materially improve posture in this saturated snapshot. The two constrained formulations produce:

HQP preserves the first level’s CoM output within numerical tolerance. Weighted QP accepts a small CoM error to reduce hand error. The planar NP example illustrates output preservation with a different robot and velocity-level model; its numerical results are not part of this Talos comparison.

An HQP backend consumes tasks grouped by level. After each solve, it records the achieved task outputs and adds their preservation equalities to subsequent levels. The task evaluation code remains unchanged.

Several tasks can share a level and use weights within it. The distinction is between tradeoffs within a level and preservation between levels. Solver names alone do not establish which hierarchy is implemented.

In particular, the TSID recordings in WBC 01 use hard constraints and one weighted soft level. Their hand and posture costs trade off within that level. They do not demonstrate a separate strict priority level for every task.

Assemble the problem outside the task

Keep the TaskRequest(J, b) from 03 unchanged. The task owns its reference, gains, and selected axes. The controller configuration assigns its request a weight, a priority level, or hard-constraint treatment. The problem assembler maps it into solver coordinates.

The same request enters a different assembly operation depending on the configured policy:

The following illustrative Python function assembles the tracking cost for one weighted level. Its input is a list of (TaskRequest, weight) pairs; the task objects do not own those weights.

import numpy as np

def tracking_cost(weighted_requests, nv, na, nlambda):
    ny = nv + na + nlambda
    H = np.zeros((ny, ny))
    c = np.zeros(ny)
    for request, weight in weighted_requests:
        A = np.zeros((request.J.shape[0], ny))
        A[:, :nv] = request.J
        H += weight * (A.T @ A)
        c -= weight * (A.T @ request.b)
    return H, c

This assumes nonnegative validated weights and matching request dimensions. The contact representation determines nlambda. For the stacked wrench convention above, it is six times the number of contacts. A point-force parameterization needs the corresponding force-generator mapping instead.

One control update then follows this sequence:

one state snapshot
    → update model once
    → evaluate active tasks and contacts
    → assign configured weights, priority levels, and hard requirements
    → assemble and solve with the selected backend
    → obtain motion commands (and torques / contact forces for inverse dynamics)
    → accept a valid feasible result or enter the chosen failure path

For a weighted backend, the assembler passes $H,c,E,e,G,g$ to one QP. An HQP backend also needs explicit level grouping and preservation of higher-level results. The equality-only null-space method works with projected task Jacobians and needs additional machinery for physical inequalities.

The assembly contract must also specify the solver’s objective convention. The code above assumes $\frac12y^THy+c^Ty$; omitting the half changes the required Hessian scaling. Frames, variable ordering, and constraint signs must agree across every block.

A solver status, finite outputs, and constraint residuals determine whether the controller accepts a result. If the hard feasible set is empty, changing a tracking weight cannot repair it. The application needs an explicit failure response and a deliberate policy for which requirements, if any, may be relaxed.