In whole-body control, the robot has many joints (DOFs) and must simultaneously satisfy several tasks. A task is typically specified in task space (end-effector pose, center-of-mass position, orientation of a camera, etc.) and can be written as an error signal \( \mathbf{e}_i(\mathbf{q}, \dot{\mathbf{q}}) \) with a desired evolution. For instance, in velocity-level control:

\[ \mathbf{e}_i(\mathbf{q}, \dot{\mathbf{q}}) \;=\; \mathbf{x}_i(\mathbf{q}) - \mathbf{x}_i^{\text{ref}}, \quad \dot{\mathbf{x}}_i \;=\; \mathbf{J}_i(\mathbf{q}) \, \dot{\mathbf{q}}, \]

where \( \mathbf{J}_i(\mathbf{q}) \) is the task Jacobian. A common control law is to choose joint velocities \( \dot{\mathbf{q}} \) such that

\[ \dot{\mathbf{x}}_i \approx \dot{\mathbf{x}}_i^{\text{ref}} - K_i \, \mathbf{e}_i, \quad K_i \succ 0. \]

For a single task, we can solve this as a least-squares problem. However, for multiple tasks, possibly conflicting and with inequality constraints (joint limits, torque limits, contact conditions), we naturally arrive at a quadratic program (QP) of the form

\[ \min_{\mathbf{u} \in \mathbb{R}^m} \; \tfrac{1}{2} \mathbf{u}^\top H \mathbf{u} + \mathbf{f}^\top \mathbf{u} \quad \text{s.t.} \quad A_{\text{ineq}} \mathbf{u} \le \mathbf{b}_{\text{ineq}}, \; A_{\text{eq}} \mathbf{u} = \mathbf{b}_{\text{eq}}, \]

where the optimization variable \( \mathbf{u} \) can be chosen as \( \dot{\mathbf{q}} \), \( \ddot{\mathbf{q}} \), or joint torques \( \tau \), depending on the control level. The matrix \( H \) and vector \( \mathbf{f} \) encode objectives (task tracking, regularization), while \( A_{\text{ineq}} \), \( A_{\text{eq}} \) capture constraints (contacts, limits).

flowchart TD
  T1["High-priority task list (e.g. balance, contact)"]
    --> B["Build task equations J_i * u = v_i_ref"]
  T2["Lower-priority tasks (e.g. posture, gaze)"]
    --> B
  B --> C["Assemble QP cost and constraints"]
  C --> S["Call QP solver"]
  S --> U["Compute control output u_k"]
  U --> Q["Send to low-level joint controllers"]
        

This lesson focuses on how to stack tasks in a prioritized order and formulate the resulting whole-body controller as one or more QPs.

A generic task \( i \) is defined as a function \( \mathbf{e}_i(\mathbf{q}, \dot{\mathbf{q}}) \in \mathbb{R}^{m_i} \). Around the current state \( (\mathbf{q}_k, \dot{\mathbf{q}}_k) \), a first-order linearization in the control variable (\( \mathbf{u} = \dot{\mathbf{q}} \) or \( \mathbf{u} = \ddot{\mathbf{q}} \)) yields

\[ \mathbf{e}_i(\mathbf{q}_k, \mathbf{u}) \;\approx\; \mathbf{e}_i(\mathbf{q}_k, \mathbf{u}_0) + \mathbf{J}_i^{(u)}(\mathbf{q}_k) \, (\mathbf{u} - \mathbf{u}_0), \]

where \( \mathbf{J}_i^{(u)} \) is the Jacobian of the task with respect to the control variable. A common velocity-level choice is

\[ \mathbf{u} = \dot{\mathbf{q}}, \quad \mathbf{J}_i^{(u)}(\mathbf{q}) = \mathbf{J}_i(\mathbf{q}), \quad \mathbf{e}_i(\mathbf{q}, \dot{\mathbf{q}}) = \mathbf{x}_i(\mathbf{q}) - \mathbf{x}_i^{\text{ref}}. \]

A standard first-order tracking law is then

