Lab 08-01: Bipedal Balance and Standing

Objectives

By the end of this lab, you will be able to:

• Implement a simple inverted pendulum balance controller
• Calculate and track Zero Moment Point (ZMP)
• Design a center of mass (CoM) controller for quiet standing
• Analyze stability margins for bipedal stance

Prerequisites

• Completed Module 05 (Dynamics and Control)
• Understanding of rigid body dynamics
• Familiarity with state feedback control

Materials

Type	Name	Tier	Notes
software	MuJoCo 3.0+	required	Physics simulation
software	Python 3.10+	required	Programming environment
software	NumPy, SciPy	required	Linear algebra, control
simulation	simple-biped.xml	required	Simplified bipedal model

Background

The Balance Problem

Standing balance requires keeping the center of mass (CoM) over the support polygon (feet). For quiet standing, this is a classic inverted pendulum problem.

Zero Moment Point (ZMP)

The ZMP is the point on the ground where the net moment of all forces equals zero:

$ZMP_x = \frac{\sum_i (m_i \ddot{z}_i x_i - m_i \ddot{x}_i z_i + m_i g x_i)}{\sum_i (m_i \ddot{z}_i + m_i g)}$

For stable standing: ZMP must lie within the support polygon.

Linear Inverted Pendulum Model (LIPM)

Simplified dynamics for CoM control:

$\ddot{x}_{CoM} = \frac{g}{z_c}(x_{CoM} - x_{ZMP})$

Where $z_c$ is the constant CoM height.

Instructions

Step 1: Load and Explore Bipedal Model

Initialize the simulation environment:

import mujoco
import numpy as np
import matplotlib.pyplot as plt
from scipy import signal

# Load simple biped model
model = mujoco.MjModel.from_xml_path(
    "textbook/assets/robot-models/mujoco/simple-biped.xml"
)
data = mujoco.MjData(model)

# Explore model structure
print(f"Number of bodies: {model.nbody}")
print(f"Number of joints: {model.njnt}")
print(f"Number of actuators: {model.nu}")

# Identify key bodies
body_names = {}
for i in range(model.nbody):
    name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i)
    if name:
        body_names[name] = i
        print(f"Body {i}: {name}")

# Find torso, feet
torso_id = body_names.get('torso', 0)
left_foot_id = body_names.get('left_foot', 0)
right_foot_id = body_names.get('right_foot', 0)

# Initialize
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)

print(f"\nInitial torso position: {data.xpos[torso_id]}")
print(f"Initial CoM: {data.subtree_com[0]}")  # subtree_com[0] is full model CoM

Expected: Model loaded with torso and foot bodies identified, CoM computed.

Step 2: Compute Zero Moment Point

Implement ZMP calculation from ground reaction forces:

def compute_zmp(model, data, foot_body_ids):
    """
    Compute Zero Moment Point from contact forces.

    Args:
        model: MuJoCo model
        data: MuJoCo data
        foot_body_ids: List of foot body IDs

    Returns:
        zmp: [x, y] position of ZMP, or None if not in contact
    """
    total_force = np.zeros(3)
    total_moment = np.zeros(3)

    # Find all contacts with ground
    for i in range(data.ncon):
        contact = data.contact[i]

        # Check if contact involves a foot
        geom1_body = model.geom_bodyid[contact.geom1]
        geom2_body = model.geom_bodyid[contact.geom2]

        if geom1_body in foot_body_ids or geom2_body in foot_body_ids:
            # Get contact force
            force = np.zeros(6)
            mujoco.mj_contactForce(model, data, i, force)

            # Contact position
            pos = contact.pos.copy()

            # Accumulate force and moment
            total_force += force[:3]
            total_moment += np.cross(pos, force[:3])

    # Compute ZMP
```python
```python
    if abs(total_force[2]) < 1.0:  # Less than 1N vertical

        return None  # Not enough contact force

    # ZMP equation: moment_x = force_z * zmp_y - force_y * zmp_z
    # Assuming z=0 (ground plane):
    zmp_x = -total_moment[1] / total_force[2]
    zmp_y = total_moment[0] / total_force[2]

    return np.array([zmp_x, zmp_y])

def compute_com(model, data):
    """Compute center of mass position and velocity."""
    com_pos = data.subtree_com[0].copy()

    # CoM velocity from momentum
    total_mass = np.sum(model.body_mass)
    com_vel = data.subtree_linvel[0].copy()

    return com_pos, com_vel

