Legged Robot MPC Controller

ROS 2 controller integration for legged robot MPC using OCS2 and Pinocchio. Tested in MuJoCo simulation environment.

The humanoid centroidal MPC & whole-body MPC are migrated from the original implementaion of wb_humanoid_mpc.

Build and Install

Clone this repo

git clone https://github.com/wei-hsuan-cheng/legged_robot_mpc_controller.git

Clone all sub-repo with vcs

cd <workspace_dir>/src
mkdir legged_robot_mpc_controller_dependencies
vcs import < legged_robot_mpc_controller/legged_robot_mpc_controller.repos

Install pinocchio library (3.9.x required; packages.ros.org only serves the newest build, which is 4.0.0 now, so install 3.9.0 from the ROS snapshot archive and hold it)

# Import the ROS snapshot archive key and add the 2026-03-29 humble snapshot
# (works on x86_64 and arm64; the arch is taken from dpkg)
curl -s "https://keyserver.ubuntu.com/pks/lookup?op=get&search=0xAD19BAB3CBF125EA" | \
  sudo gpg --batch --yes --dearmor -o /usr/share/keyrings/ros-snapshots-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-snapshots-archive-keyring.gpg] http://snapshots.ros.org/humble/2026-03-29/ubuntu jammy main" | \
  sudo tee /etc/apt/sources.list.d/ros2-snapshots.list
sudo apt update

# Install pinocchio 3.9.0 from the snapshot and pin it so a later apt upgrade
# does not pull 4.x. The exact build id differs per architecture
# (amd64: ...20260304.203533, arm64: ...20260307.163259), so resolve it from apt:
PINOCCHIO_VERSION=$(apt-cache madison ros-humble-pinocchio | awk '/snapshots.ros.org/ {print $3; exit}')
sudo apt install ros-humble-pinocchio=${PINOCCHIO_VERSION}
sudo apt-mark hold ros-humble-pinocchio

# Drop the snapshot source again afterwards
sudo rm /etc/apt/sources.list.d/ros2-snapshots.list && sudo apt update

# Check the installed version and the hold:
# expect "Version: 3.9.0-..." and flag "hi" (h = held, i = installed)
dpkg -s ros-humble-pinocchio | grep Version
dpkg -l ros-humble-pinocchio | tail -1

First install by rosdep

# rosdep install
cd <workspace_dir>
sudo rosdep init # if you never did this
rosdep update
rosdep install --ignore-src --from-paths src -y -r

Build mujoco-related pkg. (See troubleshooting section here if needed)

cd <workspace_dir>
NUM_JOBS=2 && \
export CMAKE_BUILD_PARALLEL_LEVEL=${NUM_JOBS} && \
export MAKEFLAGS=-j${NUM_JOBS} && \
export NINJAFLAGS=-j${NUM_JOBS} && \
colcon build --symlink-install \
  --packages-up-to mujoco_ros2_control mujoco_ros2_control_demos \
  --executor sequential --parallel-workers ${NUM_JOBS} \
  --cmake-force-configure \
  --cmake-args -DBUILD_TESTING=OFF -DCMAKE_BUILD_TYPE=Release && \
  . install/setup.bash

Build pkgs up-to legged_robot_mpc_controller

cd <workspace_dir>
NUM_JOBS=2 && \
export CMAKE_BUILD_PARALLEL_LEVEL=${NUM_JOBS} && \
export MAKEFLAGS=-j${NUM_JOBS} && \
export NINJAFLAGS=-j${NUM_JOBS} && \
colcon build --symlink-install \
  --packages-select legged_robot_mpc_controller \
  --executor sequential --parallel-workers ${NUM_JOBS} \
  --cmake-force-configure \
  --cmake-args -DBUILD_TESTING=OFF -DCMAKE_BUILD_TYPE=Release && \
  . install/setup.bash

Run MuJoCo Example

Launch the humanoid robot:

ros2 launch legged_robot_mpc_controller g1.launch.py \
  mpcControllerName:=humanoid_centroidal_mpc_controller \
  mpcFreq:=100 \
  mrtFreq:=1000 \
  mujoco_headless:=true \
  baseCommandGui:=true