\[ \mathbf{J}_i(\mathbf{q}_k) \, \dot{\mathbf{q}} = \dot{\mathbf{x}}_i^{\text{ref}} - K_i \, \mathbf{e}_i(\mathbf{q}_k), \]

which is linear in \( \dot{\mathbf{q}} \). When several tasks are considered simultaneously, we collect their linearized equations:

\[ \mathbf{J}_i \mathbf{u} = \mathbf{v}_i, \quad \mathbf{v}_i \;:=\; \dot{\mathbf{x}}_i^{\text{ref}} - K_i \, \mathbf{e}_i. \]

For inequality-type tasks (e.g. joint limits, distance constraints), we can write

\[ \mathbf{a}_j^\top \mathbf{u} \le b_j, \]

and group them into matrix form \( A_{\text{ineq}} \mathbf{u} \le \mathbf{b}_{\text{ineq}} \). This linear structure is precisely what is needed for QP constraints.

Consider one level containing multiple tasks indexed by \( i = 1,\dots,N \). We use a velocity-level whole-body controller with optimization variable \( \mathbf{u} = \dot{\mathbf{q}} \in \mathbb{R}^n \). We define the stacked least-squares objective

\[ J(\dot{\mathbf{q}}) \;=\; \frac{1}{2} \sum_{i=1}^N \left\| W_i \left( \mathbf{J}_i \dot{\mathbf{q}} - \mathbf{v}_i \right) \right\|^2 + \frac{\lambda}{2} \left\| W_{\text{reg}} \left( \dot{\mathbf{q}} - \dot{\mathbf{q}}_{\text{nom}} \right) \right\|^2, \]

where \( W_i \succ 0 \) weight individual tasks and \( W_{\text{reg}} \) penalizes deviation from a nominal posture velocity \( \dot{\mathbf{q}}_{\text{nom}} \). Expanding this quadratic form gives the standard QP matrices

\[ \min_{\dot{\mathbf{q}}} \; \tfrac{1}{2} \dot{\mathbf{q}}^\top H \dot{\mathbf{q}} + \mathbf{f}^\top \dot{\mathbf{q}} \quad \text{s.t.} \quad A_{\text{ineq}} \dot{\mathbf{q}} \le \mathbf{b}_{\text{ineq}}, \]

with

\[ H \;=\; \sum_{i=1}^N \mathbf{J}_i^\top W_i^\top W_i \mathbf{J}_i + \lambda \, W_{\text{reg}}^\top W_{\text{reg}}, \quad \mathbf{f} \;=\; - \sum_{i=1}^N \mathbf{J}_i^\top W_i^\top W_i \mathbf{v}_i - \lambda \, W_{\text{reg}}^\top W_{\text{reg}} \dot{\mathbf{q}}_{\text{nom}}. \]

A key observation is that if \( H \succ 0 \), the QP has a unique solution. For equality constraints \( A_{\text{eq}} \dot{\mathbf{q}} = \mathbf{b}_{\text{eq}} \) (e.g. closed kinematic chains or holonomic contact constraints), the necessary and sufficient optimality conditions (KKT conditions) are:

\[ \begin{bmatrix} H & A_{\text{eq}}^\top \\[3pt] A_{\text{eq}} & 0 \end{bmatrix} \begin{bmatrix} \dot{\mathbf{q}}^\star \\[3pt] \boldsymbol{\lambda}^\star \end{bmatrix} \;=\; - \begin{bmatrix} \mathbf{f} \\[3pt] -\mathbf{b}_{\text{eq}} \end{bmatrix}, \]

where \( \boldsymbol{\lambda}^\star \) are Lagrange multipliers. This linear system characterizes the solution and is often exploited by QP solvers.

A task stack (or stack-of-tasks) is an ordered set of task levels \( \mathcal{L}_1, \dots, \mathcal{L}_P \). Higher levels have strict priority over lower ones, in a lexicographic sense:

\[ \mathcal{L}_1 \succ \mathcal{L}_2 \succ \dots \succ \mathcal{L}_P. \]

