Browse Source

Fix IMU correction: rotate about X axis (not Y), default -6°

Match the working Phase 5 decoupled_wbc fix exactly:
- Pitch quaternion: [cos(half), sin(half), 0, 0] (X-axis rotation)
- Left-multiply: pitch_quat * base_quat_raw
- Default offset: -6° (calibrated value from Phase 5)

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
main
Joe DiPrima 1 month ago
parent
commit
b8d1f05f56
  1. 2
      gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
  2. 10
      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/include/input_interface/gamepad_manager.hpp

@ -48,7 +48,7 @@
/// Runtime-adjustable IMU pitch offset (degrees). Keys 9/0 to adjust.
/// Accessed from G1Deploy::UpdateRobotState() to correct base_quat.
inline std::atomic<double> g_imu_pitch_offset_deg{0.0};
inline std::atomic<double> g_imu_pitch_offset_deg{-6.0};
#if HAS_ROS2
#include "ros2_input_handler.hpp"

10
gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp

@ -2654,11 +2654,13 @@ class G1Deploy {
}
std::array<double, 4> base_quat_raw = float_to_double<4>(ls->imu_state().quaternion()); // qw, qx, qy, qz
// IMU pitch correction: G1's IMU may have a mounting offset.
// Runtime-adjustable via keys 9 (decrease) / 0 (increase) in 1° steps.
// IMU pitch correction: G1's pelvis IMU has a ~6° mounting offset vs simulation.
// Matches the fix from decoupled_wbc (Phase 5): rotate about X axis, left-multiply.
// Runtime-adjustable via keys 9/0 (±1° steps), default -6°.
double pitch_offset_rad = g_imu_pitch_offset_deg.load() * M_PI / 180.0;
auto imu_pitch_correction = quat_from_angle_axis_d(pitch_offset_rad, {0.0, 1.0, 0.0});
std::array<double, 4> base_quat = quat_mul_d(base_quat_raw, imu_pitch_correction);
double half = pitch_offset_rad / 2.0;
std::array<double, 4> pitch_quat = {std::cos(half), std::sin(half), 0.0, 0.0}; // w,x,y,z
std::array<double, 4> base_quat = quat_mul_d(pitch_quat, base_quat_raw);
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());

Loading…
Cancel
Save