Stiffen waist roll/pitch to simulate missing physical clamp

Robot lacks the waist clamp expected by the locked-waist policy
(mode_machine=6). Default Kp=28.5 is intentionally low to protect
gears against a physical lock, but without the clamp the waist flops
under dynamic loads (walking/running), causing backward tilt.

Bump waist_roll and waist_pitch Kp to 100, Kd to 5 so the motors
hold the waist rigid. Remove the 8-degree offset — the waist default
position is correct, it just couldn't hold it. Keep offset keys 7/8
available for tuning if needed. Remove temporary debug logging.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp

@@ -50,8 +50,9 @@
 inline std::atomic<double> g_imu_pitch_offset_deg{0.0};
 
 /// Runtime-adjustable waist pitch offset (degrees). Keys 7/8 to adjust via tmux.
-/// Positive = forward lean. Default 8° to compensate for locked-waist training.
-inline std::atomic<double> g_waist_pitch_offset_deg{8.0};
+/// Default 0° — waist should stay neutral. Higher Kp/Kd simulate the missing
+/// physical waist clamp so the motors hold rigid during dynamic motion.
+inline std::atomic<double> g_waist_pitch_offset_deg{0.0};
 
 #if HAS_ROS2
 #include "ros2_input_handler.hpp"

gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp

@@ -2836,26 +2836,20 @@ class G1Deploy {
         motor_command_tmp.kd.at(i) = kds[i];
         motor_command_tmp.dq_target.at(i) = 0.0;
       }
-      // Waist pitch offset: compensate for policy trained with locked waist.
-      // MuJoCo index 14 = waist_pitch_joint. Positive = forward lean.
-      double waist_offset_rad = g_waist_pitch_offset_deg.load() * M_PI / 180.0;
-      motor_command_tmp.q_target.at(14) += static_cast<float>(waist_offset_rad);
-      // Stiffen waist pitch to actually track the offset (default Kp=28.5 too
-      // weak to overcome gravity on upper body). Match hip-level stiffness.
+      // Stiffen waist roll/pitch to simulate the missing physical waist clamp.
+      // Policy was trained for locked-waist variant (mode_machine=6) where
+      // roll/pitch are fixed joints. Default Kp=28.5 is intentionally low to
+      // avoid stripping gears against a clamp, but without the clamp the waist
+      // flops under dynamic loads. Higher gains keep it rigid.
+      // MuJoCo 13 = waist_roll, 14 = waist_pitch.
+      motor_command_tmp.kp.at(13) = 100.0f;
+      motor_command_tmp.kd.at(13) = 5.0f;
       motor_command_tmp.kp.at(14) = 100.0f;
       motor_command_tmp.kd.at(14) = 5.0f;
-
-      // Temporary: log waist_pitch motor command every ~2s (100 ticks at 50Hz)
-      static int waist_log_cnt = 0;
-      if (++waist_log_cnt >= 100) {
-        waist_log_cnt = 0;
-        const auto ls_dbg = low_state_buffer_.GetDataWithTime().data;
-        float hw_q = ls_dbg ? ls_dbg->motor_state()[14].q() : -999.0f;
-        std::cout << "[WAIST_DBG] cmd_q=" << motor_command_tmp.q_target.at(14)
-                  << " hw_q=" << hw_q
-                  << " kp=" << motor_command_tmp.kp.at(14)
-                  << " offset=" << g_waist_pitch_offset_deg.load() << "deg"
-                  << std::endl;
+      // Optional pitch offset (keys 7/8). Default 0°.
+      double waist_offset_rad = g_waist_pitch_offset_deg.load() * M_PI / 180.0;
+      if (waist_offset_rad != 0.0) {
+        motor_command_tmp.q_target.at(14) += static_cast<float>(waist_offset_rad);
       }
 
       motor_command_buffer_.SetData(motor_command_tmp);