A convex MPC locomotion controller for the Unitree G1 humanoid, built entirely from scratch in MuJoCo. Single rigid-body dynamics, Pinocchio whole-body control, and OSQP solving the QP in under 2 ms.
BHEEMA is a model predictive control based locomotion system for the Unitree G1 humanoid. I built the entire control stack from the ground up, following the MIT Cheetah 3 convex MPC framework and adapting it for a bipedal platform with passive ankles.
The G1 has no actuated ankles, which changes everything. Most bipedal MPC formulations assume 12 degrees of force control per step. The G1 only gives you 6 since each foot is effectively a point contact. The entire MPC had to be built around that constraint.
The system runs entirely in MuJoCo with hardware-matched actuator limits. The MPC plans at 16 Hz, the whole-body controller executes at 200 Hz, and the physics runs at 2000 Hz. The robot stands indefinitely and walks forward with an alternating gait for over 20 seconds.
The MPC solves a convex QP over a finite horizon using single rigid-body dynamics. It outputs optimal contact forces for each stance leg. Those forces get mapped to joint torques by the whole-body controller through Jacobian transposes, then combined with gravity compensation and joint-level PD feedback.
The swing leg controller uses 6D operational-space impedance control to track foot trajectories generated by a Raibert heuristic footstep planner. The gait scheduler coordinates which legs are in stance and which are in swing at any given moment.
The centroidal MPC models the G1 as a single rigid body with external contact forces acting at the feet. The rotation dynamics are linearized around the current orientation to keep the optimization convex, which means the QP solves fast and reliably every cycle.
Friction cone constraints are approximated as pyramids and enforced directly in the QP. The cost function penalizes deviations from the desired body pose and velocity, with pitch and roll weighted highest to keep the linearization valid.
The MPC outputs desired contact forces. The whole-body controller's job is to turn those into actual motor commands that keep the robot balanced. Here is exactly how that happens at 200 Hz.