From floating-base dynamics to operational-space control, contact-constrained QP, balancing, and DCM walking
This post introduces the basic ideas behind Whole-Body Control (WBC) for humanoid robots. Instead of presenting WBC only as an implementation detail of a specific software package, we build the conceptual stack from floating-base dynamics to operational-space control, null-space task prioritization, contact-constrained quadratic programming, balancing, and walking pattern generation using the Divergent Component of Motion (DCM).
The examples and demonstrations are based on a PyBullet implementation using the ATLAS URDF model and PyPnC.
Humanoid robots are difficult to control because they are not fixed-base manipulators. Their bodies are high-dimensional, underactuated, and constrained by intermittent contacts with the environment. A manipulator bolted to a table can often be controlled by commanding joint motion or end-effector motion. A humanoid, however, must simultaneously decide how to move its limbs, regulate its floating base, maintain balance, and generate physically feasible contact forces at the feet or hands.
In other words, humanoid control is not just about moving joints. It is about coordinating the entire body under dynamics and contact constraints.
Whole-Body Control provides a framework for this problem. Its goal is to compute physically feasible joint torques or accelerations that satisfy robot dynamics, respect contact constraints, and achieve multiple task objectives at the same time. These tasks may include controlling the center of mass, regulating torso orientation, tracking swing-foot trajectories, maintaining stance-foot contacts, and preserving a nominal posture.
A useful way to summarize WBC is:
\[\text{WBC} = \text{task-space control} + \text{floating-base dynamics} + \text{contact constraints} + \text{prioritization}.\]This is why WBC is especially useful for humanoids. It allows the controller to reason about what the robot should accomplish with its whole body, while ensuring that the resulting motion and contact forces remain dynamically feasible.
A humanoid robot is typically modeled as a floating-base articulated system. The generalized coordinate vector is
\[q = \begin{bmatrix} q_b \\ q_j \end{bmatrix},\]where (q_b \in SE(3)) represents the six-dimensional floating base pose and (q_j \in \mathbb{R}^n) represents the actuated joint coordinates. The corresponding generalized velocity is
\[\dot{q} = \begin{bmatrix} v_b \\ \dot{q}_j \end{bmatrix}.\]The floating-base equation of motion can be written as
\[M(q)\ddot{q} + h(q, \dot{q}) = S^T \tau + J_c(q)^T \lambda,\]where (M(q)) is the mass matrix, (h(q, \dot{q})) contains Coriolis, centrifugal, and gravitational terms, (S) is the actuation selection matrix, (\tau) is the vector of actuated joint torques, (J_c(q)) is the contact Jacobian, and (\lambda) is the contact wrench or reaction force.
The selection matrix (S) is important because the floating base is not directly actuated. For a robot with (n) actuated joints, (S) selects only the actuated joint coordinates from the full floating-base system. This is one of the main differences between humanoid control and fixed-base manipulator control.
When a foot is in rigid contact with the ground, the contact point should not move relative to the environment. This gives the velocity-level contact constraint
\[J_c(q)\dot{q} = 0.\]Differentiating this constraint gives the acceleration-level constraint
\[J_c(q)\ddot{q} + \dot{J}_c(q, \dot{q})\dot{q} = 0.\]These equations express that stance feet should remain stationary during contact. In WBC, these constraints are enforced together with the robot dynamics so that the commanded motion does not violate contact consistency.
The simplest way to control a robot is to command desired joint positions. A joint-space PD controller has the form
\[\tau = K_p(q_d - q) + K_d(\dot{q}_d - \dot{q}),\]where (q_d) and (\dot{q}_d) are desired joint positions and velocities. This works well when the task itself is naturally defined in joint space. However, many robot tasks are not joint-space tasks. We usually care about the position of a hand, the orientation of a foot, the motion of the center of mass, or the pose of the torso.
Operational-Space Control (OSC) addresses this by controlling task-space quantities directly. For a task variable
\[x = f(q),\]the task velocity and acceleration are
\[\dot{x} = J(q)\dot{q},\] \[\ddot{x} = J(q)\ddot{q} + \dot{J}(q, \dot{q})\dot{q}.\]A desired task-space acceleration can be defined as
\[\ddot{x}_{\text{cmd}} = \ddot{x}_d + K_d(\dot{x}_d - \dot{x}) + K_p(x_d - x).\]The control objective is then to choose joint accelerations or torques such that
\[J(q)\ddot{q} + \dot{J}(q, \dot{q})\dot{q} \approx \ddot{x}_{\text{cmd}}.\]Joint-space control says: move each joint to this angle. Operational-space control says: move this physical body point or frame in this desired way.
For humanoid robots, this distinction is crucial. A walking robot does not merely track joint angles. It must regulate physically meaningful tasks such as center-of-mass motion, foot placement, torso orientation, and hand motion.
Humanoid robots are redundant systems. They often have more degrees of freedom than are strictly required for a given task. This redundancy allows the robot to perform secondary objectives without disturbing higher-priority tasks.
Suppose the primary task is described by
\[\dot{x}_1 = J_1(q)\dot{q}.\]A least-squares joint velocity solution is
\[\dot{q}_1 = J_1^{\#}\dot{x}_1,\]where (J_1^{#}) is a generalized inverse of (J_1). The null-space projector associated with the primary task is
\[N_1 = I - J_1^{\#}J_1.\]Any motion of the form (N_1\dot{q}_2) lies in the null space of the primary task, meaning that it does not affect (\dot{x}_1). Therefore, a two-task prioritized solution can be written as
\[\dot{q} = J_1^{\#}\dot{x}_1 + N_1J_2^{\#}\dot{x}_2.\]More carefully, the secondary task should be projected into the null space of the primary task using the projected Jacobian (J_2N_1), but the intuition is the same: lower-priority tasks should use only the remaining freedom left by higher-priority tasks.
This is the basic idea behind task prioritization. A humanoid controller may assign high priority to maintaining foot contacts and balance, medium priority to hand or swing-foot motion, and lower priority to posture regulation. If these objectives conflict, the lower-priority task is sacrificed first.
Null-space projection provides a geometric way to express task priority, but humanoid robots also need to satisfy dynamics, contacts, friction limits, torque limits, and joint limits. These requirements are naturally expressed as constraints in an optimization problem.
A common WBC formulation solves a quadratic program (QP) at each control cycle. The decision variables may include generalized acceleration, contact forces, and joint torques:
\[y = \begin{bmatrix} \ddot{q} \\ \lambda \\ \tau \end{bmatrix}.\]A typical objective minimizes task acceleration tracking error, regularizes joint accelerations, and regularizes contact forces:
\[\min_{\ddot{q},\lambda,\tau} \sum_i \left\| J_i\ddot{q} + \dot{J}_i\dot{q} - \ddot{x}_{i,\text{cmd}} \right\|_{Q_i}^2 + \left\| \ddot{q} \right\|_R^2 + \left\| \lambda \right\|_W^2.\]The equality constraints include the floating-base dynamics
\[M(q)\ddot{q} + h(q, \dot{q}) = S^T\tau + J_c(q)^T\lambda,\]and the rigid contact acceleration constraints
\[J_c(q)\ddot{q} + \dot{J}_c(q, \dot{q})\dot{q} = 0.\]The inequality constraints enforce physical feasibility. These may include friction cone constraints,
\[\sqrt{\lambda_x^2 + \lambda_y^2} \leq \mu \lambda_z,\]unilateral contact constraints,
\[\lambda_z \geq 0,\]torque limits,
\[\tau_{\min} \leq \tau \leq \tau_{\max},\]and joint acceleration limits,
\[\ddot{q}_{\min} \leq \ddot{q} \leq \ddot{q}_{\max}.\]In practice, the friction cone is often approximated by a linearized friction pyramid so that the optimization remains a QP.
This QP-based view of WBC is powerful because it turns whole-body coordination into a constrained optimization problem. The controller does not simply command a desired motion. It searches for a motion, contact force, and torque command that are all compatible with the robot dynamics and environmental contacts.
Balancing is one of the most important applications of WBC. A humanoid standing on the ground must regulate its center of mass and torso while maintaining feasible ground reaction forces. If the required contact wrench violates the friction cone or moves the center of pressure outside the support region, the robot may slip, rotate, or fall.
In a WBC formulation, balancing can be expressed through task objectives such as center-of-mass acceleration tracking,
\[\ddot{x}_{\text{com},\text{cmd}} = \ddot{x}_{\text{com},d} + K_d(\dot{x}_{\text{com},d} - \dot{x}_{\text{com}}) + K_p(x_{\text{com},d} - x_{\text{com}}),\]and torso orientation tracking,
\[\dot{\omega}_{\text{torso},\text{cmd}} = \dot{\omega}_d + K_d(\omega_d - \omega) + K_p e_R,\]where (e_R) is an orientation error. These tasks are included in the QP objective, while contact constraints ensure that the stance feet remain fixed and contact forces remain physically feasible.
The key point is that balancing is not merely posture tracking. It is the problem of producing body motion and contact wrenches that are jointly feasible under gravity, friction, and actuator limits.
WBC computes dynamically feasible control commands for a given set of desired tasks. For walking, however, the robot also needs a planner that generates stable center-of-mass and footstep trajectories. One common approach is based on the Divergent Component of Motion (DCM).
Under the Linear Inverted Pendulum Model (LIPM), the center of mass is assumed to move at approximately constant height. The natural frequency is
\[\omega = \sqrt{\frac{g}{z_{\text{com}}}},\]where (g) is gravitational acceleration and (z_{\text{com}}) is the nominal center-of-mass height. The DCM is defined as
\[\xi = x_{\text{com}} + \frac{\dot{x}_{\text{com}}}{\omega}.\]The DCM captures the unstable component of the center-of-mass dynamics. If left uncontrolled, this component diverges. Walking pattern generation can therefore be understood as choosing footsteps or reference points that guide the DCM in a stable way.
Using the Virtual Repellent Point (VRP), the DCM dynamics can be written as
\[\dot{\xi} = \omega(\xi - r_{\text{vrp}}),\]where (r_{\text{vrp}}) is the VRP. By planning a sequence of VRP positions associated with future contacts, the planner generates a DCM trajectory that guides the center of mass through the walking motion.
A useful division of labor is:
\[\text{DCM planner} \rightarrow \text{desired CoM and foot trajectories},\] \[\text{WBC} \rightarrow \text{joint torques that realize those trajectories under contact constraints}.\]In this sense, DCM planning provides the walking reference, while WBC realizes that reference on the full humanoid robot.
The concepts above can be implemented using PyPnC with the ATLAS URDF model in PyBullet. The controller combines a DCM-based walking pattern generator with a whole-body controller that tracks desired body, foot, and posture tasks while satisfying contact constraints.
The demonstration includes standing balance, forward walking, backward walking, and sideways walking. These behaviors show how the conceptual stack fits together: the DCM planner generates walking references, while the whole-body controller computes dynamically consistent commands that allow the humanoid to execute those references under contact.
Whole-Body Control is useful because humanoid robots must coordinate many tasks while respecting dynamics and contact constraints. Joint-space control is often too local, and operational-space control alone does not fully address floating-base dynamics or contact feasibility. WBC extends these ideas into a constrained optimization framework that can reason about task tracking, contact consistency, feasible wrenches, and torque limits at the same time.
For walking robots, WBC is only one part of the stack. A walking pattern generator such as a DCM planner provides stable center-of-mass and footstep references, and WBC realizes those references on the full robot body.
In short:
\[\text{Humanoid locomotion} = \text{planning stable motion} + \text{controlling the whole body under contact}.\][1] L. Sentis, J. Park, and O. Khatib, “Compliant Control of Whole-Body Multi-Contact Behaviors in Humanoid Robots,” in Robot Motion and Control, Springer, 2010.
[2] J. Englsberger, C. Ott, M. A. Roa, A. Albu-Schäffer, and G. Hirzinger, “Bipedal walking control based on capture point dynamics,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, 2011.
[3] J. Englsberger, C. Ott, and A. Albu-Schäffer, “Three-dimensional bipedal walking control based on divergent component of motion,” IEEE Transactions on Robotics, 2015.
[4] O. Khatib, “A unified approach for motion and force control of robot manipulators: The operational space formulation,” IEEE Journal on Robotics and Automation, 1987.