In whole-body control (WBC) we typically have a generalized decision vector \( \mathbf{z} \in \mathbb{R}^m \) collecting variables such as joint accelerations, contact forces, and possibly torques. Using models from earlier chapters, equality and inequality constraints encode:

• Rigid-body dynamics and actuation limits,
• Contact consistency and friction cone constraints,
• Joint and workspace limits.

On top of these constraints we place tasks such as center-of-mass (CoM) tracking, end-effector tracking, posture regulation, and regularization of torques. Let the linearized error for task \( k \) be \( \mathbf{e}_k(\mathbf{z}) \approx \mathbf{J}_k \mathbf{z} - \mathbf{b}_k \), where \( \mathbf{J}_k \) and \( \mathbf{b}_k \) are obtained from Jacobians and desired trajectories as introduced in earlier lessons.

Two main philosophies are used in WBC:

1. Strict prioritization (hierarchical control): define a stack of tasks ranked by importance, for example (1) physical consistency and safety, (2) balance, (3) end-effector tracking, (4) posture. Higher levels must be satisfied exactly (if feasible) before lower levels are considered.
2. Weighted optimal control: build a single cost that sums task errors with positive weights, allowing all tasks to trade off against each other within constraints.

The following flow diagram summarizes the high-level decision between these philosophies:

flowchart TD
  T["Define tasks: \nbalance, end-effector, \nposture, limits"] --> C1["Choose strict priorities"]
  T --> C2["Choose weighted \noptimal control"]
  C1 --> P1["Guarantee: high-priority \ntasks respected \nwhen feasible"]
  C1 --> P2["Lower tasks only \nuse remaining redundancy"]
  C2 --> P3["Single QP with \nweights for all tasks"]
  C2 --> P4["High-priority tasks \nmay be slightly violated \nfor better overall performance"]
  P1 --> D["Often preferred \nin safety-critical \nwhole-body behaviors"]
  P4 --> E["Often preferred when \ntracking quality and \nsmoothness dominate"]
        

In the rest of this lesson we formalize both approaches and quantify their trade-offs in terms of feasibility, optimality, and computational cost.

We denote the robot generalized coordinates by \( \mathbf{q} \in \mathbb{R}^n \) and velocities \( \dot{\mathbf{q}} \), which you studied in kinematics and dynamics. Whole-body controllers commonly use a decision vector

\[ \mathbf{z} = \begin{bmatrix} \ddot{\mathbf{q}} \\ \mathbf{f}_c \\ \mathbf{u} \end{bmatrix} \in \mathbb{R}^m, \]

where \( \ddot{\mathbf{q}} \) are joint accelerations, \( \mathbf{f}_c \) the stacked contact forces, and \( \mathbf{u} \) may include joint torques \( \mathbf{u} = \mathbf{S}^\top \tau \) or other control inputs. The exact structure of \( \mathbf{z} \) is fixed by the WBC design and the manipulator model already developed in earlier chapters.

We assume linear (or linearized) constraints of the form

\[ \mathbf{A} \mathbf{z} = \mathbf{b}, \qquad \mathbf{G} \mathbf{z} \leq \mathbf{h}, \]

where equality constraints encode dynamics and holonomic contacts, and inequalities encode torque limits, friction cones, and unilateral contacts.

Task \( k \) is represented as a quadratic cost

\[ J_k(\mathbf{z}) = \frac{1}{2} \left\| \mathbf{J}_k \mathbf{z} - \mathbf{b}_k \right\|_{\mathbf{W}_k}^2 = \frac{1}{2} \left( \mathbf{J}_k \mathbf{z} - \mathbf{b}_k \right)^\top \mathbf{W}_k \left( \mathbf{J}_k \mathbf{z} - \mathbf{b}_k \right), \]

where \( \mathbf{W}_k \succ 0 \) is a positive-definite weighting matrix that can emphasize directions within the task space (for example, penalizing normal motion more than tangential).

With these ingredients we can now define weighted optimal WBC and hierarchical (lexicographic) WBC.

In weighted optimal WBC we aggregate all task costs into a single quadratic objective with nonnegative scalar weights \( \alpha_k > 0 \):