At each level \( \ell \), we define a cost \( J_\ell(\mathbf{u}) \) (e.g. a weighted squared error on the tasks in that level). The ideal hierarchical problem is

\[ \begin{aligned} &\text{Minimize } J_1(\mathbf{u}) \\ &\text{subject to constraints (contacts, limits)}; \\[4pt] &\text{then minimize } J_2(\mathbf{u}) \\ &\text{subject to constraints and achieving the optimal value of } J_1; \\[4pt] &\dots \end{aligned} \]

This is a lexicographic QP. Directly encoding lexicographic order in a single QP is difficult. Two common approaches are:

1. Sequential QPs. Solve one QP per level, adding constraints that keep the optimal cost of higher levels unchanged (up to a small tolerance).
2. Null-space projection (from earlier chapters). Solve the highest-priority level, then restrict lower levels to the null space of higher-level Jacobians, which approximates lexicographic behavior in the unconstrained case.

For sequential QPs, denote by \( J_\ell^\star \) the achieved minimum at level \( \ell \). At level \( \ell+1 \), we solve

\[ \begin{aligned} &\min_{\mathbf{u}} \; J_{\ell+1}(\mathbf{u}) \\ &\text{s.t.}\; J_1(\mathbf{u}) \le J_1^\star + \epsilon_1, \dots, J_\ell(\mathbf{u}) \le J_\ell^\star + \epsilon_\ell, \\ &\quad\;\; \text{physical constraints (contacts, limits)}. \end{aligned} \]

The small tolerances \( \epsilon_i \) allow numerical robustness and ensure that lower-priority tasks cannot significantly degrade higher-priority tasks.

flowchart TD
  L1["Level 1 tasks (e.g. balance, contact)"] --> Q1["QP 1: minimize J1(u)"]
  Q1 --> S1["Solution u1, cost J1*"]
  S1 --> L2["Level 2 tasks (e.g. end-effector)"]
  L2 --> Q2["QP 2: minimize J2(u) with J1(u) <= J1* + eps"]
  Q2 --> S2["Solution u2, cost J2*"]
  S2 --> L3["Lower levels (posture, regularization, etc.)"]
        

In absence of inequality constraints, it can be shown that hierarchical QP and null-space projection yield equivalent solutions if the same cost functions and weights are used.

Consider a 7-DOF manipulator. We construct a two-level stack with variable \( \dot{\mathbf{q}} \in \mathbb{R}^7 \):

• Level 1: End-effector Cartesian velocity tracking \( \dot{\mathbf{x}} \approx \dot{\mathbf{x}}^{\text{ref}} - K_x \mathbf{e}_x \).
• Level 2: Posture regularization towards \( \mathbf{q}^{\text{ref}} \).

Level 1 QP:

\[ \min_{\dot{\mathbf{q}}} \; \frac{1}{2} \left\| W_x \left( \mathbf{J}_x \dot{\mathbf{q}} - \mathbf{v}_x \right) \right\|^2 + \frac{\lambda}{2} \| \dot{\mathbf{q}} \|^2 \quad \text{s.t.}\quad \dot{\mathbf{q}}_{\min} \le \dot{\mathbf{q}} \le \dot{\mathbf{q}}_{\max}, \]

with \( \mathbf{v}_x = \dot{\mathbf{x}}^{\text{ref}} - K_x \mathbf{e}_x \). Denote the optimal solution by \( \dot{\mathbf{q}}_1^\star \) and the minimal cost by \( J_1^\star \).

Level 2 QP:

\[ \min_{\dot{\mathbf{q}}} \; \frac{1}{2} \left\| W_q \left( \dot{\mathbf{q}} - \dot{\mathbf{q}}^{\text{ref}} \right) \right\|^2 + \frac{\lambda}{2} \| \dot{\mathbf{q}} \|^2 \quad \text{s.t.}\quad \begin{cases} \dot{\mathbf{q}}_{\min} \le \dot{\mathbf{q}} \le \dot{\mathbf{q}}_{\max}, \\ J_1(\dot{\mathbf{q}}) \le J_1^\star + \epsilon. \end{cases} \]

