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 #include -#include #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 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 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 pitch_quat = {std::cos(half), 0.0, std::sin(half), 0.0}; // w,x,y,z — Y-axis rotation - std::array base_quat = quat_mul_d(pitch_quat, base_quat_raw); + std::array base_quat = float_to_double<4>(ls->imu_state().quaternion()); // qw, qx, qy, qz std::array base_ang_vel = float_to_double<3>(ls->imu_state().gyroscope()); std::array base_accel = float_to_double<3>(ls->imu_state().accelerometer());