Browse Source

Fix IMU correction: right-multiply for body-frame pitch

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 <noreply@anthropic.com>
main
Joe DiPrima 1 month ago
parent
commit
8d9685ed3c
  1. 7
      gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp

7
gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp

@ -2654,12 +2654,13 @@ class G1Deploy {
} }
std::array<double, 4> base_quat_raw = float_to_double<4>(ls->imu_state().quaternion()); // qw, qx, qy, qz std::array<double, 4> 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 pitch_rad = g_imu_pitch_offset_deg.load() * M_PI / 180.0;
double half = pitch_rad / 2.0; double half = pitch_rad / 2.0;
std::array<double, 4> pitch_quat = {std::cos(half), 0.0, std::sin(half), 0.0}; std::array<double, 4> pitch_quat = {std::cos(half), 0.0, std::sin(half), 0.0};
std::array<double, 4> base_quat = quat_mul_d(pitch_quat, base_quat_raw);
std::array<double, 4> base_quat = quat_mul_d(base_quat_raw, pitch_quat);
std::array<double, 3> base_ang_vel = float_to_double<3>(ls->imu_state().gyroscope()); std::array<double, 3> base_ang_vel = float_to_double<3>(ls->imu_state().gyroscope());
std::array<double, 3> base_accel = float_to_double<3>(ls->imu_state().accelerometer()); std::array<double, 3> base_accel = float_to_double<3>(ls->imu_state().accelerometer());

Loading…
Cancel
Save