Lab 08-02: Walking Gait Generation

Objectives

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

• Generate walking trajectories using LIPM-based ZMP planning
• Implement footstep planning with desired velocity
• Create swing leg trajectories using polynomial interpolation
• Execute basic walking gaits in simulation

Prerequisites

• Completed Lab 08-01 (Balance and Standing)
• Understanding of trajectory generation
• Familiarity with finite state machines

Materials

Type	Name	Tier	Notes
software	MuJoCo 3.0+	required	Physics simulation
software	Python 3.10+	required	Programming environment
software	NumPy, SciPy	required	Optimization, interpolation
simulation	humanoid-walker.xml	required	Full humanoid model

Background

Walking as Controlled Falling

Walking is a series of controlled falls where:

1. CoM moves outside support polygon
2. Swing leg catches the fall
3. Weight transfers to new support
4. Process repeats with other leg

ZMP Preview Control

For smooth walking, we plan ZMP trajectory ahead and compute CoM trajectory that achieves it:

$\ddot{x}_{CoM} = \omega^2 (x_{CoM} - x_{ZMP})$

Given future ZMP trajectory, we can compute required CoM trajectory.

Walking Phases

1. Double Support (DS): Both feet on ground
2. Single Support (SS): One foot on ground, other swinging
3. Transitions: Weight shift between phases

Instructions

Step 1: Set Up Walking Model

Load a walking-capable humanoid model:

import mujoco
import numpy as np
from scipy.interpolate import CubicSpline
import matplotlib.pyplot as plt

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

# Identify leg components
def find_body_id(name):
    return mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name)

body_ids = {
    'torso': find_body_id('torso'),
    'pelvis': find_body_id('pelvis'),
    'left_hip': find_body_id('left_hip'),
    'left_knee': find_body_id('left_knee'),
    'left_ankle': find_body_id('left_ankle'),
    'left_foot': find_body_id('left_foot'),
    'right_hip': find_body_id('right_hip'),
    'right_knee': find_body_id('right_knee'),
    'right_ankle': find_body_id('right_ankle'),
    'right_foot': find_body_id('right_foot'),
}

# Find actuators for each joint
def find_actuators():
    actuators = {}
    for i in range(model.nu):
        name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
        if name:
            actuators[name] = i
    return actuators

actuator_ids = find_actuators()
print(f"Found {len(actuator_ids)} actuators")

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

print(f"Initial CoM: {data.subtree_com[0]}")

Expected: Humanoid model loaded with identified leg joints and actuators.

Step 2: Implement Footstep Planner

Generate footstep sequence for desired walking:

class FootstepPlanner:
    """Generate footstep positions for walking."""

    def __init__(self, step_length=0.15, step_width=0.1, step_time=0.5):
        """
        Initialize footstep planner.

        Args:
            step_length: Forward distance per step (m)
            step_width: Lateral distance between feet (m)
            step_time: Duration of each step (s)
        """
        self.step_length = step_length
        self.step_width = step_width
        self.step_time = step_time
        self.double_support_ratio = 0.2  # Fraction of step in DS

    def generate_footsteps(self, n_steps, start_pos=[0, 0], start_foot='left'):
        """
        Generate sequence of footstep positions.

        Args:
            n_steps: Number of steps to generate
            start_pos: [x, y] starting position
            start_foot: 'left' or 'right' foot starts

        Returns:
            footsteps: List of {'foot': str, 'pos': [x, y], 'time': float}
        """
        footsteps = []
        x = start_pos[0]
        foot = start_foot

        # Initial stance (both feet)
        y_left = self.step_width / 2
        y_right = -self.step_width / 2

        footsteps.append({
            'foot': 'left',
            'pos': [x, y_left],
            'time': 0.0,
            'type': 'stance'
        })
        footsteps.append({
            'foot': 'right',
            'pos': [x, y_right],
            'time': 0.0,
            'type': 'stance'
        })

        # Generate walking steps
        t = 0.0
        for i in range(n_steps):
            t += self.step_time

