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