From b8d1f05f56caa7dd3e0e13c82a9e27873d00544c Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Sun, 22 Feb 2026 11:40:33 -0600 Subject: [PATCH] =?UTF-8?q?Fix=20IMU=20correction:=20rotate=20about=20X=20?= =?UTF-8?q?axis=20(not=20Y),=20default=20-6=C2=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Match the working Phase 5 decoupled_wbc fix exactly: - Pitch quaternion: [cos(half), sin(half), 0, 0] (X-axis rotation) - Left-multiply: pitch_quat * base_quat_raw - Default offset: -6° (calibrated value from Phase 5) Co-Authored-By: Claude Opus 4.6 --- .../include/input_interface/gamepad_manager.hpp | 2 +- .../g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp | 10 ++++++---- 2 files changed, 7 insertions(+), 5 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 38ecfde..4e873ce 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 @@ -48,7 +48,7 @@ /// 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{0.0}; +inline std::atomic g_imu_pitch_offset_deg{-6.0}; #if HAS_ROS2 #include "ros2_input_handler.hpp" 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 ac8da80..4123dc6 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 @@ -2654,11 +2654,13 @@ class G1Deploy { } std::array base_quat_raw = float_to_double<4>(ls->imu_state().quaternion()); // qw, qx, qy, qz - // IMU pitch correction: G1's IMU may have a mounting offset. - // Runtime-adjustable via keys 9 (decrease) / 0 (increase) in 1° steps. + // 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; - auto imu_pitch_correction = quat_from_angle_axis_d(pitch_offset_rad, {0.0, 1.0, 0.0}); - std::array base_quat = quat_mul_d(base_quat_raw, imu_pitch_correction); + double half = pitch_offset_rad / 2.0; + std::array pitch_quat = {std::cos(half), std::sin(half), 0.0, 0.0}; // w,x,y,z + std::array base_quat = quat_mul_d(pitch_quat, base_quat_raw); 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());