```python
```python
            if foot == 'left':

                x += self.step_length
                footsteps.append({
                    'foot': 'left',
                    'pos': [x, y_left],
                    'time': t,
                    'type': 'swing_end'
                })
                foot = 'right'
            else:
                x += self.step_length
                footsteps.append({
                    'foot': 'right',
                    'pos': [x, y_right],
                    'time': t,
                    'type': 'swing_end'
                })
                foot = 'left'

        return footsteps

    def get_support_phase(self, t, footsteps):
        """
        Determine support phase at time t.

        Returns:
            phase: 'double', 'left_support', or 'right_support'
            progress: 0-1 progress through current phase
        """
        # Find current step
        for i in range(len(footsteps) - 1, -1, -1):
```python
```python
            if footsteps[i]['time'] <= t:

```python
```python
                if i == len(footsteps) - 1:

                    return 'double', 1.0

                next_step = footsteps[i + 1]
                step_duration = next_step['time'] - footsteps[i]['time']
                step_progress = (t - footsteps[i]['time']) / step_duration

                ds_duration = self.double_support_ratio * step_duration

```python
```python
                if step_progress < self.double_support_ratio / 2:

                    return 'double', step_progress / (self.double_support_ratio / 2)

```python
```python
                if step_progress > 1 - self.double_support_ratio / 2:

                    return 'double', (step_progress - (1 - self.double_support_ratio / 2)) / (self.double_support_ratio / 2)

                # Single support phase
```python
```python
                if next_step['foot'] == 'left':

                    return 'right_support', step_progress
                else:
                    return 'left_support', step_progress

        return 'double', 0.0

# Create planner and generate steps
planner = FootstepPlanner(step_length=0.15, step_width=0.1, step_time=0.6)
footsteps = planner.generate_footsteps(6)

print("Footstep sequence:")
for fs in footsteps:
    print(f"  t={fs['time']:.2f}s: {fs['foot']} foot at {fs['pos']}")

Expected: Footstep sequence generated with alternating left/right steps.

Step 3: Generate ZMP Trajectory

Create ZMP reference trajectory from footsteps:

class ZMPTrajectoryGenerator:
    """Generate ZMP reference trajectory for walking."""

    def __init__(self, planner):
        self.planner = planner

    def generate_zmp_trajectory(self, footsteps, dt=0.01):
        """
        Generate ZMP reference trajectory.

        ZMP moves between feet during double support,
        stays at support foot during single support.

        Args:
            footsteps: List of footstep dictionaries
            dt: Time discretization

        Returns:
            t: Time vector
            zmp_ref: Nx2 array of ZMP references
        """
        total_time = footsteps[-1]['time'] + 0.5  # Extra settling time
        t = np.arange(0, total_time, dt)
        zmp_ref = np.zeros((len(t), 2))

        for i, ti in enumerate(t):
            zmp_ref[i] = self._get_zmp_at_time(ti, footsteps)

        return t, zmp_ref

    def _get_zmp_at_time(self, t, footsteps):
        """Get desired ZMP position at time t."""
        # Find active footsteps
        left_foot_pos = None
        right_foot_pos = None

        for fs in footsteps:
```python
```python
            if fs['time'] <= t:

```python
```python
                if fs['foot'] == 'left':

                    left_foot_pos = np.array(fs['pos'])
                else:
                    right_foot_pos = np.array(fs['pos'])

        # Get phase
        phase, progress = self.planner.get_support_phase(t, footsteps)

```python
```python
        if phase == 'double':

            # Interpolate between feet
            if left_foot_pos is not None and right_foot_pos is not None:
                # Determine transition direction
                mid = (left_foot_pos + right_foot_pos) / 2
                return mid
            elif left_foot_pos is not None:
                return left_foot_pos
            else:
                return right_foot_pos if right_foot_pos is not None else np.zeros(2)

        elif phase == 'left_support':
            return left_foot_pos if left_foot_pos is not None else np.zeros(2)

        else:  # right_support
            return right_foot_pos if right_foot_pos is not None else np.zeros(2)

# Generate ZMP trajectory
zmp_gen = ZMPTrajectoryGenerator(planner)
t_zmp, zmp_ref = zmp_gen.generate_zmp_trajectory(footsteps)

print(f"ZMP trajectory: {len(t_zmp)} points over {t_zmp[-1]:.2f}s")
print(f"ZMP X range: [{zmp_ref[:, 0].min():.3f}, {zmp_ref[:, 0].max():.3f}]")

