MuJoCo Robotics Lab — Part 11: Locomotion Fundamentals

Lab 7 of 9 is the paradigm shift. Labs 1–6 lived on a fixed-base 2-link arm where every joint was actuated and "control" meant tracking a joint trajectory. Lab 7 swaps it for a Unitree G1 humanoid: 29 actuated joints, a 6-DOF unactuated floating base, and a robot that will fall the instant the controller makes a mistake. This post covers how walking is built from scratch using LIPM, a ZMP reference, preview control, a swing-foot trajectory generator, and whole-body IK.

Problem

A fixed-base arm can be position-controlled because every DOF has an actuator. The G1 cannot. At qpos[0:7] there is a 7-DOF free joint — three positions plus a quaternion — that nothing drives directly. The pelvis moves as a consequence of contact forces and joint torques, not as a command. Three consequences follow:

1. Underactuated floating-base dynamics. nq = 36 generalized coordinates, nu = 29 actuators. The missing 6 DOF are the unactuated base. You can choose joint torques but cannot command the pelvis pose.
2. Contact switching. Walking is hybrid. Support alternates between left foot, double support, right foot. Every swap changes the Jacobians constraining the base, the support polygon, and which foot is free to move.
3. Balance constraints. The Zero-Moment Point must stay strictly inside the support polygon. Violate it and the robot rotates about a foot edge.

The naive approach — set ctrl = q_stand and hope — works for a second because position servos are stiff. Then gravity droop accumulates, the CoM drifts outside the support polygon, and the robot topples. Walking requires explicit reasoning about where the CoM must be over time so the ZMP stays legal.

Theory

Linear Inverted Pendulum Model

Full humanoid dynamics have ~35 coupled DOF — planning at that level online is intractable. LIPM throws everything away except a point mass at constant height z_c:

ẍ = (g / z_c) · (x − p_x)
ÿ = (g / z_c) · (y − p_y)

(x, y) is the CoM, (p_x, p_y) the ZMP. Two independent 1D linear systems replace the full robot. The constant-height assumption is the trick — holding the CoM on a horizontal plane drops vertical dynamics and collapses the Coriolis/mass coupling.

Zero-Moment Point

The ZMP is the point on the ground where the net horizontal moment of the ground reaction force is zero. The dynamic balance criterion is simple:

ZMP(t) ∈ SupportPolygon(t)   ∀t

During single support the support polygon is one foot. During double support it is the convex hull of both feet. The footstep plan implicitly defines a ZMP reference — keep it centered under the stance foot during single support and linearly move it across during double support. That reference is what the CoM must track.

Preview Control (Kajita 2003)

Given a ZMP reference p_ref(k), find a CoM trajectory whose LIPM-predicted ZMP matches it. The catch: ZMP depends on CoM acceleration, so the CoM must start moving before the reference changes. Preview control handles this by looking ahead N samples and treating the look-ahead as a feedforward term. Discretize LIPM, augment with a ZMP error integrator, and solve the discrete algebraic Riccati equation:

x_{k+1} = A · x_k + b · u_k
p_k     = C · x_k
u_k = −K_e · e_k − K_s · x_k − Σ_{j=1..N} f(j) · p_ref(k+j)

K_e is the error-integral gain, K_s state feedback, f(j) preview gains weighting future ZMP references. At T = 0.01 s with N_preview = 200 the controller gets 2 s of look-ahead — enough to anticipate a full step. Applied independently to x and y, this produces a smooth, dynamically balanced CoM trajectory.

Swing-Leg Trajectory

During single support the swing foot moves from the last footstep to the next — horizontal interpolation is linear, vertical is a parabola peaking at step_height mid-swing:

foot_xy(t) = p_start_xy + s · (p_end_xy − p_start_xy)
foot_z(t)  = 4 · h · s · (1 − s)       s = t / T_ss

This guarantees zero vertical velocity at liftoff and touchdown. No IK is solved yet — this is purely the reference.

Whole-Body Inverse Kinematics

Everything converges at the IK stage. Three task targets per timestep: desired pelvis position (from the LIPM CoM), left-foot pose, right-foot pose. Whole-body IK converts them to joint angles via damped least squares on the Pinocchio analytical Jacobians:

dq = Jᵀ · (J · Jᵀ + λ² · I)⁻¹ · e
q  ← pin.integrate(model, q, α · dq)

pin.integrate handles the quaternion exponential on the free-flyer joint — you cannot just add dq to q with a floating base. Task priority is CoM > foot placement > posture, with λ = 1e-3 for singularity robustness near full leg extension.

Implementation

