From cc596c21252e092b2f7a15b17547ae669b0fa4c0 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Sun, 22 Feb 2026 11:17:29 -0600 Subject: [PATCH] Apply 6-degree IMU pitch offset correction for G1 hardware MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit G1's physical IMU is mounted with ~6° forward pitch bias. SONIC's NN was trained with a perfect virtual IMU, causing backward lean compensation. Pre-rotate base_quat by -6° about Y axis before it enters observations. Co-Authored-By: Claude Opus 4.6 --- .../src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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 ecd2eef..cd4ecc3 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 @@ -2653,7 +2653,13 @@ class G1Deploy { } } - std::array base_quat = float_to_double<4>(ls->imu_state().quaternion()); // qw, qx, qy, qz + 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; // negative = correct forward 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); 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());