From 8d9685ed3c759433235bf9bd699d78611f8d5beb Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Sun, 22 Feb 2026 13:06:39 -0600 Subject: [PATCH] Fix IMU correction: right-multiply for body-frame pitch MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Left-multiply applied correction in world frame, cross-coupling pitch into roll depending on robot heading. Right-multiply applies in body frame — pure pitch regardless of yaw. Co-Authored-By: Claude Opus 4.6 --- .../src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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 8808748..f20aeb7 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,13 @@ class G1Deploy { } std::array base_quat_raw = float_to_double<4>(ls->imu_state().quaternion()); // qw, qx, qy, qz - // IMU pitch correction: Y-axis rotation, left-multiply. Default -6°. - // Adjustable at runtime via keys 9/0 in tmux. + // IMU pitch correction: Y-axis rotation, right-multiply (body frame). + // Right-multiply ensures pure pitch regardless of robot heading. + // Adjustable at runtime via keys 9/0 in tmux. Default -3°. double pitch_rad = g_imu_pitch_offset_deg.load() * M_PI / 180.0; double half = pitch_rad / 2.0; std::array pitch_quat = {std::cos(half), 0.0, std::sin(half), 0.0}; - std::array base_quat = quat_mul_d(pitch_quat, base_quat_raw); + std::array base_quat = quat_mul_d(base_quat_raw, pitch_quat); 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());