Hierarchical Inverse Dynamics Controllers

• Hierarchical inverse dynamics controllers are control architectures that use cascaded quadratic programs to enforce prioritized tasks and physical constraints in whole-body robot control.
• They ensure real-time, robust performance by employing null-space projections and analytic elimination to manage redundant degrees of freedom and actuator limits.
• The integration of momentum-based control through PD and LQR approaches enables precise balance, trajectory tracking, and compliance under dynamic disturbances.

Hierarchical inverse dynamics controllers are control architectures for whole-body robot control that enforce a strict or soft prioritization of multiple tasks and physical constraints within the joint and contact-level dynamics of floating-base robots via sequential or cascaded optimization. These schemes formulate all tasks (e.g., momentum regulation, tracking, posture, contact consistency) as affine functions of generalized joint accelerations, contact forces, and joint torques, and enforce them in real time through cascades of quadratic programs (QPs) or hierarchical nonlinear least-squares programs. This design enables optimal use of redundant degrees of freedom while guaranteeing satisfaction of high-priority physical constraints even under actuator limits, external disturbances, model mismatches, and estimation errors.

1. Mathematical Formulation and Problem Structure

The core computational building block is the full rigid-body dynamics of a floating-base robot: $$M(q)\,\ddot{q} + N(q, \dot{q}) = S^T\tau + J_c^T\lambda$$ where $q = [q_j^T\; x^T]^T$ is the $\mathbb{R}^{n+6}$ configuration vector partitioned into actuated joint positions ($n$ DoF) and floating-base pose, $M(q)\in\mathbb{R}^{(n+6)\times(n+6)}$ is the inertia matrix, $N(q,\dot{q})$ aggregates Coriolis, centrifugal, gravitational, and friction terms, $S$ selects actuated joints, $\tau$ are the joint torques, $J_c$ is the contact Jacobian for all $c$ patches, and $\lambda$ are the generalized contact forces (Herzog et al., 2014, Herzog et al., 2013).

Physical consistency and actuation constraints are encoded as affine equalities or inequalities over decision variables $y = [\ddot{q}^T,\,\lambda^T,\,\tau^T]^T$, such as:

• Contact kinematics: $J_c\ddot{q} + \dot{J}_c\dot{q} = 0$
• Friction cones and CoP: linear inequalities in $\lambda$
• Joint and torque limits: $\tau_{\min}\leq \tau\leq \tau_{\max}$, $\ddot{q}_{\min}\leq \ddot{q}_j\leq \ddot{q}_{\max}$

Every task (e.g., operational space acceleration, momentum regulation) is encoded similarly as

$A\,y + a\leq 0\qquad B\,y + b = 0$

2. Cascade of Quadratic Programs and Hierarchical Prioritization

Tasks and constraints are organized into a strict hierarchy. At each priority level $r$, a single-level QP is solved: $\min_{y, v, w} \|v\|^2 + \|w\|^2 + \epsilon\|y\|^2$ subject to

$$V_r(A_r y + a_r) \leq v, \qquad W_r(B_r y + b_r) = w$$

where $v$ and $w$ are slack variables for the soft satisfaction of inequalities and equalities, $V_r$, $W_r$ encode row weights, and $\epsilon$ regularizes the Hessian.

After solving level-$r$, the solution is projected into the null space of previous levels’ equalities (basis $Z_r$) to guarantee that higher-priority tasks are unaffected by solutions at lower levels: $y = y_r^* + Z_r u_{r+1}$ This process is repeated recursively. For hard-priority hierarchies (as in (Herzog et al., 2014, Herzog et al., 2013)), no lower-priority task can degrade higher-priority objectives. Soft-priority (e.g., by weighted least-squares and large $w_i$) can be used if strict priority is not required (Zafar et al., 2018).

Significant computational acceleration is achieved by exploiting the block structure of the equations of motion (upper block for actuated DoFs, lower 6 for the floating base, i.e., Newton-Euler equations). This allows for analytic elimination of torque variables and reduces the size of the optimization problem by up to 40%, enabling real-time 1 kHz performance on 14–25 DoF humanoids (Herzog et al., 2014, Herzog et al., 2013).

The hierarchical Newton’s method, as introduced in (Pfeiffer et al., 2023), generalizes this approach to prioritized nonlinear least-squares, leveraging true second-order information through analytic Hessian assembly and KKT systems to ensure numerical stability and robust handling of kinematic/algorithmic singularities.

3. Momentum-Based Control Integration

Momentum-based tasks regulate the robot’s centroidal momentum,

$$h = \begin{bmatrix} h_{\mathrm{lin}} \ h_{\mathrm{ang}} \end{bmatrix} = H_G(q)\,\dot{q}$$

where $H_G(q)\in\mathbb{R}^{6\times(n+6)}$ is the centroidal momentum matrix.

Both kinematic ($\dot{h} = H_G\,\ddot{q} + \dot{H}_G\,\dot{q}$) and force-based ($\dot{h} = J_\lambda \lambda + [mg; 0]$) expressions can be enforced as equality tasks in the hierarchy.