# Test ZMP computation
foot_ids = [left_foot_id, right_foot_id]
zmp = compute_zmp(model, data, foot_ids)
com, com_vel = compute_com(model, data)

print(f"CoM position: {com}")
print(f"ZMP position: {zmp}")

Expected: ZMP computed within the support polygon, CoM above the feet.

Step 3: Implement LIPM Controller

Design a controller based on Linear Inverted Pendulum Model:

class LIPMBalanceController:
    """Balance controller using Linear Inverted Pendulum Model."""

    def __init__(self, model, data, com_height=0.9):
        self.model = model
        self.data = data
        self.z_c = com_height  # Constant CoM height assumption
        self.g = 9.81

        # Compute natural frequency
        self.omega = np.sqrt(self.g / self.z_c)

        # State feedback gains (from LQR or pole placement)
        # State: [x - x_ref, x_dot]
        # Want stable poles at -omega (critically damped)
        self.k_p = self.omega ** 2  # Position gain
        self.k_d = 2 * self.omega   # Velocity gain

        # Desired ZMP (ankle torque command)
        self.zmp_des = np.array([0.0, 0.0])

        # Support polygon limits
        self.foot_length = 0.2
        self.foot_width = 0.1
        self.zmp_limits = {
            'x': [-self.foot_length/2, self.foot_length/2],
            'y': [-self.foot_width/2, self.foot_width/2]
        }

    def compute_desired_zmp(self, com_pos, com_vel, com_ref=np.array([0, 0])):
        """
        Compute desired ZMP to regulate CoM to reference.

        Using LIPM: x_ddot = (g/z_c)(x_com - x_zmp)
        State feedback: x_zmp_des = x_com + (k_d/omega^2)*x_dot + (k_p/omega^2)*(x - x_ref)
        """
        # CoM error
        com_error_x = com_pos[0] - com_ref[0]
        com_error_y = com_pos[1] - com_ref[1]

        # Desired ZMP from state feedback
        zmp_des_x = com_pos[0] + (self.k_d / self.omega**2) * com_vel[0] + \
                    (self.k_p / self.omega**2) * com_error_x
        zmp_des_y = com_pos[1] + (self.k_d / self.omega**2) * com_vel[1] + \
                    (self.k_p / self.omega**2) * com_error_y

        # Saturate to support polygon
        zmp_des_x = np.clip(zmp_des_x, self.zmp_limits['x'][0], self.zmp_limits['x'][1])
        zmp_des_y = np.clip(zmp_des_y, self.zmp_limits['y'][0], self.zmp_limits['y'][1])

        self.zmp_des = np.array([zmp_des_x, zmp_des_y])
        return self.zmp_des

    def compute_ankle_torque(self, zmp_des, com_pos):
        """
        Compute ankle torques to achieve desired ZMP.

        Simplified: Ankle torque creates moment about CoP/ZMP.
        tau_ankle = m * g * (x_zmp - x_com) approximately
        """
        total_mass = np.sum(self.model.body_mass)

        # Ankle torque (sagittal plane)
        tau_x = total_mass * self.g * (zmp_des[1] - com_pos[1])  # Roll
        tau_y = -total_mass * self.g * (zmp_des[0] - com_pos[0])  # Pitch

        return np.array([tau_x, tau_y])

# Create controller
controller = LIPMBalanceController(model, data)
print(f"LIPM omega: {controller.omega:.3f} rad/s")
print(f"Gains: kp={controller.k_p:.2f}, kd={controller.k_d:.2f}")

Expected: LIPM controller created with computed gains based on pendulum dynamics.

Step 4: Simulate Quiet Standing

Run the balance controller and analyze performance:

def simulate_standing(model, data, controller, duration=5.0, dt=0.002,
                      perturbation_time=2.0, perturbation_force=50):
    """
    Simulate quiet standing with optional perturbation.

    Args:
        model: MuJoCo model
        data: MuJoCo data
        controller: LIPMBalanceController
        duration: Simulation time
        perturbation_time: When to apply perturbation
        perturbation_force: Force magnitude (N)

    Returns:
        history: Dictionary of recorded data
    """
    history = {
        'time': [],
        'com_x': [],
        'com_y': [],
        'com_z': [],
        'zmp_x': [],
        'zmp_y': [],
        'zmp_des_x': [],
        'zmp_des_y': [],
        'ankle_tau': []
    }

    foot_ids = [left_foot_id, right_foot_id]
    steps = int(duration / dt)
    perturbation_applied = False

    for step in range(steps):
        t = step * dt

        # Apply perturbation