Here \( J_1(\dot{\mathbf{q}}) \) is the same cost as level 1 evaluated at \( \dot{\mathbf{q}} \). The final whole-body velocity command is \( \dot{\mathbf{q}}^\star = \dot{\mathbf{q}}_2^\star \).

In practice, whole-body controllers typically operate at the acceleration or torque level, incorporating the rigid-body dynamics model \( M(\mathbf{q}) \ddot{\mathbf{q}} + h(\mathbf{q}, \dot{\mathbf{q}}) = \tau \), but the QP structure remains the same: a quadratic cost plus linear equality/inequality constraints.

We implement a single-level velocity QP for a 7-DOF manipulator and then show how to emulate a two-level stack using a large weight for the high-priority task (approximate hierarchy). In practice, you can also perform true sequential QPs.

import numpy as np
import cvxpy as cp

# Dimensions
nq = 7       # number of joints
mx = 6       # task-space dimension (e.g. full 6D end-effector twist)

# Example Jacobians and references (in a real system, compute from kinematics)
J_x = np.random.randn(mx, nq)  # end-effector Jacobian
J_p = np.eye(nq)               # "posture" task (identity in joint space)

x_err = np.random.randn(mx)    # current task-space error
xdot_ref = np.zeros(mx)        # desired Cartesian velocity
Kx = 5.0 * np.eye(mx)

qdot_nom = np.zeros(nq)
qdot_ref = np.zeros(nq)
Wp = 0.1 * np.eye(nq)
Wx = np.eye(mx)

# Joint limits on velocity
qdot_min = -0.5 * np.ones(nq)
qdot_max =  0.5 * np.ones(nq)

# Optimization variable
qdot = cp.Variable(nq)

v_x = xdot_ref - Kx @ x_err

# Approximate hierarchical cost:
#   Level 1 (end-effector) with large weight w1
#   Level 2 (posture) with smaller weight w2
w1 = 1000.0
w2 = 1.0
lam_reg = 1e-3

cost_level1 = cp.sum_squares(Wx @ (J_x @ qdot - v_x))
cost_level2 = cp.sum_squares(Wp @ (J_p @ (qdot - qdot_ref)))
cost_reg    = cp.sum_squares(qdot - qdot_nom)

cost = 0.5 * (w1 * cost_level1 + w2 * cost_level2 + lam_reg * cost_reg)

constraints = [
    qdot_min <= qdot,
    qdot     <= qdot_max,
]

prob = cp.Problem(cp.Minimize(cost), constraints)
prob.solve(solver=cp.OSQP)  # or cp.QP, cp.OSQP, cp.ECOS, etc.

print("Status:", prob.status)
print("Optimal qdot:", qdot.value)
      

For a true hierarchical solution, you would:

1. Solve the high-priority task alone to get \( \dot{\mathbf{q}}_1^\star \) and the minimum cost.
2. Add a constraint that the high-priority cost remains at its optimal value (within a tolerance) in the second QP and then solve for lower-priority objectives.

In C++, whole-body controllers often use Eigen for linear algebra and libraries like qpOASES or OSQP for solving QPs. Below is a sketch of forming \( H \) and \( \mathbf{g} \) for a single-level velocity QP, then calling a generic QP solver.

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

using Eigen::MatrixXd;
using Eigen::VectorXd;

