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