Whole-body control coordinates an entire robot as one coupled dynamical system, while hierarchical quadratic programming gives that controller a disciplined way to satisfy hard constraints and preserve task priorities when multiple objectives compete.

This lesson follows OSREC.010: Operational Space Control and Null-Space Redundancy. That lesson introduced task-space control, redundancy, null-space motion, and task priority. OSREC.011 extends those ideas to floating-base and highly articulated robots that must coordinate balance, contacts, manipulation, posture, joint limits, torque limits, and collision constraints at the same time.

Tokyo Robotics demonstrates whole-body motion on a humanoid platform, showing why balance, locomotion, posture, and upper-body tasks cannot be treated as unrelated control problems.

What You’ll Learn

• Define whole-body control and explain why it matters for humanoids and mobile manipulators.
• Identify common whole-body-control decision variables.
• Distinguish hard constraints from soft objectives.
• Explain the role of equality and inequality constraints.
• Understand the basic structure of a quadratic program.
• Explain how hierarchical quadratic programming preserves strict task priorities.
• Describe contact, friction, balance, joint, torque, and collision constraints.
• Understand why floating-base robots are underactuated.
• Recognize infeasibility, numerical-conditioning, model-error, and priority-design failures.
• Build a practical troubleshooting sequence for whole-body controllers.

Why Whole-Body Control Exists

A fixed-base industrial arm can often be controlled by focusing on its joints and end effector. A humanoid is different. Moving one arm shifts the robot’s center of mass. Changing posture alters ground reaction forces. A foot contact constrains motion. A hand contact introduces additional forces. A knee torque limit can restrict what the torso or arm is able to do. The body is mechanically coupled, so the controller must reason about that coupling.

Stanford’s CS327A Advanced Robotic Manipulation treats whole-body control as a unified problem involving motion coordination, environmental contacts, internal and external constraints, underactuation, and balance. The classic Stanford whole-body framework by Luis Sentis and Oussama Khatib similarly combines constraints, operational tasks, and posture objectives rather than controlling each limb as an isolated machine.

The Whole-Body Dynamics Equation

A common starting point is the rigid-body dynamics equation M(q)q̈ + h(q,q̇) = Sᵀτ + Jcᵀλ. Here M is the mass matrix, h groups gravity and velocity-dependent effects, τ contains actuator torques, S selects the actuated coordinates, Jc is a contact Jacobian, and λ represents contact forces or wrenches.

For a floating-base humanoid, the base itself is not directly actuated. The robot cannot command a motor that simply applies an arbitrary force to its torso in free space. It must create body acceleration indirectly through joint torques and contact forces at the feet, hands, knees, or other contact points. That underactuation is one reason whole-body control is fundamentally more constrained than ordinary fixed-base arm control.

What the Optimizer Solves For

A whole-body controller may solve for joint accelerations, actuator torques, contact forces, or a stacked vector containing several of these quantities. A common conceptual decision vector is z = [q̈, τ, λ]. Once that vector is defined, the controller can write robot physics and safety requirements as mathematical constraints on z.

The optimizer is not inventing arbitrary robot motion. It is searching only inside the set of commands that satisfy the dynamic model and whatever constraints the designer has imposed. The quality of the result therefore depends on both the optimizer and the correctness of the model, limits, contact state, reference frames, and priorities given to it.

Equality Constraints

An equality constraint requires a relationship to hold at the solution. Robot dynamics are often imposed as an equality. A rigid stationary foot contact can also impose a relationship requiring zero contact acceleration in selected directions. A task may request a specific Cartesian acceleration, orientation behavior, or momentum rate.

Conceptually, equalities can be written as Aeq z = beq. Some equalities are physical laws that should not be violated. Others are task equations that may need to become soft when the robot cannot satisfy everything simultaneously. That distinction matters because incorrectly treating every desire as a hard equality is a fast route to infeasibility.

Inequality Constraints

Inequality constraints describe allowable regions rather than exact values. Joint positions must remain inside limits. Joint velocities and accelerations have bounds. Motors have torque limits. Contact forces must stay inside friction limits. Collision distances must stay above safety margins. Center-of-pressure or support constraints may restrict how forces are distributed through a foot.

These constraints are commonly expressed as Aineq z ≤ bineq. Inequalities are one major reason optimization-based controllers are powerful: they let the controller reason directly about ranges and limits instead of relying only on after-the-fact clipping of an already computed command.

A whole-body humanoid demonstration provides a practical view of the many simultaneous motion and stability requirements that an optimization-based controller may need to coordinate.

Quadratic Programming