int main()
{
    const int nq = 7;
    const int mx = 6;

    MatrixXd Jx(mx, nq);  // end-effector Jacobian
    MatrixXd Jp(nq, nq);  // posture task (identity)
    Jx.setRandom();
    Jp.setIdentity();

    VectorXd x_err(mx);
    x_err.setRandom();
    VectorXd xdot_ref(mx);
    xdot_ref.setZero();

    MatrixXd Kx = 5.0 * MatrixXd::Identity(mx, mx);
    MatrixXd Wx = MatrixXd::Identity(mx, mx);
    MatrixXd Wp = 0.1 * MatrixXd::Identity(nq, nq);

    VectorXd qdot_nom = VectorXd::Zero(nq);
    VectorXd qdot_ref = VectorXd::Zero(nq);

    VectorXd v_x = xdot_ref - Kx * x_err;

    // Build H and g for the QP: 0.5 qdot^T H qdot + g^T qdot
    MatrixXd H = MatrixXd::Zero(nq, nq);
    VectorXd g = VectorXd::Zero(nq);

    double w1 = 1000.0;
    double w2 = 1.0;
    double lambda_reg = 1e-3;

    // Level 1 contribution
    H += w1 * Jx.transpose() * Wx.transpose() * Wx * Jx;
    g += -w1 * Jx.transpose() * Wx.transpose() * Wx * v_x;

    // Level 2 (posture) contribution
    H += w2 * Jp.transpose() * Wp.transpose() * Wp * Jp;
    g += -w2 * Jp.transpose() * Wp.transpose() * Wp * qdot_ref;

    // Regularization
    H += lambda_reg * MatrixXd::Identity(nq, nq);
    g += -lambda_reg * qdot_nom;

    // Velocity bounds
    VectorXd qdot_min = -0.5 * VectorXd::Ones(nq);
    VectorXd qdot_max =  0.5 * VectorXd::Ones(nq);

    // Convert to the format expected by your QP solver.
    // For example, qpOASES expects H, g, A, lb, ub, etc.
    // Here we only show the conceptual structure.

    // Example: qdot_min <= qdot <= qdot_max
    // This can be encoded as:
    //  lb = qdot_min
    //  ub = qdot_max

    VectorXd qdot_star(nq);
    // Call your QP solver here, e.g.:
    // solve_qp(H, g, A, lb, ub, qdot_star);

    return 0;
}
      

In a real whole-body controller, Jx, x_err, and the bounds are updated each control cycle using the robot's kinematics and current state estimates.

Java is less common in low-level robot control but can be used in high-level controllers or simulators. Libraries such as ojAlgo and EJML can support QP-like optimization. Below is a conceptual example using ojAlgo's quadratic solver.

import org.ojalgo.optimisation.ExpressionsBasedModel;
import org.ojalgo.optimisation.Variable;
import org.ojalgo.optimisation.Optimisation.Result;
import org.ojalgo.optimisation.QuadraticSolver;

public class WholeBodyQP {

    public static void main(String[] args) {
        int nq = 7;

        // Variables: qdot[0..nq-1]
        Variable[] qdot = new Variable[nq];
        ExpressionsBasedModel model = new ExpressionsBasedModel();
        for (int i = 0; i < nq; ++i) {
            qdot[i] = Variable.make("qdot" + i).lower(-0.5).upper(0.5);
            model.addVariable(qdot[i]);
        }

        // In a real system, you would fill H and g from your task Jacobians
        // and errors. Here we assume a diagonal H and zero g for simplicity.
        double lambdaReg = 1e-3;
        for (int i = 0; i < nq; ++i) {
            qdot[i].weight(lambdaReg);  // simple regularization
        }

        // Add linear constraints representing tasks, e.g. Jx * qdot = v_x
        // model.addExpression("task1")
        //      .setLinearFactors(qdot, coeffsForThisRow)
        //      .level(v_x_k);

        // Solve (ojAlgo chooses an appropriate solver internally)
        Result result = model.minimise();
        System.out.println("Status: " + result.getState());
        for (int i = 0; i < nq; ++i) {
            System.out.println("qdot[" + i + "] = " + result.get(i));
        }
    }
}
      

For more advanced usage, you would explicitly build a quadratic objective matching \( \tfrac{1}{2} \dot{\mathbf{q}}^\top H \dot{\mathbf{q}} + \mathbf{f}^\top \dot{\mathbf{q}} \) using ojAlgo's matrix APIs and set linear equality/inequality constraints from the task Jacobians.

MATLAB provides the quadprog function (Optimization Toolbox) which directly solves QPs. Robotics System Toolbox and Simulink blocks can handle Jacobian computation and state flow. Below is a minimal MATLAB script for the single-level velocity QP.

nq = 7;  % number of joints
mx = 6;  % task-space dimension

