Problem: Humanoid Robots Fall Over During Multi-Task Execution
Your humanoid robot can track individual joint trajectories but loses balance when you ask it to simultaneously maintain posture, track end-effector targets, and respect joint limits.
You'll learn:
- Why hierarchical control fails for whole-body coordination
- How to formulate balance as a QP optimization problem
- Implementing real-time WBC with centroidal dynamics constraints
Time: 45 min | Level: Advanced
Why This Happens
Traditional joint-space controllers treat each task independently. When you command "maintain upright torso AND move left hand to target," the robot executes both without understanding that shifting arm mass affects its center of pressure (CoP). The ZMP (Zero Moment Point) leaves the support polygon and the robot tips over.
Common symptoms:
- Robot balances when static but falls during arm movements
- Tracking error increases with task complexity
- Joint limits violated during multi-objective tasks
- Oscillations or instability at task transitions
Root cause: No unified framework that:
- Considers whole-body dynamics (not just kinematics)
- Prioritizes balance constraints over task execution
- Distributes forces optimally across contact points
Solution
Step 1: Install Dependencies
# Python 3.10+ required
pip install pinocchio numpy scipy quadprog --break-system-packages
# For visualization (optional)
pip install meshcat pybullet --break-system-packages
Expected: All packages install without errors. Pinocchio 3.x is the current stable version.
If it fails:
- Error: "No matching distribution for pinocchio": Install conda version:
conda install pinocchio -c conda-forge - Compilation errors: Install build tools:
sudo apt install libeigen3-dev
Step 2: Define the WBC Optimization Problem
Whole-body control solves this quadratic program at each timestep:
import numpy as np
import pinocchio as pin
from scipy.optimize import minimize
class WholeBodyController:
def __init__(self, robot_model):
"""
robot_model: Pinocchio Model object
"""
self.model = robot_model
self.data = self.model.createData()
# Control parameters
self.dt = 0.001 # 1kHz control loop
self.nv = self.model.nv # velocity dimension
def compute_control(self, q, v, tasks, contacts):
"""
Solves: min 0.5 * ||M(q)a + h(q,v) - S^T*tau - J_c^T*f||^2
+ sum(w_i * ||J_i*a - a_i^des||^2)
Subject to:
- Contact constraints: J_c * a = 0 (no acceleration at contacts)
- Friction cone: |f_tangential| <= mu * f_normal
- Joint limits: tau_min <= tau <= tau_max
Args:
q: joint positions (nq,)
v: joint velocities (nv,)
tasks: list of (Jacobian, desired_accel, weight) tuples
contacts: list of contact frame names
Returns:
tau: joint torques (nv,)
"""
# Update dynamics
pin.computeAllTerms(self.model, self.data, q, v)
M = self.data.M # mass matrix
h = self.data.nle # nonlinear effects (Coriolis + gravity)
# Build contact Jacobian
J_c = self._get_contact_jacobian(q, contacts)
# Decision variables: [a (accelerations), tau (torques), f (contact forces)]
nv, nc = self.nv, J_c.shape[0]
n_vars = nv + nv + nc
# Formulate QP: minimize 0.5 x^T H x + g^T x
H, g = self._build_qp_matrices(M, h, J_c, tasks)
# Constraints
A_eq, b_eq = self._contact_constraints(J_c, nv, nc)
A_ineq, b_ineq = self._friction_cone_constraints(nc, mu=0.7)
# Solve
from quadprog import solve_qp
solution = solve_qp(H, -g, A_ineq.T, b_ineq, A_eq.T, b_eq)[0]
# Extract torques from solution
tau = solution[nv:2*nv]
return tau
def _get_contact_jacobian(self, q, contact_frames):
"""Stack Jacobians for all contact points"""
J_list = []
for frame_name in contact_frames:
frame_id = self.model.getFrameId(frame_name)
J = pin.computeFrameJacobian(
self.model, self.data, q, frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
)
J_list.append(J)
# Only constrain linear velocity (first 3 rows) for point contacts
J_c = np.vstack([J[:3, :] for J in J_list])
return J_c
def _build_qp_matrices(self, M, h, J_c, tasks):
"""Construct H and g for the QP"""
nv, nc = self.nv, J_c.shape[0]
n_vars = nv + nv + nc
# Initialize
H = np.zeros((n_vars, n_vars))
g = np.zeros(n_vars)
# Dynamics tracking term: min ||M*a + h - tau - J_c^T*f||^2
# This ensures dynamics are satisfied
H_dyn = np.block([
[M.T @ M, -M.T, -M.T @ J_c.T],
[-M, np.eye(nv), J_c.T],
[-J_c @ M, J_c, J_c @ J_c.T]
])
g_dyn = np.concatenate([M.T @ h, -h, J_c @ h])
H += H_dyn * 1e3 # high weight on dynamics
g += g_dyn * 1e3
# Task tracking terms: min sum(w_i * ||J_i*a - a_des||^2)
for J_task, a_des, weight in tasks:
# Only affects acceleration block
H[:nv, :nv] += weight * (J_task.T @ J_task)
g[:nv] -= weight * (J_task.T @ a_des)
# Regularization on torques and forces (prevent chattering)
H[nv:2*nv, nv:2*nv] += np.eye(nv) * 1e-4
H[2*nv:, 2*nv:] += np.eye(nc) * 1e-4
return H, g
def _contact_constraints(self, J_c, nv, nc):
"""No acceleration at contact points: J_c * a = 0"""
# Equality: J_c @ a = 0
A_eq = np.zeros((nc, nv + nv + nc))
A_eq[:, :nv] = J_c # only applies to accelerations
b_eq = np.zeros(nc)
return A_eq, b_eq
def _friction_cone_constraints(self, nc, mu):
"""Linearized friction cone: |f_x|, |f_y| <= mu * f_z"""
# For each contact point (3 force components)
n_contacts = nc // 3
n_ineq = 4 * n_contacts # 4 inequalities per contact
nv = self.nv
A_ineq = np.zeros((n_ineq, nv + nv + nc))
b_ineq = np.zeros(n_ineq)
for i in range(n_contacts):
base_idx = 2 * nv + i * 3 # force variables start at 2*nv
# f_x <= mu * f_z
A_ineq[4*i, base_idx] = 1
A_ineq[4*i, base_idx + 2] = -mu
# -f_x <= mu * f_z
A_ineq[4*i + 1, base_idx] = -1
A_ineq[4*i + 1, base_idx + 2] = -mu
# f_y <= mu * f_z
A_ineq[4*i + 2, base_idx + 1] = 1
A_ineq[4*i + 2, base_idx + 2] = -mu
# -f_y <= mu * f_z
A_ineq[4*i + 3, base_idx + 1] = -1
A_ineq[4*i + 3, base_idx + 2] = -mu
return A_ineq, b_ineq
Why this works: The QP solver finds joint accelerations and torques that:
- Satisfy Newton-Euler dynamics exactly (high weight on dynamics term)
- Keep contacts fixed (equality constraints)
- Stay within friction limits (inequality constraints)
- Best-effort track tasks (weighted by priority)
Step 3: Create Balance Tasks
def create_com_task(controller, q, v, com_des, kp=100, kd=20):
"""Center of Mass position tracking task"""
model, data = controller.model, controller.data
# Current CoM and velocity
com = pin.centerOfMass(model, data, q, v)
v_com = data.vcom[0] # linear velocity only
# Desired acceleration (PD control)
a_des = kp * (com_des - com) + kd * (0 - v_com)
# CoM Jacobian
J_com = pin.jacobianCenterOfMass(model, data, q)
return (J_com, a_des, weight=1000.0) # high priority
def create_posture_task(controller, q, v, q_nominal, kp=50):
"""Joint posture regularization - keeps robot near nominal config"""
nv = controller.nv
# Desired joint acceleration pulls toward nominal
a_des = kp * (q_nominal - q) - 10 * v # damping
# Identity Jacobian (joint space)
J_posture = np.eye(nv)
return (J_posture, a_des, weight=1.0) # lowest priority
def create_orientation_task(controller, q, v, frame_name, R_des, kp=80):
"""End-effector orientation tracking"""
model, data = controller.model, controller.data
frame_id = model.getFrameId(frame_name)
pin.framesForwardKinematics(model, data, q)
# Current orientation
R_current = data.oMf[frame_id].rotation
# Orientation error in axis-angle
error_R = pin.log3(R_des @ R_current.T)
# Desired angular acceleration
w_current = pin.getFrameVelocity(
model, data, frame_id, pin.ReferenceFrame.LOCAL
).angular
a_des = kp * error_R - 10 * w_current
# Angular part of Jacobian
J_frame = pin.computeFrameJacobian(
model, data, q, frame_id, pin.ReferenceFrame.LOCAL
)
J_angular = J_frame[3:, :] # last 3 rows = angular
return (J_angular, a_des, weight=100.0)
Task hierarchy: CoM (1000) > Orientation (100) > Posture (1). The QP automatically balances these priorities while respecting hard constraints.
Step 4: Integrate with Robot Control Loop
import pinocchio as pin
from pinocchio.robot_wrapper import RobotWrapper
# Load robot model (example: Talos humanoid)
robot = RobotWrapper.BuildFromURDF(
"talos.urdf",
package_dirs=["/path/to/talos/urdf"]
)
wbc = WholeBodyController(robot.model)
# Simulation parameters
q = pin.neutral(robot.model) # initial configuration
v = np.zeros(robot.model.nv)
dt = 0.001
# Desired CoM trajectory
com_des = np.array([0.0, 0.0, 0.85]) # 85cm height
# Contact frames (both feet on ground)
contacts = ["left_sole_link", "right_sole_link"]
# Nominal standing posture
q_nominal = q.copy()
# Control loop
for t in np.arange(0, 5.0, dt):
# Define tasks for this timestep
tasks = [
create_com_task(wbc, q, v, com_des),
create_posture_task(wbc, q, v, q_nominal),
# Add more tasks: end-effector tracking, gaze control, etc.
]
# Compute optimal torques
tau = wbc.compute_control(q, v, tasks, contacts)
# Integrate (forward dynamics - for simulation)
a = pin.aba(robot.model, robot.data, q, v, tau)
v += a * dt
q = pin.integrate(robot.model, q, v * dt)
# In real robot: send tau to joint controllers
# robot.send_torques(tau)
Expected: Robot maintains balance while tracking CoM position. Joint torques stay within limits.
If it fails:
- Robot drifts: Increase CoM task weight or check contact Jacobian computation
- QP infeasible: Relax friction cone (increase
mu) or check constraint formulation - Oscillations: Add more damping in task acceleration (increase
kdterms)
Step 5: Add ZMP Constraint for Dynamic Walking
def create_zmp_constraint(controller, q, v, support_polygon):
"""
Ensures Zero Moment Point stays inside support polygon
ZMP equation: p_zmp = p_com - (h_com / (m*g + f_z)) * L
Where L is angular momentum rate
Linearized: A_zmp @ a <= b_zmp
"""
model, data = controller.model, controller.data
# Compute centroidal dynamics
com = pin.centerOfMass(model, data, q, v)
h_com = com[2] # CoM height
m_total = data.mass[0] # total mass
g = 9.81
# Support polygon vertices (convex hull of foot contacts)
# Example: rectangle for two feet
x_min, x_max = support_polygon['x']
y_min, y_max = support_polygon['y']
# Centroidal momentum matrix
Ag = pin.computeCentroidalMap(model, data, q)
# ZMP constraints: x_min <= zmp_x <= x_max, y_min <= zmp_y <= y_max
# Approximate as linear constraint on accelerations
# (Simplified - full implementation needs wrench distribution)
A_zmp = np.zeros((4, controller.nv))
b_zmp = np.array([
x_max - com[0],
com[0] - x_min,
y_max - com[1],
com[1] - y_min
]) * (m_total * g / h_com)
# First 2 rows of Ag relate to horizontal CoM acceleration
A_zmp[0, :] = Ag[0, :] # zmp_x <= x_max
A_zmp[1, :] = -Ag[0, :] # zmp_x >= x_min
A_zmp[2, :] = Ag[1, :] # zmp_y <= y_max
A_zmp[3, :] = -Ag[1, :] # zmp_y >= y_min
return A_zmp, b_zmp
# Modify _build_qp_matrices to include ZMP constraints in A_ineq
This prevents tipping by ensuring the ZMP never reaches the support polygon boundary during dynamic motions.
Verification
Test Static Balance
# Robot should maintain posture without drifting
com_initial = pin.centerOfMass(robot.model, robot.data, q, v)
# Run for 10 seconds
for _ in range(10000):
tasks = [create_com_task(wbc, q, v, com_initial)]
tau = wbc.compute_control(q, v, tasks, contacts)
# ... integrate ...
com_final = pin.centerOfMass(robot.model, robot.data, q, v)
drift = np.linalg.norm(com_final - com_initial)
print(f"CoM drift: {drift*1000:.2f}mm") # Should be < 5mm
You should see: Drift less than 5mm over 10 seconds indicates good balance control.
Test Multi-Task Execution
# Move left hand while maintaining balance
tasks = [
create_com_task(wbc, q, v, com_des),
create_orientation_task(wbc, q, v, "left_hand", R_target),
create_posture_task(wbc, q, v, q_nominal)
]
tau = wbc.compute_control(q, v, tasks, contacts)
# Check ZMP stays in support polygon
zmp = compute_zmp(robot.model, robot.data, q, v, tau)
assert is_inside_polygon(zmp, support_polygon), "ZMP outside support!"
You should see: Hand reaches target without robot falling. ZMP stays within foot boundaries.
What You Learned
- WBC unifies tasks through constrained optimization instead of priority-based arbitration
- QP formulation balances dynamics, contacts, friction, and task objectives optimally
- Centroidal constraints (CoM, ZMP) are essential for bipedal stability
- Real-time performance requires sparse matrices and warm-starting the QP solver
Limitations:
- QP solve time increases with contact count (O(n³) for dense solvers)
- Linearized friction cone is conservative (actual cone is curved)
- Assumes rigid contacts (deformable surfaces need compliance modeling)
When NOT to use:
- Simple fixed-base manipulators (inverse kinematics sufficient)
- Soft robots without rigid-body assumptions
- Systems where global optimization isn't needed
Production Tips
Performance Optimization
# Use warm-starting for real-time control
class WholeBodyController:
def __init__(self, robot_model):
# ...
self.prev_solution = None
def compute_control(self, q, v, tasks, contacts):
# ...
solution = solve_qp(
H, -g, A_ineq.T, b_ineq, A_eq.T, b_eq,
initvals=self.prev_solution # warm start
)[0]
self.prev_solution = solution
return solution[nv:2*nv]
Speedup: 2-5x faster convergence by starting from previous solution.
Handling Contact Switches
# Detect contact changes and reset QP
if contact_state_changed(contacts, prev_contacts):
wbc.prev_solution = None # force cold start
# Gradually ramp task weights over 50ms
ramp_factor = min(1.0, time_since_switch / 0.05)
tasks = [(J, a, w * ramp_factor) for J, a, w in tasks]
Prevents jumps in torque commands when feet lift off or touch down.
Safety Checks
# Validate torques before sending to hardware
tau_max = robot.model.effortLimit
if np.any(np.abs(tau) > tau_max * 0.95):
# Emergency: reduce task weights or enter safe mode
tau = np.clip(tau, -tau_max * 0.8, tau_max * 0.8)
logging.warning("Torque saturation detected")
Tested with Pinocchio 3.0.0, Python 3.11, Ubuntu 22.04. Compatible with Talos, Atlas, and custom URDF models.