\[ \min_{\mathbf{z}} \; J(\mathbf{z}) = \sum_{k=1}^N \alpha_k J_k(\mathbf{z}) + \frac{\lambda}{2} \|\mathbf{z}\|^2 \quad \text{subject to } \mathbf{A} \mathbf{z} = \mathbf{b},\; \mathbf{G} \mathbf{z} \leq \mathbf{h}. \]

Expanding the quadratic terms yields the standard QP form

\[ \min_{\mathbf{z}} \; \frac{1}{2} \mathbf{z}^\top \mathbf{H} \mathbf{z} + \mathbf{g}^\top \mathbf{z} \quad \text{subject to } \mathbf{A} \mathbf{z} = \mathbf{b},\; \mathbf{G} \mathbf{z} \leq \mathbf{h}, \]

where

\[ \mathbf{H} = \sum_{k=1}^N \alpha_k \mathbf{J}_k^\top \mathbf{W}_k \mathbf{J}_k + \lambda \mathbf{I}, \qquad \mathbf{g} = -\sum_{k=1}^N \alpha_k \mathbf{J}_k^\top \mathbf{W}_k \mathbf{b}_k. \]

Properties:

• If \( \mathbf{H} \succ 0 \) and the feasible set defined by \( \mathbf{A},\mathbf{G} \) is nonempty, there exists a unique optimal solution \( \mathbf{z}^\star \).
• Increasing a weight \( \alpha_k \) shrinks the task error \( \mathbf{J}_k \mathbf{z} - \mathbf{b}_k \), but may increase errors in lower-weight tasks due to coupling through \( \mathbf{z} \).
• Soft-constraint tasks (e.g., posture, torque regularization) often use moderate weights, whereas safety-related tasks (e.g., joint limits, CoM support) may be assigned very large weights to approximate hard priorities.

The main advantage of the weighted approach is that only a single QP is solved per control cycle. The disadvantage is that there is no strict guarantee that a task is exactly satisfied, even if this would be feasible, because its importance is mediated by finite weights.

In strictly prioritized whole-body control we group tasks into priority levels \( \ell = 1,\dots,L \). Level 1 typically includes hard physical consistency constraints (dynamics, contacts), which we already place into \( \mathbf{A},\mathbf{G} \). Higher levels encode progressively softer objectives (CoM, end-effector, posture, etc.).

Let \( \mathcal{J}_\ell \) be the set of tasks at priority level \( \ell \) and define the level cost

\[ J^{(\ell)}(\mathbf{z}) = \sum_{k \in \mathcal{J}_\ell} J_k(\mathbf{z}). \]

The lexicographic (hierarchical) problem is

\[ \begin{aligned} &\text{Level 1:}& &\min_{\mathbf{z}} \; J^{(1)}(\mathbf{z}) \quad \text{subject to } \mathbf{A} \mathbf{z} = \mathbf{b}, \; \mathbf{G} \mathbf{z} \leq \mathbf{h}, \\ &\text{Level 2:}& &\min_{\mathbf{z}} \; J^{(2)}(\mathbf{z}) \quad \text{subject to } \mathbf{A} \mathbf{z} = \mathbf{b},\; \mathbf{G} \mathbf{z} \leq \mathbf{h},\; J^{(1)}(\mathbf{z}) = J^{(1)}(\mathbf{z}^\star_1), \\ &\vdots & & \\ &\text{Level L:}& &\min_{\mathbf{z}} \; J^{(L)}(\mathbf{z}) \quad \text{subject to } \mathbf{A} \mathbf{z} = \mathbf{b},\; \mathbf{G} \mathbf{z} \leq \mathbf{h},\\ & & & J^{(\ell)}(\mathbf{z}) = J^{(\ell)}(\mathbf{z}^\star_\ell), \; \ell=1,\dots,L-1. \end{aligned} \]

Here \( \mathbf{z}^\star_\ell \) is the minimizer obtained at level \( \ell \). This is a lexicographic optimization: the vector of level costs \( (J^{(1)},J^{(2)},\dots,J^{(L)}) \) is minimized in dictionary order.

In practice, implementing exact constraints \( J^{(\ell)}(\mathbf{z}) = J^{(\ell)}(\mathbf{z}^\star_\ell) \) is numerically inconvenient. A common approach (introduced in redundancy resolution in task space) uses a null-space projector for equality tasks \( \mathbf{J}_1 \mathbf{z} = \mathbf{b}_1 \) at the highest level:

\[ \begin{aligned} &\text{Level 1 solution:}& &\mathbf{z}_1^\star = \mathbf{J}_1^+ \mathbf{b}_1 + \left(\mathbf{I} - \mathbf{J}_1^+ \mathbf{J}_1\right)\mathbf{z}_{\text{reg}}, \\ &\text{Null-space projector:}& &\mathbf{P}_1 = \mathbf{I} - \mathbf{J}_1^+ \mathbf{J}_1, \end{aligned} \]

where \( \mathbf{J}_1^+ \) is the Moore–Penrose pseudoinverse and \( \mathbf{z}_{\text{reg}} \) is a regularization vector. Lower-priority tasks are then optimized over the null space of higher-priority tasks by writing \( \mathbf{z} = \mathbf{z}_1^\star + \mathbf{P}_1 \boldsymbol{\eta} \) and minimizing \( J^{(2)}(\mathbf{z}_1^\star + \mathbf{P}_1 \boldsymbol{\eta}) \) with respect to \( \boldsymbol{\eta} \), and so on.

Hierarchical QP (HQP) reformulates this projector approach as a sequence of QPs with additional equality constraints that enforce invariance of higher-priority solutions. This is numerically more robust in the presence of inequality constraints and bounds.

Intuitively, strict prioritization ensures that higher-level tasks are never sacrificed for lower ones, whereas weighted optimal control allows violation of any task if sufficiently beneficial for others. We now make this more precise for two levels (the argument generalizes).

5.1 Feasible sets and guarantees

Consider two task levels with costs \( J^{(1)}(\mathbf{z}) \) and \( J^{(2)}(\mathbf{z}) \). Define

\[ F = \left\{ \mathbf{z} \,\middle|\, \mathbf{A} \mathbf{z} = \mathbf{b},\; \mathbf{G} \mathbf{z} \leq \mathbf{h} \right\} \]

and assume \( F \) is nonempty.

The set of solutions of the level-1 problem is

\[ F_1^\star = \left\{ \mathbf{z} \in F \,\middle|\, J^{(1)}(\mathbf{z}) = \inf_{\mathbf{w} \in F} J^{(1)}(\mathbf{w}) \right\}. \]

The lexicographic solution belongs to \( F_1^\star \) by construction, whereas the weighted QP solution need not, because the aggregate cost may prefer a small increase in \( J^{(1)} \) if it significantly decreases \( J^{(2)} \).

Therefore, strict prioritization provides the guarantee

\[ J^{(1)}(\mathbf{z}_{\text{lex}}) = \inf_{\mathbf{w} \in F} J^{(1)}(\mathbf{w}), \]

whereas for the weighted solution \( \mathbf{z}_{\text{w}} \) we only know that

\[ \alpha_1 J^{(1)}(\mathbf{z}_{\text{w}}) + \alpha_2 J^{(2)}(\mathbf{z}_{\text{w}}) \leq \alpha_1 J^{(1)}(\mathbf{z}_{\text{lex}}) + \alpha_2 J^{(2)}(\mathbf{z}_{\text{lex}}), \]

with no guarantee that \( J^{(1)}(\mathbf{z}_{\text{w}}) = J^{(1)}(\mathbf{z}_{\text{lex}}) \).

5.2 Limit of large weights and approximate equivalence

Let us consider the two-level weighted cost

\[ J_\alpha(\mathbf{z}) = \alpha J^{(1)}(\mathbf{z}) + J^{(2)}(\mathbf{z}), \quad \alpha > 0, \]

with solution \( \mathbf{z}_\alpha \) to the constrained QP \( \min_{\mathbf{z} \in F} J_\alpha(\mathbf{z}) \). Under mild regularity conditions (strict convexity of \( J^{(1)} \) and \( J^{(2)} \) on \( F \)), we have the following classical result:

Proposition (informal). Suppose \( F_1^\star \) is nonempty and \( J^{(2)} \) has a unique minimizer over \( F_1^\star \), denoted \( \mathbf{z}_{\text{lex}} \). Then \( \mathbf{z}_\alpha \to \mathbf{z}_{\text{lex}} \) as \( \alpha \to \infty \).