```python
```python
        if not perturbation_applied and t >= perturbation_time:

            # Apply impulse to torso
            data.xfrc_applied[torso_id, 0] = perturbation_force
            perturbation_applied = True
        elif perturbation_applied and t >= perturbation_time + 0.1:
            # Remove after 0.1s
            data.xfrc_applied[torso_id, 0] = 0

        # Get state
        com, com_vel = compute_com(model, data)
        zmp = compute_zmp(model, data, foot_ids)

        # Compute control
        zmp_des = controller.compute_desired_zmp(com, com_vel)
        tau_ankle = controller.compute_ankle_torque(zmp_des, com)

        # Apply control (assuming ankle actuators are first)
        data.ctrl[0] = tau_ankle[0]  # Roll
        data.ctrl[1] = tau_ankle[1]  # Pitch

        # Step simulation
        mujoco.mj_step(model, data)

        # Record
        history['time'].append(t)
        history['com_x'].append(com[0])
        history['com_y'].append(com[1])
        history['com_z'].append(com[2])
        if zmp is not None:
            history['zmp_x'].append(zmp[0])
            history['zmp_y'].append(zmp[1])
        else:
            history['zmp_x'].append(np.nan)
            history['zmp_y'].append(np.nan)
        history['zmp_des_x'].append(zmp_des[0])
        history['zmp_des_y'].append(zmp_des[1])
        history['ankle_tau'].append(tau_ankle.copy())

    return history

# Reset and run simulation
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)

history = simulate_standing(model, data, controller,
                           duration=5.0,
                           perturbation_time=2.0,
                           perturbation_force=100)

print(f"Simulation complete: {len(history['time'])} steps")
print(f"CoM X range: [{min(history['com_x']):.4f}, {max(history['com_x']):.4f}]")

Expected: Standing simulation completes, CoM oscillates after perturbation but returns to equilibrium.

Step 5: Analyze Stability Margins

Compute and visualize stability metrics:

def compute_stability_margin(zmp, support_polygon):
    """
    Compute distance from ZMP to support polygon boundary.

    Args:
        zmp: [x, y] ZMP position
        support_polygon: List of [x, y] vertices (counterclockwise)

    Returns:
        margin: Minimum distance to boundary (negative if outside)
    """
    n = len(support_polygon)
    min_dist = float('inf')

    for i in range(n):
        p1 = np.array(support_polygon[i])
        p2 = np.array(support_polygon[(i + 1) % n])

        # Vector from p1 to p2
        edge = p2 - p1
        edge_len = np.linalg.norm(edge)
        edge_unit = edge / edge_len

        # Vector from p1 to ZMP
        to_zmp = zmp - p1

        # Project onto edge
        proj_len = np.dot(to_zmp, edge_unit)
        proj_len = np.clip(proj_len, 0, edge_len)
        closest = p1 + proj_len * edge_unit

        # Distance to closest point on edge
        dist = np.linalg.norm(zmp - closest)

        # Check if inside (using cross product sign)
        cross = edge[0] * to_zmp[1] - edge[1] * to_zmp[0]
```python
```python
        if cross < 0:

            dist = -dist  # Outside on this edge

        min_dist = min(min_dist, dist)

    return min_dist

# Define support polygon (simplified rectangle)
support_polygon = [
    [-0.1, -0.05],  # Back left
    [0.1, -0.05],   # Front left
    [0.1, 0.05],    # Front right
    [-0.1, 0.05]    # Back right
]

# Compute stability margins over time
margins = []
for i in range(len(history['zmp_x'])):
    zmp = np.array([history['zmp_x'][i], history['zmp_y'][i]])
    if not np.isnan(zmp[0]):
        margin = compute_stability_margin(zmp, support_polygon)
        margins.append(margin)
    else:
        margins.append(np.nan)

