Browse Source

Fix IMU pitch correction sign: +6° to correct backward lean

Previous sign made the lean worse. Flipped to positive rotation.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
main
Joe DiPrima 1 month ago
parent
commit
05264193fe
  1. 2
      gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp

2
gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp

@ -2657,7 +2657,7 @@ class G1Deploy {
// IMU pitch correction: G1's IMU is mounted with ~6° forward pitch offset. // 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 // 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.). // to correct the bias before it enters observations (gravity dir, heading, etc.).
constexpr double IMU_PITCH_OFFSET_RAD = -6.0 * M_PI / 180.0; // negative = correct forward lean
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}); static const auto imu_pitch_correction = quat_from_angle_axis_d(IMU_PITCH_OFFSET_RAD, {0.0, 1.0, 0.0});
std::array<double, 4> base_quat = quat_mul_d(imu_pitch_correction, base_quat_raw); std::array<double, 4> base_quat = quat_mul_d(imu_pitch_correction, base_quat_raw);
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());

Loading…
Cancel
Save