Scripted trajectory tests (figure-eight, flat ground and ramp)

launch/figure_eight_command.py walks a closed figure-eight (Gerono lemniscate) at constant speed with a sinusoidal pelvis height, publishing the same WalkingVelocityCommand topic the GUI uses. It exercises forward speed, both turn directions and the curvature reversal at the crossing continuously, without ever leaving a small patch of floor.

Three things will bite you if you skip them:

Pick a scene without the stairs. scene.xml puts a 0.10 m riser at x = 0.75 m; the default figure-eight spans x in [-2, +2] and would walk straight into it. Use scene_flat.xml (flat) or scene_ramp.xml (3 deg ramp).
Turn the command GUI off. It publishes its slider values to the same topic at 50 Hz, so leaving it up means its zeros fight the script.
Launch from the workspace root. libFolder is a relative path (auto_generated/g1); from anywhere else the cached CppAD libraries are not found and the model is regenerated.

Flat ground:

# Terminal 1
cd <workspace_dir>
source install/setup.bash
ros2 launch legged_robot_mpc_controller g1.launch.py \
  mujoco_headless:=true \
  mujocoModelFile:=scene_flat.xml \
  baseCommandGui:=false \
  diagnosticsLog:=true diagnosticsLogPrefix:=/tmp/fig8_%t

# Terminal 2
cd <workspace_dir>
source install/setup.bash
ros2 run legged_robot_mpc_controller figure_eight_command.py --dry-run   # check the envelope
ros2 run legged_robot_mpc_controller figure_eight_command.py

--dry-run prints path length, lap time, peak/mean yaw rate and the tightest turn radius, and warns when the geometry leaves the measured-safe envelope (vx <= 0.30 m/s, |omega| <= 0.5 rad/s). That is how the defaults were chosen: --lx 4.0 --ly 1.6 --speed 0.22 gives a peak yaw rate of 0.466 rad/s. Geometry, speed, laps and all height parameters are arguments:

ros2 run legged_robot_mpc_controller figure_eight_command.py \
  --lx 4.0 --ly 1.6 --speed 0.22 --laps 2 \
  --zmin 0.77 --zmax 0.80 --zperiod 13.0

Ramp (3 degree slope, terrain test):

ros2 launch legged_robot_mpc_controller g1.launch.py \
  mujocoModelFile:=scene_ramp.xml \
  baseCommandGui:=false

scene_ramp.xml is flat until x = 1.5 m, then rises 3 degrees to z = 0.21 m at x = 5.49 m. Use a forward walk rather than the figure-eight for this, since the lemniscate stays within x <= 2 and would barely touch the slope:

ros2 run legged_robot_mpc_controller command_sequence.py --sequence walk_far

To have the swing planner actually follow the slope instead of assuming a flat floor, set ocs2.model.useTerrainHeightEstimate: true — see Configuration below for the caveat, it is off by default for a reason.

Analysing a run:

ros2 run legged_robot_mpc_controller analyze_diagnostics.py /tmp/fig8_<stamp> --timeline 5

Reports estimator-vs-ground-truth error, filter consistency (NIS), contact bookkeeping, per-joint jitter, solver health and the per-cost-term breakdown, and locates the loss of balance if there was one.

Base targets

Base twist command:

# Select twist tracking
ros2 topic pub --once /humanoid/target_mode \
  std_msgs/msg/String "{data: base_pose}"

# Publish twist command
ros2 topic pub -r 50 /humanoid/walking_velocity_command \
  ocs2_msgs/msg/WalkingVelocityCommand \
  "{linear_velocity_x: 0.25, linear_velocity_y: 0.0,
    desired_pelvis_height: 0.7925, angular_velocity_z: 0.0}"

Base pose command:

WORK IN PROGRESS

# Select pose tracking
ros2 topic pub --once /humanoid/target_mode \
  std_msgs/msg/String "{data: base_pose}"

# Publish pose command
ros2 topic pub --once /humanoid/base_pose_command \
  geometry_msgs/msg/PoseStamped \
  "{header: {frame_id: world},
    pose: {
      position: {x: 0.0, y: 0.0, z: 0.7925},
      orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
    }}"