Sketch of proof. For any \( \epsilon > 0 \) there exists \( \delta > 0 \) such that all points with \( J^{(1)}(\mathbf{z}) \leq J^{(1)}(\mathbf{z}_{\text{lex}}) + \delta \) lie in an \( \epsilon \)-neighborhood of \( F_1^\star \) (by continuity and strict convexity). If \( \mathbf{z} \in F \) violates this inequality, then \( J_\alpha(\mathbf{z}) - J_\alpha(\mathbf{z}_{\text{lex}}) \geq \alpha \delta - C \) for some constant \( C \) depending on \( J^{(2)} \). For sufficiently large \( \alpha \), such points cannot be optimal because they incur too much level-1 cost. Therefore, any minimizer \( \mathbf{z}_\alpha \) must lie close to \( F_1^\star \), and among those, the term \( J^{(2)} \) selects \( \mathbf{z}_{\text{lex}} \).

Practically, this means that very large ratios between the weights of consecutive priorities \( \alpha_1 \gg \alpha_2 \gg \dots \) can make a weighted QP numerically approximate a lexicographic solution, but:

• extremely large weights lead to ill-conditioned Hessians \( \mathbf{H} \), degrading numerical stability;
• exact equivalence is only achieved in the limit \( \alpha \to \infty \), which is not realizable in finite-precision arithmetic;
• when tasks conflict (no common solution for multiple levels), lexicographic and weighted formulations choose different compromise points.

Hence, there is a trade-off between structural guarantees and numerical conditioning that must be balanced in practical whole-body controllers.

The following diagram summarizes the computational pipeline for whole-body control and where prioritization vs optimality enters:

flowchart TD
  H["High-level tasks and priorities"] --> L["Linearize tasks: compute J_k and b_k"]
  L --> B["Assemble cost terms H and g"]
  B --> C["Add dynamics and contact constraints"]
  C --> S["Select scheme: weighted QP or hierarchical QPs"]
  S --> QP["Form one or several QP problems"]
  QP --> SOL["Call numerical QP solver"]
  SOL --> CMD["Send joint torques / accelerations to robot controller"]
        

Modern WBC implementations typically rely on specialized rigid-body dynamics libraries to build \( \mathbf{A},\mathbf{G},\mathbf{J}_k \) (e.g., Pinocchio or RBDL in C++/Python, Robotics System Toolbox in MATLAB), and generic QP solvers (e.g., OSQP, qpOASES, Gurobi) to solve the resulting optimization problems.

Strict hierarchical schemes usually solve several smaller QPs or apply null-space projectors, which may be beneficial when the highest-priority tasks involve only a subset of variables. However, lexicographic QPs can be more expensive per cycle than a single weighted QP, especially if many levels are used.

Weighted schemes solve a single QP per cycle, which is attractive for real-time constraints, but require careful manual tuning of weights and do not provide explicit guarantees for any individual task.

In this section we illustrate small building blocks for implementing weighted and prioritized task stacks in several languages. To stay focused on prioritization vs optimality, we assume that the Jacobians \( \mathbf{J}_k \) and vectors \( \mathbf{b}_k \) and constraints \( \mathbf{A},\mathbf{G},\mathbf{b},\mathbf{h} \) have already been assembled from the robot model.

7.1 Python Example (NumPy + QP Solver)

We demonstrate a 2-level scheme with a weighted QP and an approximate hierarchical solution based on a null-space projector. Popular Python toolchains for robotics include pinocchio for dynamics and qpsolvers or osqp for optimization.

import numpy as np

# Dimension of decision vector z (e.g., accelerations + contact forces)
m = 12

# Example Jacobians and desired task values
J1 = np.random.randn(6, m)   # high-priority task (e.g., balance)
b1 = np.zeros(6)
J2 = np.random.randn(3, m)   # lower-priority task (e.g., end-effector)
b2 = np.ones(3) * 0.1

W1 = np.eye(6)
W2 = np.eye(3)
lambda_reg = 1e-4

# -------- Weighted QP (single level with two tasks) --------
alpha1 = 1000.0  # high weight for task 1
alpha2 = 1.0     # lower weight for task 2

H = (alpha1 * J1.T @ W1 @ J1 +
     alpha2 * J2.T @ W2 @ J2 +
     lambda_reg * np.eye(m))