Expected: ZMP trajectory generated following footstep positions.

Step 4: Compute CoM Trajectory (Preview Control)

Use preview control to compute CoM trajectory:

class PreviewController:
    """Preview control for CoM trajectory generation."""

    def __init__(self, z_c=0.8, dt=0.01, preview_time=1.0):
        """
        Initialize preview controller.

        Args:
            z_c: CoM height (m)
            dt: Time step
            preview_time: Preview horizon (s)
        """
        self.z_c = z_c
        self.dt = dt
        self.g = 9.81
        self.omega = np.sqrt(self.g / z_c)

        # LIPM state-space model
        # x = [com, com_dot, com_ddot]^T
        # u = ZMP
        A = np.array([
            [1, dt, dt**2/2],
            [0, 1, dt],
            [0, 0, 1]
        ])
        B = np.array([
            [1 - np.cosh(self.omega * dt)],
            [-self.omega * np.sinh(self.omega * dt)],
            [-self.omega**2 * np.cosh(self.omega * dt)]
        ])
        C = np.array([[1, 0, -self.z_c / self.g]])

        self.A = A
        self.B = B
        self.C = C

        # Preview gain computation (simplified Ricatti)
        self.n_preview = int(preview_time / dt)
        self._compute_preview_gains()

    def _compute_preview_gains(self):
        """Compute preview and integral gains."""
        # Simplified gains (can use LQR for better results)
        Q = 1e6  # State cost
        R = 1    # Input cost

        # State feedback
        self.K = np.array([self.omega**2, 2*self.omega, 1]) * 0.5

        # Preview gains (simplified exponential decay)
        self.G_preview = np.zeros(self.n_preview)
        for i in range(self.n_preview):
            self.G_preview[i] = 0.3 * np.exp(-0.5 * i * self.dt)

    def generate_com_trajectory(self, zmp_ref, x0=None):
        """
        Generate CoM trajectory using preview control.

        Args:
            zmp_ref: Nx1 array of ZMP references
            x0: Initial state [com, com_dot, com_ddot] or None

        Returns:
            com_trajectory: Nx1 array of CoM positions
        """
        n = len(zmp_ref)

        if x0 is None:
            x0 = np.array([zmp_ref[0], 0, 0])

        x = x0.copy()
        com_trajectory = np.zeros(n)
        com_velocity = np.zeros(n)

        for i in range(n):
            # Current ZMP error
            zmp_current = self.C @ x

            # State feedback term
            u_fb = -self.K @ x

            # Preview term
            u_preview = 0
            for j in range(min(self.n_preview, n - i)):
                u_preview += self.G_preview[j] * zmp_ref[i + j]

            # Control input (desired ZMP adjustment)
            u = zmp_ref[i]  # Simplified: just track reference

            # State update
            x = self.A @ x + self.B.flatten() * (u - x[0])

            # Record
            com_trajectory[i] = x[0]
            com_velocity[i] = x[1]

        return com_trajectory, com_velocity

# Generate CoM trajectory for X and Y
preview = PreviewController(z_c=0.8, dt=0.01)

com_x, com_vx = preview.generate_com_trajectory(zmp_ref[:, 0])
com_y, com_vy = preview.generate_com_trajectory(zmp_ref[:, 1])

print(f"CoM trajectory generated")
print(f"CoM X: [{com_x.min():.3f}, {com_x.max():.3f}]")
print(f"CoM Y: [{com_y.min():.3f}, {com_y.max():.3f}]")

Expected: CoM trajectory smoothly follows ZMP reference with preview.

Step 5: Generate Swing Leg Trajectories

Create smooth swing foot trajectories:

class SwingLegTrajectory:
    """Generate swing leg trajectory using polynomial interpolation."""

    def __init__(self, swing_height=0.05, ground_clearance=0.02):
        """
        Args:
            swing_height: Maximum foot lift height
            ground_clearance: Height above ground at start/end
        """
        self.swing_height = swing_height
        self.ground_clearance = ground_clearance

    def generate_trajectory(self, start_pos, end_pos, duration, dt=0.01):
        """
        Generate 3D swing foot trajectory.

        Uses cubic spline for X,Y and parabolic for Z.

        Args:
            start_pos: [x, y, z] start position
            end_pos: [x, y, z] end position
            duration: Swing time (s)
            dt: Time step

        Returns:
            t: Time vector
            pos: Nx3 position trajectory
            vel: Nx3 velocity trajectory
        """
        t = np.arange(0, duration, dt)
        n = len(t)
        s = t / duration  # Normalized time [0, 1]

        pos = np.zeros((n, 3))
        vel = np.zeros((n, 3))

        # X trajectory: smooth 5th order polynomial
        # Boundary conditions: pos, vel, acc = 0 at start/end
        for i, si in enumerate(s):
            # Smooth interpolation
            h = 10*si**3 - 15*si**4 + 6*si**5  # Position
            hdot = (30*si**2 - 60*si**3 + 30*si**4) / duration  # Velocity

            pos[i, 0] = start_pos[0] + h * (end_pos[0] - start_pos[0])
            pos[i, 1] = start_pos[1] + h * (end_pos[1] - start_pos[1])
            vel[i, 0] = hdot * (end_pos[0] - start_pos[0])
            vel[i, 1] = hdot * (end_pos[1] - start_pos[1])

        # Z trajectory: parabolic arc
        for i, si in enumerate(s):
            # Parabola peaking at midpoint
            z_lift = 4 * self.swing_height * si * (1 - si)
            z_base = start_pos[2] + si * (end_pos[2] - start_pos[2])
            pos[i, 2] = z_base + z_lift

            # Z velocity
            z_lift_dot = 4 * self.swing_height * (1 - 2*si) / duration
            z_base_dot = (end_pos[2] - start_pos[2]) / duration
            vel[i, 2] = z_base_dot + z_lift_dot

        return t, pos, vel

# Test swing trajectory
swing_gen = SwingLegTrajectory(swing_height=0.08)

start = np.array([0, 0.05, 0.0])
end = np.array([0.15, 0.05, 0.0])

t_swing, swing_pos, swing_vel = swing_gen.generate_trajectory(start, end, duration=0.3)

print(f"Swing trajectory: {len(t_swing)} points")
print(f"Max height: {swing_pos[:, 2].max():.3f} m")
print(f"Final position: {swing_pos[-1]}")

Expected: Swing trajectory with smooth arc, maximum height at midpoint.

Step 6: Execute Walking Simulation

Combine all components for walking execution:

class WalkingController:
    """Full walking controller combining all trajectory generators."""

    def __init__(self, model, data, com_height=0.8):
        self.model = model
        self.data = data
        self.com_height = com_height

        # Components
        self.footstep_planner = FootstepPlanner(step_length=0.15, step_time=0.6)
        self.zmp_generator = ZMPTrajectoryGenerator(self.footstep_planner)
        self.preview_controller = PreviewController(z_c=com_height)
        self.swing_generator = SwingLegTrajectory(swing_height=0.06)

        # Tracking gains (PD)
        self.kp_com = 500
        self.kd_com = 50
        self.kp_foot = 200
        self.kd_foot = 20

    def compute_inverse_kinematics(self, com_target, left_foot_target, right_foot_target):
        """
        Simplified IK: compute joint angles for desired poses.

        In practice, use iterative IK or analytical solution.
        """
        # Placeholder: return current joint positions
        # Real implementation would solve IK here
        return self.data.qpos.copy()

    def run_walking(self, n_steps=4, dt=0.002):
        """
        Execute walking with specified number of steps.

        Returns:
            history: Walking trajectory data
        """
        # Plan footsteps
        footsteps = self.footstep_planner.generate_footsteps(n_steps)

        # Generate trajectories
        t_ref, zmp_ref = self.zmp_generator.generate_zmp_trajectory(footsteps, dt=dt)
        com_x, com_vx = self.preview_controller.generate_com_trajectory(zmp_ref[:, 0])
        com_y, com_vy = self.preview_controller.generate_com_trajectory(zmp_ref[:, 1])

        # History
        history = {
            'time': [],
            'com': [],
            'zmp': [],
            'left_foot': [],
            'right_foot': []
        }

        # Current foot positions
        left_foot_pos = np.array([0, self.footstep_planner.step_width/2, 0])
        right_foot_pos = np.array([0, -self.footstep_planner.step_width/2, 0])

        swing_foot = None
        swing_trajectory = None
        swing_index = 0
        current_step = 0

        for i, t in enumerate(t_ref):
            # Get phase
            phase, progress = self.footstep_planner.get_support_phase(t, footsteps)

            # CoM target
            com_target = np.array([com_x[i], com_y[i], self.com_height])

            # Update swing foot if entering single support