Jx = randn(mx, nq);  % example Jacobian
Jp = eye(nq);       % posture task

x_err    = randn(mx, 1);
xdot_ref = zeros(mx, 1);
Kx       = 5 * eye(mx);

Wx = eye(mx);
Wp = 0.1 * eye(nq);

qdot_nom = zeros(nq, 1);
qdot_ref = zeros(nq, 1);

v_x = xdot_ref - Kx * x_err;

w1 = 1000;     % high priority weight
w2 = 1.0;      % low priority weight
lambda_reg = 1e-3;

H = w1 * (Jx' * Wx' * Wx * Jx) + ...
    w2 * (Jp' * Wp' * Wp * Jp) + ...
    lambda_reg * eye(nq);

f = -w1 * (Jx' * Wx' * Wx * v_x) - ...
    -w2 * (Jp' * Wp' * Wp * qdot_ref) - ...
    lambda_reg * qdot_nom;

qdot_min = -0.5 * ones(nq, 1);
qdot_max =  0.5 * ones(nq, 1);

A   = [ eye(nq); -eye(nq) ];
b   = [ qdot_max; -qdot_min ];

% No equality constraints in this example
Aeq = [];
beq = [];

options = optimoptions('quadprog','Display','none');

[qdot_star, fval, exitflag] = quadprog(H, f, A, b, Aeq, beq, [], [], [], options);

disp('exitflag = '), disp(exitflag)
disp('qdot_star = '), disp(qdot_star)
      

In Simulink, you can place this computation inside a MATLAB Function block, connect the robot state and desired task signals, and send the resulting joint velocities to a joint controller block or hardware interface.

Mathematica supports quadratic optimization via QuadraticOptimization. The following example demonstrates solving a simple velocity-level QP:

nq = 7;
mx = 6;

Jx = RandomReal[NormalDistribution[0, 1], {mx, nq}];
Jp = IdentityMatrix[nq];

xErr    = RandomReal[NormalDistribution[0, 1], mx];
xdotRef = ConstantArray[0.0, mx];
Kx      = 5 IdentityMatrix[mx];

Wx = IdentityMatrix[mx];
Wp = 0.1 IdentityMatrix[nq];

qdotNom = ConstantArray[0.0, nq];
qdotRef = ConstantArray[0.0, nq];

vX = xdotRef - Kx . xErr;

w1 = 1000.0;
w2 = 1.0;
lambdaReg = 1.*^-3;

(* Build H and f *)
H = w1 Transpose[Jx].Transpose[Wx].Wx.Jx +
    w2 Transpose[Jp].Transpose[Wp].Wp.Jp +
    lambdaReg IdentityMatrix[nq];

f = -w1 Transpose[Jx].Transpose[Wx].Wx.vX -
    w2 Transpose[Jp].Transpose[Wp].Wp.qdotRef -
    lambdaReg qdotNom;

vars = Array[qdot, nq];

qdotMin = ConstantArray[-0.5, nq];
qdotMax = ConstantArray[0.5, nq];

ineqConstraints =
  Thread[qdotMin <= vars] ~Join~
  Thread[vars <= qdotMax];

objective = 1/2 vars.H.vars + f.vars;

sol = QuadraticOptimization[
  objective,
  ineqConstraints,
  vars
];

qdotStar = vars /. sol["PrimalMinimizer"];
qdotStar
      

As in other languages, you would integrate this computation into a control loop, updating Jx, xErr, and bounds at each iteration based on the robot's current state.

Problem 1 (Least-Squares QP and Pseudoinverse): Consider a single equality task \( \mathbf{J} \dot{\mathbf{q}} = \mathbf{v} \) with \( \mathbf{J} \in \mathbb{R}^{m \times n} \), \( m \le n \). Show that the unconstrained QP

\[ \min_{\dot{\mathbf{q}}} \; \frac{1}{2} \| \mathbf{J} \dot{\mathbf{q}} - \mathbf{v} \|^2 \]

has solution \( \dot{\mathbf{q}}^\star = \mathbf{J}^+ \mathbf{v} \), where \( \mathbf{J}^+ \) is the Moore–Penrose pseudoinverse.