Reference tracking of centroidal momentum is typically synthesized via:

• PD-like control: $\dot{h}_{\mathrm{ref}} = P\begin{bmatrix}m(x_{\mathrm{cog,des}}-x_{\mathrm{cog}})\0\end{bmatrix} + D(h_{\mathrm{des}}-h) + \dot{h}_{\mathrm{des}}$
• LQR-based control: feedback gain $K$ and feedforward $k_{\mathrm{ff}}$ computed from the solution to the algebraic Riccati equation on the linearized momentum-state dynamics. This LQR control law is incorporated as an equality in the QP stack ($J_\lambda(\lambda + Kx - k_{\mathrm{ff}}) = 0$), typically at intermediate priority (Herzog et al., 2014, Herzog et al., 2013).

4. Implementation and Computational Considerations

Hierarchical inverse dynamics controllers have been implemented and experimentally validated on torque-controlled humanoids (Sarcos lower body, 14–25 DoF) at 1 kHz cycle rates with peak QP cascade solve times of 0.4–0.9 ms due to the analytic elimination strategy. The Goldfarb–Idnani dual active-set QP solver, supported by efficient SVD-based null-space projections, is employed to enforce strict priorities in real time (Herzog et al., 2014, Herzog et al., 2013).

Low-level torque tracking is achieved via PID and velocity feedback control on hydraulic actuators, resulting in $<5$ Nm torque error under dynamic motion. State estimation fuses joint encoders and pelvis-mounted IMUs via extended Kalman filter, robust to contact switches and sensor noise.

Hierarchical formulations degrade gracefully under model mismatch due to the soft-slack relaxation of lower-priority tasks, ensuring that violations of strict constraints (e.g., friction or CoP) yield compliance rather than instability (Herzog et al., 2014, Herzog et al., 2013).

5. Experimental Validation and Performance Metrics

Hierarchical inverse dynamics controllers have been validated on tasks including:

• Push-recovery (double-support): robot withstands impulsive pushes at the torso (up to 290 N, 9.5 Ns) with center of gravity (CoG) position maintained within 2–5 cm, angular momentum damped in $<0.2$ s. LQR-based momentum gains achieve tighter CoG bounds (~2–3 cm vs. ~4–5 cm for diagonal PD) (Herzog et al., 2014).
• Balancing on moving platforms and seesaw: robot’s feet remain flat, with strict maintenance of CoP constraints and no foot slippage.
• CoG-tracking: experiments demonstrate tracking of low-frequency vertical/horizontal trajectories, achieving $<$2 cm tracking error (Herzog et al., 2014, Herzog et al., 2013).
• Single-support: robot transitions via force-unloading and maintains balance after lateral pushes ($\sim$150–270 N), with CoG error under 3 cm and robust swing-foot trajectory (Herzog et al., 2014).

The method does not require joint-space PD controllers: all feedback is executed at the task/operational space level, exploiting full model-based dynamics and prioritized task organization.

6. Extensions, Variants, and Related Architectures

Soft hierarchies via weighted residuals (single-QP, large $w_i$), as used for whole-body control of the Wheeled Inverted Pendulum humanoid (Zafar et al., 2018), enable computational scaling and flexibility at the cost of approximate prioritization. Multi-tiered architectures have been demonstrated in mobile manipulation, with high-level MPC shaping zero dynamics and low-level QPs enforcing hard physical constraints, achieving solve times $<1$ ms per tick at body-control rates and horizon planning over 0.5–1 s (Zafar et al., 2018).

The hierarchical Newton’s method (Pfeiffer et al., 2023) introduces a framework for prioritized nonlinear least-squares control that brings analytic second-order curvature, spectral regularization, and robust singularity handling to velocity- and acceleration-domain control, outperforming weighted-LS and other prior approaches, and reliably achieving real-time performance ($\sim$200 Hz, HRP-2Kai humanoid).

7. Practical Insights and Limitations

The demonstrated schemes highlight several recurrent features:

• Real-time feasibility at high DoF due to equation-of-motion decomposition and null-space projection.
• Robustness to model errors and noisy state estimation; moderate errors in inertia modeling and CoP prediction (e.g., up to 2 cm) do not compromise task-level performance.
• Task hierarchy ensures that if strict physical constraints are saturated (e.g., friction limits), lower-priority behaviors yield automatically.
• LQR-based momentum task integration automates gain tuning and implicitly adapts to changes in contact state and geometry.
• Proposed architectures scale to full-body, multi-contact, and underactuated platforms; further solver engineering remains critical for next-generation complexity.

These results, as demonstrated by Herzog et al., show that hierarchical inverse dynamics control enables torque-level, multi-task, whole-body regulation with strict priority enforcement and robustness under practical uncertainty on torque-controlled robots (Herzog et al., 2014, Herzog et al., 2013, Zafar et al., 2018, Pfeiffer et al., 2023).