Kazam_screencast_00049.mp4
Convex MPC locomotion controller for the Unitree Go2, in C++ on ROS 2 Humble with Gazebo Classic. Covers the whole path from spawning the robot to driving it from a GUI: stand up, MPC balance, trot, and a feedforward recovery in between.
Built for a display robot, so predictability is worth more than peak performance: the MPC is a convex QP that always has a global optimum, and every mode has an explicit fallback.
| Package | |
|---|---|
quadruped_core |
Control algorithms. No ROS dependency, unit tested without Gazebo. |
quadruped_controller |
ROS node, hardware abstraction layer, launch files, configuration. |
quadruped_description |
URDF, meshes and Gazebo / ros2_control tags. |
quadruped_msgs |
Command, status and mode change interfaces. |
quadruped_ui |
PyQt5 control panel. |
Ubuntu 22.04, ROS 2 Humble, Gazebo Classic 11, GCC 11 (C++17).
Pinocchio 3.x and ProxSuite from robotpkg,
installed under /opt/openrobots:
sudo apt install robotpkg-py310-pinocchio robotpkg-proxsuitecolcon build --symlink-install
source install/setup.bashThree terminals, always in this order:
# terminal 1 - Gazebo and the robot
ros2 launch quadruped_gazebo go2_gazebo.launch.py
# terminal 2 - control node
ros2 launch quadruped_controller controller.launch.py
# terminal 3 - control panel
ros2 launch quadruped_ui ui.launch.pyThe robot starts in PASSIVE and lies on the ground; nothing commands torque until a mode is requested. Press PRONE, then STAND UP, then BALANCE, then TROT.
| Mode | Control | |
|---|---|---|
PRONE |
fold up and hold | joint PD, feedforward trajectory |
STAND_UP |
prone, tucked, standing | joint PD, feedforward trajectory |
STAND |
hold the stand pose | joint PD |
BALANCE |
hold the stand pose | SRBD MPC |
RECOVER |
return to the stand pose | joint PD, feedforward trajectory |
TROT |
walk | gait schedule + foot trajectories + SRBD MPC |
LIE_DOWN |
standing, tucked, prone | joint PD, feedforward trajectory |
MPC. Single rigid body dynamics make the problem linear in the ground reaction forces, so it condenses into a convex QP: 13 states, 12 forces, a 10 step horizon at 30 ms, 120 decision variables and 200 inequality constraints. Measured solve time is 0.7 ms mean, 1.5 ms worst case, run at 100 Hz inside a 500 Hz control loop.
Forces become joint torques through tau = -J^T R^T f.
Trot. The gait schedule decides which legs are planted. Swing legs follow a planned trajectory (Raibert footstep placement, quintic profile, zero touch down velocity) through closed form IK; stance legs apply the MPC forces. The MPC receives the contact schedule across the whole horizon, not just the current contact set, so it does not lurch every time a foot lands.
Everything lives in
gazebo_controller.yaml.
Gains can be changed while running:
- Flat ground only. The estimator assumes every planted foot is at z = 0 and the footstep planner lands on the same plane, so a step of a few centimetres biases the height estimate and trips the guard.
- Contact is open loop. Stance and swing come from the gait schedule. Gazebo contact sensors are published but nothing subscribes yet.
- Joint torque limits are not in the QP. Only the vertical force is bounded, so the MPC can ask for a force the leg cannot produce in an extended posture.
MIT. See LICENSE.