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());