Floating-base humanoid falls forward under SAP PD stand test in Drake

[moavia90]

wb_humanoid_mpc is an open source project making humanoid robot walk using mpc . it comes with 2 types centroidal dynamics mpc i.e. whole-body kinematics and the center off mass dynamics or whole body dynamics mpc i.e. contact forces and joint accelerations with the option to compute the joint torques for each step planned accross the horizon.

Current repo is relying on pure rviz based simulation or mujoco based simulation

I am working on a project trying to test mpc policy of g1 humanoid robot in drake. I am integrating Drake as an alternative simulator for an existing MuJoCo + OCS2/MRT humanoid MPC runtime. The robot is Unitree G1.

drake_mrt_sim_node.cpp is acting as a Drake replacement for the MujocoSimInterface.cpp. It loads the G1 model into a Drake MultibodyPlant, adds ground contact, creates Meshcat visualization, converts Drake state into your common robot::model::RobotState, then lets the OCS2/MRT controller fill RobotJointAction.

Right now the launch file uses control_mode=sap_pd_stand, so MPC walking torque is intentionally disabled. The node pins the robot briefly at startup/reset, releases the floating base, and asks Drake’s built-in SAP PD to hold the actuated joints near the nominal standing pose. After release, the robot consistently pitches forward, COM drifts forward, contact support degrades, fall detection triggers, and the node resets. So the current failure is: SAP joint PD is active, but the floating-base humanoid nominal pose is not a stable Drake standing equilibrium.

What the node does

flowchart TD
    launch[ROS2 launch drake_mrt_sim_node]
    cfg[Load MPC/URDF/Drake XML config]
    plant[Create discrete MultibodyPlant]
    contact[Set PointContactOnly + SAP approximation]
    model[Parse G1 Drake XML]
    ground[Add ground contact material]
    pd[Set JointActuator controller gains]
    init[Set nominal floating-base + joint state]
    pin[Startup pin holds pose]
    release[Release floating base]
    sap[SAP PD tracks desired actuated joint state]
    state[Convert Drake state to RobotState]
    mpc[MRT controller computes RobotJointAction]
    torque[Apply MuJoCo-style joint torque if enabled]
    reset[Reset on fall]

    launch --> cfg --> plant --> contact --> model --> ground --> pd --> init --> pin --> release
    release --> sap --> state --> mpc --> torque --> state
    state --> reset --> pin

Loading

The Drake plant is configured approximately like this:

• MultibodyPlant with time_step = 0.0005
• ContactModel::kPointContactOnly
• DiscreteContactApproximation::kSap
• Ground registered manually as a box with friction/contact material
• JointActuator::set_controller_gains({kp, kd}) is called before Finalize()
• Desired state is fixed through plant.get_desired_state_input_port(model_instance)

Observed behavior

After the startup pin is released, the robot repeatedly pitches forward and resets.

Representative diagnostics from the node:

t=2.250 phase=SAP_PD_STAND
base pos=[0.005 0.000 0.790] rpy=[0.000 0.014 0.000]
com  pos=[0.028 0.000 0.718]
contacts real=[1 1] fz=[172.8 172.5] point_contacts=8
torque final_inf=0.0 raw_controller_inf=0.0

t=3.001 phase=SAP_PD_STAND
base pos=[0.093 0.000 0.785] rpy=[0.000 0.151 0.000]
com  pos=[0.110 0.000 0.710]

t=3.751 phase=SAP_PD_STAND
base pos=[0.371 0.000 0.743] rpy=[0.000 0.509 0.000]
com  pos=[0.360 0.000 0.668]
contacts real=[1 1] fz=[142.6 142.3] point_contacts=4

[drake_reset] reason=fall_detected sim_t=3.987

My understanding is that Drake's SAP PD actuator controller only stabilizes actuated joints, not the floating base directly. Therefore if the nominal joint pose plus foot contacts is not a true static equilibrium, the robot can still tip over. That appears to be what is happening.

Questions

1. Is this the expected behavior for Drake SAP PD on a floating-base humanoid, i.e. joint PD can hold joint angles but will not by itself stabilize COM/base balance?

2. For a humanoid with small foot collision geometries, is ContactModel::kPointContactOnly + DiscreteContactApproximation::kSap a reasonable setup, or should I use a different contact model / foot collision representation?

3. Is fixing desired_state_input_port(model_instance) the correct way to drive Drake's built-in actuator PD for this use case?

4. Are there recommended Drake tools or APIs to check whether a floating-base robot configuration is a valid static equilibrium under contact before running time simulation?

5. For flat-foot humanoid standing, should I prefer explicit box/convex sole collision geometries over small point/sphere contacts?

Expected outcome

Before enabling MPC walking torques, I would like the Drake model to stand for ~30 seconds in double support using only SAP PD joint posture control, or at least understand why that expectation is invalid and what Drake-supported approach is recommended.

Additional context

The node intentionally mirrors an existing MuJoCo interface:

• state is exported as a common RobotState
• base linear/angular velocities are converted into the local/root frame
• controller contact flags can be forced true to match the old MuJoCo runtime
• direct torque mode applies:
  tau = kp * (q_des - q) + kd * (qd_des - qd) + feed_forward_effort
• passive joint damping equivalent to MuJoCo dof_damping = 10 is added manually
• torque limits are manually clamped because some MJCF actuator force limit fields are not used as expected in this path

Any guidance on the correct Drake modeling/control pattern for this standing test would be appreciated.