g = -(alpha1 * J1.T @ W1 @ b1 +
      alpha2 * J2.T @ W2 @ b2)

# Assume only simple bound constraints for illustration: lb <= z <= ub
lb = -np.ones(m) * 5.0
ub =  np.ones(m) * 5.0

# Example using qpsolvers interface (install via pip if needed)
from qpsolvers import solve_qp
z_weighted = solve_qp(H, g, A=None, b=None, lb=lb, ub=ub, solver="osqp")

# -------- Approximate hierarchical solution via null-space --------
# Level 1: minimize ||J1 z - b1||^2 + lambda_reg ||z||^2
H1 = J1.T @ J1 + lambda_reg * np.eye(m)
g1 = -J1.T @ b1
z1 = np.linalg.solve(H1, -g1)

# Null-space projector of J1
J1_pinv = np.linalg.pinv(J1, rcond=1e-6)
P1 = np.eye(m) - J1_pinv @ J1

# Level 2: optimize in the null space of task 1:
#   minimize ||J2 (z1 + P1 eta) - b2||^2 w.r.t eta
H2 = P1.T @ J2.T @ J2 @ P1 + lambda_reg * np.eye(m)
g2 = P1.T @ J2.T @ (J2 @ z1 - b2)
eta = np.linalg.solve(H2, -g2)
z_hier = z1 + P1 @ eta

print("Weighted solution residuals:")
print("  Task1:", np.linalg.norm(J1 @ z_weighted - b1))
print("  Task2:", np.linalg.norm(J2 @ z_weighted - b2))

print("Hierarchical solution residuals:")
print("  Task1:", np.linalg.norm(J1 @ z_hier - b1))
print("  Task2:", np.linalg.norm(J2 @ z_hier - b2))
      

7.2 C++ Example (Eigen + QP Solver + Robotics Library)

In a C++ whole-body controller we typically use:

• Eigen for linear algebra,
• pinocchio or RBDL for robot kinematics/dynamics,
• a QP solver such as qpOASES, OSQP, or ProxQP.

#include <Eigen/Dense>
// Include your QP solver headers here, e.g. qpOASES or OSQP

// Example: build H and g for a weighted QP from two tasks
void buildWeightedQP(const Eigen::MatrixXd& J1,
                     const Eigen::VectorXd& b1,
                     const Eigen::MatrixXd& J2,
                     const Eigen::VectorXd& b2,
                     double alpha1,
                     double alpha2,
                     Eigen::MatrixXd& H,
                     Eigen::VectorXd& g)
{
    const int m = J1.cols();
    Eigen::MatrixXd W1 = Eigen::MatrixXd::Identity(J1.rows(), J1.rows());
    Eigen::MatrixXd W2 = Eigen::MatrixXd::Identity(J2.rows(), J2.rows());
    double lambda_reg = 1e-6;

    H = alpha1 * J1.transpose() * W1 * J1
      + alpha2 * J2.transpose() * W2 * J2
      + lambda_reg * Eigen::MatrixXd::Identity(m, m);

    g = -(alpha1 * J1.transpose() * W1 * b1
        + alpha2 * J2.transpose() * W2 * b2);
}

// In your control loop:
// 1. Use Pinocchio/RBDL to compute J1, J2 from the current q, qdot.
// 2. Call buildWeightedQP.
// 3. Solve the QP and map the solution vector to accelerations, torques, etc.
      

7.3 Java Example (Linear Algebra + QP via ojAlgo)

While Java is less common in classical robotics control, libraries such as ojAlgo provide quadratic programming support, and EJML provides matrix operations. A typical pattern is to assemble Hessians and gradient terms similarly to Python/C++ and then call a generic QP solver.

import org.ojalgo.matrix.Primitive64Matrix;
import org.ojalgo.optimisation.Optimisation;
import org.ojalgo.optimisation.QuadraticSolver;
import org.ojalgo.optimisation.QuadraticSolver.Builder;

public class WholeBodyQP {

    public static Optimisation.Result solveWeightedQP(
            Primitive64Matrix H,
            Primitive64Matrix g,
            Primitive64Matrix Aeq,
            Primitive64Matrix beq) {

        Builder builder = QuadraticSolver.getBuilder();
        builder.quadratic(H);
        builder.linear(g);

        if (Aeq != null) {
            builder.equalities(Aeq, beq);
        }

        QuadraticSolver solver = builder.build();
        return solver.solve();
    }
}

