@ -2654,12 +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: Y-axis rotation, left-multiply. Default -6°.
// Adjustable at runtime via keys 9/0 in tmux.
// IMU pitch correction: Y-axis rotation, right-multiply (body frame).
// Right-multiply ensures pure pitch regardless of robot heading.
// Adjustable at runtime via keys 9/0 in tmux. Default -3°.
double pitch_rad = g_imu_pitch_offset_deg . load ( ) * M_PI / 180.0 ;
double pitch_rad = g_imu_pitch_offset_deg . load ( ) * M_PI / 180.0 ;
double half = pitch_rad / 2.0 ;
double half = pitch_rad / 2.0 ;
std : : array < double , 4 > pitch_quat = { std : : cos ( half ) , 0.0 , std : : sin ( half ) , 0.0 } ;
std : : array < double , 4 > pitch_quat = { std : : cos ( half ) , 0.0 , std : : sin ( half ) , 0.0 } ;
std : : array < double , 4 > base_quat = quat_mul_d ( pitch_quat , base_quat_raw ) ;
std : : array < double , 4 > base_quat = quat_mul_d ( base_quat_raw , pitch_quat ) ;
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 ( ) ) ;