Humanoid Whole-Body Controllers

• Humanoid Whole-Body Controllers are unified frameworks that compute joint torques and contact forces to coordinate multi-task regulation in high degree-of-freedom systems.
• They integrate model-based QP/ID-stack and learning-based policy approaches to achieve agile, stable, and natural multi-contact motions.
• These controllers incorporate safety measures through control barrier functions and sensor-driven multi-contact strategies to manage complex constraints and task hierarchies.

Humanoid Whole-Body Controllers (WBCs) are unified control frameworks that compute joint torques (and optionally contact forces) for high degree-of-freedom floating-base systems, enabling simultaneous regulation of tasks such as balance, locomotion, manipulation, and compliance under a common real-time optimization layer. WBCs coordinate all limbs—legs, arms, torso, and head—under a global optimization framework that respects robot dynamics, contact feasibility, and multi-objective task hierarchies. Modern humanoid WBCs span model-based QP/ID-stack approaches and learning-based high-dimensional policy networks, enabling stable, agile, and natural multi-contact motions for both teleoperation and autonomy.

1. Mathematical Foundations and Formal Structures

Humanoid WBCs universally adopt the floating-base rigid body dynamics formalism, where the generalized configuration $q\in\mathbb{R}^{n+6}$ encodes 6-DoF base and $n$ joints, and the equations of motion are

$$M(q)\,\ddot q + C(q,\dot q)\dot q + g(q) = S^T\tau + J_c^T(q)f_c$$

with $M$ the inertia matrix, $C$ the Coriolis/centrifugal forces, $g$ gravity, $S$ the actuator selection, $J_c$ the contact Jacobian, and $f_c$ the stacked contact wrenches (Fok et al., 2015, Paredes et al., 2023, Bang et al., 2022). This structure forms the basis for both task-space and joint-space WBC formulations.

Task Formulation: Whole-body tasks are defined in operational space via Jacobians $J_i(q)$ for each primitive (CoM, end effectors, posture, etc.), and are encoded either as desired accelerations, velocities, or wrenches, depending on the WBC flavor.

Nullspace Hierarchy and Prioritization: Classical frameworks enforce strict task priorities using dynamically consistent nullspace projectors $N_i = I - J_i^\# J_i$, ensuring that lower-priority tasks do not disturb higher-priority objectives (Fok et al., 2015). Hierarchical (stack-of-tasks) formulations permit hard priorities or weighted soft-task QP stacks (Darvish et al., 2019, Sovukluk et al., 24 Jul 2025).

2. Model-Based WBC Methodologies and Task Stacking

2.1 Hierarchical Inverse-Dynamics QP/ID-WBC

Quadratic Program Core: At each control step, a QP solves for joint accelerations/velocities $\nu$ (or torques) that best achieve multiple weighted task objectives, subject to physical constraints: $$\min_\nu \sum_i w_i \| J_i(q)\nu - v_i^* \|^2 + \lambda\|\nu\|^2 \qquad \text{s.t.} \quad \begin{cases} A_\text{eq}(q)\nu = b_\text{eq} \ A_\text{ineq}(q)\nu \le b_\text{ineq} \end{cases}$$ where $w_i$ are task weights and $\lambda$ regularizes control effort (Darvish et al., 2019, Sovukluk et al., 24 Jul 2025, Bang et al., 2022).

Task Examples:

• Momentum-based balancing: primary task is centroidal momentum rate regulation, secondary is postural tracking (Darvish et al., 2019).
• DCM-based walking: stack includes DCM stabilization, CoM tracking, swing/stance foot control, and posture (Darvish et al., 2019, Kim et al., 2017).
• Operational space: Prioritized stacks for coordinated wrists, orientation, and posture tasks (Fok et al., 2015, Bang et al., 2022).

Constraint Handling: Joint, velocity, torque, friction-cone, and ZMP constraints are encoded as QP inequalities. Contact feasibility and force unilaterality are explicitly imposed (Darvish et al., 2019, Paredes et al., 2023, Bang et al., 2022).

2.2 Passivity-Based WBC and Force-Space Controllers

Passivity-based WBCs (PB-WBC) formulate control in task force space for uniform impedance regulation and stability guarantees: $$\tau = M(q)\,\ddot q + h(q,\dot q) + J^T(q)F - \Lambda(q)\dot q$$ where $F$ are desired task-space wrenches. PB-WBCs operate via two-stage QPs (task force computation, then mapping to torques), and closed-loop passivity arises from PD+impedance structure (Sovukluk et al., 24 Jul 2025). They admit uniform gain tuning but require full-rank, invertible task Jacobians.

Robustness and Tuning: Model-based WBCs offer explicit stability margins and allow analytical tuning per task emission inertia or desired closed-loop bandwidth (Sovukluk et al., 24 Jul 2025, Darvish et al., 2019). Trade-offs exist between gain scaling and disturbance sensitivity, especially in low-inertia or highly coupled task sets.

3. Extensions: Safety, Multi-Contact, and Sensor-Rich Control

3.1 Embedded Safety via Control Barrier Functions (CBFs)

CBF-based WBCs augment QPs with linear inequalities derived from control barrier functions, ensuring forward invariance of user-defined safe sets (e.g., joint limits, self-collision, ZMP margins):

• CBF constraints: For a constraint $h(x)\geq0$, synthesize inequalities of the form $$L_F^{r_b}h(x) + L_G L_F^{r_b-1}h(x)\ddot q \geq -K_\alpha \eta_b(x)$$, where $r_b$ is relative degree (Paredes et al., 2023, Khazoom et al., 2022).
• Result: Systematic, real-time safe filtering for joint, collision, and task-space constraints without compromise on task tracking, provided feasibility (Paredes et al., 2023).

3.2 Multi-Contact and Tactile WBC

