From c616b3487528d3c14a24c3fbe784489baebfa934 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Sun, 22 Feb 2026 11:35:48 -0600 Subject: [PATCH] =?UTF-8?q?Add=20runtime=20IMU=20pitch=20offset=20tuning?= =?UTF-8?q?=20(keys=209/0,=20=C2=B11=C2=B0=20steps)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- .../input_interface/gamepad_manager.hpp | 19 +++++++++++++++++++ .../src/g1_deploy_onnx_ref.cpp | 11 +++++------ 2 files changed, 24 insertions(+), 6 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 0cdd965..38ecfde 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,11 +40,16 @@ #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{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_) { 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 aa81783..ac8da80 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,12 +2654,11 @@ 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 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 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 base_quat = quat_mul_d(base_quat_raw, imu_pitch_correction); 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());