From 73da28826f5376611715247c971b46d251352da6 Mon Sep 17 00:00:00 2001
From: Joe DiPrima <epilectrik@gmail.com>
Date: Sun, 22 Feb 2026 12:12:52 -0600
Subject: [PATCH] =?UTF-8?q?Revert=20all=20IMU=20pitch=20offset=20code=20?=
 =?UTF-8?q?=E2=80=94=20back=20to=20stock=20quaternion?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Remove quaternion correction, global offset variable, and 9/0 key
handlers. Return to the last known working state (proportional stick
control without IMU modification).

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
---
 .../input_interface/gamepad_manager.hpp       | 19 -------------------
 .../src/g1_deploy_onnx_ref.cpp                |  9 +--------
 2 files changed, 1 insertion(+), 27 deletions(-)

diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
index e7480f8..0cdd965 100644
--- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
+++ b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
@@ -40,16 +40,11 @@
 #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{-8.0};
-
 #if HAS_ROS2
 #include "ros2_input_handler.hpp"
 #endif
@@ -125,20 +120,6 @@ 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_) {
diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp
index 20b14fe..ecd2eef 100644
--- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp
+++ b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp
@@ -2653,14 +2653,7 @@ 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 pelvis IMU has a ~6° mounting offset vs simulation.
-      // Matches the fix from decoupled_wbc (Phase 5): rotate about X axis, left-multiply.
-      // Runtime-adjustable via keys 9/0 (±1° steps), default -6°.
-      double pitch_offset_rad = g_imu_pitch_offset_deg.load() * M_PI / 180.0;
-      double half = pitch_offset_rad / 2.0;
-      std::array<double, 4> pitch_quat = {std::cos(half), 0.0, std::sin(half), 0.0};  // w,x,y,z — Y-axis rotation
-      std::array<double, 4> base_quat = quat_mul_d(pitch_quat, base_quat_raw);
+      std::array<double, 4> base_quat = float_to_double<4>(ls->imu_state().quaternion()); // qw, qx, qy, qz
       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());