Next-generation WBCs handle whole-body multi-contact by distributing resultant wrenches to arbitrary limb surfaces (feet, hands, elbows, thighs) via tactile sensing. A centroidal MPC (commonly DDP-based) computes desired wrenches, distributed through QP-based allocation and locally regulated using impedance control at each contact patch. Surrounding the core, contact-region updates are informed by high-resolution distributed tactile sensors, enabling robust stabilization against disturbance and environmental uncertainty (Murooka et al., 26 May 2025).

3.3 Mechanical and Kinematic-Specific WBC

Customized constraint handling for nonstandard joints (e.g., rolling-contact knees), or specific mechanical artifacts, is seamlessly integrated by appending internal-constraint equations to the QP as additional linear equality constraints (Bang et al., 2022).

4. Teleoperation, Geometric Retargeting, and Human-In-The-Loop WBC

Geometric Retargeting Pipelines: Whole-body teleoperation is enabled by mapping measured human link orientations (from motion capture or exoskeleton interfaces) to robot frame via precomputed alignment rotations and dynamic inverse kinematics: $${}^{I}\!R_{R,i}^{*} = {}^{I}\!R_{H,i}\;{}^{H}\!R_{R,i} \qquad {}^{I}\!\omega_{R,i}^{*} = {}^{I}\!\omega_{H,i}$$ Cartesian targets are then resolved to joint velocities and positions by a secondary QP before feeding to the main WBC as postural references (Darvish et al., 2019).

Online Control Structure:

• Retargeter $\rightarrow$ smoothing filter $\rightarrow$ joint reference generator $\rightarrow$ WBC QP stack.
• The WBC main loop (typically 100–1000 Hz) accepts primary (balance, gait) and secondary (teleoperation, posture) tasks in priority order, enforcing that balance- or dynamic-locomotion objectives are never sacrificed for postural tracking.

Experimental Metrics:

Controller	CoM/Traj Error (mm)	Joint Tracking (deg)	Foot Placement (cm)	QP Solve Time
Balancing (iCub)	x,y: 5, z: 10	2–5	—	5 ms @ 100 Hz
Walking (iCub)	x,y: 6	~3	2	5 ms @ 100 Hz

(Darvish et al., 2019)

5. Software Frameworks, Real-Time Implementation, and Integration

Software Layering: Modern WBC frameworks (e.g., ControlIt! (Fok et al., 2015)) expose a modular, plugin-based architecture—task and constraint objects can be dynamically loaded, scheduled into hierarchical stacks, and bound to external sources via generic parameter-binding mechanisms (e.g., ROS topics, shared memory).

• Multithreaded execution: Servo, model-update, and task-update threads allow high update frequencies (up to 2 kHz), with rigorous state locking and latency minimization. Reference designs achieve servo latency of 0.5 ms on commercial hardware (Fok et al., 2015).
• Extensibility: New tasks, constraints, and robot kinematic/dynamic models can be integrated by implementing API-compliant plugins; dynamic instantiation and configuration at runtime is standard (Fok et al., 2015, Bang et al., 2022).
• Real-World Validation: Deployed on a range of platforms (iCub, Dreamer upper-body, DRACO 3, Digit), WBCs have demonstrated sub-cm-level tracking error, agile footstep placement, robust push recovery, and online re-binding of goals in manipulation contexts (Fok et al., 2015, Darvish et al., 2019, Bang et al., 2022, Paredes et al., 2023).

6. Comparison, Trade-Offs, and Practical Recommendations

Approach	Pros	Cons	Suitability
ID WBC (QP)	Task modularity, rank-deficient handling	Task gains strongly inertia-dependent	Multi-contact, flexible task-sets
PB-WBC	Uniform gain tuning, natural impedance/passivity	Requires invertible task-maps, more complex formulation	Force/impedance, stability critical
CBF-WBC	Certified forward invariance of safety sets	Challenge in tuning and feasibility management	Safety/constrained environments

• Task Hierarchy: For aggressive locomotion or disturbance rejection, prioritize momentum/DCM and foot placement; for high-connectivity manipulation, promote end-effector tasks as primaries.
• Gain Tuning: In acceleration-space WBCs, gain selection requires scaling by task inertia; in force-space WBCs, uniform tuning is possible but task-map invertibility must be ensured (Sovukluk et al., 24 Jul 2025).
• Constraint Management: Slack variables should be introduced for infeasibility in overloaded task sets, with task weights or slack penalties set accordingly (Paredes et al., 2023, Darvish et al., 2019).

7. Limitations, Outlook, and Research Directions

Contemporary model-based WBCs enable robust multi-task coordination across complex contact scenarios and support real-time teleoperation and automation for high-DoF humanoids. Nonetheless, emerging needs—zero-shot behavior synthesis, rapid adaptation, and perceptual-semantic tasking—are driving integration with large-scale learned behavioral priors and hierarchical, multimodal learning architectures. Challenges persist in sim-to-real generalization, high-dimensional constraint satisfaction, and scaling to fully interactive human-robot environments (Paredes et al., 2023, Darvish et al., 2019, Bang et al., 2022, Fok et al., 2015).

Future avenues include the integration of safety filtering (CBFs), tactile-augmented multi-contact stabilization, large-scale data-driven skill transfer, and seamless layering with higher-level perceptual and instruction-following policies. Ongoing efforts in modular software (e.g., ControlIt!), validation on both legacy (iCub, DRACO) and state-of-the-art (Digit) platforms, and explicit cross-platform retargeting pipelines continue to establish the foundational role of WBCs in humanoid research and deployment (Paredes et al., 2023, Sovukluk et al., 24 Jul 2025, Murooka et al., 26 May 2025, Bang et al., 2022, Fok et al., 2015, Darvish et al., 2019).