Add runtime IMU pitch offset tuning (keys 9/0, ±1° steps)

Replace fixed 6° correction with live-adjustable offset starting at 0°.
Fix quaternion multiplication order (body-frame). Press 9 to decrease
pitch offset, 0 to increase, printed to console.

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

@@ -40,11 +40,16 @@
 #include <thread>
 #include <chrono>
 
+#include <atomic>
 #include "input_interface.hpp"
 #include "zmq_endpoint_interface.hpp"
 #include "gamepad.hpp"
 #include "../localmotion_kplanner.hpp"  // For LocomotionMode enum
 
+/// Runtime-adjustable IMU pitch offset (degrees). Keys 9/0 to adjust.
+/// Accessed from G1Deploy::UpdateRobotState() to correct base_quat.
+inline std::atomic<double> g_imu_pitch_offset_deg{0.0};
+
 #if HAS_ROS2
 #include "ros2_input_handler.hpp"
 #endif
@@ -120,6 +125,20 @@ class GamepadManager : public InputInterface {
             is_manager_key = true;
             std::cout << "[GamepadManager] EMERGENCY STOP triggered (O/o key pressed)" << std::endl;
             break;
+          case '9': {
+            double prev = g_imu_pitch_offset_deg.load();
+            g_imu_pitch_offset_deg.store(prev - 1.0);
+            std::cout << "[IMU] Pitch offset: " << (prev - 1.0) << " deg" << std::endl;
+            is_manager_key = true;
+            break;
+          }
+          case '0': {
+            double prev = g_imu_pitch_offset_deg.load();
+            g_imu_pitch_offset_deg.store(prev + 1.0);
+            std::cout << "[IMU] Pitch offset: " << (prev + 1.0) << " 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

@@ -2654,12 +2654,11 @@ class G1Deploy {
       }
 
       std::array<double, 4> base_quat_raw = float_to_double<4>(ls->imu_state().quaternion()); // qw, qx, qy, qz
-      // IMU pitch correction: G1's IMU is mounted with ~6° forward pitch offset.
-      // The NN was trained with a perfect IMU, so we pre-rotate by -6° about Y axis
-      // to correct the bias before it enters observations (gravity dir, heading, etc.).
-      constexpr double IMU_PITCH_OFFSET_RAD = 6.0 * M_PI / 180.0;  // positive = correct backward lean
-      static const auto imu_pitch_correction = quat_from_angle_axis_d(IMU_PITCH_OFFSET_RAD, {0.0, 1.0, 0.0});
-      std::array<double, 4> base_quat = quat_mul_d(imu_pitch_correction, base_quat_raw);
+      // IMU pitch correction: G1's IMU may have a mounting offset.
+      // Runtime-adjustable via keys 9 (decrease) / 0 (increase) in 1° steps.
+      double pitch_offset_rad = g_imu_pitch_offset_deg.load() * M_PI / 180.0;
+      auto imu_pitch_correction = quat_from_angle_axis_d(pitch_offset_rad, {0.0, 1.0, 0.0});
+      std::array<double, 4> base_quat = quat_mul_d(base_quat_raw, imu_pitch_correction);
       std::array<double, 3> base_ang_vel = float_to_double<3>(ls->imu_state().gyroscope());
       std::array<double, 3> base_accel = float_to_double<3>(ls->imu_state().accelerometer());