Solution: The cost function is \( J(\dot{\mathbf{q}}) = \tfrac{1}{2} (\mathbf{J} \dot{\mathbf{q}} - \mathbf{v})^\top (\mathbf{J} \dot{\mathbf{q}} - \mathbf{v}) \). The gradient is

\[ \nabla J(\dot{\mathbf{q}}) = \mathbf{J}^\top (\mathbf{J} \dot{\mathbf{q}} - \mathbf{v}). \]

Setting this to zero gives the normal equations

\[ \mathbf{J}^\top \mathbf{J} \dot{\mathbf{q}}^\star = \mathbf{J}^\top \mathbf{v}. \]

If \( \mathbf{J} \) has full row rank, the minimum-norm solution is \( \dot{\mathbf{q}}^\star = \mathbf{J}^+ \mathbf{v} = \mathbf{J}^\top (\mathbf{J} \mathbf{J}^\top)^{-1} \mathbf{v} \), which is exactly the standard pseudoinverse solution. This coincides with the unique minimizer of the quadratic cost.

Problem 2 (Equivalence of Hierarchical QP and Null-Space Projection): Assume two levels of tasks with costs \( J_1(\dot{\mathbf{q}}) = \tfrac{1}{2} \| \mathbf{J}_1 \dot{\mathbf{q}} - \mathbf{v}_1 \|^2 \), \( J_2(\dot{\mathbf{q}}) = \tfrac{1}{2} \| \mathbf{J}_2 \dot{\mathbf{q}} - \mathbf{v}_2 \|^2 \), and no inequalities. Show that if \( \mathbf{J}_1 \) is full row rank, the hierarchical solution

\[ \dot{\mathbf{q}}^\star = \dot{\mathbf{q}}_1^\star + \mathbf{N}_1 \dot{\mathbf{q}}_2^\star, \]

with \( \dot{\mathbf{q}}_1^\star = \mathbf{J}_1^+ \mathbf{v}_1 \) and \( \mathbf{N}_1 = I - \mathbf{J}_1^+ \mathbf{J}_1 \), minimizes \( J_2 \) subject to the exact satisfaction of the level-1 task.

Solution: Any velocity satisfying the level-1 task can be written as \( \dot{\mathbf{q}} = \dot{\mathbf{q}}_1^\star + \mathbf{N}_1 \mathbf{z} \), where \( \mathbf{z} \) is arbitrary. Substituting into \( J_2 \) gives a quadratic in \( \mathbf{z} \):

\[ J_2(\mathbf{z}) = \tfrac{1}{2} \left\| \mathbf{J}_2 (\dot{\mathbf{q}}_1^\star + \mathbf{N}_1 \mathbf{z}) - \mathbf{v}_2 \right\|^2. \]

This is a standard unconstrained quadratic program in \( \mathbf{z} \) with unique minimizer \( \mathbf{z}^\star \) if the associated Hessian is positive definite on the image of \( \mathbf{N}_1 \). The resulting \( \dot{\mathbf{q}}^\star = \dot{\mathbf{q}}_1^\star + \mathbf{N}_1 \mathbf{z}^\star \) is precisely the hierarchical solution that minimizes \( J_2 \) while exactly satisfying the level-1 task.

Problem 3 (KKT Conditions for a Constrained Velocity QP): For the generic QP

\[ \min_{\dot{\mathbf{q}}} \tfrac{1}{2} \dot{\mathbf{q}}^\top H \dot{\mathbf{q}} + \mathbf{f}^\top \dot{\mathbf{q}} \quad \text{s.t.}\quad A_{\text{eq}} \dot{\mathbf{q}} = \mathbf{b}_{\text{eq}}, \; A_{\text{ineq}} \dot{\mathbf{q}} \le \mathbf{b}_{\text{ineq}}, \]

derive the Karush–Kuhn–Tucker (KKT) conditions.

Solution: Introduce multipliers \( \boldsymbol{\lambda} \) for equalities and \( \boldsymbol{\mu} \) for inequalities. The Lagrangian is

