Consider a floating-base humanoid or planar biped with generalized coordinates \( q \in \mathbb{R}^n \) and generalized velocities \( \dot{q} \). We study a simple task:

• Maintain balance while standing on one foot (stance foot in contact).
• Regulate the center of mass (CoM) position above the stance foot.
• Move the swing foot along a prescribed Cartesian trajectory.
• Keep the posture close to a comfortable nominal configuration.

At the control level, we assume an acceleration-based whole-body controller that computes desired joint accelerations \( \ddot{q}^{\star} \) and contact forces \( \boldsymbol{\lambda} \), and then maps them into actuator torques \( \tau \) using the robot's known dynamics model (imported from the robotics course):

\[ \mathbf{M}(q)\ddot{q} + \mathbf{h}(q,\dot{q}) = \mathbf{S}^{\top} \boldsymbol{\tau} + \mathbf{J}_c(q)^{\top}\boldsymbol{\lambda}, \]

where \( \mathbf{M}(q) \) is the inertia matrix, \( \mathbf{h}(q,\dot{q}) \) collects Coriolis, centrifugal, and gravity terms, \( \mathbf{S} \) selects actuated joints, and \( \mathbf{J}_c(q) \) is the contact Jacobian for the stance foot.

The high-level structure of the controller is:

flowchart TD
  REF["Task references (com, swing foot, posture)"]
    --> TASK["Compute task errors and desired task accelerations"]
  TASK --> QP["Build quadratic program (H, g, Aeq, beq, Aineq, bineq)"]
  QP --> SOL["Solve QP for qdd, lambda, tau"]
  SOL --> TAU["Low-level torque command tau(t)"]
  TAU --> ROBOT["Legged robot dynamics"]
  ROBOT --> SENS["State estimation (q, qd, contacts)"]
  SENS --> TASK
        

Our case study is thus an example of the general whole-body QP control framework from previous lessons, but instantiated for a simple legged / humanoid task with explicit equations.

We focus on four main tasks, expressed at the acceleration level for a whole-body controller:

1. CoM regulation task.
2. Stance foot contact consistency (holonomic constraint).
3. Swing foot tracking task.
4. Posture regularization task.

2.1 CoM regulation task

Let \( \mathbf{p}_{\text{com}}(q) \in \mathbb{R}^3 \) be the CoM position. Its acceleration is

\[ \ddot{\mathbf{p}}_{\text{com}} = \mathbf{J}_{\text{com}}(q)\ddot{q} + \dot{\mathbf{J}}_{\text{com}}(q,\dot{q})\dot{q}, \]

where \( \mathbf{J}_{\text{com}}(q) \) is the CoM Jacobian. A PD law in task space defines a desired CoM acceleration:

\[ \ddot{\mathbf{p}}_{\text{com}}^{\star} = \ddot{\mathbf{p}}_{\text{com,ref}} + \mathbf{K}_p^{\text{com}}\bigl( \mathbf{p}_{\text{com,ref}} - \mathbf{p}_{\text{com}}(q) \bigr) + \mathbf{K}_d^{\text{com}}\bigl( \dot{\mathbf{p}}_{\text{com,ref}} - \dot{\mathbf{p}}_{\text{com}}(q,\dot{q}) \bigr). \]

The CoM task then becomes a linear equation in \( \ddot{q} \):

\[ \mathbf{J}_{\text{com}}(q)\ddot{q} = \ddot{\mathbf{p}}_{\text{com}}^{\star} - \dot{\mathbf{J}}_{\text{com}}(q,\dot{q})\dot{q}. \]

2.2 Stance foot contact constraint

Let \( \mathbf{x}_c(q) \) denote the stance foot Cartesian pose. Assuming it is rigidly fixed to the ground (no slip, no lift-off), its velocity and acceleration must vanish:

\[ \dot{\mathbf{x}}_c(q,\dot{q}) = \mathbf{0}, \quad \ddot{\mathbf{x}}_c(q,\dot{q},\ddot{q}) = \mathbf{0}. \]