The lab loads the Unitree G1 from the MuJoCo Menagerie directly — no separate URDF. Pinocchio's buildModelFromMJCF with JointModelFreeFlyer() produces nq = 36, nv = 35, matching MuJoCo. One gotcha: MuJoCo stores free-joint quaternions as (w, x, y, z); Pinocchio FreeFlyer uses (x, y, z, w). Every state handoff routes through mj_qpos_to_pin() / pin_q_to_mj() helpers that swap the scalar component.

The full pipeline is precomputed offline then replayed online:

FootstepPlanner → ZMP reference → LIPMPreviewController → CoM(t)
                                ↓
                       swing_trajectory(t)
                                ↓
                WholeBodyIK(CoM, lfoot, rfoot) → q_des(t)
                                ↓
                    MuJoCo position servos @ 500 Hz

Precomputing the trajectory before the simulation runs keeps the online loop cheap — it is a table lookup plus one IK solve per step.

Preview Controller

The core of the planner is compact once the DARE is solved:

class LIPMPreviewController:
    def __init__(self, T, z_c, g, Q_e, R, N_preview):
        A = np.array([[1, T, T*T/2], [0, 1, T], [0, 0, 1]])
        b = np.array([[T**3 / 6], [T*T / 2], [T]])
        c = np.array([[1, 0, -z_c / g]])

        # Augmented system with ZMP error integrator
        A_tilde = np.block([[1.0, c @ A], [np.zeros((3, 1)), A]])
        b_tilde = np.vstack([c @ b, b])
        Q = np.zeros((4, 4)); Q[0, 0] = Q_e

        P = solve_discrete_are(A_tilde, b_tilde, Q, R)
        K = np.linalg.solve(R + b_tilde.T @ P @ b_tilde,
                            b_tilde.T @ P @ A_tilde)
        self.K_e, self.K_s = K[0, 0], K[0, 1:]

        # Preview gains via backward recursion on the closed-loop system
        self.f = np.zeros(N_preview)
        Ac = A_tilde - b_tilde @ K
        X  = -Ac.T @ P @ np.vstack([[1.0], np.zeros((3, 1))])
        for j in range(N_preview):
            self.f[j] = np.linalg.solve(
                R + b_tilde.T @ P @ b_tilde, b_tilde.T @ X).item()
            X = Ac.T @ X

    def step(self, p_ref_k, preview, state, err):
        u = -self.K_e * err - self.K_s @ state - self.f @ preview
        state_next = self.A @ state + self.b.flatten() * u
        p_actual   = float(self.c @ state_next)
        return state_next, err + (p_actual - p_ref_k), p_actual

Applied separately to x and y with the same footstep-derived p_ref, this produces the CoM trajectory the walking controller tracks.

Whole-Body IK and Control Loop

IK uses Pinocchio computeFrameJacobian on the left_foot and right_foot frames (not bodies — G1 exposes foot sites which import as frames). The left-leg Jacobian block lives at v[6:12], right leg at v[12:18]. Each leg solves independently against its foot target, pelvis clamped to the desired CoM position and upright orientation.

One subtlety is gravity droop: at kp = 500 the stiff position servos still sag under the G1's weight. The fix is a feedforward term that cancels gravity and adds a velocity lead:

ctrl = q_des + qfrc_bias / kp + kd · qd_des / kp

qfrc_bias is MuJoCo's gravity + Coriolis vector. Dividing by kp and adding to the position command is the exact offset needed for the servo to reach q_des at steady state instead of settling below it.

Results

Twelve-step walking demo on flat ground, precomputed trajectory, 500 Hz physics, 100 Hz control.

The ZMP never leaves the support polygon and the G1 walks end-to-end without a recovery step. Visually the gait is slow and cautious — LIPM + preview control optimizes for feasibility, not speed — but it is dynamically balanced from the first footfall to the last.

Key Takeaways

• Floating base breaks the fixed-base playbook. You cannot position-control the pelvis — ZMP, LIPM, preview control, and whole-body IK all exist because the base must be induced to move rather than commanded.
• LIPM is a design choice, not a physics model. Constant CoM height is a simplification enforced by IK. The reward is two decoupled 1D linear systems trivial to plan on.
• Preview control is the minimum viable CoM planner. Tracking ZMP requires anticipation; 2 s of look-ahead collapses the problem to a linear feedback law with feedforward preview gains.
• Pinocchio's free-flyer integration is mandatory. Adding dq to q naively corrupts the base quaternion. pin.integrate applies the quaternion exponential and is the only correct update.
• Gravity feedforward makes position servos usable on a humanoid. ctrl = q_des + qfrc_bias/kp closes the droop gap that would otherwise accumulate into a fall.
• Precompute what you can. ZMP, CoM, and foot targets are deterministic given the footstep plan — solve offline, leaving the online loop as a lookup plus one IK solve.

Resources

• Explore this lab on NotebookLM
• Source code on GitHub
• Full YouTube playlist