\[ \mathcal{L}(\dot{\mathbf{q}}, \boldsymbol{\lambda}, \boldsymbol{\mu}) = \tfrac{1}{2} \dot{\mathbf{q}}^\top H \dot{\mathbf{q}} + \mathbf{f}^\top \dot{\mathbf{q}} + \boldsymbol{\lambda}^\top (A_{\text{eq}} \dot{\mathbf{q}} - \mathbf{b}_{\text{eq}}) + \boldsymbol{\mu}^\top (A_{\text{ineq}} \dot{\mathbf{q}} - \mathbf{b}_{\text{ineq}}). \]

The KKT conditions are:

• Stationarity: \( H \dot{\mathbf{q}}^\star + \mathbf{f} + A_{\text{eq}}^\top \boldsymbol{\lambda}^\star + A_{\text{ineq}}^\top \boldsymbol{\mu}^\star = 0. \)
• Primal feasibility: \( A_{\text{eq}} \dot{\mathbf{q}}^\star = \mathbf{b}_{\text{eq}}, \; A_{\text{ineq}} \dot{\mathbf{q}}^\star \le \mathbf{b}_{\text{ineq}}. \)
• Dual feasibility: \( \boldsymbol{\mu}^\star \ge 0. \)
• Complementarity: \( \mu_i^\star \big( (A_{\text{ineq}} \dot{\mathbf{q}}^\star - \mathbf{b}_{\text{ineq}})_i \big) = 0 \) for all \( i \).

Problem 4 (Joint-Velocity Bounds as Linear Inequalities): Show how the box constraints \( \dot{\mathbf{q}}_{\min} \le \dot{\mathbf{q}} \le \dot{\mathbf{q}}_{\max} \) can be written in the standard form \( A_{\text{ineq}} \dot{\mathbf{q}} \le \mathbf{b}_{\text{ineq}} \).

Solution: Rewrite the bounds as \( \dot{\mathbf{q}} \le \dot{\mathbf{q}}_{\max} \) and \( -\dot{\mathbf{q}} \le -\dot{\mathbf{q}}_{\min} \). Stacking them:

\[ A_{\text{ineq}} = \begin{bmatrix} I \\ -I \end{bmatrix}, \quad \mathbf{b}_{\text{ineq}} = \begin{bmatrix} \dot{\mathbf{q}}_{\max} \\ -\dot{\mathbf{q}}_{\min} \end{bmatrix}. \]

This is exactly the form used in the MATLAB and C++ examples.

Problem 5 (Approximate vs Exact Hierarchy via Weights): Consider two tasks with costs \( J_1 \) and \( J_2 \). We approximate a hierarchy by minimizing

\[ J_\alpha(\mathbf{u}) = \alpha J_1(\mathbf{u}) + J_2(\mathbf{u}), \]

with \( \alpha \gg 1 \). Explain under what conditions the minimizing sequence \( \mathbf{u}_\alpha^\star \) converges to the lexicographic solution (minimize \( J_1 \) first, then \( J_2 \) on its minimizer set).

Solution: If both \( J_1 \) and \( J_2 \) are strictly convex quadratics and the feasible set is compact, then for sufficiently large \( \alpha \) the minimizer \( \mathbf{u}_\alpha^\star \) first makes \( J_1 \) arbitrarily close to its minimum. The set of minimizers of \( J_1 \) is then approximately enforced, and within this set, the contribution of \( J_2 \) dominates the remaining variation. Formally, the limit of \( \mathbf{u}_\alpha^\star \) as \( \alpha \rightarrow \infty \) coincides with the unique lexicographic minimizer when these convexity and uniqueness assumptions hold.

In this lesson, we formalized whole-body tasks as linearized equality and inequality constraints in joint velocities (or accelerations/torques) and assembled them into quadratic programs. We introduced the notion of task stacks and lexicographic priorities, showed how hierarchical QPs implement strict priority among levels, and related this to the null-space projection methods studied earlier in the course. Finally, we demonstrated implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, providing a practical bridge between the theory of QPs and real-time whole-body controllers.