Fixed-sequence stair climbing (`stair_climb` target mode) (centroidal MPC only)

The centroidal MPC provides an example of climbing a staircase with known ground-truth geometry using a pre-compiled, fixed sequence: gait (mode schedule), foothold placements, swing lift-off/touch-down heights, and a pelvis reference (zero pitch/roll) are all generated once from config/g1/terrain/stair_climbing/*.yaml at trigger time. No perception / plane segmentation is involved.

Run example:

# Launch
ros2 launch legged_robot_mpc_controller g1.launch.py \
  mpcControllerName:=humanoid_centroidal_mpc_controller \
  mpcFreq:=100 \
  mrtFreq:=1000 \
  mujoco_headless:=true \
  baseCommandGui:=false

# Trigger the target mode
ros2 topic pub --once /humanoid/target_mode std_msgs/msg/String "{data: terrain_walk}"

# Then the stair climbing motion starts automatically

Auto-test: tests/stair_climbing_test.sh launches the simulation headless, triggers the climb, monitors the pelvis ground-truth odometry and both foot TF frames, and prints one of VERDICT: SUCCESS | INCOMPLETE | FALL | NO_ODOM (exit code 0 only on SUCCESS, so it can gate CI):

# Workspace sourced; runs from anywhere (cds to the workspace root internally
# so the CppAD cache in auto_generated/ is found)
ros2 run legged_robot_mpc_controller stair_climbing_test.sh
# or with explicit args / from the source tree:
./tests/stair_climbing_test.sh /tmp/stair_climbing_test.log 45 90   # [log] [monitor_s] [startup_wait_s]

Terrain-aware walking (`terrain_walk` target mode) (centroidal MPC only)

Perception-free online terrain locomotion: Instead of a pre-scripted sequence, the robot follows a plain velocity command while a TerrainFootholdPlanner selects footholds each solver cycle over the same ground-truth staircase geometry (implementation of the T-RO 2023 perceptive-locomotion pipeline, without the elevation-map / plane-segmentation integrated).

Each cycle it:

Extrapolates a nominal foothold under the hip (Raibert heuristic + capture-point velocity feedback)
Projects it onto the terrain surface, prefers stepping up onto a reachable tread (step-up bonus)
Anchors the stance laterally to the stair centerline
Feeds the per-phase support heights to the swing planner, and terrain-adapts the pelvis height (zero pitch/roll) while gating forward momentum so the CoM cannot overrun the feet at a riser.

Run example:

# Launch
ros2 launch legged_robot_mpc_controller g1.launch.py \
  mpcControllerName:=humanoid_centroidal_mpc_controller \
  mpcFreq:=100 \
  mrtFreq:=1000 \
  mujoco_headless:=true \
  baseCommandGui:=true

# Trigger the target mode
ros2 topic pub --once /humanoid/target_mode std_msgs/msg/String "{data: terrain_walk}"

# Then send base twist command to play with the robot

Auto-test (headless, drives the climb and prints a verdict; exit 0 only on SUCCESS):

VX=0.08 ros2 run legged_robot_mpc_controller terrain_walk_test.sh /tmp/terrain_walk.log 90

Arm joint / frame-relation targets (`/humanoid/mpc_targets`)

`command_type`	Meaning
`joint`	Track arm joint positions. One trajectory; states cover exactly the tracked arm joints (shoulder + elbow), ordered by `joint_names`. Overrides the built-in gait arm swing.
`frame_relation`	Track the relative pose of a leaf frame expressed in a reference frame. Convention (matching `mpc_controllers`): `source_frames[i]` is the reference (root) frame — a robot frame such as `pelvis`, or a global frame (`world`) — and `target_frames[i]` is the tracked leaf frame (a hand). One trajectory per pair; states are `[position xyz, quaternion xyzw]` of target expressed in source. Optional `frame_relation_tracking_weights` (6 per pair: position xyz, orientation xyz) override the configured defaults.
`joint_frame_relation`	Both at once: the joint trajectory first in `target_trajectories`, then one per frame pair.
`default`	Clear all external targets and revert to the built-in posture + gait arm swing.

Sample target publishers in launch/command/mpc_targets/:

# Sine arm-joint swing (command_type: joint)
ros2 run legged_robot_mpc_controller joint_tracking_target.py

# Hold both hands at pelvis-relative poses (command_type: frame_relation)
ros2 run legged_robot_mpc_controller frame_relation_tracking_target.py

# Fixed arm posture + left-hand pose target together (command_type: joint_frame_relation)
ros2 run legged_robot_mpc_controller joint_frame_relation_tracking_target.py

# Switch back to the built-in arm-swing reference
ros2 topic pub --once /humanoid/mpc_targets ocs2_msgs/msg/MpcTargets "{command_type: 'default'}"

Minimal one-shot CLI examples:

# Hold both arms at a raised posture (command_type: joint)
ros2 topic pub --once /humanoid/mpc_targets ocs2_msgs/msg/MpcTargets "{
  command_type: 'joint',
  joint_names: [left_shoulder_pitch_joint, left_shoulder_roll_joint, left_shoulder_yaw_joint, left_elbow_joint,
                right_shoulder_pitch_joint, right_shoulder_roll_joint, right_shoulder_yaw_joint, right_elbow_joint],
  target_trajectories: [{time_trajectory: [0.0],
                         state_trajectory: [{value: [0.3, 0.0, 0.0, 0.6, 0.3, 0.0, 0.0, 0.6]}],
                         input_trajectory: [{value: []}]}]}"

# Move the left hand to a pelvis-relative pose (command_type: frame_relation;
# source = reference frame, target = tracked leaf frame)
ros2 topic pub --once /humanoid/mpc_targets ocs2_msgs/msg/MpcTargets "{
  command_type: 'frame_relation',
  source_frames: [pelvis],
  target_frames: [left_rubber_hand],
  target_trajectories: [{time_trajectory: [0.0],
                         state_trajectory: [{value: [0.32, 0.15, 0.20, 0.0, 0.0, 0.0, 1.0]}],
                         input_trajectory: [{value: []}]}]}"

Both channels are soft costs balanced against the rest of the MPC; raise the command weights to track tighter. frame_relation is currently registered for the centroidal controller only.

Useful launch args:

baseCommandGui:=true | false
spawnMpcController:=true | false         # false is only for environment smoke tests; the robot will not balance
mpcControllerName:=humanoid_centroidal_mpc_controller | humanoid_wb_mpc_controller
use_mujoco_sim:=true | false             # false: plain ros2_control_node (fake hardware)
use_fake_hardware:=false | true          # mock_components/GenericSystem when not using MuJoCo
ros2ControlCommandInterface:=effort | effort_pd | position
mujocoEffortCommandMode:=actuator | qfrc_applied
initialPoseFile:=<...>/initial_pose.yaml # initial joint state (config/g1/initial_pose.yaml)
rviz:=true | false
mujoco_headless:=true | false
mujoco_wait_to_start:=true | false       # paused start + /mujoco_ros2_control/start service
mujoco_real_time_factor:=1.0             # double
mujoco_publish_rate:=100.0               # double
gt_enabled:=true | false                 # floating-base ground-truth odometry for visualization / ROS consumers
gt_body_frame:=pelvis                    # MuJoCo body published as ground truth
mpcFreq:=100                             # integer
mrtFreq:=1000                            # integer
libFolder:=auto_generated/g1             # CppAD codegen output
mujocoModelFile:=scene.xml               # swap scene: boxes / stairs / slope in description/g1/mujoco
stairClimbingFile:=stair_climbing.yaml
terrainWalkingFile:terrain_walking.yaml

Useful topics:

# Floating-base ground truth from mujoco_ros2_control (nav_msgs/Odometry).
# MPC reads the same pelvis body directly through ros2_control state interfaces.
ros2 topic echo /mujoco/ground_truth/odom

# Actuated joint states
ros2 topic echo /joint_states

Configuration

All MPC settings live in ROS 2 parameters:

Controller	Parameter declaration	Config adapter	Interface
`humanoid_wb_mpc_controller`	`src/humanoid_wb_mpc/humanoid_wb_mpc_controller_parameter.yaml`	`src/humanoid_wb_mpc/wb_mpc_config_builder.cpp`	`WBMpcInterface::Config`
`humanoid_centroidal_mpc_controller`	`src/humanoid_centroidal_mpc/humanoid_centroidal_mpc_controller_parameter.yaml`	`src/humanoid_centroidal_mpc/centroidal_mpc_config_builder.cpp`	`CentroidalMpcInterface::Config`

Both declare model settings, foot-constraint gains, swing trajectory, SQP/rollout/MPC solver settings, initial state, Q/R/Q_final cost diagonals, task-space foot cost weights, and the friction-cone / contact-moment / joint-limit / foot-collision constraint parameters.

The whole-body state is:

[base pose, joint positions, base velocity, joint velocities] with joint accelerations + contact wrenches as inputs.

The centroidal state is:

[normalized centroidal momentum, base pose, joint positions] with joint velocities + contact wrenches as inputs.
Additional centroidal-only costs (ICP, torso task-space tracking via costs.taskSpaceCosts, leg external-torque costs via costs.legTorqueCost).

Loaders shared by both controllers (gait map, reference config, cost-matrix assembly) live in common/config/config_builder_utils.hpp.

Robot-specific values:

config/g1/gait.yaml is the named gait library (mode sequence templates), referenced by ocs2.gait.gaitLibraryFile and shared by both controllers.
config/g1/initial_pose.yaml sets the simulation start pose consumed by the ros2_control xacro.

Terrain height (`ocs2.model.useTerrainHeightEstimate`)

adaptToCurrentGroundHeight() computes a ground-height estimate from the stance feet and historically discarded it (terrainHeight = 0.0). That constant is what the swing trajectory is built around, so it hard-codes a flat floor at the world origin — the single thing preventing this stack from walking on sloped or stepped ground, however well the estimator tracks the terrain.

Setting useTerrainHeightEstimate: true uses the estimate instead, rate-limited by maxTerrainHeightStep per solver iteration. Pair it with a stateEstimator.height.source that does not itself assume a plane — inekf or anchored. kinematic and blend pin the stance feet to groundZ and would feed the flat assumption straight back in.

Off by default, and not yet trustworthy. On the 3 degree ramp it more than doubles survival (40.7 s → 94.1 s) and halves peak pitch (0.50 → 0.22 rad), but the reported height drifts: |e_z| reaches 0.39 m on flat ground and 0.13 m on the ramp. The two are the same effect — tightening maxTerrainHeightStep removes the drift and the benefit together (0.01 → 149 s walked / 0.39 m drift; 0.002 → 68 s / 0.079 m; 0.0005 → 64 s / 0.050 m). The gain is largely the base-height target being allowed to run away with the drifting terrain reference, not terrain adaptation.

The loop is: with anchored, touchdown anchors are computed from the filter's own height, so enabling the terrain reference lets filter drift feed the anchors, which feed the reference. inekf (no blend) shows it far less — |e_z| 0.053 instead of 0.390 under the same setting. Bounding this is the open problem before terrain walking can be relied on.

InEKF height and contact sources

Measured on the figure-eight, flat ground, survival time:

`height.source`	`contact.source: scheduled`	`contact.source: torque`
`kinematic`	127.8 s	12.7 s
`blend`	60.3 s	8.2 s
`anchored`	43.2 s	6.6 s
`inekf`	26.3 s	7.4 s

torque contact detection collapses everything to 6–13 s. ContactEstimator assumes contact i is served by joints 3i…3i+2 — a 12-DoF quadruped layout that maps G1's heel/toe onto the wrong joints entirely. This is structural, not a beta0/beta1 tuning problem; use scheduled.

kinematic wins on flat ground because it pins the feet to the same plane the controller assumes while useTerrainHeightEstimate is off — that is consistency, not terrain generality, which is exactly why terrain support needs both changes together.

Floating-Base State (ground truth vs. state estimator)

The floating-base feedback source is chosen with the floatingBaseSource launch argument (centroidal MPC):

# (default) simulator/hardware body state - MuJoCo ground truth in simulation
ros2 launch legged_robot_mpc_controller g1.launch.py \
  mpcControllerName:=humanoid_centroidal_mpc_controller \
  floatingBaseSource:=ground_truth_state

# proprioceptive InEKF drives the MPC: IMU + joint encoders + scheduled contacts,
# no ground-truth body in the control loop
ros2 launch legged_robot_mpc_controller g1.launch.py \
  mpcControllerName:=humanoid_centroidal_mpc_controller \
  floatingBaseSource:=state_estimator

`floatingBaseSource`	Feedback used by the MPC
`ground_truth_state` (default)	Body pose/twist from `ros2_control` state interfaces (MuJoCo ground truth in simulation; the robot's own base state on hardware).
`state_estimator`	Contact-aided InEKF estimate (pose, world linear velocity, body angular velocity). Ground truth is used only for the brief filter warm-up and for evaluation.

The estimator can also run in parallel while ground truth drives control, which is the way to compare it against GT without risking the robot — set stateEstimator.enabled: true and keep floatingBaseSource:=ground_truth_state. Either way it publishes its estimate for evaluation:

ros2 topic echo /mujoco/ground_truth/odom        # ground truth (also used for RViz/TF)
ros2 topic echo /humanoid/state_estimate/odom    # InEKF estimate

Closed-loop status: verified on flat ground — tests/state_estimator_closed_loop_test.sh reports VERDICT: SUCCESS (2.1 m of walking, height error 1.8 mm, roll/pitch < 0.6°, body-velocity error 0.026 m/s). Backward, lateral and turning motions all match the ground-truth-driven behaviour. Note the default scene has a staircase in front of the robot, so plain forward walking runs into it and falls with ground truth as well — flat-ground tests command motion away from the stairs.

See docs/humanoid_state_estimation.md for the equations, implementation and the terrain limitation of the kinematic height anchor.

Acknowledgements

The original code implementation of humanoid centroidal MPC and whole-body MPC: manumerous/wb_humanoid_mpc

Contact

Author: Wei-Hsuan Cheng (johnathancheng0125@gmail.com)
Homepage: wei-hsuan-cheng
GitHub: wei-hsuan-cheng

Navigation Menu

Repository navigation

About

Repository files navigation

Legged Robot MPC Controller

Build and Install

Run MuJoCo Example

Scripted trajectory tests (figure-eight, flat ground and ramp)

Base targets

Fixed-sequence stair climbing (`stair_climb` target mode) (centroidal MPC only)

Terrain-aware walking (`terrain_walk` target mode) (centroidal MPC only)

Arm joint / frame-relation targets (`/humanoid/mpc_targets`)

Configuration

Terrain height (`ocs2.model.useTerrainHeightEstimate`)

InEKF height and contact sources

Floating-Base State (ground truth vs. state estimator)

Acknowledgements

Contact

About

Releases

Packages

Contributors

Languages

Name		Name	Last commit message	Last commit date
cmake		cmake
config		config
description/g1		description/g1
docs		docs
include		include
launch		launch
src		src
tests		tests
.gitignore		.gitignore
CMakeLists.txt		CMakeLists.txt
Claude.md		Claude.md
README.md		README.md
legged_robot_mpc_controller.repos		legged_robot_mpc_controller.repos
legged_robot_mpc_controller_plugin.xml		legged_robot_mpc_controller_plugin.xml
package.xml		package.xml

Latest commit

History

Folders and files

Repository files navigation

Legged Robot MPC Controller

Build and Install

Run MuJoCo Example

Scripted trajectory tests (figure-eight, flat ground and ramp)

Base targets

Fixed-sequence stair climbing (stair_climb target mode) (centroidal MPC only)

Terrain-aware walking (terrain_walk target mode) (centroidal MPC only)

Arm joint / frame-relation targets (/humanoid/mpc_targets)

Configuration

Terrain height (ocs2.model.useTerrainHeightEstimate)

InEKF height and contact sources

Floating-Base State (ground truth vs. state estimator)

Acknowledgements

Contact

About

Topics

Resources

Stars

Watchers

Forks

Releases

Packages

Contributors

Languages

Fixed-sequence stair climbing (`stair_climb` target mode) (centroidal MPC only)

Terrain-aware walking (`terrain_walk` target mode) (centroidal MPC only)

Arm joint / frame-relation targets (`/humanoid/mpc_targets`)

Terrain height (`ocs2.model.useTerrainHeightEstimate`)