Add waist pitch offset to compensate for locked-waist training

The SONIC policy was likely trained on mode_machine=6 (locked waist variant)
where waist_roll and waist_pitch are fixed joints. On our mode_machine=5
robot the waist is free but the NN outputs near-zero for it. This adds a
configurable waist_pitch offset (default +8°) applied directly to the motor
command. Keys 7/8 adjust ±1° at runtime via tmux.

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

@@ -49,6 +49,10 @@
 /// Runtime-adjustable IMU pitch offset (degrees). Keys 9/0 to adjust via tmux.
 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};
+
 #if HAS_ROS2
 #include "ros2_input_handler.hpp"
 #endif
@@ -138,6 +142,20 @@ class GamepadManager : public InputInterface {
             is_manager_key = true;
             break;
           }
+          case '7': {
+            double val = g_waist_pitch_offset_deg.load() - 1.0;
+            g_waist_pitch_offset_deg.store(val);
+            std::cout << "[WAIST] Pitch offset: " << val << " deg" << std::endl;
+            is_manager_key = true;
+            break;
+          }
+          case '8': {
+            double val = g_waist_pitch_offset_deg.load() + 1.0;
+            g_waist_pitch_offset_deg.store(val);
+            std::cout << "[WAIST] Pitch offset: " << val << " deg" << std::endl;
+            is_manager_key = true;
+            break;
+          }
         }
 
         if (!is_manager_key && current_) {

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

@@ -2836,6 +2836,11 @@ 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);
+
       motor_command_buffer_.SetData(motor_command_tmp);
       return true;
     }