Since \( \dot{\mathbf{x}}_c = \mathbf{J}_c(q)\dot{q} \) and \( \ddot{\mathbf{x}}_c = \mathbf{J}_c(q)\ddot{q} + \dot{\mathbf{J}}_c(q,\dot{q})\dot{q} \), this yields the acceleration-level holonomic constraint

\[ \mathbf{J}_c(q)\ddot{q} + \dot{\mathbf{J}}_c(q,\dot{q})\dot{q} = \mathbf{0}. \]

2.3 Swing foot tracking task

For the swing foot pose \( \mathbf{x}_s(q) \), we define a desired acceleration based on a reference trajectory \( \mathbf{x}_{s,\text{ref}}(t) \):

\[ \ddot{\mathbf{x}}_s^{\star} = \ddot{\mathbf{x}}_{s,\text{ref}} + \mathbf{K}_p^{s}\bigl( \mathbf{x}_{s,\text{ref}} - \mathbf{x}_s(q) \bigr) + \mathbf{K}_d^{s}\bigl( \dot{\mathbf{x}}_{s,\text{ref}} - \dot{\mathbf{x}}_s(q,\dot{q}) \bigr), \]

and the corresponding task equation

\[ \mathbf{J}_s(q)\ddot{q} = \ddot{\mathbf{x}}_s^{\star} - \dot{\mathbf{J}}_s(q,\dot{q})\dot{q}. \]

2.4 Posture task

Finally, we define a joint-space posture task that attracts the robot toward a nominal configuration \( q_0 \), e.g. a comfortable upright stance. A simple choice is

\[ \ddot{q}^{\text{post},\star} = \ddot{q}_{0} + \mathbf{K}_p^{q}(q_0 - q) + \mathbf{K}_d^{q}(\dot{q}_0 - \dot{q}), \]

with task matrix \( \mathbf{J}_{\text{post}} = \mathbf{I}_n \):

\[ \mathbf{J}_{\text{post}}\ddot{q} = \ddot{q}^{\text{post},\star}. \]

In this case the posture task is directly an equality in \( \ddot{q} \).

We now construct a quadratic program in the decision variable \( z \) defined as

\[ z = \begin{bmatrix} \ddot{q} \\ \boldsymbol{\tau} \\ \boldsymbol{\lambda} \end{bmatrix} \in \mathbb{R}^{n + n_a + n_{\lambda}}, \]

where \( n_a \) is the number of actuated joints and \( n_{\lambda} \) is the dimension of the contact wrench vector.

3.1 Quadratic cost from multiple tasks

Each acceleration-level task can be written in the affine form \( \mathbf{A}_k \ddot{q} - \mathbf{b}_k = \mathbf{0} \), e.g. for CoM:

\[ \mathbf{A}_{\text{com}} = \mathbf{J}_{\text{com}}(q),\quad \mathbf{b}_{\text{com}} = \ddot{\mathbf{p}}_{\text{com}}^{\star} - \dot{\mathbf{J}}_{\text{com}}\dot{q}. \]

We embed these into a quadratic cost

\[ J(z) = \sum_{k \in \{\text{com},s,\text{post}\}} \frac{1}{2} w_k \bigl\| \mathbf{A}_k \ddot{q} - \mathbf{b}_k \bigr\|^2 + \frac{1}{2} w_{\text{reg}}\|\ddot{q}\|^2, \]

where \( w_k > 0 \) are task weights and \( w_{\text{reg}} \) regularizes the solution. Stacking all tasks we obtain the standard QP form

\[ \min_{z} \;\; \frac{1}{2} z^{\top}\mathbf{H}z + \mathbf{g}^{\top}z. \]

In particular, if the cost depends only on \( \ddot{q} \), \( \mathbf{H} \) has a block-diagonal structure with a nonzero \( \ddot{q} \) block and zeros elsewhere (for \( \boldsymbol{\tau} \), \( \boldsymbol{\lambda} \)).

3.2 Equality and inequality constraints

We incorporate the dynamics and contact constraints as equalities:

• Dynamics:

  \[ \mathbf{M}(q)\ddot{q} + \mathbf{h}(q,\dot{q}) - \mathbf{S}^{\top}\boldsymbol{\tau} - \mathbf{J}_c^{\top}\boldsymbol{\lambda} = \mathbf{0}. \]

• Stance foot contact:

  \[ \mathbf{J}_c(q)\ddot{q} + \dot{\mathbf{J}}_c(q,\dot{q})\dot{q} = \mathbf{0}. \]

These equalities can be written compactly as \( \mathbf{A}_{\text{eq}} z = \mathbf{b}_{\text{eq}} \).

Inequality constraints encode actuation limits and friction cones:

• Torque limits:

  \[ \boldsymbol{\tau}_{\min} \le \boldsymbol{\tau} \le \boldsymbol{\tau}_{\max}. \]

• Linearized friction cone:

  \[ \mathbf{F}\boldsymbol{\lambda} \le \mathbf{0}, \]

  where \( \mathbf{F} \) encodes the facets of the friction pyramid at the stance foot.

In matrix form we have \( \mathbf{A}_{\text{ineq}} z \le \mathbf{b}_{\text{ineq}} \).

The resulting whole-body QP for the humanoid case study is therefore:

\[ \begin{aligned} &\min_{z} && \frac{1}{2} z^{\top}\mathbf{H}z + \mathbf{g}^{\top}z \\ &\text{subject to} && \mathbf{A}_{\text{eq}}z = \mathbf{b}_{\text{eq}}, \\ & & & \mathbf{A}_{\text{ineq}}z \le \mathbf{b}_{\text{ineq}}. \end{aligned} \]

From a control perspective, the QP acts as a multi-objective inverse kinematics and inverse dynamics solver that respects contact and actuation limits.

We now sketch a Python implementation using pinocchio for rigid-body dynamics and qpsolvers (or any similar QP package) for solving the whole-body QP. The code illustrates how to build task matrices and constraints; it omits robot-specific details such as URDF paths.

import numpy as np
import pinocchio as pin
from qpsolvers import solve_qp

# Assume we have a Pinocchio model of a simple humanoid
# with data structure `model`, `data` already created.
# CoM and foot frames:
com_ref = np.array([0.0, 0.0, 0.8])   # desired CoM
swing_ref = np.array([0.1, 0.1, 0.4]) # desired swing foot position
posture_ref = None  # to be set to a nominal configuration q0

Kp_com = 50.0
Kd_com = 10.0
Kp_swing = 150.0
Kd_swing = 20.0
Kp_post = 20.0
Kd_post = 5.0