A quadratic program, or QP, minimizes a quadratic objective while satisfying linear equality and inequality constraints. One common form is min ½zᵀHz + fᵀz subject to the controller’s equality and inequality constraints. In robotics, the quadratic cost can penalize tracking error, excessive torque, large acceleration, posture deviation, or another quantity that can be expressed in a suitable least-squares form.

A weighted QP can combine multiple objectives by assigning each one a numerical weight. That approach is useful but has an important limitation: weights express relative importance, not necessarily strict priority. If two objectives conflict strongly enough, a supposedly secondary objective can still alter a more important one unless the formulation is carefully designed.

Why Hierarchical Quadratic Programming Is Different

Hierarchical quadratic programming, or HQP, organizes objectives into priority levels. The controller solves the highest-priority level first, then allows lower-priority optimization only within the remaining freedom that does not degrade the protected result above it. This is the optimization counterpart of the task-priority and null-space concepts introduced in OSREC.010.

The influential work by Adrien Escande, Nicolas Mansard, and Pierre-Brice Wieber showed how strict hierarchies containing both equalities and inequalities can be solved fast enough for whole-body humanoid problems. Their formulation was designed specifically for cases where multiple robot objectives are incompatible and must be respected in an ordered hierarchy rather than merely blended together.

A simple hierarchy might place physical consistency and contact constraints at the top, collision avoidance and joint limits next, balance and center-of-mass control after that, a hand-tracking task below those requirements, and posture or effort minimization at the bottom. The exact order depends on the machine and application, but the engineering principle is stable: lower-priority behavior must not silently destroy a higher-priority requirement.

Contact Constraints and Friction

A robot standing on the ground cannot assume that any ground-reaction force is available. Contact forces are constrained by friction and contact geometry. A common model restricts tangential force relative to normal force through a friction cone, often approximated by a friction pyramid so that the condition can be represented with linear inequalities inside a QP.

If the optimizer asks for a force outside that feasible region, the real foot may slip even though the mathematical motion target looks reasonable. This is why contact-force constraints belong inside the control problem rather than being treated as an unrelated low-level detail.

Balance Is a Whole-Body Constraint

Balance is not controlled only by the ankles. Arm motion, torso lean, knee flexion, step location, contact timing, center-of-mass acceleration, and ground-reaction forces all affect whether the robot remains supportable. A whole-body controller can coordinate these quantities so that reaching or manipulation does not unintentionally consume the stability margin required for standing or walking.

The modern view is therefore to treat locomotion and manipulation as coupled whenever they share the same body and contacts. Stanford’s whole-body work explicitly addresses multiple distributed tasks and postures together with multiple contacts and constraints rather than decomposing balance into a separate subsystem that is unaware of manipulation.

Hard Constraints Versus Soft Objectives

A hard constraint defines a region the optimizer is not allowed to leave. A soft objective defines behavior the controller should pursue when feasible. Confusing these categories can make a controller brittle. For example, a desired hand pose may need to relax slightly when strict joint, torque, collision, friction, or balance limits would otherwise be violated.

Slack variables are one tool for controlled relaxation. Instead of declaring the entire optimization infeasible when a soft requirement cannot be met exactly, the controller can permit a quantified violation and penalize that violation strongly. The penalty makes relaxation expensive without pretending that the physical system can always satisfy every desired task exactly.

Why Infeasibility Matters

An optimization problem is infeasible when no candidate command satisfies all of its active hard constraints. In robotics, this can happen when a requested task lies outside the reachable set, contact assumptions are wrong, torque bounds are too restrictive, joint limits conflict with the task, or multiple high-priority constraints contradict each other.

A production controller must detect infeasibility explicitly. It should not assume that a solver will always return a meaningful motion command. Safe responses can include holding position, reducing task demands, changing contact strategy, replanning, activating a fallback controller, or entering a defined safe state.

This humanoid demonstration shows coordinated full-body motion on a platform whose control must respect actuation, balance, contact, and posture limits together.

Numerical Conditioning

Even a theoretically feasible problem can become numerically difficult. Poorly scaled variables, nearly dependent constraints, singular or near-singular Jacobians, extreme task weights, inaccurate contact geometry, and insufficient regularization can make the optimization unstable or sensitive to small changes.

Engineers should monitor solver status, residuals, active constraints, iteration count, condition indicators, command discontinuities, and the magnitude of slack variables. Solver success is not merely a Boolean result; the surrounding diagnostic information is part of the controller’s observability.

Model Error Still Exists

Optimization cannot compensate perfectly for a bad physical model. Incorrect payload mass, wrong inertia, poor joint calibration, time delay, friction, structural flex, contact compliance, sensor bias, or an incorrect contact state can cause the real robot to violate the behavior predicted by the optimizer.