```python
```python
            if phase == 'left_support' and swing_foot != 'right':

                # Start right swing
                swing_foot = 'right'
```python
```python
                if current_step + 2 < len(footsteps):

```python
```python
                    next_fs = [fs for fs in footsteps if fs['foot'] == 'right' and fs['time'] > t]

                    if next_fs:
                        target = np.array([next_fs[0]['pos'][0], next_fs[0]['pos'][1], 0])
                        _, swing_trajectory, _ = self.swing_generator.generate_trajectory(
                            right_foot_pos, target, 0.3, dt
                        )
                        swing_index = 0

            elif phase == 'right_support' and swing_foot != 'left':
                swing_foot = 'left'
```python
```python
                if current_step + 2 < len(footsteps):

```python
```python
                    next_fs = [fs for fs in footsteps if fs['foot'] == 'left' and fs['time'] > t]

                    if next_fs:
                        target = np.array([next_fs[0]['pos'][0], next_fs[0]['pos'][1], 0])
                        _, swing_trajectory, _ = self.swing_generator.generate_trajectory(
                            left_foot_pos, target, 0.3, dt
                        )
                        swing_index = 0

            elif phase == 'double':
                swing_foot = None

            # Update swing foot position
```python
```python
            if swing_trajectory is not None and swing_index < len(swing_trajectory):

```python
```python
                if swing_foot == 'left':

                    left_foot_pos = swing_trajectory[swing_index]
                else:
                    right_foot_pos = swing_trajectory[swing_index]
                swing_index += 1

            # Record
            history['time'].append(t)
            history['com'].append(com_target.copy())
            history['zmp'].append(zmp_ref[i].copy())
            history['left_foot'].append(left_foot_pos.copy())
            history['right_foot'].append(right_foot_pos.copy())

        return history

# Create controller and run
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)

walker = WalkingController(model, data)
history = walker.run_walking(n_steps=6)

print(f"\nWalking simulation: {len(history['time'])} steps")
print(f"Final CoM X: {history['com'][-1][0]:.3f} m")
print(f"Distance walked: {history['com'][-1][0] - history['com'][0][0]:.3f} m")

Expected: Walking trajectories generated for CoM and both feet over 6 steps.

Expected Outcomes

After completing this lab, you should have:

1. Code artifacts:

   • footstep_planner.py: Footstep sequence generation
   • zmp_trajectory.py: ZMP reference trajectory
   • preview_controller.py: CoM trajectory from ZMP
   • swing_trajectory.py: Swing leg motion
   • walking_controller.py: Integration of all components

2. Understanding:

   • Footstep planning principles
   • ZMP preview control concept
   • Gait phase transitions
   • Swing leg trajectory requirements

3. Experimental results:

   • Walking trajectory plots
   • Phase timing analysis
   • ZMP tracking performance

Rubric

Criterion	Points	Description
Footstep Planning	20	Correct alternating step sequence
ZMP Trajectory	20	Smooth ZMP following footsteps
CoM Generation	20	Preview control produces stable trajectory
Swing Trajectories	20	Smooth foot arcs with clearance
Integration	20	Complete walking sequence
Total	100

Common Errors

Error: CoM trajectory diverges Solution: Check preview gains, ensure ZMP is within support polygon.

Error: Feet collide during walking Solution: Increase swing height, check step width parameter.

Error: Discontinuities in trajectories Solution: Verify phase transition timing, smooth interpolation.

Extensions

For advanced students:

1. Turning: Add footstep rotation for curved walking
2. Variable Speed: Adapt step length/time for speed changes
3. Push Recovery: Add stepping response to perturbations
4. Rough Terrain: Height variation in swing trajectories

Related Content

• Theory: Module 08 theory.md, Section 8.2 (Gait Generation)
• Previous Lab: Lab 08-01 (Balance and Standing)
• Next Lab: Lab 08-03 (Dynamic Locomotion)
• Application: Humanoid robots, exoskeletons, prosthetics