def build_whole_body_qp(model, data, q, qd, support_frame, swing_frame):
    """
    Build the matrices H, g, Aeq, beq, Aineq, bineq
    for a simple whole-body QP with CoM, swing foot,
    and posture tasks.
    """
    nv = model.nv  # velocity dimension
    na = model.nv  # assume all joints actuated for simplicity

    # 1) Compute dynamics terms
    pin.computeAllTerms(model, data, q, qd)
    M = data.M.copy()
    h = data.nle.copy()   # non-linear effects

    # Selection matrix for actuated joints (identity here)
    S = np.eye(na, nv)

    # 2) CoM quantities
    com_pos = pin.centerOfMass(model, data, q)
    com_vel = data.vcom[0]
    J_com = pin.jacobianCenterOfMass(model, data, q)

    com_pos_err = com_ref - com_pos
    com_vel_err = -com_vel
    com_acc_des = (0.0
                   + Kp_com * com_pos_err
                   + Kd_com * com_vel_err)

    # CoM task: J_com * qdd = com_acc_des - Jdot_com * qd
    # Pinocchio does not give Jdot_com directly; we can approximate with finite difference
    # or use a dedicated function in more complete code.
    Jdot_com_qd = np.zeros(3)  # placeholder for simplicity
    A_com = J_com
    b_com = com_acc_des - Jdot_com_qd

    # 3) Swing foot task
    # Frame placement and Jacobian
    swing_id = model.getFrameId(swing_frame)
    pin.updateFramePlacement(model, data, swing_id)
    x_swing = data.oMf[swing_id].translation
    J_swing = pin.computeFrameJacobian(
        model, data, q, swing_id, pin.LOCAL_WORLD_ALIGNED
    )[:3, :]

    # Approximate velocity and Jdot * qd
    swing_vel = J_swing.dot(qd)
    x_err = swing_ref - x_swing
    v_err = -swing_vel
    acc_des_swing = (0.0
                     + Kp_swing * x_err
                     + Kd_swing * v_err)
    Jdot_swing_qd = np.zeros(3)  # placeholder

    A_swing = J_swing
    b_swing = acc_des_swing - Jdot_swing_qd

    # 4) Posture task
    global posture_ref
    if posture_ref is None:
        posture_ref = q.copy()
    q_err = posture_ref - q
    qd_err = -qd
    acc_des_post = (0.0
                    + Kp_post * q_err
                    + Kd_post * qd_err)
    A_post = np.eye(nv)
    b_post = acc_des_post

    # Stack tasks into least-squares cost:
    w_com = 1.0
    w_swing = 1.0
    w_post = 0.1
    w_reg = 1e-4

    A_tasks = np.vstack([
        np.sqrt(w_com) * A_com,
        np.sqrt(w_swing) * A_swing,
        np.sqrt(w_post) * A_post
    ])
    b_tasks = np.concatenate([
        np.sqrt(w_com) * b_com,
        np.sqrt(w_swing) * b_swing,
        np.sqrt(w_post) * b_post
    ])

    # Cost: 0.5 * ||A_tasks * qdd - b_tasks||^2 + 0.5 * w_reg ||qdd||^2
    H_qdd = A_tasks.T @ A_tasks + w_reg * np.eye(nv)
    g_qdd = -A_tasks.T @ b_tasks

    # Decision vector: z = [qdd, tau] (ignore contact forces for simplicity)
    # Introduce approximate dynamics: M * qdd + h = S.T * tau
    # so tau = (S^-T) * (M * qdd + h); here S is identity.
    # We eliminate tau instead of including it in z.
    H = H_qdd
    g = g_qdd

    # Equality constraints: contact acceleration (stance foot)
    support_id = model.getFrameId(support_frame)
    pin.updateFramePlacement(model, data, support_id)
    Jc = pin.computeFrameJacobian(
        model, data, q, support_id, pin.LOCAL_WORLD_ALIGNED
    )[:3, :]
    Jcdot_qd = np.zeros(3)  # placeholder
    Aeq = Jc
    beq = -Jcdot_qd

    # Inequalities (e.g. joint acceleration bounds)
    qdd_max = 20.0 * np.ones(nv)
    qdd_min = -20.0 * np.ones(nv)
    Aineq = np.vstack([np.eye(nv), -np.eye(nv)])
    bineq = np.concatenate([qdd_max, -qdd_min])

    return H, g, Aeq, beq, Aineq, bineq

def solve_whole_body_qp(H, g, Aeq, beq, Aineq, bineq):
    # qpsolvers interface: solve_qp(P, q, G, h, A, b)
    qdd_star = solve_qp(H, g, Aineq, bineq, Aeq, beq, solver="osqp")
    return qdd_star
      

In a full implementation, one would: (i) use exact expressions for \( \dot{\mathbf{J}}_k\dot{q} \), (ii) keep \( \boldsymbol{\lambda} \) and \( \boldsymbol{\tau} \) as decision variables in the QP, (iii) include friction inequalities, and (iv) wrap this computation inside a real-time control loop.

In C++, it is natural to combine Eigen for linear algebra, pinocchio or another rigid-body library for dynamics, and OSQP-Eigen for QP solving. Below is a minimal sketch that assumes that the task matrices for the legged case have already been assembled into H, g, Aeq, and beq.

#include <Eigen/Dense>
#include <OsqpEigen/OsqpEigen.h>