# Analyze
valid_margins = [m for m in margins if not np.isnan(m)]
print(f"\nStability Margin Statistics:")
print(f"  Minimum: {min(valid_margins):.4f} m")
print(f"  Maximum: {max(valid_margins):.4f} m")
print(f"  Mean: {np.mean(valid_margins):.4f} m")

# Check if balance was maintained
```python
```python
if min(valid_margins) > 0:

    print("  Status: STABLE (ZMP always within support polygon)")
else:
    print("  Status: UNSTABLE (ZMP exited support polygon)")

Expected: Stability margins computed, system remains stable with positive margins.

Step 6: Capture Point Analysis

Implement capture point for dynamic stability:

def compute_capture_point(com_pos, com_vel, omega):
    """
    Compute capture point (Instantaneous Capture Point).

    The capture point is where the robot needs to step to
    come to rest.

    CP = CoM + CoM_velocity / omega

    Args:
        com_pos: [x, y, z] CoM position
        com_vel: [vx, vy, vz] CoM velocity
        omega: Natural frequency sqrt(g/z_c)

    Returns:
        cp: [x, y] Capture point position
    """
    cp_x = com_pos[0] + com_vel[0] / omega
    cp_y = com_pos[1] + com_vel[1] / omega
    return np.array([cp_x, cp_y])

def analyze_capture_point(history, controller, support_polygon):
    """Analyze capture point trajectory."""
    capture_points = []

    for i in range(len(history['time'])):
        com = np.array([history['com_x'][i], history['com_y'][i], history['com_z'][i]])
        # Estimate velocity from finite difference
```python
```python
        if i > 0:

            dt = history['time'][i] - history['time'][i-1]
            com_vel = np.array([
                (history['com_x'][i] - history['com_x'][i-1]) / dt,
                (history['com_y'][i] - history['com_y'][i-1]) / dt,
                0
            ])
        else:
            com_vel = np.zeros(3)

        cp = compute_capture_point(com, com_vel, controller.omega)
        capture_points.append(cp)

    # Compute capture point margins
    cp_margins = []
    for cp in capture_points:
        margin = compute_stability_margin(cp, support_polygon)
        cp_margins.append(margin)

    print(f"\nCapture Point Analysis:")
    print(f"  Min margin: {min(cp_margins):.4f} m")
    print(f"  Max excursion from center: {max(np.linalg.norm(np.array(capture_points), axis=1)):.4f} m")

    # Capture point within support = recoverable without stepping
```python
```python
    recoverable_steps = sum(1 for m in cp_margins if m > 0)

    print(f"  Recoverable (no step needed): {recoverable_steps}/{len(cp_margins)} steps")

    return capture_points, cp_margins

cp_points, cp_margins = analyze_capture_point(history, controller, support_polygon)

Expected: Capture point analysis complete, showing dynamic stability margin.

Expected Outcomes

After completing this lab, you should have:

1. Code artifacts:

   • zmp_computation.py: ZMP calculation from contacts
   • lipm_controller.py: Balance controller implementation
   • stability_analysis.py: Margin and capture point analysis

2. Understanding:

   • Relationship between CoM, ZMP, and balance
   • LIPM approximation and its limitations
   • Stability margins for standing balance
   • Capture point concept for dynamic stability

3. Experimental results:

   • Response to perturbation
   • Stability margin time series
   • Capture point trajectories

Rubric

Criterion	Points	Description
ZMP Computation	20	Correct ZMP from contact forces
LIPM Controller	25	Working balance control with proper gains
Perturbation Response	20	Recovery from push perturbation
Stability Analysis	20	Margin and capture point computation
Documentation	15	Clear code and analysis
Total	100

Common Errors

Error: ZMP computation returns None Solution: Check contact detection, ensure feet are on ground.

Error: System falls over after perturbation Solution: Increase controller gains, check actuator limits.

Error: Oscillations don't dampen Solution: Increase damping gain kd, check for sensor noise.

Extensions

For advanced students:

1. 3D Balance: Extend to sagittal + frontal plane coupling
2. Model Predictive Control: MPC-based balance with preview
3. Compliant Standing: Add ankle compliance for rougher terrain
4. Sensory Integration: Add vestibular-like sensing for tilt

Related Content

• Theory: Module 08 theory.md, Section 8.1 (Balance Fundamentals)
• Next Lab: Lab 08-02 (Walking Gait Generation)
• Application: Humanoid standing, exoskeleton balance