// Usage (inside a control loop):
// 1. Fill H and g from your tasks (using the same formulas as in Python).
// 2. Call WholeBodyQP.solveWeightedQP(H, g, Aeq, beq).
// 3. Convert the solution vector into joint commands.
      

7.4 MATLAB / Simulink Example (Robotics System Toolbox + quadprog)

In MATLAB, the Robotics System Toolbox or custom dynamics code can generate Jacobians and constraints, and the Optimization Toolbox provides quadprog for QPs. A simple 2-level weighted QP looks like:

function z = whole_body_qp_step(J1, b1, J2, b2, Aeq, beq, lb, ub)
    m = size(J1, 2);
    W1 = eye(size(J1, 1));
    W2 = eye(size(J2, 1));
    alpha1 = 1e3;
    alpha2 = 1.0;
    lambda_reg = 1e-6;

    H = alpha1 * (J1' * W1 * J1) + ...
        alpha2 * (J2' * W2 * J2) + ...
        lambda_reg * eye(m);

    g = -(alpha1 * (J1' * W1 * b1) + ...
          alpha2 * (J2' * W2 * b2));

    options = optimoptions('quadprog', ...
                           'Algorithm','interior-point-convex', ...
                           'Display','off');

    z = quadprog(H, g, [], [], Aeq, beq, lb, ub, [], options);
end
      

In Simulink, this function can be called from a MATLAB Function block inside a high-frequency control loop, with robot state and task references supplied as inputs, and joint torques or accelerations as outputs.

7.5 Wolfram Mathematica Example (QuadraticOptimization)

Mathematica offers high-level convex optimization via QuadraticOptimization. The following snippet constructs a weighted WBC QP symbolically and solves it:

(* Dimensions *)
m = 12;
J1 = RandomReal[{-1, 1}, {6, m}];
b1 = ConstantArray[0.0, 6];
J2 = RandomReal[{-1, 1}, {3, m}];
b2 = ConstantArray[0.1, 3];

alpha1 = 1000.;
alpha2 = 1.;
lambdaReg = 10.^(-4);

H = alpha1 Transpose[J1].J1 +
    alpha2 Transpose[J2].J2 +
    lambdaReg IdentityMatrix[m];

g = -(alpha1 Transpose[J1].b1 +
      alpha2 Transpose[J2].b2);

(* Simple box constraints: -5 <= z_i <= 5 *)
vars = Array[z, m];
lb = ConstantArray[-5., m];
ub = ConstantArray[5., m];

zSol = QuadraticOptimization[
   1/2 vars.H.vars + g.vars,
   Thread[lb <= vars <= ub],
   vars
];

zSolVector = vars /. zSol;
      

This symbolic environment is useful for prototyping task weightings and exploring the impact of prioritization vs optimality in small models before deploying on real-time controllers.

Problem 1 (Feasible set and guarantees). Consider a two-level whole-body control problem with feasible set \( F \) as in Section 5 and strictly convex quadratic level-1 cost \( J^{(1)}(\mathbf{z}) \). Show that the lexicographic solution \( \mathbf{z}_{\text{lex}} \) is the unique solution of the constrained optimization problem

\[ \min_{\mathbf{z} \in F_1^\star} J^{(2)}(\mathbf{z}), \]

where \( F_1^\star \) is the set of level-1 minimizers defined in Section 5.

Solution. By definition of lexicographic optimization, level 1 first computes \( F_1^\star \), the set of minimizers of \( J^{(1)} \) on \( F \). Strict convexity of \( J^{(1)} \) on \( F \) implies that \( F_1^\star \) is nonempty and convex. At level 2, we minimize \( J^{(2)} \) subject to \( \mathbf{z} \in F \) and the additional condition \( J^{(1)}(\mathbf{z}) = J^{(1)}(\mathbf{z}_1^\star) \) for some \( \mathbf{z}_1^\star \in F_1^\star \). But this condition is equivalent to \( \mathbf{z} \in F_1^\star \) by definition, hence the level-2 optimization reduces exactly to \( \min_{\mathbf{z} \in F_1^\star} J^{(2)}(\mathbf{z}) \). If \( J^{(2)} \) is strictly convex on \( F_1^\star \), the minimizer is unique and equals \( \mathbf{z}_{\text{lex}} \).

Problem 2 (Limit of large weights). Let \( J_\alpha(\mathbf{z}) = \alpha J^{(1)}(\mathbf{z}) + J^{(2)}(\mathbf{z}) \) with \( \alpha > 0 \) and suppose the conditions of the proposition in Section 5.2 hold. Prove that any limit point of \( \mathbf{z}_\alpha \) as \( \alpha \to \infty \) must lie in \( F_1^\star \).

Solution. Assume by contradiction that a subsequence \( \mathbf{z}_{\alpha_k} \) converges to \( \bar{\mathbf{z}} \notin F_1^\star \). Then there exists \( \delta > 0 \) such that \( J^{(1)}(\bar{\mathbf{z}}) \geq J^{(1)}(\mathbf{z}_1^\star) + 2\delta \) for any \( \mathbf{z}_1^\star \in F_1^\star \) by continuity and separation of the closed sets \( F_1^\star \) and \( \{\bar{\mathbf{z}}\} \). For sufficiently large \( k \), \( J^{(1)}(\mathbf{z}_{\alpha_k}) \geq J^{(1)}(\mathbf{z}_1^\star) + \delta \). Then

\[ J_{\alpha_k}(\mathbf{z}_{\alpha_k}) \geq \alpha_k \bigl(J^{(1)}(\mathbf{z}_1^\star) + \delta\bigr) + J^{(2)}(\mathbf{z}_{\alpha_k}) \geq \alpha_k J^{(1)}(\mathbf{z}_1^\star) + \alpha_k \delta - C, \]

for some constant \( C \) such that \( J^{(2)}(\mathbf{z}_{\alpha_k}) \geq -C \) on the compact sublevel set of interest. On the other hand, at \( \mathbf{z}_1^\star \) we have \( J_{\alpha_k}(\mathbf{z}_1^\star) = \alpha_k J^{(1)}(\mathbf{z}_1^\star) + J^{(2)}(\mathbf{z}_1^\star) \). For large \( k \) we get \( J_{\alpha_k}(\mathbf{z}_{\alpha_k}) > J_{\alpha_k}(\mathbf{z}_1^\star) \), contradicting optimality of \( \mathbf{z}_{\alpha_k} \). Thus any limit point must lie in \( F_1^\star \).

Problem 3 (Simple 2D example). Consider a 2D decision variable \( \mathbf{z} = [z_1,z_2]^\top \) with no constraints (so \( F = \mathbb{R}^2 \)) and task costs

\[ J^{(1)}(\mathbf{z}) = \frac{1}{2}(z_1 - 1)^2, \qquad J^{(2)}(\mathbf{z}) = \frac{1}{2}(z_2 - 2)^2 + \frac{1}{2}(z_1 - 3)^2. \]

1. Compute the lexicographic solution.
2. Compute the weighted solution for \( J_\alpha = \alpha J^{(1)} + J^{(2)} \) and show that it converges to the lexicographic solution as \( \alpha \to \infty \).

Solution. (1) For level 1, we minimize \( J^{(1)} \) over \( \mathbb{R}^2 \): the minimizer is \( z_1 = 1 \), and \( z_2 \) is unconstrained. Thus \( F_1^\star = \{(1,z_2)\,|\,z_2 \in \mathbb{R}\} \). At level 2 we minimize \( J^{(2)} \) over \( F_1^\star \), i.e.

\[ J^{(2)}(1,z_2) = \frac{1}{2}(z_2 - 2)^2 + \frac{1}{2}(1 - 3)^2 = \frac{1}{2}(z_2 - 2)^2 + 2. \]

The minimizer is \( z_2 = 2 \), hence \( \mathbf{z}_{\text{lex}} = (1,2)^\top \).

(2) For the weighted cost,

\[ J_\alpha(\mathbf{z}) = \frac{\alpha}{2}(z_1 - 1)^2 + \frac{1}{2}(z_2 - 2)^2 + \frac{1}{2}(z_1 - 3)^2. \]

Taking gradients and setting to zero:

\[ \begin{aligned} \partial_{z_1} J_\alpha &= \alpha (z_1 - 1) + (z_1 - 3) = (\alpha + 1) z_1 - (\alpha + 3) = 0, \\ \partial_{z_2} J_\alpha &= (z_2 - 2) = 0. \end{aligned} \]

Thus

\[ z_2^\star(\alpha) = 2, \qquad z_1^\star(\alpha) = \frac{\alpha + 3}{\alpha + 1} = 1 + \frac{2}{\alpha + 1}. \]

Taking \( \alpha \to \infty \), we obtain \( z_1^\star(\alpha) \to 1 \) and the weighted solution converges to \( (1,2)^\top = \mathbf{z}_{\text{lex}} \). For moderate \( \alpha \), however, \( z_1^\star(\alpha) \neq 1 \), showing the approximate nature of prioritization under finite weights.

Problem 4 (Conditioning of Hessian). Suppose we use weights \( \alpha_1 = \kappa \), \( \alpha_2 = 1 \) with \( \kappa \gg 1 \) in the weighted QP \( J_\kappa = \kappa J^{(1)} + J^{(2)} \). Explain why the condition number of \( \mathbf{H} \) scales approximately like \( \kappa \) and discuss the impact on numerical stability.

Solution. Since \( \mathbf{H} = \kappa \mathbf{H}_1 + \mathbf{H}_2 + \lambda \mathbf{I} \) with positive semidefinite \( \mathbf{H}_1,\mathbf{H}_2 \), the dominant eigenvalues grow roughly linearly with \( \kappa \) in directions where \( \mathbf{H}_1 \) is nonzero, while eigenvalues in the null-space of \( \mathbf{H}_1 \) remain bounded by those of \( \mathbf{H}_2 + \lambda \mathbf{I} \). The condition number (ratio of largest to smallest eigenvalue) therefore scales approximately like \( \kappa \). A large condition number amplifies floating-point errors and slows down iterative solvers, making the QP harder to solve robustly in real time. This illustrates a key trade-off: mimicking strict priority via huge weights can degrade solver performance.

Problem 5 (Design flow choice). Consider a whole-body controller for a humanoid that must (i) never violate unilateral contact constraints, (ii) prioritize balance and joint limits over hand trajectory tracking, and (iii) run at 1 kHz. Propose whether you would favor a strictly hierarchical HQP or a single weighted QP, and justify your choice based on the analysis in this lesson.

Solution. Contact consistency and unilateral constraints are naturally handled as hard constraints \( \mathbf{A},\mathbf{G} \) in any QP. The choice concerns the relative importance of balance, joint limits, and hand tracking. Because balance and joint limits are safety critical, a strict hierarchy with balance and limits at higher priority levels is attractive: it guarantees that hand tracking cannot violate them if a feasible solution exists. However, hierarchical approaches require solving several QPs per cycle, which may challenge a 1 kHz loop unless the problems are small and highly optimized. An alternative is a single weighted QP with very large weights for balance and limits, but this sacrifices formal guarantees and may yield ill-conditioned Hessians. A common compromise is to use a small number of priority levels (e.g., physical consistency and safety in level 1, all other tasks weighted in level 2) with an efficient HQP solver designed for real-time use. This preserves safety guarantees while keeping the computational cost manageable.

In this lesson we formalized how whole-body controllers handle multiple tasks using either strict prioritization (lexicographic optimization) or weighted optimal control. We expressed tasks as quadratic costs in a common decision vector \( \mathbf{z} \) subject to dynamics and contact constraints, constructed weighted QPs, and described hierarchical formulations using null-space projectors and sequential QPs.

Theoretical analysis showed that, under suitable convexity assumptions, weighted QPs with very large weight ratios approximate lexicographic behavior in the limit, but finite weights provide only approximate prioritization and introduce conditioning issues. Hierarchical schemes provide strict guarantees for high-priority tasks but may incur higher computational cost due to multiple QP solves.

Implementation snippets in Python, C++, Java, MATLAB/Simulink, and Mathematica illustrated how to assemble whole-body QPs and highlighted the role of robotics-specific libraries (Pinocchio, RBDL, Robotics System Toolbox) and numerical optimizers (OSQP, qpOASES, quadprog, QuadraticOptimization). These tools will be used in the next lesson to implement whole-body behaviors for simple legged or humanoid case studies.