// H, g, Aeq, beq, Aineq, bineq constructed elsewhere
struct WholeBodyQP {
  Eigen::MatrixXd H;
  Eigen::VectorXd g;
  Eigen::MatrixXd Aeq;
  Eigen::VectorXd beq;
  Eigen::MatrixXd Aineq;
  Eigen::VectorXd bineq;
};

Eigen::VectorXd solveWholeBodyQP(const WholeBodyQP& qp) {
  const int nv = static_cast<int>(qp.H.rows());

  OsqpEigen::Solver solver;
  solver.settings().setVerbosity(false);
  solver.settings().setWarmStart(true);

  // OSQP uses sparse matrices
  solver.data()->setNumberOfVariables(nv);
  solver.data()->setNumberOfConstraints(
      qp.Aeq.rows() + qp.Aineq.rows()
  );

  // Build constraint matrix and bounds:
  Eigen::MatrixXd Aall(qp.Aeq.rows() + qp.Aineq.rows(), nv);
  Aall << qp.Aeq,
          qp.Aineq;

  Eigen::VectorXd lower(qp.Aeq.rows() + qp.Aineq.rows());
  Eigen::VectorXd upper(qp.Aeq.rows() + qp.Aineq.rows());

  // Equalities: Aeq * z = beq
  lower.head(qp.Aeq.rows()) = qp.beq;
  upper.head(qp.Aeq.rows()) = qp.beq;

  // Inequalities: Aineq * z <= bineq
  lower.tail(qp.Aineq.rows()) =
      Eigen::VectorXd::Constant(qp.Aineq.rows(), -OsqpEigen::INFTY);
  upper.tail(qp.Aineq.rows()) = qp.bineq;

  solver.data()->setHessianMatrix(qp.H.sparseView());
  solver.data()->setGradient(qp.g);
  solver.data()->setLinearConstraintsMatrix(Aall.sparseView());
  solver.data()->setLowerBound(lower);
  solver.data()->setUpperBound(upper);

  if (!solver.initSolver()) {
    throw std::runtime_error("OSQP init failed");
  }

  if (!solver.solve()) {
    throw std::runtime_error("OSQP solve failed");
  }

  return solver.getSolution();
}
      

The robot-specific part is the construction of the WholeBodyQP matrices from CoM, swing-foot, posture tasks and contact/dynamics constraints, exactly as in the Python sketch but using C++ bindings of the robotics library.

While Java is less common in low-level whole-body control, we can still implement the QP using libraries such as EJML (for linear algebra) and ojAlgo (for optimization). The following sketch assumes a simplified QP with only inequality bounds on joint accelerations and lumped tasks into H and g.

import org.ejml.simple.SimpleMatrix;
import org.ojalgo.optimisation.ExpressionsBasedModel;
import org.ojalgo.optimisation.Variable;
import org.ojalgo.optimisation.Optimisation.Result;

public class WholeBodyQPJava {

    public static double[] solveQP(SimpleMatrix H, SimpleMatrix g,
                                   double[] qddMin, double[] qddMax) {

        int n = H.numRows();
        ExpressionsBasedModel model = new ExpressionsBasedModel();

        Variable[] qddVars = new Variable[n];
        for (int i = 0; i < n; ++i) {
            qddVars[i] = Variable.make("qdd" + i)
                    .lower(qddMin[i])
                    .upper(qddMax[i]);
            model.addVariable(qddVars[i]);
        }

        // Quadratic objective: 0.5 * qdd^T H qdd + g^T qdd
        var expr = model.addExpression("cost");
        for (int i = 0; i < n; ++i) {
            expr.set(qddVars[i], g.get(i, 0));
            for (int j = 0; j < n; ++j) {
                double hij = 0.5 * H.get(i, j);
                expr.setQuadraticFactor(qddVars[i], qddVars[j], hij);
            }
        }
        expr.weight(1.0);

        Result result = model.minimise();
        double[] qdd = new double[n];
        for (int i = 0; i < n; ++i) {
            qdd[i] = result.get(i).doubleValue();
        }
        return qdd;
    }
}
      