This is why simulation, hardware-in-the-loop validation, conservative limits, telemetry, and staged deployment matter. Stanford’s control-and-simulation work emphasizes physically realistic simulation as a way to evaluate control methods before expensive or unsafe physical experiments.

A Practical Troubleshooting Workflow

1. Confirm the contact state. Verify which feet, hands, wheels, or other bodies are actually constrained by the environment.
2. Verify coordinate frames. Check world, base, contact, tool, center-of-mass, and sensor frames.
3. Check the dynamics model. Validate mass, inertia, gravity, payload, and actuator mapping.
4. Inspect task priorities. Confirm that safety and physical constraints are above preference-level objectives.
5. Disable low-priority tasks. Reduce the controller to the smallest feasible hierarchy.
6. Inspect inequalities. Determine which joint, torque, friction, or collision bounds are active.
7. Check solver status and residuals. Do not rely only on whether a command was returned.
8. Check conditioning and scaling. Normalize variables and inspect near-dependent constraints.
9. Add tasks one at a time. Identify which task introduces infeasibility, oscillation, or command discontinuity.
10. Compare predicted and measured forces. Large disagreement often points to model or contact-state error.
11. Test transitions. Pay special attention when a foot lifts, a hand makes contact, a joint limit activates, or a priority level changes.
12. Verify fallback behavior. Intentionally create an infeasible request in simulation and confirm the system enters the expected safe state.

Engineering Exercise

1. Model a floating-base humanoid standing with two feet in contact with the ground.
2. Choose a conceptual decision vector containing joint accelerations, actuator torques, and contact forces.
3. Identify which part of the rigid-body dynamics should be imposed as an equality constraint.
4. Define inequality constraints for joint torque limits and foot friction.
5. Create a three-level task hierarchy: balance, hand tracking, and posture.
6. Explain what should happen when the requested hand target would require violating the friction constraint.
7. Add a slack variable to the hand task and explain why the balance and friction constraints should remain protected.
8. Describe one situation that makes the QP infeasible.
9. Describe one situation that is feasible mathematically but fails on hardware because the model is wrong.
10. List the telemetry you would log before allowing the controller to run on a physical humanoid.

Knowledge Check

1. What is whole-body control?
2. Why is a floating-base humanoid underactuated?
3. Name three common decision variables in optimization-based whole-body control.
4. What is the difference between an equality and an inequality constraint?
5. Why are friction constraints important?
6. What does a quadratic program minimize?
7. What is the key difference between a weighted QP and a strict hierarchical QP?
8. Why can a hard hand-position requirement make a controller infeasible?
9. What is a slack variable used for?
10. Why can a mathematically correct whole-body controller still fail on real hardware?

Answers

1. A control approach that coordinates the robot’s coupled body dynamics, tasks, contacts, and constraints as one system.
2. Its free-floating base has no direct actuators; body motion must be produced through joints and environmental contacts.
3. Joint accelerations, actuator torques, and contact forces or wrenches.
4. An equality specifies an exact relationship; an inequality specifies an allowable range or boundary.
5. They keep requested contact forces inside physically supportable regions and help prevent slip.
6. A quadratic objective function subject to its constraints.
7. Weights trade objectives numerically, while HQP protects higher-priority results before optimizing lower-priority tasks.
8. The requested pose may conflict with reachability, torque, collision, balance, or contact limits.
9. To allow controlled relaxation of a requirement while penalizing the amount of violation.
10. Real hardware contains model error, delay, friction, compliance, calibration error, noise, and imperfect contact assumptions.

Key Takeaway

Whole-body control turns a complex robot into one coordinated dynamical system; hierarchical quadratic programming gives that system a rigorous way to decide what must never be violated, what should be achieved first, and what can be relaxed when the machine runs out of physical freedom.

References

Primary technical references: Stanford CS327A — Advanced Robotic Manipulation; Stanford Robotics — Control and Simulation; Sentis and Khatib — A Whole-Body Control Framework for Humanoids Operating in Human Environments; Escande, Mansard, and Wieber — Hierarchical Quadratic Programming: Fast Online Humanoid-Robot Motion Generation; and Moro and Sentis — Whole-Body Control of Humanoid Robots.

Next: OSREC.012 will move from whole-body optimization into multi-contact planning and contact-transition control.

Safety: whole-body controllers can generate large, fast, and unintuitive multi-joint motions when tasks conflict or contact assumptions change. Validate in simulation first, enforce approved joint/velocity/torque/contact limits, use qualified safeguarding and emergency-stop procedures, and test physical hardware only under controlled engineering conditions.