The dynamics and task construction steps mirror the Python and C++ implementations; only the linear algebra and optimization libraries change. The state update and low-level torque commands would typically be handled in a real-time loop written in another language or via JNI.

MATLAB is widely used for rapid prototyping of whole-body controllers. We can use the Robotics System Toolbox for kinematics/dynamics and quadprog for the QP. A simple script for the legged case is:

% Assume rigidBodyTree object 'humanoid' from Robotics System Toolbox
% and kinematics helpers have been defined.
function qdd_star = wholeBodyQP_step(humanoid, q, qd, supportBody, swingBody)

  % Compute dynamics
  [M, C, G] = manipulatorDynamics(humanoid, q, qd);
  h = C * qd.' + G;

  % CoM Jacobian and position
  Jcom = centerOfMassJacobian(humanoid, q);
  pcom = centerOfMass(humanoid, q);

  pcomRef = [0, 0, 0.8];
  Kp_com = 50; Kd_com = 10;

  vcom = Jcom * qd.';
  comErr = pcomRef - pcom;
  comVelErr = -vcom;
  comAccDes = Kp_com * comErr + Kd_com * comVelErr;
  JdotComQd = zeros(1, size(Jcom, 2)); % placeholder

  Acom = Jcom;
  bcom = comAccDes - JdotComQd;

  % Swing foot task
  swingId = bodyIndex(humanoid, swingBody);
  [Tswing, Jswing] = forwardKinematics(humanoid, q, swingId);
  xswing = tform2trvec(Tswing);
  vswing = Jswing * qd.';

  swingRef = [0.1, 0.1, 0.4];
  Kp_s = 150; Kd_s = 20;
  xErr = swingRef - xswing;
  vErr = -vswing;
  accSwingDes = Kp_s * xErr + Kd_s * vErr;
  JdotSwingQd = zeros(1, size(Jswing, 2)); % placeholder

  Aswing = Jswing(1:3, :);
  bswing = accSwingDes - JdotSwingQd(1:3);

  % Posture task
  q0 = q; % nominal posture
  Kp_q = 20; Kd_q = 5;
  qErr = q0 - q;
  qdErr = -qd;
  accPostDes = Kp_q * qErr + Kd_q * qdErr;

  Apost = eye(numel(q));
  bpost = accPostDes;

  % Stack tasks
  w_com = 1.0; w_swing = 1.0; w_post = 0.1; w_reg = 1e-4;

  At = [sqrt(w_com) * Acom;
        sqrt(w_swing) * Aswing;
        sqrt(w_post) * Apost];

  bt = [sqrt(w_com) * bcom.';
        sqrt(w_swing) * bswing.';
        sqrt(w_post) * bpost.'];

  H = At.' * At + w_reg * eye(size(At, 2));
  f = -At.' * bt;

  % Contact acceleration constraint (holonomic, stance foot)
  supportId = bodyIndex(humanoid, supportBody);
  [Tsup, Jsup] = forwardKinematics(humanoid, q, supportId);
  Jc = Jsup(1:3, :);
  JcdotQd = zeros(3, 1); % placeholder

  Aeq = Jc;
  beq = -JcdotQd;

  % Acceleration bounds
  n = numel(q);
  qddMax = 20 * ones(n, 1);
  qddMin = -20 * ones(n, 1);
  Aineq = [eye(n); -eye(n)];
  bineq = [qddMax; -qddMin];

  opts = optimoptions('quadprog', 'Display', 'off');
  qdd_star = quadprog(H, f, Aineq, bineq, Aeq, beq, [], [], [], opts).';
end
      

In Simulink, this function can be wrapped inside a MATLAB Function block that runs at the control rate; the block inputs are \( q \), \( \dot{q} \), contact information, and reference trajectories, while the outputs are \( \ddot{q}^{\star} \) or directly \( \boldsymbol{\tau} \) via \( \mathbf{M}(q)\ddot{q}^{\star} + \mathbf{h} \).

In Wolfram Mathematica, the acceleration-level QP can be encoded using QuadraticOptimization or NMinimize. The following sketch considers only joint accelerations \( \ddot{q} \) as decision variables and aggregates CoM, swing, and posture tasks:

(* Dimensions *)
nv = 6; (* for example *)

(* Symbolic decision variables for joint accelerations *)
qdd = Array[qdd, nv];

(* Task matrices and vectors (numeric arrays from kinematics/dynamics) *)
Acom   = { {(* ... fill with J_com entries ... *)} };
bcom   = {(* ... com_acc_des - Jdot_com.qd ... *)};

Aswing = { {(* ... J_swing ... *)} };
bswing = {(* ... swing_acc_des - Jdot_swing.qd ... *)};

Apost  = IdentityMatrix[nv];
bpost  = {(* ... posture_acc_des ... *)};

wcom   = 1.0;
wswing = 1.0;
wpost  = 0.1;
wreg   = 10.^(-4);

At = Join[
  Sqrt[wcom]  Acom,
  Sqrt[wswing] Aswing,
  Sqrt[wpost] Apost
];

bt = Join[
  Sqrt[wcom]  bcom,
  Sqrt[wswing] bswing,
  Sqrt[wpost] bpost
];

H = Transpose[At].At + wreg IdentityMatrix[nv];
g = -Transpose[At].bt;

(* Contact constraint Jc qdd + Jcdot_qd == 0 *)
Jc        = { {(* ... stance foot Jacobian rows ... *)} };
Jcdot_qd  = {(* ... precomputed numeric vector ... *)};

eqConstr = {
  Jc.qdd + Jcdot_qd == ConstantArray[0, Length[Jcdot_qd]]
};

(* Acceleration bounds *)
qddMin = -20 ConstantArray[1, nv];
qddMax =  20 ConstantArray[1, nv];

ineqConstr = Thread[qddMin <= qdd <= qddMax];

(* Quadratic optimization *)
cost[qv_] := 1/2 qv.H.qv + g.qv;

sol = NMinimize[
  { cost[qdd], Join[eqConstr, ineqConstr] },
  qdd
];

qddStar = qdd /. Last[sol];
      

In a more advanced setup, one may represent the kinematics symbolically and generate optimized C code from Mathematica, but that lies beyond the scope of this case study.

Problem 1 (Contact constraint derivation). For a stance foot point with Cartesian position \( \mathbf{x}_c(q) \), show that enforcing zero velocity and acceleration at the contact yields the constraint \( \mathbf{J}_c(q)\ddot{q} + \dot{\mathbf{J}}_c(q,\dot{q})\dot{q} = \mathbf{0} \). Explain its physical meaning.

Solution. The stance foot velocity is \( \dot{\mathbf{x}}_c = \mathbf{J}_c(q)\dot{q} \). Differentiating once more in time:

\[ \ddot{\mathbf{x}}_c = \frac{\mathrm{d}}{\mathrm{d}t} \bigl( \mathbf{J}_c(q)\dot{q} \bigr) = \dot{\mathbf{J}}_c(q,\dot{q})\dot{q} + \mathbf{J}_c(q)\ddot{q}. \]

Rigid contact with the environment requires \( \dot{\mathbf{x}}_c = \mathbf{0} \) and \( \ddot{\mathbf{x}}_c = \mathbf{0} \). Thus,

\[ \mathbf{J}_c(q)\ddot{q} + \dot{\mathbf{J}}_c(q,\dot{q})\dot{q} = \mathbf{0}, \]

which is the desired constraint. Physically, it ensures that the contact point neither moves nor accelerates relative to the ground, preventing slip and lift-off at the acceleration level.

Problem 2 (Closed-form solution for 1-DOF two-task QP). Consider a single joint with acceleration \( \ddot{q} \) and two tasks expressed as desired accelerations \( a_1^{\star} \) (e.g. CoM-related) and \( a_2^{\star} \) (e.g. posture-related). The QP is

\[ \min_{\ddot{q}} \; \frac{1}{2} w_1(\ddot{q} - a_1^{\star})^2 + \frac{1}{2} w_2(\ddot{q} - a_2^{\star})^2, \]

with \( w_1, w_2 > 0 \). Derive the optimal \( \ddot{q}^{\star} \) explicitly.

Solution. Differentiate the cost with respect to \( \ddot{q} \), set the derivative to zero:

\[ \frac{\partial}{\partial \ddot{q}} \left( \frac{1}{2} w_1(\ddot{q} - a_1^{\star})^2 + \frac{1}{2} w_2(\ddot{q} - a_2^{\star})^2 \right) = w_1(\ddot{q} - a_1^{\star}) + w_2(\ddot{q} - a_2^{\star}) = 0. \]

Solving for \( \ddot{q} \):

\[ (w_1 + w_2)\ddot{q} = w_1 a_1^{\star} + w_2 a_2^{\star} \quad\Rightarrow\quad \ddot{q}^{\star} = \frac{w_1 a_1^{\star} + w_2 a_2^{\star}}{w_1 + w_2}. \]

Thus the optimal acceleration is a weighted average of the two task accelerations, consistent with the general multi-task QP structure.

Problem 3 (Limit of large task weight and hierarchy). In the previous problem, assume that task 1 (e.g. CoM regulation) is more important than task 2. Show that, as \( w_1 \to \infty \) with \( w_2 \) fixed, the solution satisfies \( \ddot{q}^{\star} \to a_1^{\star} \). Interpret this as an approximation of strict task hierarchy.

Solution. We have

\[ \ddot{q}^{\star}(w_1, w_2) = \frac{w_1 a_1^{\star} + w_2 a_2^{\star}}{w_1 + w_2}. \]

Rewrite as

\[ \ddot{q}^{\star} = a_1^{\star} + \frac{w_2}{w_1 + w_2} (a_2^{\star} - a_1^{\star}). \]

As \( w_1 \to \infty \), the factor \( \frac{w_2}{w_1 + w_2} \to 0 \), hence

\[ \lim_{w_1 \to \infty} \ddot{q}^{\star} = a_1^{\star}. \]

In the limit of very large weight, the solution enforces task 1 exactly while task 2 only influences the solution when there is residual redundancy. This illustrates how large weights approximate strict task priority in a QP, though true hierarchical QPs are numerically more robust.

Problem 4 (Stacked task ordering for a legged humanoid). For the standing-on-one-foot case, propose a priority ordering of the following tasks and discuss the rationale:

• (A) Contact consistency for the stance foot.
• (B) CoM regulation.
• (C) Swing foot tracking.
• (D) Posture regularization.

Solution. A natural ordering from highest to lowest priority is (A) → (B) → (C) → (D). The reasoning can be summarized by the following schematic:

flowchart TD
  ROOT["Start"] --> T1["Highest: stance contact constraints (A)"]
  T1 --> T2["High: balance via com regulation (B)"]
  T2 --> T3["Medium: swing foot tracking (C)"]
  T3 --> T4["Low: posture regularization (D)"]
  T4 --> SOL["Solve QP and send torques"]
        

Contact consistency (A) is essential for physical feasibility; breaking it invalidates the whole model. CoM regulation (B) is critical for balance; a loss of CoM control leads to falls. Swing foot tracking (C) is important but can be sacrificed temporarily to avoid losing balance. Posture regularization (D) is purely aesthetic / comfort-related and should never compromise the higher-priority tasks.

In this lesson we applied the general whole-body QP control framework to a concrete legged / humanoid scenario: balancing on one foot while regulating the CoM, tracking a swing-foot trajectory, and maintaining a comfortable posture. We expressed each task as an acceleration-level linear relation in \( \ddot{q} \), aggregated them into a quadratic cost, and enforced dynamics, contact, and actuation limits as equality and inequality constraints.

We then showed how to implement this controller sketch in multiple programming environments (Python, C++, Java, MATLAB/Simulink, Wolfram Mathematica), emphasizing the construction of the QP matrices rather than library-specific details. Finally, we solved several analytical problems to deepen intuition about contact constraints, multi-task blending, and hierarchical priorities. These ideas provide a bridge between abstract whole-body control theory and practical implementations on legged and humanoid robots.