diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 09d7a4ae..db2c2db4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -21,6 +21,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -38,13 +39,19 @@ value_broadcaster: - /shoot/heat - /shoot/referee_heat - /shoot/heat_limit - + - /chassis/left_front_joint/physical_angle + - /chassis/left_front_joint/physical_velocity + - /chassis/left_back_joint/physical_angle + - /chassis/left_back_joint/physical_velocity + - /chassis/right_back_joint/physical_angle + - /chassis/right_back_joint/physical_velocity + - /chassis/right_front_joint/physical_angle + - /chassis/right_front_joint/physical_velocity deformable_infantry: ros__parameters: serial_filter_rmcs_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" - serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" - serial_filter_imu: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" + serial_filter_top_board: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" left_front_zero_point: 7173 left_back_zero_point: 5167 right_back_zero_point: 3098 @@ -58,28 +65,52 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 8.0 - max_angle: 58.0 - launch_ramp_shortcut_enabled: false + min_angle: 5.0 + max_angle: 59.0 active_suspension_enable: true spin_ratio: 1.0 +deformable_suspension: + ros__parameters: # IMU attitude correction at min-angle stance. - active_suspension_pitch_kp: 8.0 - active_suspension_pitch_ki: 0.35 - active_suspension_pitch_kd: 0.28 - - active_suspension_roll_kp: 8.0 - active_suspension_roll_ki: 0.35 - active_suspension_roll_kd: 0.28 - - active_suspension_pitch_angle_diff_limit_deg: 45.0 - active_suspension_roll_angle_diff_limit_deg: 45.0 - active_suspension_pid_integral_limit_deg: 20.0 + active_suspension_pitch_outer_kp: 12.0 + active_suspension_pitch_outer_ki: 0.02 + active_suspension_pitch_outer_kd: 0.0 + active_suspension_pitch_outer_integral_min: -2.0 + active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_output_min: -3.0 + active_suspension_pitch_outer_output_max: 3.0 + + active_suspension_pitch_inner_kp: 0.45 + active_suspension_pitch_inner_ki: 0.0 + active_suspension_pitch_inner_kd: 0.0 + active_suspension_pitch_inner_integral_min: -1.0 + active_suspension_pitch_inner_integral_max: 1.0 + active_suspension_pitch_inner_output_min: -0.785 + active_suspension_pitch_inner_output_max: 0.785 + + active_suspension_roll_outer_kp: 12.0 + active_suspension_roll_outer_ki: 0.02 + active_suspension_roll_outer_kd: 0.0 + active_suspension_roll_outer_integral_min: -2.0 + active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_output_min: -3.0 + active_suspension_roll_outer_output_max: 3.0 + + active_suspension_roll_inner_kp: 0.45 + active_suspension_roll_inner_ki: 0.0 + active_suspension_roll_inner_kd: 0.0 + active_suspension_roll_inner_integral_min: -1.0 + active_suspension_roll_inner_integral_max: 1.0 + active_suspension_roll_inner_output_min: -0.785 + active_suspension_roll_inner_output_max: 0.785 # Chassis-owned joint intent trajectory limits while attitude correction is active. active_suspension_target_velocity_limit_deg: 80.0 active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_correction_velocity_limit_deg: 720.0 + active_suspension_correction_acceleration_limit_deg: 3600.0 + active_suspension_rate_lpf_cutoff_hz: 10.0 # Automatic IMU mounting-error calibration. # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. @@ -88,10 +119,9 @@ chassis_controller: gimbal_controller: ros__parameters: - upper_limit: -0.65 # -35 deg - lower_limit: 0.05 # 6 deg + upper_limit: -0.47123 # -27 deg + lower_limit: 0.15707 # 9 deg ctrl_hold_pitch_target_angle: 0.0 - ctrl_hold_chassis_yaw_velocity_max: 3.0 yaw_angle_kp: 30.0 yaw_angle_ki: 0.0 @@ -104,11 +134,11 @@ gimbal_controller: yaw_vel_ff_gain: 0.47 yaw_acc_ff_gain: 0.00 - pitch_angle_kp: 25.0 + pitch_angle_kp: 7.2 pitch_angle_ki: 0.0 pitch_angle_kd: 0.0 - pitch_velocity_kp: 2.2 + pitch_velocity_kp: 3.0 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 @@ -118,9 +148,6 @@ gimbal_controller: pitch_torque_control: true - pitch_fusion_enabled: true - pitch_fusion_alpha: 0.98 - friction_wheel_controller: ros__parameters: friction_wheels: @@ -129,8 +156,6 @@ friction_wheel_controller: friction_velocities: - 580.0 - 580.0 - friction_velocity_min: 550.0 - friction_velocity_max: 600.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -178,11 +203,11 @@ bullet_feeder_velocity_pid_controller: deformable_chassis_controller: ros__parameters: - mass: 23.0 + mass: 25.5 moment_of_inertia: 1.0 chassis_radius: 0.2341741 - rod_length: 0.150 - wheel_radius: 0.07 + rod_length: 0.140 + wheel_radius: 0.075 friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 @@ -190,11 +215,10 @@ deformable_chassis_controller: lf_joint_controller: ros__parameters: - # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle setpoint_angle: /chassis/left_front_joint/target_physical_angle + setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque - dt: 0.001 b0: -1.0 kt: 1.0 @@ -211,11 +235,12 @@ lf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + lb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle setpoint_angle: /chassis/left_back_joint/target_physical_angle + setpoint_velocity: /chassis/left_back_joint/target_physical_velocity control: /chassis/left_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -233,11 +258,12 @@ lb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + rb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle setpoint_angle: /chassis/right_back_joint/target_physical_angle + setpoint_velocity: /chassis/right_back_joint/target_physical_velocity control: /chassis/right_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -255,11 +281,12 @@ rb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + rf_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle setpoint_angle: /chassis/right_front_joint/target_physical_angle + setpoint_velocity: /chassis/right_front_joint/target_physical_velocity control: /chassis/right_front_joint/control_torque dt: 0.001 b0: -1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 742bf86f..1f565d9e 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -21,6 +21,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -37,18 +38,25 @@ value_broadcaster: forward_list: - /gimbal/yaw/angle - /gimbal/yaw/velocity + - /chassis/left_front_joint/physical_angle + - /chassis/left_front_joint/physical_velocity + - /chassis/left_back_joint/physical_angle + - /chassis/left_back_joint/physical_velocity + - /chassis/right_back_joint/physical_angle + - /chassis/right_back_joint/physical_velocity + - /chassis/right_front_joint/physical_angle + - /chassis/right_front_joint/physical_velocity deformable_infantry: ros__parameters: serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" - serial_filter_top_board: "AF-8AE3-4EC1-03C3-C494-88FE-2DC4-3018-0298" - serial_filter_imu: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" + serial_filter_top_board: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" left_front_zero_point: 374 left_back_zero_point: 5801 right_back_zero_point: 7817 right_front_zero_point: 7136 yaw_motor_zero_point: 43365 - pitch_motor_zero_point: 6853 + pitch_motor_zero_point: 6432 debug_log_supercap: false debug_log_wheel_motor: false debug_log_deformable_joint_motor: false @@ -56,28 +64,52 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 8.0 - max_angle: 58.0 - launch_ramp_shortcut_enabled: false + min_angle: 5.0 + max_angle: 59.0 active_suspension_enable: true spin_ratio: 1.0 +deformable_suspension: + ros__parameters: # IMU attitude correction at min-angle stance. - active_suspension_pitch_kp: 8.0 - active_suspension_pitch_ki: 0.35 - active_suspension_pitch_kd: 0.28 - - active_suspension_roll_kp: 8.0 - active_suspension_roll_ki: 0.35 - active_suspension_roll_kd: 0.28 - - active_suspension_pitch_angle_diff_limit_deg: 45.0 - active_suspension_roll_angle_diff_limit_deg: 45.0 - active_suspension_pid_integral_limit_deg: 20.0 + active_suspension_pitch_outer_kp: 12.0 + active_suspension_pitch_outer_ki: 0.02 + active_suspension_pitch_outer_kd: 0.0 + active_suspension_pitch_outer_integral_min: -2.0 + active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_output_min: -3.0 + active_suspension_pitch_outer_output_max: 3.0 + + active_suspension_pitch_inner_kp: 0.45 + active_suspension_pitch_inner_ki: 0.0 + active_suspension_pitch_inner_kd: 0.0 + active_suspension_pitch_inner_integral_min: -1.0 + active_suspension_pitch_inner_integral_max: 1.0 + active_suspension_pitch_inner_output_min: -0.785 + active_suspension_pitch_inner_output_max: 0.785 + + active_suspension_roll_outer_kp: 12.0 + active_suspension_roll_outer_ki: 0.02 + active_suspension_roll_outer_kd: 0.0 + active_suspension_roll_outer_integral_min: -2.0 + active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_output_min: -3.0 + active_suspension_roll_outer_output_max: 3.0 + + active_suspension_roll_inner_kp: 0.45 + active_suspension_roll_inner_ki: 0.0 + active_suspension_roll_inner_kd: 0.0 + active_suspension_roll_inner_integral_min: -1.0 + active_suspension_roll_inner_integral_max: 1.0 + active_suspension_roll_inner_output_min: -0.785 + active_suspension_roll_inner_output_max: 0.785 # Chassis-owned joint intent trajectory limits while attitude correction is active. active_suspension_target_velocity_limit_deg: 80.0 active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_correction_velocity_limit_deg: 720.0 + active_suspension_correction_acceleration_limit_deg: 3600.0 + active_suspension_rate_lpf_cutoff_hz: 10.0 # Automatic IMU mounting-error calibration. # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. @@ -86,10 +118,9 @@ chassis_controller: gimbal_controller: ros__parameters: - upper_limit: -0.65 # -35 deg - lower_limit: 0.05 # 6 deg + upper_limit: -0.47123 # -27 deg + lower_limit: 0.15707 # 9 deg ctrl_hold_pitch_target_angle: 0.0 - ctrl_hold_chassis_yaw_velocity_max: 3.0 yaw_angle_kp: 30.0 yaw_angle_ki: 0.0 @@ -102,11 +133,11 @@ gimbal_controller: yaw_vel_ff_gain: 0.47 yaw_acc_ff_gain: 0.00 - pitch_angle_kp: 25.0 + pitch_angle_kp: 7.2 pitch_angle_ki: 0.0 pitch_angle_kd: 0.0 - pitch_velocity_kp: 2.2 + pitch_velocity_kp: 3.0 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 @@ -116,9 +147,6 @@ gimbal_controller: pitch_torque_control: true - pitch_fusion_enabled: true - pitch_fusion_alpha: 0.98 - friction_wheel_controller: ros__parameters: friction_wheels: @@ -127,14 +155,12 @@ friction_wheel_controller: friction_velocities: - 580.0 - 580.0 - friction_velocity_min: 550.0 - friction_velocity_max: 600.0 friction_soft_start_stop_time: 1.0 heat_controller: ros__parameters: heat_per_shot: 10 - reserved_heat: 0 + reserved_heat: 15 bullet_feeder_controller: ros__parameters: @@ -176,11 +202,11 @@ bullet_feeder_velocity_pid_controller: deformable_chassis_controller: ros__parameters: - mass: 22.5 + mass: 25.5 moment_of_inertia: 1.0 chassis_radius: 0.2341741 - rod_length: 0.150 - wheel_radius: 0.07 + rod_length: 0.140 + wheel_radius: 0.075 friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 @@ -188,13 +214,12 @@ deformable_chassis_controller: lf_joint_controller: ros__parameters: - # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle setpoint_angle: /chassis/left_front_joint/target_physical_angle + setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque - dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -209,14 +234,15 @@ lf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + lb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle setpoint_angle: /chassis/left_back_joint/target_physical_angle + setpoint_velocity: /chassis/left_back_joint/target_physical_velocity control: /chassis/left_back_joint/control_torque dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -231,14 +257,15 @@ lb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + rb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle setpoint_angle: /chassis/right_back_joint/target_physical_angle + setpoint_velocity: /chassis/right_back_joint/target_physical_velocity control: /chassis/right_back_joint/control_torque dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -253,14 +280,15 @@ rb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + rf_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle setpoint_angle: /chassis/right_front_joint/target_physical_angle + setpoint_velocity: /chassis/right_front_joint/target_physical_velocity control: /chassis/right_front_joint/control_torque dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml deleted file mode 100644 index d8de651a..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ /dev/null @@ -1,267 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::DeformableInfantryV2 -> deformable_infantry - - - rmcs_core::referee::Status -> referee_status - - rmcs_core::referee::Command -> referee_command - - - rmcs_core::referee::command::Interaction -> referee_interaction - - rmcs_core::referee::command::interaction::Ui -> referee_ui - - rmcs_core::referee::app::ui::DeformableInfantry -> referee_ui_infantry - - - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller - - - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - - rmcs_core::controller::shooting::HeatController -> heat_controller - - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller - - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::DeformableChassisController -> deformable_chassis_controller - - - rmcs_core::controller::chassis::DeformableJointController -> lf_joint_controller - - rmcs_core::controller::chassis::DeformableJointController -> lb_joint_controller - - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - -value_broadcaster: - ros__parameters: - forward_list: - - /gimbal/yaw/angle - - /gimbal/yaw/velocity - - -deformable_infantry: - ros__parameters: - serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" - serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" - serial_filter_imu: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" - left_front_zero_point: 7173 - left_back_zero_point: 5167 - right_back_zero_point: 3098 - right_front_zero_point: 6485 - yaw_motor_zero_point: 39442 - pitch_motor_zero_point: 56556 - debug_log_supercap: false - debug_log_wheel_motor: false - debug_log_deformable_joint_motor: false - -chassis_controller: - ros__parameters: - # Deploy geometry / chassis-owned joint intent - min_angle: 20.0 - max_angle: 50.0 - active_suspension_enable: true - spin_ratio: 1.0 - - # IMU attitude correction at min-angle stance. - active_suspension_pitch_kp: 8.0 - active_suspension_pitch_ki: 0.35 - active_suspension_pitch_kd: 0.28 - - active_suspension_roll_kp: 8.0 - active_suspension_roll_ki: 0.35 - active_suspension_roll_kd: 0.28 - - active_suspension_pitch_angle_diff_limit_deg: 45.0 - active_suspension_roll_angle_diff_limit_deg: 45.0 - active_suspension_pid_integral_limit_deg: 20.0 - - # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 - - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. - chassis_imu_calibration_wait_s: 2.0 - chassis_imu_calibration_sample_s: 3.0 - -gimbal_controller: - ros__parameters: - inertia: 1.0 # kg·m² - friction: 1.65 # Nm/(rad/s) - - upper_limit: -0.65 # -35 deg - lower_limit: 0.05 # 6 deg - ctrl_hold_pitch_target_angle: 0.0 - ctrl_hold_chassis_yaw_velocity_max: 3.0 - - yaw_angle_kp: 10.0 - yaw_angle_ki: 0.0 - yaw_angle_kd: 0.0 - - yaw_velocity_kp: 8.0 - yaw_velocity_ki: 0.0 - yaw_velocity_kd: 0.0 - - pitch_angle_kp: 40.0 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 - - pitch_velocity_kp: 3.0 - pitch_velocity_ki: 0.0 - pitch_velocity_kd: 0.0 - - pitch_torque_control: true - -friction_wheel_controller: - ros__parameters: - friction_wheels: - - /gimbal/left_friction - - /gimbal/right_friction - friction_velocities: - - 580.0 - - 580.0 - friction_soft_start_stop_time: 1.0 - -heat_controller: - ros__parameters: - heat_per_shot: 10 - reserved_heat: 0 - -bullet_feeder_controller: - ros__parameters: - bullets_per_feeder_turn: 8.0 - shot_frequency: 30.0 - safe_shot_frequency: 10.0 - eject_frequency: 10.0 - eject_time: 0.05 - deep_eject_frequency: 5.0 - deep_eject_time: 0.2 - single_shot_max_stop_delay: 2.0 - -left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/left_friction/velocity - setpoint: /gimbal/left_friction/control_velocity - control: /gimbal/left_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/right_friction/velocity - setpoint: /gimbal/right_friction/control_velocity - control: /gimbal/right_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -bullet_feeder_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/bullet_feeder/velocity - setpoint: /gimbal/bullet_feeder/control_velocity - control: /gimbal/bullet_feeder/control_torque - kp: 1.4 - ki: 0.0 - kd: 0.0 - -deformable_chassis_controller: - ros__parameters: - mass: 23.0 - moment_of_inertia: 1.0 - chassis_radius: 0.2341741 - rod_length: 0.150 - wheel_radius: 0.07 - friction_coefficient: 0.6 - k1: 2.958580e+00 - k2: 3.082190e-03 - no_load_power: 11.37 - -lf_joint_controller: - ros__parameters: - # Joint-local servo inputs produced by chassis intent generation - measurement_angle: /chassis/left_front_joint/physical_angle - setpoint_angle: /chassis/left_front_joint/target_physical_angle - control: /chassis/left_front_joint/control_torque - - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 -lb_joint_controller: - ros__parameters: - # Same joint-servo layout as lf_joint_controller - measurement_angle: /chassis/left_back_joint/physical_angle - setpoint_angle: /chassis/left_back_joint/target_physical_angle - control: /chassis/left_back_joint/control_torque - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 -rb_joint_controller: - ros__parameters: - # Same joint-servo layout as lf_joint_controller - measurement_angle: /chassis/right_back_joint/physical_angle - setpoint_angle: /chassis/right_back_joint/target_physical_angle - control: /chassis/right_back_joint/control_torque - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 -rf_joint_controller: - ros__parameters: - # Same joint-servo layout as lf_joint_controller - measurement_angle: /chassis/right_front_joint/physical_angle - setpoint_angle: /chassis/right_front_joint/target_physical_angle - control: /chassis/right_front_joint/control_torque - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 08264072..8a2c5858 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -18,8 +18,8 @@ + - diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 8c4b38e1..13900781 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -93,9 +93,9 @@ class ChassisPowerController } void reset_power_control() { - virtual_buffer_energy_ = virtual_buffer_energy_limit_; - boost_mode_ = false; - *supercap_control_enabled_ = false; + virtual_buffer_energy_ = virtual_buffer_energy_limit_; + boost_mode_ = false; + *supercap_control_enabled_ = false; *chassis_control_power_limit_ = 0.0; } diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 9d17ebdf..9cea6ef5 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -1,13 +1,12 @@ #include #include #include -#include -#include #include #include -#include +#include #include +#include #include #include @@ -17,6 +16,7 @@ #include #include +#include "controller/chassis/deformable_mode.hpp" #include "controller/pid/pid_calculator.hpp" namespace rmcs_core::controller::chassis { @@ -25,105 +25,16 @@ class DeformableChassis : public rmcs_executor::Component , public rclcpp::Node { public: - enum JointIndex : size_t { - kLeftFront = 0, - kLeftBack = 1, - kRightBack = 2, - kRightFront = 3, - kJointCount = 4, - }; - - struct AttitudePidAxis { - double kp = 20.0; - double ki = 0.0; - double kd = 0.0; - double integral = 0.0; - double integral_limit = std::numeric_limits::infinity(); - double output_limit = std::numeric_limits::infinity(); - - void reset() { integral = 0.0; } - - double update(double error, double rate, double dt) { - if (!std::isfinite(error) || !std::isfinite(rate) || !std::isfinite(dt) || dt <= 0.0) { - reset(); - return std::numeric_limits::quiet_NaN(); - } - - integral = std::clamp(integral + error * dt, -integral_limit, integral_limit); - const double output = kp * error + ki * integral - kd * rate; - return std::clamp(output, -output_limit, output_limit); - } - }; - - DeformableChassis() + explicit DeformableChassis() : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(10.0, 0.0, 0.0) , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) - , launch_ramp_shortcut_enabled_(get_parameter_or("launch_ramp_shortcut_enabled", true)) - - , min_angle_(get_parameter_or("min_angle", 7.0)) - , max_angle_(get_parameter_or("max_angle", 58.0)) - , target_physical_velocity_limit_( - std::max( - deg_to_rad(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), - 1e-6)) - , target_physical_acceleration_limit_( - std::max( - deg_to_rad( - std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0))), - 1e-6)) - , active_suspension_enable_(get_parameter_or("active_suspension_enable", false)) - , pitch_kp_(get_parameter_or("active_suspension_pitch_kp", 200.0)) - , pitch_ki_(get_parameter_or("active_suspension_pitch_ki", 0.0)) - , pitch_kd_(get_parameter_or("active_suspension_pitch_kd", 20.0)) - , roll_kp_(get_parameter_or("active_suspension_roll_kp", 200.0)) - , roll_ki_(get_parameter_or("active_suspension_roll_ki", 0.0)) - , roll_kd_(get_parameter_or("active_suspension_roll_kd", 20.0)) - , suspension_velocity_limit_( - std::max( - deg_to_rad( - std::abs(get_parameter_or( - "active_suspension_target_velocity_limit_deg", - get_parameter_or("target_physical_velocity_limit", 180.0)))), - 1e-6)) - , suspension_acceleration_limit_( - std::max( - deg_to_rad( - std::abs(get_parameter_or( - "active_suspension_target_acceleration_limit_deg", - get_parameter_or("target_physical_acceleration_limit", 720.0)))), - 1e-6)) - , pitch_diff_limit_( - std::abs(get_parameter_or( - "active_suspension_pitch_angle_diff_limit_deg", max_angle_ - min_angle_)) - * std::numbers::pi / 180.0) - , roll_diff_limit_( - std::abs(get_parameter_or( - "active_suspension_roll_angle_diff_limit_deg", max_angle_ - min_angle_)) - * std::numbers::pi / 180.0) - , pid_integral_limit_( - std::abs(get_parameter_or( - "active_suspension_pid_integral_limit_deg", max_angle_ - min_angle_)) - * std::numbers::pi / 180.0) - , chassis_imu_calibration_wait_time_( - std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0)) - , chassis_imu_calibration_sample_time_( - std::max(get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6)) { + , joint_mode_mgr_(*this) { following_velocity_controller_.output_max = angular_velocity_max_; following_velocity_controller_.output_min = -angular_velocity_max_; - pitch_pid_.kp = pitch_kp_; - pitch_pid_.ki = pitch_ki_; - pitch_pid_.kd = pitch_kd_; - pitch_pid_.integral_limit = pid_integral_limit_; - pitch_pid_.output_limit = pitch_diff_limit_; - roll_pid_.kp = roll_kp_; - roll_pid_.ki = roll_ki_; - roll_pid_.kd = roll_kd_; - roll_pid_.integral_limit = pid_integral_limit_; - roll_pid_.output_limit = roll_diff_limit_; register_input("/remote/joystick/right", joystick_right_); register_input("/remote/switch/right", switch_right_); @@ -134,81 +45,35 @@ class DeformableChassis register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - register_input("/chassis/manual_yaw_velocity_override", manual_yaw_velocity_override_, false); - - register_input( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, false); - register_input( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, false); - register_input( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, false); - register_input( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, false); - register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); - register_input("/chassis/imu/roll", chassis_imu_roll_, false); - register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); - register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); - - register_output("/gimbal/scope/control_torque", scope_motor_control_torque, nan_); register_output("/chassis/angle", chassis_angle_, nan_); register_output("/chassis/control_angle", chassis_control_angle_, nan_); - register_output("/chassis/control_mode", mode_); register_output("/chassis/control_velocity", chassis_control_velocity_); - register_output("/chassis/ctrl_hold_active", ctrl_hold_active_, false); - - register_output("/chassis/left_front_joint/control_angle_error", lf_angle_error_, nan_); - register_output("/chassis/left_back_joint/control_angle_error", lb_angle_error_, nan_); - register_output("/chassis/right_front_joint/control_angle_error", rf_angle_error_, nan_); - register_output("/chassis/right_back_joint/control_angle_error", rb_angle_error_, nan_); - - register_output( - "/chassis/left_front_joint/target_physical_angle", - left_front_joint_target_physical_angle_, nan_); - register_output( - "/chassis/left_back_joint/target_physical_angle", - left_back_joint_target_physical_angle_, nan_); - register_output( - "/chassis/right_back_joint/target_physical_angle", - right_back_joint_target_physical_angle_, nan_); - register_output( - "/chassis/right_front_joint/target_physical_angle", - right_front_joint_target_physical_angle_, nan_); - register_output( - "/chassis/left_front_joint/target_physical_velocity", - left_front_joint_target_physical_velocity_, nan_); - register_output( - "/chassis/left_back_joint/target_physical_velocity", - left_back_joint_target_physical_velocity_, nan_); - register_output( - "/chassis/right_back_joint/target_physical_velocity", - right_back_joint_target_physical_velocity_, nan_); - register_output( - "/chassis/right_front_joint/target_physical_velocity", - right_front_joint_target_physical_velocity_, nan_); - register_output( - "/chassis/left_front_joint/target_physical_acceleration", - left_front_joint_target_physical_acceleration_, nan_); - register_output( - "/chassis/left_back_joint/target_physical_acceleration", - left_back_joint_target_physical_acceleration_, nan_); - register_output( - "/chassis/right_back_joint/target_physical_acceleration", - right_back_joint_target_physical_acceleration_, nan_); + register_output("/chassis/pitch_lock_active", pitch_lock_active_, false); + register_output("/chassis/active_suspension/active", active_suspension_active_, false); + register_output("/chassis/deformable/low_prone_active", low_prone_active_, false); + register_output("/chassis/deformable/symmetric_posture_target", symmetric_posture_target_, true); + register_output("/chassis/deformable/correction_inverted", correction_inverted_, false); + register_output("/chassis/deformable/min_angle_deg", min_angle_deg_, joint_mode_mgr_.min_angle()); + register_output("/chassis/deformable/max_angle_deg", max_angle_deg_, joint_mode_mgr_.max_angle()); register_output( - "/chassis/right_front_joint/target_physical_acceleration", - right_front_joint_target_physical_acceleration_, nan_); + "/chassis/deformable/suspension_reference_angle_deg", + suspension_reference_angle_deg_, joint_mode_mgr_.suspension_reference_angle_deg()); + register_output("/chassis/deformable/reset_count", deformable_reset_count_, static_cast(0)); + for (size_t i = 0; i < kJointCount; ++i) { + register_output( + fmt::format("/chassis/deformable/{}_joint/posture_target_angle", kJointName[i]), + joint_posture_target_angle_rad_[i], deg_to_rad(joint_mode_mgr_.max_angle())); + } *mode_ = rmcs_msgs::ChassisMode::AUTO; - *ctrl_hold_active_ = false; + *pitch_lock_active_ = false; + *active_suspension_active_ = false; + *low_prone_active_ = false; + *symmetric_posture_target_ = true; + *correction_inverted_ = false; chassis_control_velocity_->vector << nan_, nan_, nan_; - - current_target_angle_ = max_angle_; - lf_current_target_angle_ = max_angle_; - lb_current_target_angle_ = max_angle_; - rf_current_target_angle_ = max_angle_; - rb_current_target_angle_ = max_angle_; } void before_updating() override { @@ -219,19 +84,9 @@ class DeformableChassis if (!gimbal_yaw_angle_error_.ready()) { gimbal_yaw_angle_error_.make_and_bind_directly(0.0); RCLCPP_WARN( - get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". " + "Set to 0.0."); } - if (!manual_yaw_velocity_override_.ready()) - manual_yaw_velocity_override_.make_and_bind_directly(nan_); - if (!chassis_imu_pitch_.ready()) - chassis_imu_pitch_.make_and_bind_directly(0.0); - if (!chassis_imu_roll_.ready()) - chassis_imu_roll_.make_and_bind_directly(0.0); - if (!chassis_imu_pitch_rate_.ready()) - chassis_imu_pitch_rate_.make_and_bind_directly(0.0); - if (!chassis_imu_roll_rate_.ready()) - chassis_imu_roll_rate_.make_and_bind_directly(0.0); - validate_joint_feedback_inputs(); } void update() override { @@ -248,437 +103,50 @@ class DeformableChassis break; } - update_mode_from_inputs_(switch_left, switch_right, keyboard); - update_suspension_toggle_from_inputs_(switch_left, switch_right); - *ctrl_hold_active_ = ctrl_hold_requested_by_input_(); + double rotary_knob = rotary_knob_.ready() ? *rotary_knob_ : 0.0; + + joint_mode_mgr_.update(switch_left, switch_right, keyboard, rotary_knob, update_dt()); + + *mode_ = joint_mode_mgr_.mode(); + *pitch_lock_active_ = joint_mode_mgr_.pitch_lock_active(); + *active_suspension_active_ = joint_mode_mgr_.suspension_active(); + *low_prone_active_ = joint_mode_mgr_.low_prone_active(); + *symmetric_posture_target_ = joint_mode_mgr_.symmetric_posture_target(); + *correction_inverted_ = joint_mode_mgr_.correction_inverted(); + *min_angle_deg_ = joint_mode_mgr_.min_angle(); + *max_angle_deg_ = joint_mode_mgr_.max_angle(); + *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); + publish_joint_posture_targets_(); + update_velocity_control(); - update_lift_target_toggle(keyboard); - run_joint_intent_pipeline_(); } while (false); - - last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; } private: + static constexpr size_t kJointCount = 4; static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double translational_velocity_max_ = 10.0; static constexpr double angular_velocity_max_ = 30.0; - static constexpr double rad_to_deg_ = 180.0 / std::numbers::pi; - static constexpr double imu_calibration_offset_limit_rad_ = - 1.0 * std::numbers::pi / 180.0; - - void validate_joint_feedback_inputs() const { - if (left_front_joint_physical_angle_.ready() && left_back_joint_physical_angle_.ready() - && right_front_joint_physical_angle_.ready() && right_back_joint_physical_angle_.ready()) - return; - - throw std::runtime_error( - "missing deformable chassis feedback interfaces: expected /chassis/*_joint/physical_angle"); - } - - void update_mode_from_inputs_( - rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard) { - auto mode = *mode_; - const bool q_pressed = keyboard.q; - const bool e_pressed = keyboard.e; - const bool last_q_pressed = last_keyboard_.q; - const bool last_e_pressed = last_keyboard_.e; - const bool last_c_pressed = last_keyboard_.c; - const bool qe_combo_pressed = q_pressed && e_pressed; - const bool last_qe_combo_pressed = last_q_pressed && last_e_pressed; - const bool e_rising = !last_e_pressed && e_pressed; - const bool c_rising = !last_c_pressed && keyboard.c; - const bool qe_combo_rising = !last_qe_combo_pressed && qe_combo_pressed; - if (switch_left == rmcs_msgs::Switch::DOWN) { - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - return; - } - - if (qe_complex_spin_active_) { - if (c_rising) { - deactivate_qe_complex_spin_(); - apply_symmetric_target = true; - lf_current_target_angle_ = current_target_angle_; - lb_current_target_angle_ = current_target_angle_; - rb_current_target_angle_ = current_target_angle_; - rf_current_target_angle_ = current_target_angle_; - mode = rmcs_msgs::ChassisMode::SPIN; - } - } else if (qe_combo_rising) { - deactivate_complex_spin_(); - activate_qe_complex_spin_(mode); - } else if (e_rising && !q_pressed) { - if (complex_spin_active_) { - deactivate_complex_spin_(); - if (mode == rmcs_msgs::ChassisMode::SPIN) - mode = rmcs_msgs::ChassisMode::AUTO; - } else { - activate_complex_spin_(mode); - } - } else if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE - && switch_right == rmcs_msgs::Switch::DOWN) { - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - if (mode == rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::STEP_DOWN; - } else { - mode = rmcs_msgs::ChassisMode::SPIN; - spinning_forward_ = !spinning_forward_; - } - } else if (!last_keyboard_.c && keyboard.c) { - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - if (mode == rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::AUTO; - } else { - mode = rmcs_msgs::ChassisMode::SPIN; - spinning_forward_ = !spinning_forward_; - } - } else if (launch_ramp_shortcut_enabled_ && !last_keyboard_.x && keyboard.x) { - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - mode = mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP - ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::LAUNCH_RAMP; - } else if (!last_keyboard_.z && keyboard.z) { - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::STEP_DOWN; - } - - if (complex_spin_active_ || qe_complex_spin_active_) - mode = rmcs_msgs::ChassisMode::SPIN; - - *mode_ = mode; - } - - void activate_complex_spin_(rmcs_msgs::ChassisMode& mode) { - complex_spin_active_ = true; - complex_spin_elapsed_ = 0.0; - apply_symmetric_target = true; - if (mode != rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::SPIN; - spinning_forward_ = !spinning_forward_; - } - } - - void deactivate_complex_spin_() { - complex_spin_active_ = false; - complex_spin_elapsed_ = 0.0; - } - - void activate_qe_complex_spin_(rmcs_msgs::ChassisMode& mode) { - qe_complex_spin_active_ = true; - qe_last_toggle_elapsed_ = 0.0; - qe_front_high_rear_low_ = true; - apply_front_high_rear_low_target_(); - if (mode != rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::SPIN; - spinning_forward_ = !spinning_forward_; - } - } - - void deactivate_qe_complex_spin_() { - qe_complex_spin_active_ = false; - qe_last_toggle_elapsed_ = 0.0; - } - - void apply_front_high_rear_low_target_() { - lf_current_target_angle_ = max_angle_; - rf_current_target_angle_ = max_angle_; - lb_current_target_angle_ = min_angle_; - rb_current_target_angle_ = min_angle_; - apply_symmetric_target = false; - qe_front_high_rear_low_ = true; - } - - void apply_front_low_rear_high_target_() { - lf_current_target_angle_ = min_angle_; - rf_current_target_angle_ = min_angle_; - lb_current_target_angle_ = max_angle_; - rb_current_target_angle_ = max_angle_; - apply_symmetric_target = false; - qe_front_high_rear_low_ = false; - } - - void toggle_bg_target_() { - if (qe_front_high_rear_low_) { - apply_front_low_rear_high_target_(); - } else { - apply_front_high_rear_low_target_(); - } - } - - void toggle_qe_complex_spin_target_() { - toggle_bg_target_(); - } - - void update_qe_complex_spin_toggle_() { - constexpr double qe_complex_spin_toggle_period = 1.0; - - qe_last_toggle_elapsed_ += update_dt(); - size_t qe_complex_spin_toggle_count = 0; - while (qe_last_toggle_elapsed_ >= qe_complex_spin_toggle_period) { - qe_last_toggle_elapsed_ -= qe_complex_spin_toggle_period; - ++qe_complex_spin_toggle_count; - } - - if ((qe_complex_spin_toggle_count % 2) == 1) - toggle_qe_complex_spin_target_(); - } - - std::array read_current_joint_physical_angles_() const { - const std::array*, kJointCount> physical_angle_inputs{ - &left_front_joint_physical_angle_, &left_back_joint_physical_angle_, - &right_back_joint_physical_angle_, &right_front_joint_physical_angle_}; - - std::array current_physical_angles{}; - current_physical_angles.fill(nan_); - for (size_t i = 0; i < kJointCount; ++i) { - if (physical_angle_inputs[i]->ready() && std::isfinite(*(*physical_angle_inputs[i]))) { - current_physical_angles[i] = *(*physical_angle_inputs[i]); - } - } - - return current_physical_angles; - } - - bool prone_override_requested_by_keyboard() const { return keyboard_.ready() && keyboard_->ctrl; } - - bool suspension_toggle_requested_by_switch_( - rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) const { - return switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP - && last_switch_right_ == rmcs_msgs::Switch::MIDDLE; - } - - void update_suspension_toggle_from_inputs_( - rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { - if (suspension_toggle_requested_by_switch_(switch_left, switch_right)) { - suspension_on_by_switch = !suspension_on_by_switch; - } - } - - bool ctrl_hold_requested_by_input_() const { - return prone_override_requested_by_keyboard() || suspension_on_by_switch; - } - - bool suspension_requested_by_input_() const { - return active_suspension_enable_ && ctrl_hold_requested_by_input_(); - } - - bool symmetric_joint_target_requested_() const { - constexpr double epsilon = 1e-6; - return std::abs(lf_current_target_angle_ - lb_current_target_angle_) <= epsilon - && std::abs(lf_current_target_angle_ - rb_current_target_angle_) <= epsilon - && std::abs(lf_current_target_angle_ - rf_current_target_angle_) <= epsilon; - } - - void reset_attitude_correction_state_() { - pitch_pid_.reset(); - roll_pid_.reset(); - joint_suspension_active_.fill(false); - } - - void reset_chassis_imu_calibration_window_() { - chassis_imu_calibration_hold_elapsed_ = 0.0; - chassis_imu_calibration_sample_count_ = 0; - chassis_imu_pitch_sum_ = 0.0; - chassis_imu_roll_sum_ = 0.0; - chassis_imu_calibration_completed_for_window_ = false; - } - - void update_chassis_imu_calibration_() { - if (chassis_imu_calibrated_once_) - return; - - if (!symmetric_joint_target_requested_()) { - reset_chassis_imu_calibration_window_(); - return; - } - - const double raw_pitch = *chassis_imu_pitch_; - const double raw_roll = *chassis_imu_roll_; - if (!std::isfinite(raw_pitch) || !std::isfinite(raw_roll)) - return; - - chassis_imu_calibration_hold_elapsed_ += update_dt(); - if (chassis_imu_calibration_hold_elapsed_ < chassis_imu_calibration_wait_time_) - return; - - const double calibration_end_time = - chassis_imu_calibration_wait_time_ + chassis_imu_calibration_sample_time_; - if (chassis_imu_calibration_hold_elapsed_ < calibration_end_time) { - chassis_imu_pitch_sum_ += raw_pitch; - chassis_imu_roll_sum_ += raw_roll; - ++chassis_imu_calibration_sample_count_; - return; - } - - if (chassis_imu_calibration_completed_for_window_) - return; - - chassis_imu_calibration_completed_for_window_ = true; - if (chassis_imu_calibration_sample_count_ == 0) { - RCLCPP_WARN( - get_logger(), - "[chassis imu calibration] skipped because no valid samples were collected"); - return; - } - - chassis_imu_pitch_offset_ = std::clamp( - chassis_imu_pitch_sum_ / static_cast(chassis_imu_calibration_sample_count_), - -imu_calibration_offset_limit_rad_, imu_calibration_offset_limit_rad_); - chassis_imu_roll_offset_ = std::clamp( - chassis_imu_roll_sum_ / static_cast(chassis_imu_calibration_sample_count_), - -imu_calibration_offset_limit_rad_, imu_calibration_offset_limit_rad_); - chassis_imu_calibrated_once_ = true; - RCLCPP_INFO( - get_logger(), - "[chassis imu calibration] pitch_offset=% .3f deg roll_offset=% .3f deg " - "(samples=%zu)", - chassis_imu_pitch_offset_ * rad_to_deg_, chassis_imu_roll_offset_ * rad_to_deg_, - chassis_imu_calibration_sample_count_); - } - - bool ensure_joint_target_states_from_feedback( - const std::array& current_physical_angles) { - bool any_active = false; - for (size_t i = 0; i < kJointCount; ++i) { - if (std::isfinite(current_physical_angles[i]) && !joint_target_active_[i]) { - joint_target_physical_angle_state_rad_[i] = current_physical_angles[i]; - joint_target_physical_velocity_state_rad_[i] = 0.0; - joint_target_physical_acceleration_state_rad_[i] = 0.0; - current_target_physical_angles_rad_[i] = current_physical_angles[i]; - joint_target_active_[i] = true; - } - - any_active = any_active || joint_target_active_[i]; - } - - return any_active; - } - - bool any_joint_target_active_() const { - return std::any_of( - joint_target_active_.begin(), joint_target_active_.end(), [](bool active) { - return active; - }); - } - - double active_suspension_min_angle_rad_() const { - return deg_to_rad(min_angle_ - 5.0); - } - - void update_active_suspension_() { - if (!suspension_requested_by_input_()) { - reset_attitude_correction_state_(); - return; - } - - constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; - const double base_target_angle = active_suspension_min_angle_rad_(); - const double max_target_angle = deg_to_rad(max_angle_); - const double corrected_pitch = - std::clamp(*chassis_imu_pitch_ - chassis_imu_pitch_offset_, -max_attitude, max_attitude); - const double corrected_roll = - std::clamp(*chassis_imu_roll_ - chassis_imu_roll_offset_, -max_attitude, max_attitude); - const double corrected_pitch_rate = *chassis_imu_pitch_rate_; - const double corrected_roll_rate = *chassis_imu_roll_rate_; - - const double dt = update_dt(); - const double pitch_angle_diff = - pitch_pid_.update(-corrected_pitch, corrected_pitch_rate, dt); - const double roll_angle_diff = - roll_pid_.update(corrected_roll, -corrected_roll_rate, dt); - if (!std::isfinite(pitch_angle_diff) || !std::isfinite(roll_angle_diff)) { - reset_attitude_correction_state_(); - current_target_physical_angles_rad_.fill(base_target_angle); - return; - } - - // Positive pitch_angle_diff raises the rear pair. Positive roll_angle_diff raises the left - // pair. Every leg starts from the active-suspension minimum and only receives additive - // corrections so at least one leg always stays at that minimum. - const double front_pitch_add = std::max(-pitch_angle_diff, 0.0); - const double back_pitch_add = std::max(pitch_angle_diff, 0.0); - const double left_roll_add = std::max(roll_angle_diff, 0.0); - const double right_roll_add = std::max(-roll_angle_diff, 0.0); - - current_target_physical_angles_rad_[kLeftFront] = - std::clamp( - base_target_angle + front_pitch_add + left_roll_add, base_target_angle, - max_target_angle); - current_target_physical_angles_rad_[kLeftBack] = - std::clamp( - base_target_angle + back_pitch_add + left_roll_add, base_target_angle, - max_target_angle); - current_target_physical_angles_rad_[kRightBack] = - std::clamp( - base_target_angle + back_pitch_add + right_roll_add, base_target_angle, - max_target_angle); - current_target_physical_angles_rad_[kRightFront] = - std::clamp( - base_target_angle + front_pitch_add + right_roll_add, base_target_angle, - max_target_angle); - - joint_suspension_active_.fill(true); - } + static constexpr double default_dt_ = 1e-3; void reset_all_controls() { + joint_mode_mgr_.reset(); + *deformable_reset_count_ += 1; + *mode_ = rmcs_msgs::ChassisMode::AUTO; - *ctrl_hold_active_ = false; - reset_attitude_correction_state_(); - reset_chassis_imu_calibration_window_(); + *pitch_lock_active_ = false; + *active_suspension_active_ = false; + *low_prone_active_ = false; + *symmetric_posture_target_ = true; + *correction_inverted_ = false; + *min_angle_deg_ = joint_mode_mgr_.min_angle(); + *max_angle_deg_ = joint_mode_mgr_.max_angle(); + *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); + publish_joint_posture_targets_(); chassis_control_velocity_->vector << nan_, nan_, nan_; *chassis_angle_ = nan_; *chassis_control_angle_ = nan_; - - current_target_angle_ = max_angle_; - lf_current_target_angle_ = current_target_angle_; - lb_current_target_angle_ = current_target_angle_; - rb_current_target_angle_ = current_target_angle_; - rf_current_target_angle_ = current_target_angle_; - joint_target_active_.fill(false); - current_target_physical_angles_rad_.fill(nan_); - joint_target_physical_angle_state_rad_.fill(nan_); - joint_target_physical_velocity_state_rad_.fill(0.0); - joint_target_physical_acceleration_state_rad_.fill(0.0); - suspension_on_by_switch = false; - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - - *scope_motor_control_torque = nan_; - - *lf_angle_error_ = nan_; - *lb_angle_error_ = nan_; - *rf_angle_error_ = nan_; - *rb_angle_error_ = nan_; - - *left_front_joint_target_physical_angle_ = nan_; - *left_back_joint_target_physical_angle_ = nan_; - *right_back_joint_target_physical_angle_ = nan_; - *right_front_joint_target_physical_angle_ = nan_; - *left_front_joint_target_physical_velocity_ = nan_; - *left_back_joint_target_physical_velocity_ = nan_; - *right_back_joint_target_physical_velocity_ = nan_; - *right_front_joint_target_physical_velocity_ = nan_; - *left_front_joint_target_physical_acceleration_ = nan_; - *left_back_joint_target_physical_acceleration_ = nan_; - *right_back_joint_target_physical_acceleration_ = nan_; - *right_front_joint_target_physical_acceleration_ = nan_; - - } - - void update_velocity_control() { - const Eigen::Vector2d translational_velocity = update_translational_velocity_control(); - const double angular_velocity = update_angular_velocity_control(); - chassis_control_velocity_->vector << translational_velocity, angular_velocity; } double update_dt() const { @@ -687,12 +155,18 @@ class DeformableChassis return default_dt_; } + void update_velocity_control() { + Eigen::Vector2d translational_velocity = update_translational_velocity_control(); + double angular_velocity = update_angular_velocity_control(); + chassis_control_velocity_->vector << translational_velocity, angular_velocity; + } + Eigen::Vector2d update_translational_velocity_control() { const auto keyboard = *keyboard_; - const Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; + Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; Eigen::Vector2d translational_velocity = - Eigen::Rotation2Dd{*gimbal_yaw_angle_} * ((*joystick_right_) + keyboard_move); + Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move); if (translational_velocity.norm() > 1.0) translational_velocity.normalize(); @@ -702,13 +176,6 @@ class DeformableChassis } double update_angular_velocity_control() { - if (*ctrl_hold_active_ && std::isfinite(*manual_yaw_velocity_override_)) { - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; - *chassis_control_angle_ = nan_; - return std::clamp( - *manual_yaw_velocity_override_, -angular_velocity_max_, angular_velocity_max_); - } - double angular_velocity = 0.0; double chassis_control_angle = nan_; @@ -716,35 +183,26 @@ class DeformableChassis case rmcs_msgs::ChassisMode::AUTO: break; case rmcs_msgs::ChassisMode::SPIN: { + bool forward = joint_mode_mgr_.spinning_forward(); angular_velocity = - spin_ratio_ * (spinning_forward_ ? angular_velocity_max_ : -angular_velocity_max_); + spin_ratio_ * (forward ? angular_velocity_max_ : -angular_velocity_max_); angular_velocity = std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); } break; case rmcs_msgs::ChassisMode::STEP_DOWN: { - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); - // In step-down mode, front/back can both be used for alignment. constexpr double alignment = std::numbers::pi; - while (err > alignment / 2) { + while (chassis_angle_error > alignment / 2) { chassis_control_angle -= alignment; if (chassis_control_angle < 0) chassis_control_angle += 2 * std::numbers::pi; - err -= alignment; + chassis_angle_error -= alignment; } - angular_velocity = following_velocity_controller_.update(err); - } break; - - case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - - constexpr double alignment = 2 * std::numbers::pi; - if (err > alignment / 2) - err -= alignment; - - angular_velocity = following_velocity_controller_.update(err); + angular_velocity = following_velocity_controller_.update(chassis_angle_error); } break; default: break; @@ -761,226 +219,30 @@ class DeformableChassis if (chassis_control_angle < 0) chassis_control_angle += 2 * std::numbers::pi; - double err = chassis_control_angle + *gimbal_yaw_angle_; - if (err >= 2 * std::numbers::pi) - err -= 2 * std::numbers::pi; - - return err; - } - - void update_lift_target_toggle(rmcs_msgs::Keyboard keyboard) { - constexpr double rotary_knob_symmetric_edge_threshold = 0.7; - constexpr double rotary_knob_bg_edge_threshold = -0.9; - constexpr double complex_spin_toggle_period = 0.5; - - const bool keyboard_toggle_condition = - !qe_complex_spin_active_ && !last_keyboard_.q && keyboard.q && !keyboard.e; - const bool rotary_knob_toggle_condition = - last_rotary_knob_ < rotary_knob_symmetric_edge_threshold - && *rotary_knob_ >= rotary_knob_symmetric_edge_threshold; - const bool rotary_knob_bg_toggle_condition = - !qe_complex_spin_active_ && last_rotary_knob_ > rotary_knob_bg_edge_threshold - && *rotary_knob_ <= rotary_knob_bg_edge_threshold; - const bool front_high_rear_low = !qe_complex_spin_active_ && !last_keyboard_.b && keyboard.b; - const bool front_low_rear_high = !qe_complex_spin_active_ && !last_keyboard_.g && keyboard.g; - bool complex_spin_toggle_condition = false; - - if (complex_spin_active_) { - complex_spin_elapsed_ += update_dt(); - size_t complex_spin_toggle_count = 0; - while (complex_spin_elapsed_ >= complex_spin_toggle_period) { - complex_spin_elapsed_ -= complex_spin_toggle_period; - ++complex_spin_toggle_count; - } - complex_spin_toggle_condition = (complex_spin_toggle_count % 2) == 1; - } - - if (qe_complex_spin_active_) - update_qe_complex_spin_toggle_(); + double unsigned_angle_error = chassis_control_angle + *gimbal_yaw_angle_; + if (unsigned_angle_error >= 2 * std::numbers::pi) + unsigned_angle_error -= 2 * std::numbers::pi; - if (apply_symmetric_target) { - lf_current_target_angle_ = current_target_angle_; - lb_current_target_angle_ = current_target_angle_; - rb_current_target_angle_ = current_target_angle_; - rf_current_target_angle_ = current_target_angle_; - } - - if (rotary_knob_toggle_condition || keyboard_toggle_condition || complex_spin_toggle_condition) { - current_target_angle_ = - (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; - apply_symmetric_target = true; - } else if (rotary_knob_bg_toggle_condition) { - toggle_bg_target_(); - } else if (front_high_rear_low) { - apply_front_high_rear_low_target_(); - } else if (front_low_rear_high) { - apply_front_low_rear_high_target_(); - } - - last_rotary_knob_ = *rotary_knob_; - } - - // Chassis owns the high-level joint intent pipeline: read feedback, generate deploy targets, - // coordinate suspension overrides, then publish the resulting joint intent for the servo layer. - void run_joint_intent_pipeline_() { - const auto current_physical_angles = read_current_joint_physical_angles_(); - const bool suspension_requested = suspension_requested_by_input_(); - - if (!ensure_joint_target_states_from_feedback(current_physical_angles)) { - publish_nan_joint_targets(); - return; - } - - current_target_physical_angles_rad_[kLeftFront] = deg_to_rad(lf_current_target_angle_); - current_target_physical_angles_rad_[kLeftBack] = deg_to_rad(lb_current_target_angle_); - current_target_physical_angles_rad_[kRightBack] = deg_to_rad(rb_current_target_angle_); - current_target_physical_angles_rad_[kRightFront] = deg_to_rad(rf_current_target_angle_); - if (suspension_requested) { - current_target_physical_angles_rad_.fill(active_suspension_min_angle_rad_()); - } - - update_chassis_imu_calibration_(); - scope_motor_control(suspension_requested); - update_active_suspension_(); - - update_joint_target_trajectory(); - publish_joint_target_angles(current_physical_angles); + return unsigned_angle_error; } static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } - void scope_motor_control(bool suspension_requested = false) { - const bool prone_target_active = suspension_requested; - if (prone_target_active && *mode_ != rmcs_msgs::ChassisMode::SPIN) { - *scope_motor_control_torque = -0.3; - } else { - *scope_motor_control_torque = 0.3; - } - } - - void update_joint_target_trajectory() { - const double dt = update_dt(); - for (size_t i = 0; i < kJointCount; ++i) { - if (!joint_target_active_[i]) - continue; - - double& angle_state = joint_target_physical_angle_state_rad_[i]; - double& velocity_state = joint_target_physical_velocity_state_rad_[i]; - double& acceleration_state = joint_target_physical_acceleration_state_rad_[i]; - const double target_angle = current_target_physical_angles_rad_[i]; - const double velocity_limit = joint_suspension_active_[i] - ? suspension_velocity_limit_ - : target_physical_velocity_limit_; - const double acceleration_limit = - joint_suspension_active_[i] ? suspension_acceleration_limit_ - : target_physical_acceleration_limit_; - - if (!std::isfinite(target_angle) || !std::isfinite(angle_state)) { - continue; - } - - const double position_error = target_angle - angle_state; - const double stopping_distance = - velocity_state * velocity_state / (2.0 * acceleration_limit); - - double desired_velocity = 0.0; - if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) { - desired_velocity = std::copysign(velocity_limit, position_error); - } - - const double velocity_error = desired_velocity - velocity_state; - acceleration_state = - std::clamp(velocity_error / dt, -acceleration_limit, acceleration_limit); - - velocity_state += acceleration_state * dt; - velocity_state = std::clamp(velocity_state, -velocity_limit, velocity_limit); - angle_state += velocity_state * dt; - - const double next_error = target_angle - angle_state; - if ((position_error > 0.0 && next_error < 0.0) - || (position_error < 0.0 && next_error > 0.0) - || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { - angle_state = target_angle; - velocity_state = 0.0; - acceleration_state = 0.0; - } - } - } - - void publish_joint_target_angles( - const std::array& current_physical_angles) { - if (!any_joint_target_active_()) { - publish_nan_joint_targets(); - return; - } - - const auto publish_joint = [this, ¤t_physical_angles]( - size_t index, OutputInterface& angle_output, - OutputInterface& velocity_output, - OutputInterface& acceleration_output, - OutputInterface& angle_error_output) { - if (!joint_target_active_[index]) { - *angle_output = nan_; - *velocity_output = nan_; - *acceleration_output = nan_; - *angle_error_output = nan_; - return; - } - - *angle_output = joint_target_physical_angle_state_rad_[index]; - *velocity_output = joint_target_physical_velocity_state_rad_[index]; - *acceleration_output = joint_target_physical_acceleration_state_rad_[index]; - *angle_error_output = std::isfinite(current_physical_angles[index]) - ? current_physical_angles[index] - - joint_target_physical_angle_state_rad_[index] - : nan_; - }; + void publish_joint_posture_targets_() { + std::array targets_deg{}; + joint_mode_mgr_.copy_joint_posture_target_deg(targets_deg); - publish_joint( - kLeftFront, left_front_joint_target_physical_angle_, - left_front_joint_target_physical_velocity_, - left_front_joint_target_physical_acceleration_, lf_angle_error_); - publish_joint( - kLeftBack, left_back_joint_target_physical_angle_, - left_back_joint_target_physical_velocity_, - left_back_joint_target_physical_acceleration_, lb_angle_error_); - publish_joint( - kRightBack, right_back_joint_target_physical_angle_, - right_back_joint_target_physical_velocity_, - right_back_joint_target_physical_acceleration_, rb_angle_error_); - publish_joint( - kRightFront, right_front_joint_target_physical_angle_, - right_front_joint_target_physical_velocity_, - right_front_joint_target_physical_acceleration_, rf_angle_error_); + for (size_t i = 0; i < kJointCount; ++i) + *joint_posture_target_angle_rad_[i] = deg_to_rad(targets_deg[i]); } - void publish_nan_joint_targets() { - reset_attitude_correction_state_(); - joint_target_active_.fill(false); - - *left_front_joint_target_physical_angle_ = nan_; - *left_back_joint_target_physical_angle_ = nan_; - *right_back_joint_target_physical_angle_ = nan_; - *right_front_joint_target_physical_angle_ = nan_; - - *left_front_joint_target_physical_velocity_ = nan_; - *left_back_joint_target_physical_velocity_ = nan_; - *right_back_joint_target_physical_velocity_ = nan_; - *right_front_joint_target_physical_velocity_ = nan_; - - *left_front_joint_target_physical_acceleration_ = nan_; - *left_back_joint_target_physical_acceleration_ = nan_; - *right_back_joint_target_physical_acceleration_ = nan_; - *right_front_joint_target_physical_acceleration_ = nan_; - - *lf_angle_error_ = nan_; - *lb_angle_error_ = nan_; - *rb_angle_error_ = nan_; - *rf_angle_error_ = nan_; - - } + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; -private: InputInterface joystick_right_; InputInterface switch_right_; InputInterface switch_left_; @@ -988,107 +250,30 @@ class DeformableChassis InputInterface rotary_knob_; InputInterface update_rate_; - rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); - double last_rotary_knob_ = 0.0; - InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; - InputInterface manual_yaw_velocity_override_; OutputInterface chassis_angle_, chassis_control_angle_; OutputInterface mode_; OutputInterface chassis_control_velocity_; - OutputInterface ctrl_hold_active_; + OutputInterface pitch_lock_active_; + OutputInterface active_suspension_active_; + OutputInterface low_prone_active_; + OutputInterface symmetric_posture_target_; + OutputInterface correction_inverted_; + OutputInterface min_angle_deg_; + OutputInterface max_angle_deg_; + OutputInterface suspension_reference_angle_deg_; + OutputInterface deformable_reset_count_; + std::array, kJointCount> joint_posture_target_angle_rad_; - bool spinning_forward_ = true; - bool apply_symmetric_target = true; - bool complex_spin_active_ = false; - double complex_spin_elapsed_ = 0.0; - bool qe_complex_spin_active_ = false; - bool qe_front_high_rear_low_ = true; - double qe_last_toggle_elapsed_ = 0.0; pid::PidCalculator following_velocity_controller_; const double spin_ratio_; - const bool launch_ramp_shortcut_enabled_; - - InputInterface left_front_joint_physical_angle_; - InputInterface left_back_joint_physical_angle_; - InputInterface right_front_joint_physical_angle_; - InputInterface right_back_joint_physical_angle_; - InputInterface chassis_imu_pitch_; - InputInterface chassis_imu_roll_; - InputInterface chassis_imu_pitch_rate_; - InputInterface chassis_imu_roll_rate_; - - OutputInterface scope_motor_control_torque; - - OutputInterface lf_angle_error_; - OutputInterface lb_angle_error_; - OutputInterface rf_angle_error_; - OutputInterface rb_angle_error_; - - OutputInterface left_front_joint_target_physical_angle_; - OutputInterface left_back_joint_target_physical_angle_; - OutputInterface right_back_joint_target_physical_angle_; - OutputInterface right_front_joint_target_physical_angle_; - OutputInterface left_front_joint_target_physical_velocity_; - OutputInterface left_back_joint_target_physical_velocity_; - OutputInterface right_back_joint_target_physical_velocity_; - OutputInterface right_front_joint_target_physical_velocity_; - OutputInterface left_front_joint_target_physical_acceleration_; - OutputInterface left_back_joint_target_physical_acceleration_; - OutputInterface right_back_joint_target_physical_acceleration_; - OutputInterface right_front_joint_target_physical_acceleration_; - double min_angle_; - double max_angle_; - - double current_target_angle_; - double lf_current_target_angle_, lb_current_target_angle_, rb_current_target_angle_, - rf_current_target_angle_; - - std::array current_target_physical_angles_rad_ = {0.0, 0.0, 0.0, 0.0}; - - std::array joint_target_active_ = {false, false, false, false}; - std::array joint_target_physical_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - std::array joint_target_physical_velocity_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - std::array joint_target_physical_acceleration_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - - double target_physical_velocity_limit_; - double target_physical_acceleration_limit_; - bool active_suspension_enable_; - bool suspension_on_by_switch = false; - double pitch_kp_; - double pitch_ki_; - double pitch_kd_; - double roll_kp_; - double roll_ki_; - double roll_kd_; - double suspension_velocity_limit_; - double suspension_acceleration_limit_; - double pitch_diff_limit_; - double roll_diff_limit_; - double pid_integral_limit_; - std::array joint_suspension_active_ = {false, false, false, false}; - AttitudePidAxis pitch_pid_; - AttitudePidAxis roll_pid_; - double chassis_imu_pitch_offset_ = 0.0; - double chassis_imu_roll_offset_ = 0.0; - double chassis_imu_calibration_wait_time_; - double chassis_imu_calibration_sample_time_; - double chassis_imu_calibration_hold_elapsed_ = 0.0; - size_t chassis_imu_calibration_sample_count_ = 0; - double chassis_imu_pitch_sum_ = 0.0; - double chassis_imu_roll_sum_ = 0.0; - bool chassis_imu_calibration_completed_for_window_ = false; - bool chassis_imu_calibrated_once_ = false; - static constexpr double default_dt_ = 1e-3; + DeformableChassisModeManager joint_mode_mgr_; }; } // namespace rmcs_core::controller::chassis #include + PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp index 14623175..38bb7ec1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp @@ -32,26 +32,19 @@ class DeformableJointController : public rmcs_executor::Component , public rclcpp::Node { public: - // Joint controller owns only the local angle-servo execution. Chassis publishes the - // higher-level target angle trajectory; this controller turns that target into motor torque. - struct ControllerConfig { - rmcs_core::controller::adrc::TD::Config td; - rmcs_core::controller::adrc::ESO::Config eso; - rmcs_core::controller::adrc::NLESF::Config nlesf; - double output_min = -std::numeric_limits::infinity(); - double output_max = std::numeric_limits::infinity(); - }; - - struct InputSnapshot { - double measurement_angle = std::numeric_limits::quiet_NaN(); - double setpoint_angle = std::numeric_limits::quiet_NaN(); - }; - - DeformableJointController() + explicit DeformableJointController() : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { - register_interfaces_(); + register_input(get_parameter("measurement_angle").as_string(), measurement_angle_); + register_input(get_parameter("setpoint_angle").as_string(), setpoint_angle_); + if (has_parameter("setpoint_velocity")) { + register_input( + get_parameter("setpoint_velocity").as_string(), setpoint_velocity_, false); + use_setpoint_velocity_ = true; + } + register_output(get_parameter("control").as_string(), control_torque_, nan_); + load_config_(); apply_config_(); } @@ -74,11 +67,21 @@ class DeformableJointController } private: - void register_interfaces_() { - register_input(get_parameter("measurement_angle").as_string(), measurement_angle_); - register_input(get_parameter("setpoint_angle").as_string(), setpoint_angle_); - register_output(get_parameter("control").as_string(), control_torque_, nan_); - } + // Joint controller owns only the local angle-servo execution. Chassis publishes the + // higher-level target angle trajectory; this controller turns that target into motor torque. + struct ControllerConfig { + rmcs_core::controller::adrc::TD::Config td; + rmcs_core::controller::adrc::ESO::Config eso; + rmcs_core::controller::adrc::NLESF::Config nlesf; + double output_min = -std::numeric_limits::infinity(); + double output_max = std::numeric_limits::infinity(); + }; + + struct InputSnapshot { + double measurement_angle = std::numeric_limits::quiet_NaN(); + double setpoint_angle = std::numeric_limits::quiet_NaN(); + double setpoint_velocity = std::numeric_limits::quiet_NaN(); + }; void load_config_() { dt_ = load_parameter_or(*this, "dt", 0.001); @@ -124,6 +127,10 @@ class DeformableJointController bool read_inputs_(InputSnapshot& inputs) const { inputs.measurement_angle = *measurement_angle_; inputs.setpoint_angle = *setpoint_angle_; + if (use_setpoint_velocity_ && setpoint_velocity_.ready() + && std::isfinite(*setpoint_velocity_)) { + inputs.setpoint_velocity = *setpoint_velocity_; + } return std::isfinite(inputs.measurement_angle) && std::isfinite(inputs.setpoint_angle); } @@ -142,11 +149,18 @@ class DeformableJointController } bool run_joint_servo_(const InputSnapshot& inputs, double& control_torque) { - const auto td_out = td_.update(inputs.setpoint_angle); const auto eso_out = eso_.update(inputs.measurement_angle, last_u_); - const double e1 = td_out.x1 - eso_out.z1; - const double e2 = td_out.x2 - eso_out.z2; + double reference_angle = inputs.setpoint_angle; + double reference_velocity = inputs.setpoint_velocity; + if (!std::isfinite(reference_velocity)) { + const auto td_out = td_.update(inputs.setpoint_angle); + reference_angle = td_out.x1; + reference_velocity = td_out.x2; + } + + const double e1 = reference_angle - eso_out.z1; + const double e2 = reference_velocity - eso_out.z2; control_torque = kt_ * nlesf_.compute(e1, e2, eso_out.z3, b0_).u; control_torque = std::clamp(control_torque, config_.output_min, config_.output_max); @@ -174,6 +188,7 @@ class DeformableJointController InputInterface measurement_angle_; InputInterface setpoint_angle_; + InputInterface setpoint_velocity_; OutputInterface control_torque_; @@ -187,6 +202,7 @@ class DeformableJointController double b0_ = 1.0; double kt_ = 1.0; double last_u_ = 0.0; + bool use_setpoint_velocity_ = false; bool initialized_ = false; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp new file mode 100644 index 00000000..923a9ff2 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -0,0 +1,333 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +class DeformableChassisModeManager { +public: + enum class SuspensionMode : uint8_t { + OFF = 0, + ACTIVE = 1, + }; + + struct JointPostureState { + rmcs_msgs::ChassisMode mode = rmcs_msgs::ChassisMode::AUTO; + bool ctrl_low_prone_active = false; + bool low_prone_active = false; + bool pitch_lock_active = false; + bool suspension_active = false; + SuspensionMode suspension_mode = SuspensionMode::OFF; + bool symmetric_posture_target = true; + bool spinning_forward = true; + std::array joint_posture_target_deg = {58.0, 58.0, 58.0, 58.0}; + double suspension_reference_angle_deg = 58.0; + }; + + explicit DeformableChassisModeManager(rclcpp::Node& node) + : min_angle_(node.get_parameter_or("min_angle", 5.0)) + , max_angle_(node.get_parameter_or("max_angle", 59.0)) + , active_suspension_base_angle_( + std::clamp( + node.get_parameter_or("active_suspension_base_angle", max_angle_), + min_angle_ - 5.0, max_angle_)) + , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) { + current_target_angle_ = max_angle_; + joint_current_target_angle_.fill(max_angle_); + update_joint_posture_state_(false); + } + + void reset() { + joint_posture_state_.mode = rmcs_msgs::ChassisMode::AUTO; + joint_posture_state_.ctrl_low_prone_active = false; + joint_posture_state_.low_prone_active = false; + joint_posture_state_.pitch_lock_active = false; + joint_posture_state_.suspension_active = false; + joint_posture_state_.suspension_mode = SuspensionMode::OFF; + joint_posture_state_.symmetric_posture_target = true; + joint_posture_state_.spinning_forward = true; + joint_posture_state_.joint_posture_target_deg.fill(max_angle_); + joint_posture_state_.suspension_reference_angle_deg = max_angle_; + + current_target_angle_ = max_angle_; + active_suspension_base_angle_ = max_angle_; + joint_current_target_angle_.fill(max_angle_); + apply_symmetric_target_ = true; + suspension_enabled_by_toggle_ = false; + low_prone_enabled_by_toggle_ = false; + + last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + last_keyboard_ = rmcs_msgs::Keyboard::zero(); + last_rotary_knob_ = 0.0; + + update_joint_posture_state_(false); + } + + void update( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { + + update_mode_from_inputs_(switch_left, switch_right, keyboard); + update_low_prone_toggle_from_inputs_(switch_left, switch_right); + + joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; + joint_posture_state_.low_prone_active = + joint_posture_state_.ctrl_low_prone_active || low_prone_enabled_by_toggle_; + joint_posture_state_.pitch_lock_active = + joint_posture_state_.ctrl_low_prone_active; + + update_suspension_mode_from_inputs_(switch_left, switch_right, keyboard, rotary_knob); + update_posture_target_from_inputs_(switch_left, switch_right, keyboard, rotary_knob, dt); + update_joint_posture_state_(joint_posture_state_.low_prone_active); + + last_switch_right_ = switch_right; + last_keyboard_ = keyboard; + } + + rmcs_msgs::ChassisMode mode() const { return joint_posture_state_.mode; } + bool pitch_lock_active() const { return joint_posture_state_.pitch_lock_active; } + bool suspension_active() const { return joint_posture_state_.suspension_active; } + bool low_prone_active() const { return joint_posture_state_.low_prone_active; } + bool symmetric_posture_target() const { return joint_posture_state_.symmetric_posture_target; } + bool spinning_forward() const { return joint_posture_state_.spinning_forward; } + double suspension_reference_angle_deg() const { + return joint_posture_state_.suspension_reference_angle_deg; + } + void copy_joint_posture_target_deg(std::array& out) const { + out = joint_posture_state_.joint_posture_target_deg; + } + + const JointPostureState& joint_posture_state() const { return joint_posture_state_; } + + double min_angle() const { return min_angle_; } + double max_angle() const { return max_angle_; } + double max_angle_rad() const { return deg_to_rad_(max_angle_); } + + double active_suspension_min_angle_rad() const { return deg_to_rad_(min_angle_ - 5.0); } + + bool correction_inverted() const { + double midpoint = (min_angle_ - 5.0 + max_angle_) / 2.0; + return joint_posture_state_.suspension_reference_angle_deg > midpoint; + } + +private: + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + static constexpr size_t kJointCount = 4; + + static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } + + static bool + symmetric_joint_target_requested_(const std::array& joint_target_deg) { + constexpr double epsilon = 1e-6; + return std::all_of(joint_target_deg.begin() + 1, joint_target_deg.end(), [&](double v) { + return std::abs(v - joint_target_deg.front()) <= epsilon; + }); + } + + void update_mode_from_inputs_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard) { + + auto next_mode = joint_posture_state_.mode; + if (switch_left == rmcs_msgs::Switch::DOWN) { + joint_posture_state_.mode = next_mode; + return; + } + + if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE + && switch_right == rmcs_msgs::Switch::DOWN) { + if (next_mode == rmcs_msgs::ChassisMode::SPIN) { + next_mode = rmcs_msgs::ChassisMode::STEP_DOWN; + } else { + next_mode = rmcs_msgs::ChassisMode::SPIN; + joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; + } + } else if (!last_keyboard_.c && keyboard.c) { + if (next_mode == rmcs_msgs::ChassisMode::SPIN) { + next_mode = rmcs_msgs::ChassisMode::AUTO; + } else { + next_mode = rmcs_msgs::ChassisMode::SPIN; + joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; + } + } else if (!last_keyboard_.z && keyboard.z) { + next_mode = next_mode == rmcs_msgs::ChassisMode::STEP_DOWN + ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::STEP_DOWN; + } + + joint_posture_state_.mode = next_mode; + } + + void apply_front_high_rear_low_target_() { + joint_current_target_angle_[kLeftFront] = max_angle_; + joint_current_target_angle_[kRightFront] = max_angle_; + joint_current_target_angle_[kLeftBack] = min_angle_; + joint_current_target_angle_[kRightBack] = min_angle_; + apply_symmetric_target_ = false; + } + + void apply_front_low_rear_high_target_() { + joint_current_target_angle_[kLeftFront] = min_angle_; + joint_current_target_angle_[kRightFront] = min_angle_; + joint_current_target_angle_[kLeftBack] = max_angle_; + joint_current_target_angle_[kRightBack] = max_angle_; + apply_symmetric_target_ = false; + } + + void toggle_front_back_posture_target_() { + if (joint_current_target_angle_[kLeftFront] > joint_current_target_angle_[kLeftBack]) + apply_front_low_rear_high_target_(); + else + apply_front_high_rear_low_target_(); + } + + void update_suspension_mode_from_inputs_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard, double rotary_knob) { + const bool remote_suspension_rotary_mode = + switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::MIDDLE; + const bool remote_active_toggle_requested = + remote_suspension_rotary_mode && rotary_knob_down_edge_(rotary_knob); + + const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.e && keyboard.e; + if (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested) + suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; + + const bool active_requested = + suspension_enable_ + && (joint_posture_state_.low_prone_active || suspension_enabled_by_toggle_); + + joint_posture_state_.suspension_mode = SuspensionMode::OFF; + if (active_requested) + joint_posture_state_.suspension_mode = SuspensionMode::ACTIVE; + + joint_posture_state_.suspension_active = + joint_posture_state_.suspension_mode == SuspensionMode::ACTIVE; + } + + void update_low_prone_toggle_from_inputs_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { + if (switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP + && last_switch_right_ == rmcs_msgs::Switch::MIDDLE) { + low_prone_enabled_by_toggle_ = !low_prone_enabled_by_toggle_; + } + } + + void update_posture_target_from_inputs_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double /*dt*/) { + const bool remote_joint_posture_rotary_mode = + switch_left == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::MIDDLE; + + const bool keyboard_posture_toggle_condition = !last_keyboard_.q && keyboard.q; + const bool remote_posture_toggle_condition = + remote_joint_posture_rotary_mode && rotary_knob_down_edge_(rotary_knob); + const bool remote_front_back_posture_toggle_condition = + remote_joint_posture_rotary_mode && rotary_knob_up_edge_(rotary_knob); + const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; + const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; + + if (apply_symmetric_target_) + joint_current_target_angle_.fill(current_target_angle_); + + const bool posture_toggle_requested = + remote_posture_toggle_condition || keyboard_posture_toggle_condition; + + if (posture_toggle_requested) { + if (joint_posture_state_.suspension_active) { + active_suspension_base_angle_ = + (std::abs(active_suspension_base_angle_ - max_angle_) < 1e-6) + ? min_angle_ + : max_angle_; + current_target_angle_ = active_suspension_base_angle_; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(current_target_angle_); + } else { + current_target_angle_ = + (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(current_target_angle_); + } + } else if (remote_front_back_posture_toggle_condition) { + toggle_front_back_posture_target_(); + } else if (front_high_rear_low) { + apply_front_high_rear_low_target_(); + } else if (front_low_rear_high) { + apply_front_low_rear_high_target_(); + } + + last_rotary_knob_ = rotary_knob; + } + + bool rotary_knob_down_edge_(double rotary_knob) const { + constexpr double rotary_knob_edge_threshold = 0.7; + return last_rotary_knob_ < rotary_knob_edge_threshold + && rotary_knob >= rotary_knob_edge_threshold; + } + + bool rotary_knob_up_edge_(double rotary_knob) const { + constexpr double rotary_knob_edge_threshold = 0.7; + return last_rotary_knob_ > -rotary_knob_edge_threshold + && rotary_knob <= -rotary_knob_edge_threshold; + } + + void update_joint_posture_state_(bool low_prone_active) { + std::array effective_joint_posture_target_deg = + joint_current_target_angle_; + if (low_prone_active) + effective_joint_posture_target_deg.fill(min_angle_ - 5.0); + + joint_posture_state_.joint_posture_target_deg = effective_joint_posture_target_deg; + joint_posture_state_.symmetric_posture_target = + symmetric_joint_target_requested_(effective_joint_posture_target_deg); + + if (joint_posture_state_.suspension_active) { + joint_posture_state_.suspension_reference_angle_deg = + low_prone_active ? min_angle_ : active_suspension_base_angle_; + return; + } + + if (joint_posture_state_.symmetric_posture_target) { + joint_posture_state_.suspension_reference_angle_deg = + effective_joint_posture_target_deg.front(); + return; + } + + double posture_angle_sum = 0.0; + for (double angle_deg : effective_joint_posture_target_deg) + posture_angle_sum += angle_deg; + joint_posture_state_.suspension_reference_angle_deg = + posture_angle_sum / static_cast(kJointCount); + } + + JointPostureState joint_posture_state_; + + double min_angle_; + double max_angle_; + double active_suspension_base_angle_; + bool suspension_enable_; + + double current_target_angle_; + std::array joint_current_target_angle_; + bool apply_symmetric_target_ = true; + bool suspension_enabled_by_toggle_ = false; + bool low_prone_enabled_by_toggle_ = false; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + double last_rotary_knob_ = 0.0; +}; + +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp index ec5922b8..74bc3d6b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp @@ -1,11 +1,13 @@ #include #include +#include #include #include #include #include +#include #include #include #include @@ -41,22 +43,17 @@ class DeformableOmniWheelController register_input("/chassis/left_front_wheel/max_torque", wheel_motor_max_control_torque_); - register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); + for (size_t i = 0; i < kWheelCount; ++i) { + register_input( + fmt::format("/chassis/{}_wheel/velocity", kWheelName[i]), wheel_velocity_[i]); + register_output( + fmt::format("/chassis/{}_wheel/control_torque", kWheelName[i]), + wheel_control_torque_[i], nan_); + } register_input("/chassis/control_velocity", chassis_control_velocity_); register_input("/chassis/control_power_limit", power_limit_); register_input("/chassis/radius", chassis_radius_); - - register_output( - "/chassis/left_front_wheel/control_torque", left_front_control_torque_, nan_); - register_output("/chassis/left_back_wheel/control_torque", left_back_control_torque_, nan_); - register_output( - "/chassis/right_back_wheel/control_torque", right_back_control_torque_, nan_); - register_output( - "/chassis/right_front_wheel/control_torque", right_front_control_torque_, nan_); } void before_updating() override { @@ -76,9 +73,9 @@ class DeformableOmniWheelController return; } - Eigen::Vector4d wheel_velocities = { - *left_front_velocity_, *left_back_velocity_, *right_back_velocity_, - *right_front_velocity_}; + Eigen::Vector4d wheel_velocities; + for (size_t i = 0; i < kWheelCount; ++i) + wheel_velocities[i] = *wheel_velocity_[i]; const auto chassis_velocity = calculate_chassis_velocity(wheel_velocities); auto chassis_control_torque = calculate_chassis_control_torque(chassis_velocity); @@ -89,28 +86,34 @@ class DeformableOmniWheelController const auto wheel_control_torques = calculate_wheel_control_torques(chassis_control_torque, wheel_pid_torques); - *left_front_control_torque_ = wheel_control_torques[0]; - *left_back_control_torque_ = wheel_control_torques[1]; - *right_back_control_torque_ = wheel_control_torques[2]; - *right_front_control_torque_ = wheel_control_torques[3]; + for (size_t i = 0; i < kWheelCount; ++i) + *wheel_control_torque_[i] = wheel_control_torques[i]; } private: + static constexpr size_t kWheelCount = 4; + static constexpr const char* kWheelName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double g_ = 9.81; + struct ChassisControlTorque { Eigen::Vector2d torque; Eigen::Vector2d lambda; }; void reset_all_controls() { - *left_front_control_torque_ = 0.0; - *left_back_control_torque_ = 0.0; - *right_back_control_torque_ = 0.0; - *right_front_control_torque_ = 0.0; + for (size_t i = 0; i < kWheelCount; ++i) + *wheel_control_torque_[i] = 0.0; } Eigen::Vector3d calculate_chassis_velocity(const Eigen::Vector4d& wheel_velocities) const { const auto& [w1, w2, w3, w4] = wheel_velocities; - const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); + const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); Eigen::Vector3d velocity; velocity.x() = -w1 - w2 + w3 + w4; velocity.y() = w1 - w2 - w3 + w4; @@ -122,23 +125,24 @@ class DeformableOmniWheelController ChassisControlTorque calculate_chassis_control_torque(const Eigen::Vector3d& chassis_velocity) { ChassisControlTorque result; - Eigen::Vector3d err = chassis_control_velocity_->vector - chassis_velocity; + Eigen::Vector3d chassis_velocity_error = + chassis_control_velocity_->vector - chassis_velocity; Eigen::Vector2d translational_torque = (-std::numbers::sqrt2 / 4 * wheel_radius_) * mass_ - * translational_velocity_pid_calculator_.update(err.head<2>()); + * translational_velocity_pid_calculator_.update(chassis_velocity_error.head<2>()); result.torque.x() = translational_torque.norm(); const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); - result.torque.y() = (-std::numbers::sqrt2 / 4 * wheel_radius_) - * (moment_of_inertia_ / a_plus_b) - * angular_velocity_pid_calculator_.update(err[2]); + result.torque.y() = (-std::numbers::sqrt2 / 4 * wheel_radius_) + * (moment_of_inertia_ / a_plus_b) + * angular_velocity_pid_calculator_.update(chassis_velocity_error[2]); Eigen::Vector2d translational_torque_direction; if (result.torque.x() > 0) translational_torque_direction = translational_torque / result.torque.x(); else translational_torque_direction = Eigen::Vector2d::UnitX(); - auto& [x, y] = translational_torque_direction; + auto& [x, y] = translational_torque_direction; result.lambda = {-x + y, -x - y}; return result; @@ -163,13 +167,13 @@ class DeformableOmniWheelController const Eigen::Vector4d& wheel_pid_torques) const { const auto& [w1, w2, w3, w4] = wheel_velocities; - const auto& [x_max, y_max] = chassis_control_torque.torque; - const double y_sign = y_max > 0 ? 1.0 : -1.0; + const auto& [x_max, y_max] = chassis_control_torque.torque; + const double y_sign = y_max > 0 ? 1.0 : -1.0; const auto& [lambda_1, lambda_2] = chassis_control_torque.lambda; const auto& [t1, t2, t3, t4] = wheel_pid_torques; - const double rhombus_top = (friction_coefficient_ * mass_ * g_ * wheel_radius_) / 4; + const double rhombus_top = (friction_coefficient_ * mass_ * g_ * wheel_radius_) / 4; const double rhombus_right = rhombus_top / std::max(std::abs(lambda_1), std::abs(lambda_2)); const double a = 4 * k1_; @@ -185,14 +189,14 @@ class DeformableOmniWheelController Eigen::Vector2d result = Eigen::Vector2d::Constant(nan_); if (com_height_ > 1e-6) { - const double dir_x = -(lambda_1 + lambda_2) / 2.0; - const double dir_y = (lambda_1 - lambda_2) / 2.0; - const double coeff = -com_height_ / (std::numbers::sqrt2 * wheel_radius_); + const double dir_x = -(lambda_1 + lambda_2) / 2.0; + const double dir_y = (lambda_1 - lambda_2) / 2.0; + const double coeff = -com_height_ / (std::numbers::sqrt2 * wheel_radius_); const double gamma_1 = coeff * (+dir_x / chassis_radius_x_ + dir_y / chassis_radius_y_); const double gamma_2 = coeff * (-dir_x / chassis_radius_x_ + dir_y / chassis_radius_y_); const double force_to_torque = friction_coefficient_ * wheel_radius_; - const double rhs = force_to_torque * mass_ * g_ / 4.0; + const double rhs = force_to_torque * mass_ * g_ / 4.0; const std::vector half_planes = { {lambda_1 - force_to_torque * gamma_1, y_sign, rhs}, {-lambda_1 - force_to_torque * gamma_1, -y_sign, rhs}, @@ -219,7 +223,7 @@ class DeformableOmniWheelController static Eigen::Vector4d calculate_wheel_control_torques( ChassisControlTorque chassis_control_torque, Eigen::Vector4d wheel_pid_torques) { const auto& [lambda_1, lambda_2] = chassis_control_torque.lambda; - Eigen::Vector4d wheel_torques = { + Eigen::Vector4d wheel_torques = { +lambda_1 * chassis_control_torque.torque.x(), +lambda_2 * chassis_control_torque.torque.x(), -lambda_1 * chassis_control_torque.torque.x(), @@ -230,10 +234,6 @@ class DeformableOmniWheelController return wheel_torques; } - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - static constexpr double g_ = 9.81; - const double mass_; const double moment_of_inertia_; const double wheel_radius_; @@ -245,10 +245,8 @@ class DeformableOmniWheelController InputInterface wheel_motor_max_control_torque_; - InputInterface left_front_velocity_; - InputInterface left_back_velocity_; - InputInterface right_back_velocity_; - InputInterface right_front_velocity_; + std::array, kWheelCount> wheel_velocity_; + std::array, kWheelCount> wheel_control_torque_; InputInterface chassis_control_velocity_; InputInterface power_limit_; @@ -260,11 +258,6 @@ class DeformableOmniWheelController pid::MatrixPidCalculator<4> wheel_velocity_pid_; QcpSolver qcp_solver_; - - OutputInterface left_front_control_torque_; - OutputInterface left_back_control_torque_; - OutputInterface right_back_control_torque_; - OutputInterface right_front_control_torque_; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp new file mode 100644 index 00000000..3f562ba0 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -0,0 +1,628 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "controller/pid/pid_calculator.hpp" +#include "filter/low_pass_filter.hpp" + +namespace rmcs_core::controller::chassis { + +class DeformableSuspension + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableSuspension() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + load_config_(); + + register_input("/predefined/update_rate", update_rate_, false); + + register_input("/chassis/active_suspension/active", active_suspension_active_); + register_input("/chassis/deformable/reset_count", reset_count_, false); + register_input("/chassis/deformable/low_prone_active", low_prone_active_); + register_input( + "/chassis/deformable/symmetric_posture_target", symmetric_posture_target_); + register_input("/chassis/deformable/correction_inverted", correction_inverted_); + register_input("/chassis/deformable/min_angle_deg", min_angle_deg_); + register_input("/chassis/deformable/max_angle_deg", max_angle_deg_); + register_input( + "/chassis/deformable/suspension_reference_angle_deg", + suspension_reference_angle_deg_); + + register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); + register_input("/chassis/imu/roll", chassis_imu_roll_, false); + register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); + register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); + + for (size_t i = 0; i < kJointCount; ++i) { + register_input( + std::string{"/chassis/deformable/"} + kJointName[i] + "_joint/posture_target_angle", + joint_posture_target_angle_rad_[i]); + register_input( + std::string{"/chassis/"} + kJointName[i] + "_joint/physical_angle", + joint_physical_angle_[i], false); + register_output( + std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_angle", + joint_target_angle_[i], nan_); + register_output( + std::string{"/chassis/"} + kJointName[i] + + "_joint/target_physical_velocity", + joint_target_velocity_[i], nan_); + register_output( + std::string{"/chassis/"} + kJointName[i] + + "_joint/target_physical_acceleration", + joint_target_acceleration_[i], nan_); + register_output( + std::string{"/chassis/"} + kJointName[i] + "_joint/control_angle_error", + joint_angle_error_[i], nan_); + } + } + + void before_updating() override { + if (!update_rate_.ready()) + update_rate_.make_and_bind_directly(1000.0); + if (!reset_count_.ready()) + reset_count_.make_and_bind_directly(static_cast(0)); + if (!chassis_imu_pitch_.ready()) + chassis_imu_pitch_.make_and_bind_directly(0.0); + if (!chassis_imu_roll_.ready()) + chassis_imu_roll_.make_and_bind_directly(0.0); + if (!chassis_imu_pitch_rate_.ready()) + chassis_imu_pitch_rate_.make_and_bind_directly(0.0); + if (!chassis_imu_roll_rate_.ready()) + chassis_imu_roll_rate_.make_and_bind_directly(0.0); + + configure_active_rate_filters_(1.0 / update_dt_()); + validate_joint_feedback_inputs_(); + reset_all_controls_(); + last_reset_count_ = *reset_count_; + } + + void update() override { + if (*reset_count_ != last_reset_count_) { + reset_all_controls_(); + last_reset_count_ = *reset_count_; + return; + } + + const auto current_physical_angles = read_feedback_(); + + if (!init_joint_targets_from_feedback_(current_physical_angles)) { + publish_nan_joint_targets_(); + return; + } + + const auto posture_target_angles_rad = read_posture_target_angles_rad_(); + const auto dt = update_dt_(); + + double filtered_pitch_rate = *chassis_imu_pitch_rate_; + double filtered_roll_rate = *chassis_imu_roll_rate_; + filter_attitude_rates_(filtered_pitch_rate, filtered_roll_rate); + + if (*active_suspension_active_) + calibrate_(*chassis_imu_pitch_, *chassis_imu_roll_, *symmetric_posture_target_, dt); + + std::array joint_angle_states{}; + copy_joint_angle_states_(joint_angle_states); + update_suspension_state_( + *chassis_imu_pitch_ - pitch_offset_value_, *chassis_imu_roll_ - roll_offset_value_, + filtered_pitch_rate, filtered_roll_rate, *active_suspension_active_, + *low_prone_active_, *min_angle_deg_, *max_angle_deg_, + *suspension_reference_angle_deg_, *correction_inverted_, joint_angle_states, dt); + + const auto target_angles_rad = compute_joint_trajectory_targets_( + posture_target_angles_rad, *active_suspension_active_, *low_prone_active_, + *min_angle_deg_, *suspension_reference_angle_deg_); + + run_joint_trajectory_(target_angles_rad, *active_suspension_active_, dt); + publish_joint_targets_(current_physical_angles); + } + +private: + static constexpr size_t kJointCount = 4; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double offset_limit_rad_ = 1.0 * std::numbers::pi / 180.0; + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; + + static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } + + void validate_joint_feedback_inputs_() const { + for (size_t i = 0; i < kJointCount; ++i) + if (!joint_physical_angle_[i].ready()) + throw std::runtime_error( + "missing deformable chassis feedback interfaces: expected " + "/chassis/*_joint/physical_angle"); + } + + double update_dt_() const { + if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) + return 1.0 / *update_rate_; + return 1e-3; + } + + void load_pid_( + const std::string& prefix, pid::PidCalculator& pid, double kp_default, + double ki_default, double kd_default, double integral_min_default, + double integral_max_default, double output_min_default, double output_max_default) { + pid.kp = get_parameter_or(prefix + "kp", kp_default); + pid.ki = get_parameter_or(prefix + "ki", ki_default); + pid.kd = get_parameter_or(prefix + "kd", kd_default); + pid.integral_min = get_parameter_or(prefix + "integral_min", integral_min_default); + pid.integral_max = get_parameter_or(prefix + "integral_max", integral_max_default); + pid.output_min = get_parameter_or(prefix + "output_min", output_min_default); + pid.output_max = get_parameter_or(prefix + "output_max", output_max_default); + } + + void load_config_() { + joint_target_vel_limit_ = std::max( + deg_to_rad_(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), + 1e-6); + joint_target_acc_limit_ = std::max( + deg_to_rad_(std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0))), + 1e-6); + suspension_target_vel_limit_ = std::max( + deg_to_rad_(std::abs(get_parameter_or( + "active_suspension_target_velocity_limit_deg", + get_parameter_or("target_physical_velocity_limit", 180.0)))), + 1e-6); + suspension_target_acc_limit_ = std::max( + deg_to_rad_(std::abs(get_parameter_or( + "active_suspension_target_acceleration_limit_deg", + get_parameter_or("target_physical_acceleration_limit", 720.0)))), + 1e-6); + + load_pid_( + "active_suspension_pitch_outer_", pitch_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, + -3.0, 3.0); + load_pid_( + "active_suspension_pitch_inner_", pitch_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, + -0.785, 0.785); + load_pid_( + "active_suspension_roll_outer_", roll_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, + -3.0, 3.0); + load_pid_( + "active_suspension_roll_inner_", roll_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, + -0.785, 0.785); + + active_correction_vel_limit_ = std::max( + deg_to_rad_(std::abs( + get_parameter_or("active_suspension_correction_velocity_limit_deg", 720.0))), + 1e-6); + active_correction_acc_limit_ = std::max( + deg_to_rad_(std::abs( + get_parameter_or("active_suspension_correction_acceleration_limit_deg", 3600.0))), + 1e-6); + active_rate_lpf_cutoff_hz_ = std::max( + get_parameter_or("active_suspension_rate_lpf_cutoff_hz", 10.0), 1e-6); + + calibration_wait_time_ = std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); + calibration_sample_time_ = + std::max(get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); + } + + std::array read_feedback_() const { + std::array angles; + angles.fill(nan_); + + for (size_t i = 0; i < kJointCount; ++i) + if (joint_physical_angle_[i].ready() && std::isfinite(*joint_physical_angle_[i])) + angles[i] = *joint_physical_angle_[i]; + + return angles; + } + + std::array read_posture_target_angles_rad_() const { + std::array targets{}; + for (size_t i = 0; i < kJointCount; ++i) + targets[i] = *joint_posture_target_angle_rad_[i]; + return targets; + } + + std::array compute_joint_trajectory_targets_( + const std::array& posture_target_angles_rad, bool suspension_active, + bool low_prone_active, double min_angle_deg, double suspension_reference_angle_deg) const { + if (!suspension_active) + return posture_target_angles_rad; + + std::array target_angles_rad{}; + double target_angle_rad = low_prone_active ? deg_to_rad_(min_angle_deg - 5.0) + : deg_to_rad_(suspension_reference_angle_deg); + target_angles_rad.fill(target_angle_rad); + return target_angles_rad; + } + + void reset_attitude_() { + pitch_outer_pid_.reset(); + pitch_inner_pid_.reset(); + roll_outer_pid_.reset(); + roll_inner_pid_.reset(); + correction_target_rad_.fill(0.0); + } + + void reset_calibration_window_() { + calibration_hold_elapsed_ = 0.0; + sample_count_ = 0; + pitch_sum_ = 0.0; + roll_sum_ = 0.0; + calibration_completed_for_window_ = false; + } + + void reset_all_controls_() { + reset_attitude_(); + pitch_rate_filter_.reset(); + roll_rate_filter_.reset(); + correction_state_rad_.fill(0.0); + correction_velocity_state_rad_.fill(0.0); + correction_acceleration_state_rad_.fill(0.0); + joint_target_active_.fill(false); + joint_target_angle_state_rad_.fill(nan_); + joint_target_velocity_state_rad_.fill(0.0); + joint_target_acceleration_state_rad_.fill(0.0); + reset_calibration_window_(); + calibrated_once_ = false; + pitch_offset_value_ = 0.0; + roll_offset_value_ = 0.0; + + for (size_t i = 0; i < kJointCount; ++i) { + *joint_target_angle_[i] = nan_; + *joint_target_velocity_[i] = nan_; + *joint_target_acceleration_[i] = nan_; + *joint_angle_error_[i] = nan_; + } + } + + void calibrate_(double pitch, double roll, bool symmetric_target, double dt) { + if (calibrated_once_) + return; + + if (!symmetric_target) { + reset_calibration_window_(); + return; + } + + if (!std::isfinite(pitch) || !std::isfinite(roll)) + return; + + calibration_hold_elapsed_ += dt; + if (calibration_hold_elapsed_ < calibration_wait_time_) + return; + + const double calibration_end = calibration_wait_time_ + calibration_sample_time_; + if (calibration_hold_elapsed_ < calibration_end) { + pitch_sum_ += pitch; + roll_sum_ += roll; + ++sample_count_; + return; + } + + if (calibration_completed_for_window_) + return; + + calibration_completed_for_window_ = true; + if (sample_count_ == 0) + return; + + pitch_offset_value_ = std::clamp( + pitch_sum_ / static_cast(sample_count_), -offset_limit_rad_, offset_limit_rad_); + roll_offset_value_ = std::clamp( + roll_sum_ / static_cast(sample_count_), -offset_limit_rad_, offset_limit_rad_); + calibrated_once_ = true; + } + + bool init_joint_targets_from_feedback_(const std::array& physical_angles) { + bool any_active_value = false; + for (size_t i = 0; i < kJointCount; ++i) { + if (std::isfinite(physical_angles[i]) && !joint_target_active_[i]) { + joint_target_angle_state_rad_[i] = physical_angles[i]; + joint_target_velocity_state_rad_[i] = 0.0; + joint_target_acceleration_state_rad_[i] = 0.0; + joint_target_active_[i] = true; + } + any_active_value = any_active_value || joint_target_active_[i]; + } + return any_active_value; + } + + void configure_active_rate_filters_(double sampling_frequency) { + const double clamped_sampling_frequency = std::max(sampling_frequency, 1e-6); + if (std::abs(active_rate_filter_sampling_hz_ - clamped_sampling_frequency) < 1e-6) + return; + + pitch_rate_filter_.set_cutoff(active_rate_lpf_cutoff_hz_, clamped_sampling_frequency); + roll_rate_filter_.set_cutoff(active_rate_lpf_cutoff_hz_, clamped_sampling_frequency); + active_rate_filter_sampling_hz_ = clamped_sampling_frequency; + } + + void filter_attitude_rates_(double& pitch_rate, double& roll_rate) { + if (std::isfinite(pitch_rate)) + pitch_rate = pitch_rate_filter_.update(pitch_rate); + if (std::isfinite(roll_rate)) + roll_rate = roll_rate_filter_.update(roll_rate); + } + + void compute_correction_targets_(double pitch_diff, double roll_diff, bool inverted) { + if (inverted) { + const double front_pitch_contribution = std::max(pitch_diff, 0.0); + const double back_pitch_contribution = std::max(-pitch_diff, 0.0); + const double left_roll_contribution = std::max(-roll_diff, 0.0); + const double right_roll_contribution = std::max(roll_diff, 0.0); + correction_target_rad_[kLeftFront] = + -(front_pitch_contribution + left_roll_contribution); + correction_target_rad_[kLeftBack] = + -(back_pitch_contribution + left_roll_contribution); + correction_target_rad_[kRightBack] = + -(back_pitch_contribution + right_roll_contribution); + correction_target_rad_[kRightFront] = + -(front_pitch_contribution + right_roll_contribution); + } else { + const double front_pitch_contribution = std::max(-pitch_diff, 0.0); + const double back_pitch_contribution = std::max(pitch_diff, 0.0); + const double left_roll_contribution = std::max(roll_diff, 0.0); + const double right_roll_contribution = std::max(-roll_diff, 0.0); + correction_target_rad_[kLeftFront] = front_pitch_contribution + left_roll_contribution; + correction_target_rad_[kLeftBack] = back_pitch_contribution + left_roll_contribution; + correction_target_rad_[kRightBack] = back_pitch_contribution + right_roll_contribution; + correction_target_rad_[kRightFront] = front_pitch_contribution + right_roll_contribution; + } + } + + void run_correction_trajectory_( + bool low_prone_override_active, double min_angle_deg, double max_angle_deg, + double base_angle_deg, const std::array& base_joint_angles, + double correction_vel_limit, double correction_acc_limit, double dt) { + const double max_target_rad = deg_to_rad_(max_angle_deg); + const double min_susp_rad = deg_to_rad_(min_angle_deg - 5.0); + + for (size_t i = 0; i < kJointCount; ++i) { + const double base_angle = std::isfinite(base_joint_angles[i]) + ? base_joint_angles[i] + : (low_prone_override_active ? min_susp_rad + : deg_to_rad_(base_angle_deg)); + + const double correction_min = min_susp_rad - base_angle; + const double correction_max = max_target_rad - base_angle; + const double target = + std::clamp(correction_target_rad_[i], correction_min, correction_max); + + double& angle_state = correction_state_rad_[i]; + double& velocity_state = correction_velocity_state_rad_[i]; + double& acceleration_state = correction_acceleration_state_rad_[i]; + + const double position_error = target - angle_state; + const double stopping_distance = + velocity_state * velocity_state / (2.0 * correction_acc_limit); + + double desired_velocity = 0.0; + if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) + desired_velocity = std::copysign(correction_vel_limit, position_error); + + const double velocity_error = desired_velocity - velocity_state; + acceleration_state = + std::clamp(velocity_error / dt, -correction_acc_limit, correction_acc_limit); + + velocity_state += acceleration_state * dt; + velocity_state = std::clamp(velocity_state, -correction_vel_limit, correction_vel_limit); + angle_state += velocity_state * dt; + + const double next_error = target - angle_state; + if ((position_error > 0.0 && next_error < 0.0) + || (position_error < 0.0 && next_error > 0.0) + || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { + angle_state = target; + velocity_state = 0.0; + acceleration_state = 0.0; + } + } + } + + void update_suspension_state_( + double pitch, double roll, double pitch_rate, double roll_rate, bool suspension_active, + bool low_prone_override_active, double min_angle_deg, double max_angle_deg, + double base_angle_deg, bool correction_inverted, + const std::array& base_joint_angles, double dt) { + if (!suspension_active) { + reset_attitude_(); + run_correction_trajectory_( + low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, + base_joint_angles, active_correction_vel_limit_, active_correction_acc_limit_, dt); + return; + } + + constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; + const double clamped_pitch = std::clamp(pitch, -max_attitude, max_attitude); + const double clamped_roll = std::clamp(roll, -max_attitude, max_attitude); + + const double pitch_outer = pitch_outer_pid_.update(-clamped_pitch); + const double roll_outer = roll_outer_pid_.update(clamped_roll); + const double pitch_diff = pitch_inner_pid_.update(pitch_outer - pitch_rate); + const double roll_diff = roll_inner_pid_.update(roll_outer + roll_rate); + + if (!std::isfinite(pitch_diff) || !std::isfinite(roll_diff)) { + reset_attitude_(); + return; + } + + compute_correction_targets_(pitch_diff, roll_diff, correction_inverted); + run_correction_trajectory_( + low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, + base_joint_angles, active_correction_vel_limit_, active_correction_acc_limit_, dt); + } + + void run_joint_trajectory_( + const std::array& target_angles_rad, bool suspension_active, + double dt) { + for (size_t i = 0; i < kJointCount; ++i) { + if (!joint_target_active_[i]) + continue; + + double& angle_state = joint_target_angle_state_rad_[i]; + double& velocity_state = joint_target_velocity_state_rad_[i]; + double& acceleration_state = joint_target_acceleration_state_rad_[i]; + const double target = target_angles_rad[i]; + + const double vel_limit = + suspension_active ? suspension_target_vel_limit_ : joint_target_vel_limit_; + const double acc_limit = + suspension_active ? suspension_target_acc_limit_ : joint_target_acc_limit_; + + if (!std::isfinite(target) || !std::isfinite(angle_state)) + continue; + + const double position_error = target - angle_state; + const double stopping_distance = velocity_state * velocity_state / (2.0 * acc_limit); + + double desired_velocity = 0.0; + if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) + desired_velocity = std::copysign(vel_limit, position_error); + + const double velocity_error = desired_velocity - velocity_state; + acceleration_state = std::clamp(velocity_error / dt, -acc_limit, acc_limit); + + velocity_state += acceleration_state * dt; + velocity_state = std::clamp(velocity_state, -vel_limit, vel_limit); + angle_state += velocity_state * dt; + + const double next_error = target - angle_state; + if ((position_error > 0.0 && next_error < 0.0) + || (position_error < 0.0 && next_error > 0.0) + || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { + angle_state = target; + velocity_state = 0.0; + acceleration_state = 0.0; + } + } + } + + bool any_joint_target_active_() const { + for (size_t i = 0; i < kJointCount; ++i) + if (joint_target_active_[i]) + return true; + return false; + } + + void copy_joint_angle_states_(std::array& out) const { + out = joint_target_angle_state_rad_; + } + + void publish_joint_targets_(const std::array& feedback_angles) { + const double min_angle_rad = deg_to_rad_(*min_angle_deg_ - 5.0); + const double max_angle_rad = deg_to_rad_(*max_angle_deg_); + + if (!any_joint_target_active_()) { + publish_nan_joint_targets_(); + return; + } + + for (size_t i = 0; i < kJointCount; ++i) { + if (!joint_target_active_[i]) { + *joint_target_angle_[i] = nan_; + *joint_target_velocity_[i] = nan_; + *joint_target_acceleration_[i] = nan_; + *joint_angle_error_[i] = nan_; + continue; + } + + const double target = joint_target_angle_state_rad_[i] + correction_state_rad_[i]; + *joint_target_angle_[i] = std::clamp(target, min_angle_rad, max_angle_rad); + *joint_target_velocity_[i] = + joint_target_velocity_state_rad_[i] + correction_velocity_state_rad_[i]; + *joint_target_acceleration_[i] = + joint_target_acceleration_state_rad_[i] + correction_acceleration_state_rad_[i]; + *joint_angle_error_[i] = std::isfinite(feedback_angles[i]) + ? feedback_angles[i] - *joint_target_angle_[i] + : nan_; + } + } + + void publish_nan_joint_targets_() { + reset_all_controls_(); + } + + InputInterface update_rate_; + + InputInterface active_suspension_active_; + InputInterface reset_count_; + InputInterface low_prone_active_; + InputInterface symmetric_posture_target_; + InputInterface correction_inverted_; + InputInterface min_angle_deg_; + InputInterface max_angle_deg_; + InputInterface suspension_reference_angle_deg_; + + InputInterface chassis_imu_pitch_; + InputInterface chassis_imu_roll_; + InputInterface chassis_imu_pitch_rate_; + InputInterface chassis_imu_roll_rate_; + + std::array, kJointCount> joint_posture_target_angle_rad_; + std::array, kJointCount> joint_physical_angle_; + + std::array, kJointCount> joint_target_angle_; + std::array, kJointCount> joint_target_velocity_; + std::array, kJointCount> joint_target_acceleration_; + std::array, kJointCount> joint_angle_error_; + + pid::PidCalculator pitch_outer_pid_{}; + pid::PidCalculator pitch_inner_pid_{}; + pid::PidCalculator roll_outer_pid_{}; + pid::PidCalculator roll_inner_pid_{}; + filter::LowPassFilter<1> pitch_rate_filter_{1.0}; + filter::LowPassFilter<1> roll_rate_filter_{1.0}; + + double active_correction_vel_limit_ = 40.0; + double active_correction_acc_limit_ = 200.0; + double active_rate_lpf_cutoff_hz_ = 10.0; + double active_rate_filter_sampling_hz_ = 0.0; + + double calibration_wait_time_ = 2.0; + double calibration_sample_time_ = 3.0; + double calibration_hold_elapsed_ = 0.0; + size_t sample_count_ = 0; + double pitch_sum_ = 0.0; + double roll_sum_ = 0.0; + bool calibration_completed_for_window_ = false; + bool calibrated_once_ = false; + double pitch_offset_value_ = 0.0; + double roll_offset_value_ = 0.0; + + std::array correction_target_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + + std::array joint_target_active_ = {false, false, false, false}; + std::array joint_target_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array joint_target_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array joint_target_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + + double joint_target_vel_limit_ = 0.0; + double joint_target_acc_limit_ = 0.0; + double suspension_target_vel_limit_ = 0.0; + double suspension_target_acc_limit_ = 0.0; + size_t last_reset_count_ = 0; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::DeformableSuspension, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp deleted file mode 100644 index e25d9cdb..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp +++ /dev/null @@ -1,873 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include "controller/chassis/qcp_solver.hpp" -#include "controller/pid/matrix_pid_calculator.hpp" -#include "controller/pid/pid_calculator.hpp" -#include "filter/low_pass_filter.hpp" - -namespace rmcs_core::controller::chassis { - -class DeformableChassisController - : public rmcs_executor::Component - , public rclcpp::Node { - - enum class WheelIndex : size_t { - LeftFront = 0, - LeftBack = 1, - RightBack = 2, - RightFront = 3, - Count = 4 - }; - - static constexpr size_t kWheelCount = static_cast(WheelIndex::Count); - - struct EllipseParameters { - double a, b, c, d, e, f; - }; - -public: - explicit DeformableChassisController() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , mass_(get_parameter("mass").as_double()) - , moment_of_inertia_(get_parameter("moment_of_inertia").as_double()) - , chassis_radius_(get_parameter("chassis_radius").as_double()) - , rod_length_(get_parameter("rod_length").as_double()) - , wheel_radius_(get_parameter("wheel_radius").as_double()) - , friction_coefficient_(get_parameter("friction_coefficient").as_double()) - , k1_(get_parameter("k1").as_double()) - , k2_(get_parameter("k2").as_double()) - , no_load_power_(get_parameter("no_load_power").as_double()) - , ellipse_coeff_quadratic_translational_( - k1_ * mass_ * mass_ * wheel_radius_ * wheel_radius_ / 16.0) - , ellipse_coeff_cross_term_( - k1_ * mass_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_ / 8.0) - , ellipse_coeff_quadratic_angular_( - k1_ * moment_of_inertia_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_ / 16.0) - , ellipse_coeff_linear_translational_(mass_ * wheel_radius_ / 4.0) - , ellipse_coeff_linear_angular_(moment_of_inertia_ * wheel_radius_ / 4.0) - , vehicle_radius_(Eigen::Vector4d::Constant(chassis_radius_ + rod_length_)) - , control_acceleration_filter_(5.0, 1000.0) - , chassis_velocity_expected_(Eigen::Vector3d::Zero()) - , chassis_translational_velocity_pid_(5.0, 0.0, 1.0) - , chassis_angular_velocity_pid_(5.0, 0.0, 1.0) - , steering_velocity_pid_(0.15, 0.0, 0.0) - , steering_angle_pid_(30.0, 0.0, 0.0) - , wheel_velocity_pid_(0.6, 0.0, 0.0) { - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - - register_input("/chassis/left_front_steering/angle", left_front_steering_angle_); - register_input("/chassis/left_back_steering/angle", left_back_steering_angle_); - register_input("/chassis/right_back_steering/angle", right_back_steering_angle_); - register_input("/chassis/right_front_steering/angle", right_front_steering_angle_); - - register_input("/chassis/left_front_steering/velocity", left_front_steering_velocity_); - register_input("/chassis/left_back_steering/velocity", left_back_steering_velocity_); - register_input("/chassis/right_back_steering/velocity", right_back_steering_velocity_); - register_input("/chassis/right_front_steering/velocity", right_front_steering_velocity_); - - register_input("/chassis/left_front_wheel/velocity", left_front_wheel_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_wheel_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_wheel_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_wheel_velocity_); - - register_input("/chassis/left_front_joint/physical_angle", left_front_joint_angle_); - register_input("/chassis/left_back_joint/physical_angle", left_back_joint_angle_); - register_input("/chassis/right_back_joint/physical_angle", right_back_joint_angle_); - register_input("/chassis/right_front_joint/physical_angle", right_front_joint_angle_); - - register_input("/chassis/left_front_joint/physical_velocity", left_front_joint_velocity_); - register_input("/chassis/left_back_joint/physical_velocity", left_back_joint_velocity_); - register_input("/chassis/right_back_joint/physical_velocity", right_back_joint_velocity_); - register_input("/chassis/right_front_joint/physical_velocity", right_front_joint_velocity_); - - register_input( - "/chassis/left_front_joint/target_physical_angle", - left_front_joint_target_physical_angle_, false); - register_input( - "/chassis/left_back_joint/target_physical_angle", - left_back_joint_target_physical_angle_, false); - register_input( - "/chassis/right_back_joint/target_physical_angle", - right_back_joint_target_physical_angle_, false); - register_input( - "/chassis/right_front_joint/target_physical_angle", - right_front_joint_target_physical_angle_, false); - register_input( - "/chassis/left_front_joint/target_physical_velocity", - left_front_joint_target_physical_velocity_, false); - register_input( - "/chassis/left_back_joint/target_physical_velocity", - left_back_joint_target_physical_velocity_, false); - register_input( - "/chassis/right_back_joint/target_physical_velocity", - right_back_joint_target_physical_velocity_, false); - register_input( - "/chassis/right_front_joint/target_physical_velocity", - right_front_joint_target_physical_velocity_, false); - register_input( - "/chassis/left_front_joint/target_physical_acceleration", - left_front_joint_target_physical_acceleration_, false); - register_input( - "/chassis/left_back_joint/target_physical_acceleration", - left_back_joint_target_physical_acceleration_, false); - register_input( - "/chassis/right_back_joint/target_physical_acceleration", - right_back_joint_target_physical_acceleration_, false); - register_input( - "/chassis/right_front_joint/target_physical_acceleration", - right_front_joint_target_physical_acceleration_, false); - - register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); - register_input("/chassis/control_velocity", chassis_control_velocity_); - register_input("/chassis/control_power_limit", power_limit_); - - register_output( - "/chassis/left_front_steering/control_torque", left_front_steering_control_torque_); - register_output( - "/chassis/left_back_steering/control_torque", left_back_steering_control_torque_); - register_output( - "/chassis/right_back_steering/control_torque", right_back_steering_control_torque_); - register_output( - "/chassis/right_front_steering/control_torque", right_front_steering_control_torque_); - - register_output( - "/chassis/left_front_wheel/control_torque", left_front_wheel_control_torque_); - register_output("/chassis/left_back_wheel/control_torque", left_back_wheel_control_torque_); - register_output( - "/chassis/right_back_wheel/control_torque", right_back_wheel_control_torque_); - register_output( - "/chassis/right_front_wheel/control_torque", right_front_wheel_control_torque_); - } - - void update() override { - if (std::isnan(chassis_control_velocity_->vector[0])) { - reset_all_controls(); - return; - } - - const JointFeedbackStates joint_feedback = update_joint_feedback_states_(); - const JointTargetStates joint_target = update_joint_target_states_(); - if (joint_feedback.valid) { - vehicle_radius_ = joint_feedback.radius; - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 1000, - "physical joint angle[deg] lf=%.2f lb=%.2f rb=%.2f rf=%.2f, radius[m] lf=%.3f " - "lb=%.3f rb=%.3f rf=%.3f", - joint_feedback.alpha_rad[0] * 180.0 / std::numbers::pi, - joint_feedback.alpha_rad[1] * 180.0 / std::numbers::pi, - joint_feedback.alpha_rad[2] * 180.0 / std::numbers::pi, - joint_feedback.alpha_rad[3] * 180.0 / std::numbers::pi, vehicle_radius_[0], - vehicle_radius_[1], vehicle_radius_[2], vehicle_radius_[3]); - } - - integral_yaw_angle_imu(); - - const auto steering_status = calculate_steering_status(); - const auto wheel_velocities = calculate_wheel_velocities(); - const auto chassis_velocity = - calculate_chassis_velocity(steering_status, wheel_velocities, joint_feedback); - auto chassis_status_expected = - calculate_chassis_status_expected(chassis_velocity, joint_target, joint_feedback); - const auto chassis_control_velocity = calculate_chassis_control_velocity(); - const auto chassis_acceleration = calculate_chassis_control_acceleration( - chassis_status_expected.velocity, chassis_control_velocity); - const double power_limit = - *power_limit_ - no_load_power_ - k2_ * wheel_velocities.array().pow(2).sum(); - const auto wheel_pid_torques = - calculate_wheel_pid_torques(steering_status, wheel_velocities, chassis_status_expected); - const auto constrained_chassis_acceleration = constrain_chassis_control_acceleration( - steering_status, wheel_velocities, joint_target, chassis_acceleration, - wheel_pid_torques, power_limit); - const auto filtered_chassis_acceleration = - odom_to_base_link_vector(control_acceleration_filter_.update( - base_link_to_odom_vector(constrained_chassis_acceleration))); - const auto steering_torques = calculate_steering_control_torques( - steering_status, chassis_status_expected, joint_target, joint_feedback, - filtered_chassis_acceleration); - const auto wheel_torques = calculate_wheel_control_torques( - steering_status, joint_target, joint_feedback, filtered_chassis_acceleration, - wheel_pid_torques); - - update_control_torques(steering_torques, wheel_torques); - update_chassis_velocity_expected(filtered_chassis_acceleration); - } - -private: - struct SteeringStatus { - Eigen::Vector4d angle = Eigen::Vector4d::Zero(); - Eigen::Vector4d cos_angle = Eigen::Vector4d::Zero(); - Eigen::Vector4d sin_angle = Eigen::Vector4d::Zero(); - Eigen::Vector4d velocity = Eigen::Vector4d::Zero(); - Eigen::Vector4d sin_angle_minus_phi = Eigen::Vector4d::Zero(); - Eigen::Vector4d cos_angle_minus_phi = Eigen::Vector4d::Zero(); - }; - - struct ChassisStatus { - Eigen::Vector3d velocity = Eigen::Vector3d::Zero(); - Eigen::Vector4d wheel_velocity_x = Eigen::Vector4d::Zero(); - Eigen::Vector4d wheel_velocity_y = Eigen::Vector4d::Zero(); - }; - - struct JointStateData { - Eigen::Vector4d alpha_rad = Eigen::Vector4d::Zero(); - Eigen::Vector4d alpha_dot_rad = Eigen::Vector4d::Zero(); - Eigen::Vector4d alpha_ddot_rad = Eigen::Vector4d::Zero(); - Eigen::Vector4d radius = Eigen::Vector4d::Zero(); - Eigen::Vector4d radius_dot = Eigen::Vector4d::Zero(); - Eigen::Vector4d radius_ddot = Eigen::Vector4d::Zero(); - bool valid = false; - }; - - struct JointFeedbackStates : JointStateData {}; - - struct JointTargetStates : JointStateData { - bool has_velocity = false; - bool has_acceleration = false; - }; - - enum class JointStateSource : uint8_t { Target, Feedback }; - - struct JointStateView { - const Eigen::Vector4d& alpha_rad; - const Eigen::Vector4d& alpha_dot_rad; - const Eigen::Vector4d& alpha_ddot_rad; - const Eigen::Vector4d& radius; - const Eigen::Vector4d& radius_dot; - const Eigen::Vector4d& radius_ddot; - JointStateSource source; - bool valid; - }; - - static JointStateView - select_joint_state(const JointTargetStates& target, const JointFeedbackStates& feedback) { - if (target.valid) { - return { - target.alpha_rad, target.alpha_dot_rad, target.alpha_ddot_rad, target.radius, - target.radius_dot, target.radius_ddot, JointStateSource::Target, target.valid}; - } - - return {feedback.alpha_rad, feedback.alpha_dot_rad, - feedback.alpha_ddot_rad, feedback.radius, - feedback.radius_dot, feedback.radius_ddot, - JointStateSource::Feedback, feedback.valid}; - } - - [[nodiscard]] static Eigen::Vector4d read_required_inputs_( - const InputInterface& left_front, const InputInterface& left_back, - const InputInterface& right_back, const InputInterface& right_front) { - return {*left_front, *left_back, *right_back, *right_front}; - } - - [[nodiscard]] static Eigen::Vector4d read_optional_inputs_( - const InputInterface& left_front, const InputInterface& left_back, - const InputInterface& right_back, const InputInterface& right_front) { - return { - left_front.ready() ? *left_front : nan_, - left_back.ready() ? *left_back : nan_, - right_back.ready() ? *right_back : nan_, - right_front.ready() ? *right_front : nan_, - }; - } - - static void populate_joint_geometry_( - const Eigen::Vector4d& alpha_rad, const Eigen::Vector4d& alpha_dot_rad, - const Eigen::Vector4d& alpha_ddot_rad, double chassis_radius, double rod_length, - Eigen::Vector4d& radius, Eigen::Vector4d& radius_dot, Eigen::Vector4d& radius_ddot) { - radius = chassis_radius + rod_length * alpha_rad.array().cos(); - radius_dot = -rod_length * alpha_rad.array().sin() * alpha_dot_rad.array(); - radius_ddot = -rod_length * alpha_rad.array().cos() * alpha_dot_rad.array().square() - - rod_length * alpha_rad.array().sin() * alpha_ddot_rad.array(); - } - - [[nodiscard]] JointFeedbackStates update_joint_feedback_states_() { - JointFeedbackStates joint; - joint.alpha_rad = read_required_inputs_( - left_front_joint_angle_, left_back_joint_angle_, right_back_joint_angle_, - right_front_joint_angle_); - joint.alpha_dot_rad = read_required_inputs_( - left_front_joint_velocity_, left_back_joint_velocity_, right_back_joint_velocity_, - right_front_joint_velocity_); - - if (!joint.alpha_rad.array().isFinite().all() - || !joint.alpha_dot_rad.array().isFinite().all()) - return joint; - - if (last_joint_velocity_valid_) { - joint.alpha_ddot_rad = (joint.alpha_dot_rad - last_joint_velocity_) / dt_; - } - - last_joint_velocity_ = joint.alpha_dot_rad; - last_joint_velocity_valid_ = true; - - populate_joint_geometry_( - joint.alpha_rad, joint.alpha_dot_rad, joint.alpha_ddot_rad, chassis_radius_, - rod_length_, joint.radius, joint.radius_dot, joint.radius_ddot); - - joint.valid = joint.radius.array().isFinite().all() - && joint.radius_dot.array().isFinite().all() - && joint.radius_ddot.array().isFinite().all(); - - return joint; - } - - [[nodiscard]] JointTargetStates update_joint_target_states_() { - JointTargetStates joint; - joint.alpha_rad = read_optional_inputs_( - left_front_joint_target_physical_angle_, left_back_joint_target_physical_angle_, - right_back_joint_target_physical_angle_, right_front_joint_target_physical_angle_); - - if (!joint.alpha_rad.array().isFinite().all()) - return joint; - - const Eigen::Vector4d target_velocity = read_optional_inputs_( - left_front_joint_target_physical_velocity_, left_back_joint_target_physical_velocity_, - right_back_joint_target_physical_velocity_, - right_front_joint_target_physical_velocity_); - if (target_velocity.array().isFinite().all()) { - joint.alpha_dot_rad = target_velocity; - joint.has_velocity = true; - } else if (last_joint_target_angle_valid_) { - joint.alpha_dot_rad = (joint.alpha_rad - last_joint_target_angle_) / dt_; - joint.has_velocity = true; - } - - const Eigen::Vector4d target_acceleration = read_optional_inputs_( - left_front_joint_target_physical_acceleration_, - left_back_joint_target_physical_acceleration_, - right_back_joint_target_physical_acceleration_, - right_front_joint_target_physical_acceleration_); - if (target_acceleration.array().isFinite().all()) { - joint.alpha_ddot_rad = target_acceleration; - joint.has_acceleration = true; - } - - last_joint_target_angle_ = joint.alpha_rad; - last_joint_target_angle_valid_ = true; - - populate_joint_geometry_( - joint.alpha_rad, joint.alpha_dot_rad, joint.alpha_ddot_rad, chassis_radius_, - rod_length_, joint.radius, joint.radius_dot, joint.radius_ddot); - - joint.valid = joint.radius.array().isFinite().all() - && joint.radius_dot.array().isFinite().all() - && joint.radius_ddot.array().isFinite().all(); - return joint; - } - - void reset_all_controls() { - control_acceleration_filter_.reset(); - - chassis_yaw_angle_imu_ = 0.0; - chassis_velocity_expected_ = Eigen::Vector3d::Zero(); - vehicle_radius_ = Eigen::Vector4d::Constant(chassis_radius_ + rod_length_); - last_joint_velocity_ = Eigen::Vector4d::Zero(); - last_joint_velocity_valid_ = false; - last_joint_target_angle_ = Eigen::Vector4d::Zero(); - last_joint_target_angle_valid_ = false; - - *left_front_steering_control_torque_ = 0.0; - *left_back_steering_control_torque_ = 0.0; - *right_back_steering_control_torque_ = 0.0; - *right_front_steering_control_torque_ = 0.0; - - *left_front_wheel_control_torque_ = 0.0; - *left_back_wheel_control_torque_ = 0.0; - *right_back_wheel_control_torque_ = 0.0; - *right_front_wheel_control_torque_ = 0.0; - } - - void integral_yaw_angle_imu() { - chassis_yaw_angle_imu_ += *chassis_yaw_velocity_imu_ * dt_; - chassis_yaw_angle_imu_ = std::fmod(chassis_yaw_angle_imu_, 2 * std::numbers::pi); - } - - [[nodiscard]] SteeringStatus calculate_steering_status() const { - SteeringStatus steering_status; - steering_status.angle = read_required_inputs_( - left_front_steering_angle_, left_back_steering_angle_, right_back_steering_angle_, - right_front_steering_angle_); - steering_status.angle.array() -= std::numbers::pi / 4; - steering_status.cos_angle = steering_status.angle.array().cos(); - steering_status.sin_angle = steering_status.angle.array().sin(); - - for (size_t i = 0; i < kWheelCount; ++i) { - const double angle_minus_phi = steering_status.angle[i] - phi_[i]; - steering_status.sin_angle_minus_phi[i] = std::sin(angle_minus_phi); - steering_status.cos_angle_minus_phi[i] = std::cos(angle_minus_phi); - } - - steering_status.velocity = read_required_inputs_( - left_front_steering_velocity_, left_back_steering_velocity_, - right_back_steering_velocity_, right_front_steering_velocity_); - return steering_status; - } - - [[nodiscard]] Eigen::Vector4d calculate_wheel_velocities() const { - return read_required_inputs_( - left_front_wheel_velocity_, left_back_wheel_velocity_, right_back_wheel_velocity_, - right_front_wheel_velocity_); - } - - /** - * @brief Observe chassis velocity from wheel velocities using least squares - * - * Solves: A·x = b for x = [vx, vy, ωz] - * where A_i = [cos(ζᵢ), sin(ζᵢ), R_i·sin(ζᵢ - φᵢ)] - * b_i = r·ωᵢ - Ṙᵢ·cos(ζᵢ - φᵢ) - */ - [[nodiscard]] Eigen::Vector3d calculate_chassis_velocity( - const SteeringStatus& steering_status, Eigen::Ref wheel_velocities, - const JointFeedbackStates& joint) const { - Eigen::Vector4d wheel_velocities_eff = wheel_velocities; - if (joint.valid) { - const Eigen::Vector4d clamped_radius_dot = - joint.radius_dot.cwiseMax(-0.1).cwiseMin(0.1); - const Eigen::Vector4d wheel_omega_mech = - (clamped_radius_dot.array() * phi_cos_vec_.array() - * steering_status.cos_angle.array() - + clamped_radius_dot.array() * phi_sin_vec_.array() - * steering_status.sin_angle.array()) - / wheel_radius_; - wheel_velocities_eff -= wheel_omega_mech; - } - - const double one_quarter_r = wheel_radius_ / 4.0; - Eigen::Vector3d velocity; - velocity.x() = one_quarter_r * wheel_velocities_eff.dot(steering_status.cos_angle); - velocity.y() = one_quarter_r * wheel_velocities_eff.dot(steering_status.sin_angle); - velocity.z() = - -one_quarter_r - * (-wheel_velocities_eff[0] * steering_status.sin_angle[0] / vehicle_radius_[0] - + wheel_velocities_eff[1] * steering_status.cos_angle[1] / vehicle_radius_[1] - + wheel_velocities_eff[2] * steering_status.sin_angle[2] / vehicle_radius_[2] - - wheel_velocities_eff[3] * steering_status.cos_angle[3] / vehicle_radius_[3]); - return velocity; - } - - /** - * @brief Calculate expected chassis status with energy scaling - * - * Wheel center velocity: v_i = v + ω·R_i·e_t,i + Ṙᵢ·e_r,i - */ - [[nodiscard]] ChassisStatus calculate_chassis_status_expected( - Eigen::Ref chassis_velocity, const JointTargetStates& joint_target, - const JointFeedbackStates& joint_feedback) { - const double chassis_energy = calculate_chassis_energy(chassis_velocity); - const double chassis_energy_expected = calculate_chassis_energy(chassis_velocity_expected_); - - if (std::isfinite(chassis_energy) && std::isfinite(chassis_energy_expected) - && chassis_energy_expected > chassis_energy && chassis_energy_expected > 1e-12) { - const double k = std::sqrt(chassis_energy / chassis_energy_expected); - if (std::isfinite(k) && k >= 0.0) - chassis_velocity_expected_ *= k; - } - - ChassisStatus chassis_status_expected; - chassis_status_expected.velocity = odom_to_base_link_vector(chassis_velocity_expected_); - - const auto joint = select_joint_state(joint_target, joint_feedback); - - const double vx = chassis_status_expected.velocity.x(); - const double vy = chassis_status_expected.velocity.y(); - const double vz = chassis_status_expected.velocity.z(); - for (size_t i = 0; i < kWheelCount; ++i) { - const double radius = joint.valid ? joint.radius[i] : vehicle_radius_[i]; - const double clamped_radius_dot = - joint.valid ? std::clamp(joint.radius_dot[i], -0.1, 0.1) : 0.0; - const Eigen::Vector2d wheel_velocity = Eigen::Vector2d(vx, vy) - + vz * radius * tangential_unit_fast_(i) - + clamped_radius_dot * radial_unit_fast_(i); - chassis_status_expected.wheel_velocity_x[i] = wheel_velocity.x(); - chassis_status_expected.wheel_velocity_y[i] = wheel_velocity.y(); - } - - return chassis_status_expected; - } - - [[nodiscard]] Eigen::Vector3d calculate_chassis_control_velocity() const { - Eigen::Vector3d chassis_control_velocity = chassis_control_velocity_->vector; - chassis_control_velocity.head<2>() = - Eigen::Rotation2Dd(-std::numbers::pi / 4) * chassis_control_velocity.head<2>(); - return chassis_control_velocity; - } - - [[nodiscard]] Eigen::Vector3d calculate_chassis_control_acceleration( - Eigen::Ref chassis_velocity_expected, - Eigen::Ref chassis_control_velocity) { - Eigen::Vector2d translational_control_acceleration = - chassis_translational_velocity_pid_.update( - chassis_control_velocity.head<2>() - chassis_velocity_expected.head<2>()); - - const double angular_control_acceleration = chassis_angular_velocity_pid_.update( - chassis_control_velocity[2] - chassis_velocity_expected[2]); - - Eigen::Vector3d chassis_control_acceleration; - chassis_control_acceleration << translational_control_acceleration, - angular_control_acceleration; - if (chassis_control_acceleration.lpNorm<1>() < 1e-1) - chassis_control_acceleration.setZero(); - return chassis_control_acceleration; - } - - [[nodiscard]] Eigen::Vector4d calculate_wheel_pid_torques( - const SteeringStatus& steering_status, Eigen::Ref wheel_velocities, - const ChassisStatus& chassis_status_expected) { - const Eigen::Vector4d wheel_control_velocity = - chassis_status_expected.wheel_velocity_x.array() * steering_status.cos_angle.array() - + chassis_status_expected.wheel_velocity_y.array() * steering_status.sin_angle.array(); - return wheel_velocity_pid_.update( - wheel_control_velocity / wheel_radius_ - wheel_velocities); - } - - [[nodiscard]] Eigen::Vector3d constrain_chassis_control_acceleration( - const SteeringStatus& steering_status, Eigen::Ref wheel_velocities, - const JointTargetStates& joint_target, - Eigen::Ref chassis_acceleration, - Eigen::Ref wheel_pid_torques, const double& power_limit) { - Eigen::Vector2d translational_acceleration_direction = chassis_acceleration.head<2>(); - double translational_acceleration_max = translational_acceleration_direction.norm(); - if (translational_acceleration_max > 0.0) - translational_acceleration_direction /= translational_acceleration_max; - - double angular_acceleration_max = chassis_acceleration.z(); - double angular_acceleration_direction = angular_acceleration_max > 0 ? 1.0 : -1.0; - angular_acceleration_max *= angular_acceleration_direction; - - const double rhombus_right = friction_coefficient_ * g_; - const double constraint_radius = - joint_target.valid ? joint_target.radius.mean() : vehicle_radius_.mean(); - const double rhombus_top = rhombus_right * mass_ * constraint_radius / moment_of_inertia_; - - const auto params = calculate_ellipse_parameters( - steering_status, wheel_velocities, joint_target, translational_acceleration_direction, - angular_acceleration_direction, wheel_pid_torques); - - const QcpSolver::QuadraticConstraint quadratic_constraint{ - params.a, params.b, params.c, params.d, params.e, params.f - power_limit}; - - Eigen::Vector2d best_point = qcp_solver_.solve( - {1.0, 0.2}, {translational_acceleration_max, angular_acceleration_max}, - {rhombus_right, rhombus_top}, quadratic_constraint); - - const double min_translational = 0.3 * rhombus_right; - if (best_point.x() < min_translational - && translational_acceleration_max > min_translational) - best_point.x() = min_translational; - - Eigen::Vector3d best_acceleration; - best_acceleration << best_point.x() * translational_acceleration_direction, - best_point.y() * angular_acceleration_direction; - return best_acceleration; - } - - [[nodiscard]] EllipseParameters calculate_ellipse_parameters( - const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, - const JointTargetStates& joint_target, - const Eigen::Vector2d& translational_acceleration_direction, - const double& angular_acceleration_direction, - const Eigen::Vector4d& wheel_torque_base) const { - EllipseParameters params{0, 0, 0, 0, 0, 0}; - - for (size_t i = 0; i < kWheelCount; ++i) { - const double constraint_radius = - joint_target.valid ? joint_target.radius[i] : vehicle_radius_[i]; - const double cos_alpha_minus_gamma = - steering_status.cos_angle[i] * translational_acceleration_direction.x() - + steering_status.sin_angle[i] * translational_acceleration_direction.y(); - const double sin_alpha_minus_varphi = steering_status.sin_angle_minus_phi[i]; - const double double_k1_torque_base_plus_wheel_velocity = - 2 * k1_ * wheel_torque_base[i] + wheel_velocities[i]; - - params.a += ellipse_coeff_quadratic_translational_ * cos_alpha_minus_gamma - * cos_alpha_minus_gamma; - params.b += ellipse_coeff_cross_term_ * angular_acceleration_direction - * cos_alpha_minus_gamma * sin_alpha_minus_varphi / constraint_radius; - params.c += ellipse_coeff_quadratic_angular_ * sin_alpha_minus_varphi - * sin_alpha_minus_varphi / (constraint_radius * constraint_radius); - params.d += ellipse_coeff_linear_translational_ - * double_k1_torque_base_plus_wheel_velocity * cos_alpha_minus_gamma; - params.e += ellipse_coeff_linear_angular_ * angular_acceleration_direction - * double_k1_torque_base_plus_wheel_velocity * sin_alpha_minus_varphi - / constraint_radius; - params.f += wheel_torque_base[i] * (k1_ * wheel_torque_base[i] + wheel_velocities[i]); - } - - return params; - } - - [[nodiscard]] Eigen::Vector4d calculate_steering_control_torques( - const SteeringStatus& steering_status, const ChassisStatus& chassis_status_expected, - const JointTargetStates& joint_target, const JointFeedbackStates& joint_feedback, - const Eigen::Vector3d& chassis_acceleration) { - const double vx = chassis_status_expected.velocity.x(); - const double vy = chassis_status_expected.velocity.y(); - const double vz = chassis_status_expected.velocity.z(); - const double ax = chassis_acceleration.x(); - const double ay = chassis_acceleration.y(); - const double az = chassis_acceleration.z(); - - const auto joint = select_joint_state(joint_target, joint_feedback); - if (!joint.valid) [[unlikely]] - return Eigen::Vector4d::Zero(); - - Eigen::Vector4d dot_r_squared = chassis_status_expected.wheel_velocity_x.array().square() - + chassis_status_expected.wheel_velocity_y.array().square(); - - Eigen::Vector4d steering_control_velocity = - vx * ay - vy * ax - vz * (vx * vx + vy * vy) - + joint.radius.array() * (az * vx - vz * (ax + vz * vy)) * phi_cos_vec_.array() - + joint.radius.array() * (az * vy - vz * (ay - vz * vx)) * phi_sin_vec_.array(); - Eigen::Vector4d steering_control_angle; - - for (size_t i = 0; i < kWheelCount; ++i) { - if (dot_r_squared[i] > 1e-2) { - steering_control_velocity[i] /= dot_r_squared[i]; - steering_control_angle[i] = std::atan2( - chassis_status_expected.wheel_velocity_y[i], - chassis_status_expected.wheel_velocity_x[i]); - } else { - const double x = - ax - joint.radius[i] * (az * phi_sin_vec_[i] + vz * vz * phi_cos_vec_[i]); - const double y = - ay + joint.radius[i] * (az * phi_cos_vec_[i] - vz * vz * phi_sin_vec_[i]); - if (x * x + y * y > 1e-6) { - steering_control_velocity[i] = 0.0; - steering_control_angle[i] = std::atan2(y, x); - } else { - steering_control_velocity[i] = nan_; - steering_control_angle[i] = nan_; - } - } - } - - Eigen::Vector4d steering_torque = steering_velocity_pid_.update( - steering_control_velocity - + steering_angle_pid_.update( - (steering_control_angle - steering_status.angle).unaryExpr([](double diff) { - diff = std::fmod(diff, std::numbers::pi); - if (diff < -std::numbers::pi / 2) - diff += std::numbers::pi; - else if (diff > std::numbers::pi / 2) - diff -= std::numbers::pi; - return diff; - })) - - steering_status.velocity); - - return steering_torque.unaryExpr([](double v) { return std::isnan(v) ? 0.0 : v; }); - } - - [[nodiscard]] Eigen::Vector4d calculate_wheel_control_torques( - const SteeringStatus& steering_status, const JointTargetStates& joint_target, - const JointFeedbackStates& joint_feedback, const Eigen::Vector3d& chassis_acceleration, - const Eigen::Vector4d& wheel_pid_torques) const { - const auto joint = select_joint_state(joint_target, joint_feedback); - - const double ax = chassis_acceleration.x(); - const double ay = chassis_acceleration.y(); - const double az = chassis_acceleration.z(); - - Eigen::Vector4d wheel_torque = - wheel_radius_ - * (ax * mass_ * steering_status.cos_angle.array() - + ay * mass_ * steering_status.sin_angle.array() - + az * moment_of_inertia_ * steering_status.sin_angle_minus_phi.array() - / joint.radius.array()) - / 4.0; - - wheel_torque += wheel_pid_torques; - return wheel_torque; - } - - void update_control_torques( - const Eigen::Vector4d& steering_torque, const Eigen::Vector4d& wheel_torque) { - *left_front_steering_control_torque_ = steering_torque[0]; - *left_back_steering_control_torque_ = steering_torque[1]; - *right_back_steering_control_torque_ = steering_torque[2]; - *right_front_steering_control_torque_ = steering_torque[3]; - - *left_front_wheel_control_torque_ = wheel_torque[0]; - *left_back_wheel_control_torque_ = wheel_torque[1]; - *right_back_wheel_control_torque_ = wheel_torque[2]; - *right_front_wheel_control_torque_ = wheel_torque[3]; - } - - void update_chassis_velocity_expected(const Eigen::Vector3d& chassis_acceleration) { - chassis_velocity_expected_ += dt_ * base_link_to_odom_vector(chassis_acceleration); - } - - Eigen::Vector3d base_link_to_odom_vector(Eigen::Vector3d vector) const { - vector.head<2>() = Eigen::Rotation2Dd(chassis_yaw_angle_imu_) * vector.head<2>(); - return vector; - } - - Eigen::Vector3d odom_to_base_link_vector(Eigen::Vector3d vector) const { - vector.head<2>() = Eigen::Rotation2Dd(-chassis_yaw_angle_imu_) * vector.head<2>(); - return vector; - } - - [[nodiscard]] double calculate_chassis_energy(const Eigen::Vector3d& velocity) const { - return mass_ * velocity.head<2>().squaredNorm() - + moment_of_inertia_ * velocity.z() * velocity.z(); - } - - static Eigen::Vector2d radial_unit_(double phi) { return {std::cos(phi), std::sin(phi)}; } - - static Eigen::Vector2d tangential_unit_(double phi) { return {-std::sin(phi), std::cos(phi)}; } - - [[nodiscard]] Eigen::Vector2d radial_unit_fast_(size_t wheel_index) const { - return {phi_cos_[wheel_index], phi_sin_[wheel_index]}; - } - - [[nodiscard]] Eigen::Vector2d tangential_unit_fast_(size_t wheel_index) const { - return {-phi_sin_[wheel_index], phi_cos_[wheel_index]}; - } - - static double wrap_to_half_pi_(double diff) { - diff = std::fmod(diff, std::numbers::pi); - if (diff < -std::numbers::pi / 2) - diff += std::numbers::pi; - else if (diff > std::numbers::pi / 2) - diff -= std::numbers::pi; - return diff; - } - - static constexpr std::array phi_ = { - 0.0, - std::numbers::pi / 2, - std::numbers::pi, - -std::numbers::pi / 2, - }; - - static constexpr std::array phi_cos_ = { - 1.0, - 0.0, - -1.0, - 0.0, - }; - - static constexpr std::array phi_sin_ = { - 0.0, - 1.0, - 0.0, - -1.0, - }; - - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double dt_ = 1e-3; - static constexpr double g_ = 9.81; - - const double mass_; - const double moment_of_inertia_; - const double chassis_radius_; - const double rod_length_; - const double wheel_radius_; - const double friction_coefficient_; - const double k1_; - const double k2_; - const double no_load_power_; - - // Precomputed constants for calculate_ellipse_parameters - const double ellipse_coeff_quadratic_translational_; // k1 * mass^2 * wheel_radius^2 / 16 - const double ellipse_coeff_cross_term_; // k1 * mass * moment_of_inertia * wheel_radius^2 / 8 - const double ellipse_coeff_quadratic_angular_; // k1 * moment_of_inertia^2 * wheel_radius^2 / 16 - const double ellipse_coeff_linear_translational_; // mass * wheel_radius / 4 - const double ellipse_coeff_linear_angular_; // moment_of_inertia * wheel_radius / 4 - - Eigen::Vector4d vehicle_radius_; - const Eigen::Vector4d phi_cos_vec_{1.0, 0.0, -1.0, 0.0}; - const Eigen::Vector4d phi_sin_vec_{0.0, 1.0, 0.0, -1.0}; - Eigen::Vector4d last_joint_velocity_ = Eigen::Vector4d::Zero(); - bool last_joint_velocity_valid_ = false; - Eigen::Vector4d last_joint_target_angle_ = Eigen::Vector4d::Zero(); - bool last_joint_target_angle_valid_ = false; - - InputInterface joystick_right_; - InputInterface joystick_left_; - - InputInterface left_front_steering_angle_; - InputInterface left_back_steering_angle_; - InputInterface right_back_steering_angle_; - InputInterface right_front_steering_angle_; - - InputInterface left_front_steering_velocity_; - InputInterface left_back_steering_velocity_; - InputInterface right_back_steering_velocity_; - InputInterface right_front_steering_velocity_; - - InputInterface left_front_wheel_velocity_; - InputInterface left_back_wheel_velocity_; - InputInterface right_back_wheel_velocity_; - InputInterface right_front_wheel_velocity_; - - InputInterface left_front_joint_angle_; - InputInterface left_back_joint_angle_; - InputInterface right_back_joint_angle_; - InputInterface right_front_joint_angle_; - - InputInterface left_front_joint_velocity_; - InputInterface left_back_joint_velocity_; - InputInterface right_back_joint_velocity_; - InputInterface right_front_joint_velocity_; - - InputInterface left_front_joint_target_physical_angle_; - InputInterface left_back_joint_target_physical_angle_; - InputInterface right_back_joint_target_physical_angle_; - InputInterface right_front_joint_target_physical_angle_; - InputInterface left_front_joint_target_physical_velocity_; - InputInterface left_back_joint_target_physical_velocity_; - InputInterface right_back_joint_target_physical_velocity_; - InputInterface right_front_joint_target_physical_velocity_; - InputInterface left_front_joint_target_physical_acceleration_; - InputInterface left_back_joint_target_physical_acceleration_; - InputInterface right_back_joint_target_physical_acceleration_; - InputInterface right_front_joint_target_physical_acceleration_; - - InputInterface chassis_yaw_velocity_imu_; - InputInterface chassis_control_velocity_; - InputInterface power_limit_; - - OutputInterface left_front_steering_control_torque_; - OutputInterface left_back_steering_control_torque_; - OutputInterface right_back_steering_control_torque_; - OutputInterface right_front_steering_control_torque_; - - OutputInterface left_front_wheel_control_torque_; - OutputInterface left_back_wheel_control_torque_; - OutputInterface right_back_wheel_control_torque_; - OutputInterface right_front_wheel_control_torque_; - - QcpSolver qcp_solver_; - filter::LowPassFilter<3> control_acceleration_filter_; - - double chassis_yaw_angle_imu_ = 0.0; - Eigen::Vector3d chassis_velocity_expected_ = Eigen::Vector3d::Zero(); - - pid::MatrixPidCalculator<2> chassis_translational_velocity_pid_; - pid::PidCalculator chassis_angular_velocity_pid_; - pid::MatrixPidCalculator<4> steering_velocity_pid_; - pid::MatrixPidCalculator<4> steering_angle_pid_; - pid::MatrixPidCalculator<4> wheel_velocity_pid_; -}; - -} // namespace rmcs_core::controller::chassis - -#include - -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::DeformableChassisController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp index fc194e4a..46733c51 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp @@ -1,10 +1,12 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -31,21 +33,17 @@ class OmniWheelController register_input("/chassis/left_front_wheel/max_torque", wheel_motor_max_control_torque_); - register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); + for (size_t i = 0; i < kWheelCount; ++i) { + register_input( + fmt::format("/chassis/{}_wheel/velocity", kWheelName[i]), + wheel_velocity_[i]); + register_output( + fmt::format("/chassis/{}_wheel/control_torque", kWheelName[i]), + wheel_control_torque_[i], nan_); + } register_input("/chassis/control_velocity", chassis_control_velocity_); register_input("/chassis/control_power_limit", power_limit_); - - register_output( - "/chassis/left_front_wheel/control_torque", left_front_control_torque_, nan_); - register_output("/chassis/left_back_wheel/control_torque", left_back_control_torque_, nan_); - register_output( - "/chassis/right_back_wheel/control_torque", right_back_control_torque_, nan_); - register_output( - "/chassis/right_front_wheel/control_torque", right_front_control_torque_, nan_); } void before_updating() override { @@ -60,10 +58,9 @@ class OmniWheelController return; } - Eigen::Vector4d wheel_velocities = { - *left_front_velocity_, *left_back_velocity_, // - *right_back_velocity_, *right_front_velocity_ // - }; + Eigen::Vector4d wheel_velocities; + for (size_t i = 0; i < kWheelCount; ++i) + wheel_velocities[i] = *wheel_velocity_[i]; const auto chassis_velocity = calculate_chassis_velocity(wheel_velocities); auto chassis_control_torque = calculate_chassis_control_torque(chassis_velocity); @@ -74,23 +71,24 @@ class OmniWheelController const auto wheel_control_torques = calculate_wheel_control_torques(chassis_control_torque, wheel_pid_torques); - *left_front_control_torque_ = wheel_control_torques[0]; - *left_back_control_torque_ = wheel_control_torques[1]; - *right_back_control_torque_ = wheel_control_torques[2]; - *right_front_control_torque_ = wheel_control_torques[3]; + for (size_t i = 0; i < kWheelCount; ++i) + *wheel_control_torque_[i] = wheel_control_torques[i]; } private: + static constexpr const char* kWheelName[] = { + "left_front", "left_back", "right_back", "right_front", + }; + static constexpr size_t kWheelCount = 4; + struct ChassisControlTorque { Eigen::Vector2d torque; Eigen::Vector2d lambda; }; void reset_all_controls() { - *left_front_control_torque_ = 0.0; - *left_back_control_torque_ = 0.0; - *right_back_control_torque_ = 0.0; - *right_front_control_torque_ = 0.0; + for (size_t i = 0; i < kWheelCount; ++i) + *wheel_control_torque_[i] = 0.0; } static Eigen::Vector3d calculate_chassis_velocity(const Eigen::Vector4d& wheel_velocities) { @@ -200,10 +198,8 @@ class OmniWheelController InputInterface wheel_motor_max_control_torque_; - InputInterface left_front_velocity_; - InputInterface left_back_velocity_; - InputInterface right_back_velocity_; - InputInterface right_front_velocity_; + std::array, kWheelCount> + wheel_velocity_; InputInterface chassis_control_velocity_; InputInterface power_limit_; @@ -215,10 +211,8 @@ class OmniWheelController QcpSolver qcp_solver_; - OutputInterface left_front_control_torque_; - OutputInterface left_back_control_torque_; - OutputInterface right_back_control_torque_; - OutputInterface right_front_control_torque_; + std::array, kWheelCount> + wheel_control_torque_; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index a0b1a33f..bf4588b1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -38,32 +38,28 @@ class DeformableInfantryGimbalController get_parameter_or("pitch_gravity_ff_gain", pitch_gravity_ff_gain_, 0.0); get_parameter_or("pitch_gravity_ff_phase", pitch_gravity_ff_phase_, 0.0); get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); - get_parameter_or( - "ctrl_hold_chassis_yaw_velocity_max", ctrl_hold_chassis_yaw_velocity_max_, 30.0); } auto update() -> void override { const auto switch_right = *input_.switch_right; - const auto switch_left = *input_.switch_left; + const auto switch_left = *input_.switch_left; + const auto keyboard = *input_.keyboard; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - suspension_on_by_switch_ = false; - last_switch_right_ = switch_right; reset_all_controls(); return; } - update_ctrl_hold_request_state(switch_left, switch_right); + update_pitch_lock_state(switch_left, switch_right, keyboard); if (ctrl_hold_requested()) { update_ctrl_hold_control(); - return; + } else { + deactivate_ctrl_hold(); } - deactivate_ctrl_hold(); - const auto auto_aim_active = auto_aim_requested() && input_.auto_aim_should_control.ready() && *input_.auto_aim_should_control && input_.auto_aim_control_direction.ready() @@ -73,50 +69,67 @@ class DeformableInfantryGimbalController auto_aim_active ? update_auto_aim_control() : update_manual_control(); *output_.yaw_angle_error = angle_error.yaw_angle_error; - *output_.pitch_angle_error = angle_error.pitch_angle_error; + if (!ctrl_hold_active_) + *output_.pitch_angle_error = angle_error.pitch_angle_error; - if (!std::isfinite(angle_error.yaw_angle_error) - || !std::isfinite(angle_error.pitch_angle_error)) { - reset_control_outputs(); - return; + if (!std::isfinite(angle_error.yaw_angle_error)) { + yaw_angle_pid_.reset(); + yaw_velocity_pid_.reset(); + *output_.yaw_control_torque = kNaN; } - const auto feedforward_enabled = auto_aim_active && input_.auto_aim_feedforward_valid.ready() + const auto feedforward_enabled = auto_aim_active + && input_.auto_aim_feedforward_valid.ready() && *input_.auto_aim_feedforward_valid; - const auto yaw_velocity_ff = feedforward_enabled && input_.auto_aim_yaw_rate.ready() - && std::isfinite(*input_.auto_aim_yaw_rate) - ? yaw_vel_ff_gain_ * *input_.auto_aim_yaw_rate - : 0.0; - const auto yaw_acc_ff = feedforward_enabled && input_.auto_aim_yaw_acc.ready() - && std::isfinite(*input_.auto_aim_yaw_acc) - ? yaw_acc_ff_gain_ * *input_.auto_aim_yaw_acc - : 0.0; - const auto pitch_acc_ff = feedforward_enabled && input_.auto_aim_pitch_acc.ready() - && std::isfinite(*input_.auto_aim_pitch_acc) - ? pitch_acc_ff_gain_ * *input_.auto_aim_pitch_acc - : 0.0; - const auto pitch_gravity_ff = pitch_gravity_feedforward(); - const auto yaw_velocity_ref = - yaw_angle_pid_.update(angle_error.yaw_angle_error) + yaw_velocity_ff; - const auto pitch_velocity_ref = pitch_angle_pid_.update(angle_error.pitch_angle_error); - - *output_.yaw_control_torque = - yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu) + yaw_acc_ff; - if (pitch_torque_control_enabled_) { - *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = - pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) + + if (std::isfinite(angle_error.yaw_angle_error)) { + const auto yaw_velocity_ff = feedforward_enabled && input_.auto_aim_yaw_rate.ready() + && std::isfinite(*input_.auto_aim_yaw_rate) + ? yaw_vel_ff_gain_ * *input_.auto_aim_yaw_rate + : 0.0; + const auto yaw_acc_ff = feedforward_enabled && input_.auto_aim_yaw_acc.ready() + && std::isfinite(*input_.auto_aim_yaw_acc) + ? yaw_acc_ff_gain_ * *input_.auto_aim_yaw_acc + : 0.0; + + const auto yaw_velocity_ref = + yaw_angle_pid_.update(angle_error.yaw_angle_error) + yaw_velocity_ff; + *output_.yaw_control_torque = + yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu) + yaw_acc_ff; + } + + if (!ctrl_hold_active_) { + if (!std::isfinite(angle_error.pitch_angle_error)) { + pitch_angle_pid_.reset(); + pitch_velocity_pid_.reset(); + *output_.pitch_control_velocity = kNaN; + *output_.pitch_control_torque = kNaN; + } else { + const auto pitch_acc_ff = feedforward_enabled && input_.auto_aim_pitch_acc.ready() + && std::isfinite(*input_.auto_aim_pitch_acc) + ? pitch_acc_ff_gain_ * *input_.auto_aim_pitch_acc + : 0.0; + const auto pitch_gravity_ff = pitch_gravity_feedforward(); + const auto pitch_velocity_ref = + pitch_angle_pid_.update(angle_error.pitch_angle_error); + + if (pitch_torque_control_enabled_) { + *output_.pitch_control_velocity = kNaN; + *output_.pitch_control_torque = + pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) + pitch_acc_ff + pitch_gravity_ff; - } else { - pitch_velocity_pid_.reset(); - *output_.pitch_control_velocity = pitch_velocity_ref; - *output_.pitch_control_torque = kNaN; + } else { + pitch_velocity_pid_.reset(); + *output_.pitch_control_velocity = pitch_velocity_ref; + *output_.pitch_control_torque = kNaN; + } + } } } private: - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); static constexpr auto kDefaultDt = 1e-3; auto configure_pid(const std::string& prefix, pid::PidCalculator& calculator) -> void { @@ -138,7 +151,6 @@ class DeformableInfantryGimbalController component.register_input("/remote/mouse", mouse); component.register_input("/predefined/update_rate", update_rate, false); - component.register_input("/tf", tf); component.register_input("/gimbal/yaw/angle", yaw_angle); component.register_input("/gimbal/yaw/velocity", yaw_velocity); component.register_input("/gimbal/pitch/angle", pitch_angle); @@ -164,7 +176,6 @@ class DeformableInfantryGimbalController InputInterface mouse; InputInterface update_rate; - InputInterface tf; InputInterface yaw_angle; InputInterface yaw_velocity; InputInterface pitch_angle; @@ -190,9 +201,6 @@ class DeformableInfantryGimbalController component.register_output("/gimbal/pitch/control_angle", pitch_control_angle, kNaN); component.register_output("/gimbal/yaw/control_angle_error", yaw_angle_error, kNaN); component.register_output("/gimbal/pitch/control_angle_error", pitch_angle_error, kNaN); - component.register_output( - "/chassis/manual_yaw_velocity_override", chassis_manual_yaw_velocity_override, - kNaN); } OutputInterface yaw_control_torque; @@ -202,24 +210,15 @@ class DeformableInfantryGimbalController OutputInterface pitch_control_angle; OutputInterface yaw_angle_error; OutputInterface pitch_angle_error; - OutputInterface chassis_manual_yaw_velocity_override; } output_{*this}; auto ctrl_hold_requested() const -> bool { - return (input_.keyboard.ready() && input_.keyboard->ctrl) || suspension_on_by_switch_; - } - - auto update_ctrl_hold_request_state( - rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) -> void { - if (switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP - && last_switch_right_ == rmcs_msgs::Switch::MIDDLE) { - suspension_on_by_switch_ = !suspension_on_by_switch_; - } - last_switch_right_ = switch_right; + return pitch_lock_active_; } auto update_dt() const -> double { - if (input_.update_rate.ready() && std::isfinite(*input_.update_rate) && *input_.update_rate > 1e-6) + if (input_.update_rate.ready() && std::isfinite(*input_.update_rate) + && *input_.update_rate > 1e-6) return 1.0 / *input_.update_rate; return kDefaultDt; } @@ -251,20 +250,29 @@ class DeformableInfantryGimbalController } auto pitch_gravity_feedforward() const -> double { + if (ctrl_hold_active_) + return 0.0; if (!input_.pitch_angle.ready() || !std::isfinite(*input_.pitch_angle)) return 0.0; - else return pitch_gravity_ff_gain_ * std::sin(*input_.pitch_angle - pitch_gravity_ff_phase_); } + auto update_pitch_lock_state( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard) -> void { + if (switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP + && last_switch_right_ == rmcs_msgs::Switch::MIDDLE) { + suspension_on_by_switch_ = !suspension_on_by_switch_; + } + + pitch_lock_active_ = keyboard.ctrl || suspension_on_by_switch_; + last_switch_right_ = switch_right; + } + auto activate_ctrl_hold() -> void { ctrl_hold_active_ = true; - locked_yaw_angle_ = (input_.yaw_angle.ready() && std::isfinite(*input_.yaw_angle)) - ? *input_.yaw_angle - : 0.0; pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); - gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); } auto deactivate_ctrl_hold() -> void { @@ -272,29 +280,19 @@ class DeformableInfantryGimbalController return; ctrl_hold_active_ = false; - locked_yaw_angle_ = kNaN; - *output_.yaw_control_angle = kNaN; + pitch_angle_pid_.reset(); + pitch_velocity_pid_.reset(); *output_.pitch_control_angle = kNaN; - *output_.chassis_manual_yaw_velocity_override = kNaN; - gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); } auto update_ctrl_hold_control() -> void { if (!ctrl_hold_active_) activate_ctrl_hold(); - yaw_angle_pid_.reset(); - yaw_velocity_pid_.reset(); - *output_.yaw_control_torque = kNaN; - *output_.yaw_control_angle = kNaN; + *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = kNaN; - *output_.pitch_control_angle = kNaN; - *output_.chassis_manual_yaw_velocity_override = kNaN; - *output_.yaw_angle_error = kNaN; - *output_.pitch_angle_error = kNaN; - *output_.yaw_control_angle = locked_yaw_angle_; - *output_.pitch_control_angle = kNaN; + *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; if (input_.pitch_angle.ready() && std::isfinite(*input_.pitch_angle)) { auto pitch_target_error = ctrl_hold_pitch_target_angle_ - *input_.pitch_angle; @@ -303,7 +301,7 @@ class DeformableInfantryGimbalController else if (pitch_target_error < -std::numbers::pi) pitch_target_error += 2 * std::numbers::pi; - *output_.pitch_angle_error = pitch_target_error; + *output_.pitch_angle_error = pitch_target_error; const auto pitch_velocity_ref = pitch_angle_pid_.update(pitch_target_error); if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; @@ -313,14 +311,9 @@ class DeformableInfantryGimbalController } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; - *output_.pitch_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; } } - - const auto yaw_velocity_override = std::clamp( - manual_yaw_shift() / update_dt(), -ctrl_hold_chassis_yaw_velocity_max_, - ctrl_hold_chassis_yaw_velocity_max_); - *output_.chassis_manual_yaw_velocity_override = yaw_velocity_override; } auto reset_control_outputs() -> void { @@ -328,18 +321,20 @@ class DeformableInfantryGimbalController yaw_velocity_pid_.reset(); pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); - *output_.yaw_control_torque = kNaN; - *output_.yaw_control_angle = kNaN; + *output_.yaw_control_torque = kNaN; + *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = kNaN; - *output_.pitch_control_angle = kNaN; - *output_.chassis_manual_yaw_velocity_override = kNaN; + *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; } auto reset_all_controls() -> void { deactivate_ctrl_hold(); + pitch_lock_active_ = false; + suspension_on_by_switch_ = false; + last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); - *output_.yaw_angle_error = kNaN; + *output_.yaw_angle_error = kNaN; *output_.pitch_angle_error = kNaN; reset_control_outputs(); } @@ -368,20 +363,19 @@ class DeformableInfantryGimbalController get_parameter("pitch_velocity_kd").as_double(), }; - double joystick_sensitivity_ = 0.003; - double mouse_sensitivity_ = 0.5; - bool pitch_torque_control_enabled_ = false; - double yaw_vel_ff_gain_ = 0.0; - double yaw_acc_ff_gain_ = 0.0; - double pitch_acc_ff_gain_ = 0.0; + double joystick_sensitivity_ = 0.003; + double mouse_sensitivity_ = 0.5; + bool pitch_torque_control_enabled_ = false; + double yaw_vel_ff_gain_ = 0.0; + double yaw_acc_ff_gain_ = 0.0; + double pitch_acc_ff_gain_ = 0.0; double ctrl_hold_pitch_target_angle_ = 0.0; - double ctrl_hold_chassis_yaw_velocity_max_ = 30.0; double pitch_gravity_ff_gain_ = 0.0; double pitch_gravity_ff_phase_ = 0.0; + bool pitch_lock_active_ = false; bool suspension_on_by_switch_ = false; - bool ctrl_hold_active_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - double locked_yaw_angle_ = kNaN; + bool ctrl_hold_active_ = false; }; } // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 17562eb8..5c048d0e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -59,37 +59,6 @@ class TwoAxisGimbalSolver { } }; - class SetToLevelYawShift : public Operation { - public: - explicit SetToLevelYawShift(double yaw_shift) - : yaw_shift_(yaw_shift) {} - - private: - PitchLink::DirectionVector update(TwoAxisGimbalSolver& super) const override { - OdomImu::DirectionVector odom_dir; - if (!super.control_enabled_) { - odom_dir = fast_tf::cast( - PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *super.tf_); - } else { - odom_dir = super.control_direction_; - } - - if (std::abs(odom_dir->x()) < 1e-6 && std::abs(odom_dir->y()) < 1e-6) - return {}; - - super.control_enabled_ = true; - odom_dir->z() = 0; - odom_dir->normalize(); - auto dir = fast_tf::cast(odom_dir, *super.tf_); - dir->normalize(); - - const auto yaw_transform = Eigen::AngleAxisd{yaw_shift_, Eigen::Vector3d::UnitZ()}; - return PitchLink::DirectionVector{yaw_transform * (*dir)}; - } - - double yaw_shift_; - }; - class SetControlDirection : public Operation { public: explicit SetControlDirection(OdomImu::DirectionVector target) @@ -244,7 +213,6 @@ class TwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); const Eigen::Vector2d upper_limit_, lower_limit_; - rmcs_executor::Component::InputInterface tf_; double yaw_cw_min_ = 0.; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index 3018b3f8..f0b277f1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -31,7 +31,6 @@ class FrictionWheelController register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); register_input("/remote/keyboard", keyboard_); - register_input("/remote/mouse/mouse_wheel", mouse_wheel_); auto friction_wheels = get_parameter("friction_wheels").as_string_array(); auto friction_working_velocities = get_parameter("friction_velocities").as_double_array(); @@ -59,36 +58,6 @@ class FrictionWheelController friction_wheels[i] + "/control_velocity", friction_control_velocities_[i], nan_); } - const bool has_adjustable_min = has_parameter("friction_velocity_min"); - const bool has_adjustable_max = has_parameter("friction_velocity_max"); - if (has_adjustable_min != has_adjustable_max) - throw std::runtime_error( - "Parameter mismatch: 'friction_velocity_min' and 'friction_velocity_max' must be " - "provided together."); - - friction_velocity_adjustment_enabled_ = has_adjustable_min; - if (friction_velocity_adjustment_enabled_) { - friction_velocity_min_ = get_parameter("friction_velocity_min").as_double(); - friction_velocity_max_ = get_parameter("friction_velocity_max").as_double(); - if (friction_velocity_min_ > friction_velocity_max_) - throw std::runtime_error( - "Invalid friction velocity range: 'friction_velocity_min' must be less than " - "or equal to 'friction_velocity_max'."); - - friction_adjustable_velocity_ = friction_working_velocities_[0]; - for (size_t i = 1; i < friction_count_; i++) { - if (std::abs(friction_working_velocities_[i] - friction_adjustable_velocity_) > 1e-6) - throw std::runtime_error( - "Adjustable friction velocity requires all 'friction_velocities' to be " - "identical."); - } - - if (friction_adjustable_velocity_ < friction_velocity_min_ - || friction_adjustable_velocity_ > friction_velocity_max_) - throw std::runtime_error( - "Initial friction velocity is outside the configured adjustable range."); - } - friction_soft_start_stop_step_ = (1 / 1000.0) / get_parameter("friction_soft_start_stop_time").as_double(); @@ -101,7 +70,6 @@ class FrictionWheelController const auto switch_right = *switch_right_; const auto switch_left = *switch_left_; const auto keyboard = *keyboard_; - const auto mouse_wheel = *mouse_wheel_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -111,7 +79,6 @@ class FrictionWheelController } if (switch_right != Switch::DOWN) { - update_adjustable_friction_velocity(keyboard, mouse_wheel); update_friction_working_velocity_outputs(); if ((!last_keyboard_.v && keyboard.v) @@ -148,18 +115,6 @@ class FrictionWheelController *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; } - void update_adjustable_friction_velocity(const rmcs_msgs::Keyboard& keyboard, double mouse_wheel) { - if (!friction_velocity_adjustment_enabled_ || !keyboard.x) - return; - - friction_adjustable_velocity_ -= mouse_wheel * 5.0 * 10; - friction_adjustable_velocity_ = - std::clamp(friction_adjustable_velocity_, friction_velocity_min_, friction_velocity_max_); - - for (size_t i = 0; i < friction_count_; i++) - friction_working_velocities_[i] = friction_adjustable_velocity_; - } - void update_friction_working_velocity_outputs() { for (size_t i = 0; i < friction_count_; i++) *friction_working_velocity_outputs_[i] = friction_working_velocities_[i]; @@ -245,7 +200,6 @@ class FrictionWheelController InputInterface switch_right_; InputInterface switch_left_; InputInterface keyboard_; - InputInterface mouse_wheel_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -259,10 +213,6 @@ class FrictionWheelController std::unique_ptr[]> friction_working_velocity_outputs_; bool friction_enabled_ = false; - bool friction_velocity_adjustment_enabled_ = false; - double friction_adjustable_velocity_ = 0.0; - double friction_velocity_min_ = 0.0; - double friction_velocity_max_ = 0.0; double friction_soft_start_stop_step_; double friction_soft_start_stop_percentage_ = nan_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index fe906fe9..38dd8a86 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -6,11 +7,13 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -50,7 +53,6 @@ class DeformableInfantryOmniB get_component_name() + "_command", *this)) { using namespace rmcs_description; - register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); register_output( "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); @@ -60,39 +62,28 @@ class DeformableInfantryOmniB tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); // For command: remote-status - using Srv = std_srvs::srv::Trigger; + using Srv = std_srvs::srv::Trigger; status_service_ = create_service( "/rmcs/service/robot_status", [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { status_service_callback(response); }); - std::string serial_filter_imu; - get_parameter_or("serial_filter_imu", serial_filter_imu, std::string{}); - - rmcs_board_lite = std::make_unique( + bottom_board_ = std::make_unique( *this, *deformable_infantry_command_, get_parameter("serial_filter_rmcs_board").as_string()); top_board_ = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_top_board").as_string(), !serial_filter_imu.empty()); - if (!serial_filter_imu.empty()) - imu_board_ = std::make_unique(*this, vt13_, serial_filter_imu); - remote_control_ = std::make_unique(*this, rmcs_board_lite->dr16_, vt13_); + *this, *deformable_infantry_command_, vt13_, + get_parameter("serial_filter_top_board").as_string()); + remote_control_ = + std::make_unique(*this, bottom_board_->dr16_, vt13_); } ~DeformableInfantryOmniB() override = default; - void before_updating() override { - top_board_->request_hard_sync_read(); - next_hard_sync_log_time_ = Clock::now() + std::chrono::seconds(1); - } - void update() override { - rmcs_board_lite->update(); + bottom_board_->update(); top_board_->update(); - if (imu_board_) - imu_board_->update(); vt13_.update_status(); remote_control_->update(); @@ -105,14 +96,24 @@ class DeformableInfantryOmniB void command_update() { const bool even = ((cmd_tick_++ & 1u) == 0u); - rmcs_board_lite->command_update(even); + bottom_board_->command_update(even); top_board_->command_update(); } private: + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; + class DeformableInfantryOmniBCommand; class BottomBoard; - class ImuBoard; class TopBoard; class DeformableInfantryOmniBCommand : public rmcs_executor::Component { @@ -161,9 +162,8 @@ class DeformableInfantryOmniB for (auto& motor : chassis_wheel_motors_) motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(13.0) - .enable_multi_turn_angle() - .set_reversed()); + .set_reduction_ratio(19.0) + .enable_multi_turn_angle()); // V2: LK MG5010 i36 direct-drive joint motors, built-in encoder zero point for (auto& motor : chassis_joint_motors_) @@ -189,27 +189,17 @@ class DeformableInfantryOmniB "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); deformableInfantry.register_output( "/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, - nan_); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_velocity", - right_front_joint_physical_velocity_, nan_); + for (size_t i = 0; i < 4; ++i) { + deformableInfantry.register_output( + fmt::format( + "/chassis/{}_joint/physical_angle", DeformableInfantryOmniB::kJointName[i]), + joint_physical_angle_[i], nan_); + deformableInfantry.register_output( + fmt::format( + "/chassis/{}_joint/physical_velocity", + DeformableInfantryOmniB::kJointName[i]), + joint_physical_velocity_[i], nan_); + } deformableInfantry.register_output("/chassis/encoder/alpha", encoder_alpha_, nan_); deformableInfantry.register_output( "/chassis/encoder/alpha_dot", encoder_alpha_dot_, nan_); @@ -234,7 +224,7 @@ class DeformableInfantryOmniB const double q3 = imu_.q3(); double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); + sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); const double standard_pitch = std::asin(sin_pitch); const double standard_roll = @@ -242,10 +232,10 @@ class DeformableInfantryOmniB // Export chassis attitude using the requested convention: // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; + *chassis_imu_pitch_ = -standard_pitch; + *chassis_imu_roll_ = standard_roll; *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); + *chassis_imu_roll_rate_ = imu_.gx(); } for (auto& motor : chassis_wheel_motors_) @@ -253,14 +243,9 @@ class DeformableInfantryOmniB for (auto& motor : chassis_joint_motors_) motor.update_status(); - update_joint_physical_feedback_( - 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); - update_joint_physical_feedback_( - 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); + for (size_t i = 0; i < 4; ++i) + update_joint_physical_feedback_( + i, joint_physical_angle_[i], joint_physical_velocity_[i]); update_geometry_feedback_(); if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) @@ -285,7 +270,7 @@ class DeformableInfantryOmniB .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[kLeftFront].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -296,7 +281,7 @@ class DeformableInfantryOmniB .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[kLeftBack].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -307,7 +292,7 @@ class DeformableInfantryOmniB .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[kRightBack].generate_command(), device::CanPacket8::PaddingQuarter{}, gimbal_bullet_feeder_.generate_command(), device::CanPacket8::PaddingQuarter{}, @@ -318,7 +303,7 @@ class DeformableInfantryOmniB .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[3].generate_command(), + chassis_wheel_motors_[kRightFront].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -326,7 +311,7 @@ class DeformableInfantryOmniB .as_bytes(), }); builder.can2_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), }); builder.can1_transmit({ @@ -341,22 +326,34 @@ class DeformableInfantryOmniB .as_bytes(), }); } else { - builder.can0_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), - }); + for (size_t i = 0; i < 4; ++i) { + switch (i) { + case kLeftFront: + builder.can0_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kLeftBack: + builder.can1_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightBack: + builder.can2_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightFront: + builder.can3_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + } + } } } @@ -365,8 +362,8 @@ class DeformableInfantryOmniB rmcs_executor::Component& command_; static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; + static constexpr double chassis_radius_base_ = 0.2341741; + static constexpr double rod_length_ = 0.150; static constexpr double default_radius_ = 0.5 * rod_length_ + chassis_radius_base_; static double to_physical_angle_(double motor_angle) { @@ -379,27 +376,27 @@ class DeformableInfantryOmniB size_t index, OutputInterface& angle_output, OutputInterface& velocity_output) { if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = nan_; + *angle_output = nan_; *velocity_output = nan_; return; } - *angle_output = to_physical_angle_(chassis_joint_motors_[index].angle()); + *angle_output = to_physical_angle_(chassis_joint_motors_[index].angle()); *velocity_output = to_physical_velocity_(chassis_joint_motors_[index].velocity()); } void update_geometry_feedback_() { const Eigen::Vector4d alpha_rad{ - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; const Eigen::Vector4d alpha_dot_rad{ - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = nan_; + *encoder_alpha_ = nan_; *encoder_alpha_dot_ = nan_; - *radius_ = default_radius_; + *radius_ = default_radius_; RCLCPP_WARN_THROTTLE( deformable_infantry_.get_logger(), *deformable_infantry_.get_clock(), 1000, "deformable joint feedback invalid, fallback chassis radius to default %.3f m", @@ -407,7 +404,7 @@ class DeformableInfantryOmniB return; } - *encoder_alpha_ = alpha_rad.mean(); + *encoder_alpha_ = alpha_rad.mean(); *encoder_alpha_dot_ = alpha_dot_rad.mean(); *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); } @@ -425,29 +422,44 @@ class DeformableInfantryOmniB }; if (debug_log_wheel_motor_) { + std::string wheel_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + wheel_rx_str.push_back(' '); + wheel_rx_str.push_back(wheel_rx(i)); + } RCLCPP_INFO( deformable_infantry_.get_logger(), "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%c %c %c %c]", - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), - wheel_rx(1), wheel_rx(2), wheel_rx(3)); + "rx=[%s]", + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); } if (debug_log_deformable_joint_motor_) { + std::string joint_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + joint_rx_str.push_back(' '); + joint_rx_str.push_back(joint_rx(i)); + } RCLCPP_INFO( deformable_infantry_.get_logger(), "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%c %c %c %c]", - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, - joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); + "rx=[%s]", + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], + joint_rx_str.c_str()); } next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); @@ -458,7 +470,7 @@ class DeformableInfantryOmniB if (now < next_supercap_feedback_log_time_) return; - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); + const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); @@ -466,8 +478,7 @@ class DeformableInfantryOmniB deformable_infantry_.get_logger(), "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', - supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, supercap_rx ? supercap_.supercap_voltage() : nan_, supercap_rx ? supercap_.chassis_voltage() : nan_, supercap_rx ? supercap_.chassis_power() : nan_, @@ -487,28 +498,25 @@ class DeformableInfantryOmniB dr16_.store_status(data.uart_data); } - void can0_receive_callback(const librmcs::data::CanDataView& data) override { + void process_chassis_can_receive_(size_t index, const librmcs::data::CanDataView& data) { if (data.is_extended_can_id || data.is_remote_transmission) return; if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); + chassis_wheel_motors_[index].store_status(data.can_data); + wheel_status_received_[index].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); } } + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(0, data); + } + void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x300) { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { if (data.can_data.size() == 8) latest_supercap_status_.store( device::CanPacket8{data.can_data}, std::memory_order_relaxed); @@ -518,31 +526,17 @@ class DeformableInfantryOmniB } void can2_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { + if (data.can_id == 0x142) gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { + else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); - } } void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); - } + process_chassis_can_receive_(3, data); } void uart0_receive_callback(const librmcs::data::UartDataView& data) override { @@ -563,43 +557,31 @@ class DeformableInfantryOmniB OutputInterface& tf_; device::Bmi088 imu_{1000, 0.2, 0.0}; - device::LkMotor gimbal_yaw_motor_{ - deformable_infantry_, command_, "/gimbal/yaw"}; + device::LkMotor gimbal_yaw_motor_{deformable_infantry_, command_, "/gimbal/yaw"}; device::Dr16 dr16_; device::DjiMotor chassis_wheel_motors_[4]{ - device::DjiMotor{ - deformable_infantry_, command_, "/chassis/left_front_wheel"}, - device::DjiMotor{ - deformable_infantry_, command_, "/chassis/left_back_wheel"}, - device::DjiMotor{ - deformable_infantry_, command_, "/chassis/right_back_wheel"}, - device::DjiMotor{ - deformable_infantry_, command_, "/chassis/right_front_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/left_front_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/left_back_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/right_back_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/right_front_wheel"}, }; device::LkMotor chassis_joint_motors_[4]{ - device::LkMotor{ - deformable_infantry_, command_, "/chassis/left_front_joint"}, - device::LkMotor{ - deformable_infantry_, command_, "/chassis/left_back_joint"}, - device::LkMotor{ - deformable_infantry_, command_, "/chassis/right_back_joint"}, - device::LkMotor{ - deformable_infantry_, command_, "/chassis/right_front_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/left_front_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/left_back_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/right_back_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/right_front_joint"}, }; std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - Clock::time_point next_chassis_feedback_log_time_{ - Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{ - Clock::now() + std::chrono::seconds(1)}; + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; device::Supercap supercap_{deformable_infantry_, command_}; - std::atomic latest_supercap_status_{ - device::CanPacket8{uint64_t{0}}}; + std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; device::DjiMotor gimbal_bullet_feeder_{ deformable_infantry_, command_, "/gimbal/bullet_feeder"}; @@ -612,95 +594,32 @@ class DeformableInfantryOmniB OutputInterface chassis_imu_roll_; OutputInterface chassis_imu_pitch_rate_; OutputInterface chassis_imu_roll_rate_; - OutputInterface left_front_joint_physical_angle_; - OutputInterface left_back_joint_physical_angle_; - OutputInterface right_back_joint_physical_angle_; - OutputInterface right_front_joint_physical_angle_; - OutputInterface left_front_joint_physical_velocity_; - OutputInterface left_back_joint_physical_velocity_; - OutputInterface right_back_joint_physical_velocity_; - OutputInterface right_front_joint_physical_velocity_; + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; OutputInterface encoder_alpha_; OutputInterface encoder_alpha_dot_; OutputInterface radius_; }; - class ImuBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmniB; - - public: - explicit ImuBoard( - DeformableInfantryOmniB& deformableInfantry, device::Vt13& vt13, - const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , tf_{deformableInfantry.tf_} - , vt13_{vt13} - , bmi088_{1000, 0.2, 0.0} { - - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - } - - ~ImuBoard() override = default; - - void update() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); - } - - private: - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - vt13_.store_status(data.uart_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - OutputInterface gimbal_pitch_velocity_imu_; - device::Vt13& vt13_; - - device::Bmi088 bmi088_; - }; - class TopBoard final : private librmcs::agent::RmcsBoardLite { public: friend class DeformableInfantryOmniB; explicit TopBoard( DeformableInfantryOmniB& deformableInfantry, - DeformableInfantryOmniBCommand& deformableInfantry_command, - std::string serial_filter = {}, bool has_external_imu_board = false) + DeformableInfantryOmniBCommand& deformableInfantry_command, device::Vt13& vt13, + const std::string& serial_filter = {}) : librmcs::agent::RmcsBoardLite( serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , hard_sync_pending_(deformableInfantry.hard_sync_pending_) - , has_external_imu_board_(has_external_imu_board) , tf_(deformableInfantry.tf_) - , bmi088_(1000, 0.2, 0.0) + , vt13_{vt13} + , gimbal_imu_{1000, 0.2, 0.0} , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") , gimbal_left_friction_( deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") , gimbal_right_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") - , scope_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/scope") { + deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") { gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} @@ -716,25 +635,19 @@ class DeformableInfantryOmniB gimbal_right_friction_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - deformableInfantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - if (!has_external_imu_board_) - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + deformableInfantry.register_output( + "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(-x, -y, z); - }); + gimbal_imu_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); } ~TopBoard() override = default; [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_bmi088_; + return *gimbal_yaw_velocity_imu_; } void request_hard_sync_read() { @@ -743,34 +656,21 @@ class DeformableInfantryOmniB } void update() { - bmi088_.update_status(); + gimbal_imu_.update_status(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); - scope_motor_.update_status(); - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + Eigen::Quaterniond const gimbal_imu_pose{ + gimbal_imu_.q0(), gimbal_imu_.q1(), gimbal_imu_.q2(), gimbal_imu_.q3()}; + tf_->set_transform( + gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - if (!has_external_imu_board_) { - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); - - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> - // OdomImu, so use the encoder pitch from the TF tree to move the - // yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); - } + *gimbal_pitch_velocity_imu_ = gimbal_imu_.gy(); + *gimbal_yaw_velocity_imu_ = gimbal_imu_.gz(); + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); tf_->set_state( pitch_encoder_angle); } @@ -778,17 +678,27 @@ class DeformableInfantryOmniB void command_update() { auto builder = start_transmit(); builder.can0_transmit({ - .can_id = 0x141, + .can_id = 0x141, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); - builder.can1_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, gimbal_right_friction_.generate_command(), - scope_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, } .as_bytes(), @@ -796,6 +706,10 @@ class DeformableInfantryOmniB } private: + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + vt13_.store_status(data.uart_data); + } + void uart1_receive_callback(const librmcs::data::UartDataView&) override {} void can0_receive_callback(const librmcs::data::CanDataView& data) override { @@ -810,33 +724,34 @@ class DeformableInfantryOmniB return; if (data.can_id == 0x201) gimbal_left_friction_.store_status(data.can_data); - else if (data.can_id == 0x202) + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (data.can_id == 0x202) gimbal_right_friction_.store_status(data.can_data); - else if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); } void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + gimbal_imu_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + gimbal_imu_.store_gyroscope_status(data.x, data.y, data.z); } - std::atomic& hard_sync_pending_; - bool has_external_imu_board_ = false; OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_encoder_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface gimbal_yaw_velocity_imu_; + device::Vt13& vt13_; - device::Bmi088 bmi088_; + device::Bmi088 gimbal_imu_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; }; auto status_service_callback(const std::shared_ptr& response) @@ -849,14 +764,19 @@ class DeformableInfantryOmniB }; text("Gimbal Status"); - text("- Yaw: {}", rmcs_board_lite->gimbal_yaw_motor_.last_raw_angle()); + text("- Yaw: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); text("Chassis Status"); - text("- left front: {}", rmcs_board_lite->chassis_joint_motors_[0].last_raw_angle()); - text("- left back: {}", rmcs_board_lite->chassis_joint_motors_[1].last_raw_angle()); - text("- right back: {}", rmcs_board_lite->chassis_joint_motors_[2].last_raw_angle()); - text("- right front: {}", rmcs_board_lite->chassis_joint_motors_[3].last_raw_angle()); + constexpr auto kPosition = + std::array{"left front", "left back", "right back", "right front"}; + constexpr auto kMaxLength = + std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); + + for (auto&& [index, motor] : + std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { + text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); + } response->message = feedback_message.str(); } @@ -867,13 +787,9 @@ class DeformableInfantryOmniB OutputInterface auto_aim_yaw_velocity_; InputInterface timestamp_; device::Vt13 vt13_; - std::atomic hard_sync_pending_{false}; - size_t hard_sync_snapshot_count_ = 0; - Clock::time_point next_hard_sync_log_time_{}; std::shared_ptr deformable_infantry_command_; - std::unique_ptr rmcs_board_lite; - std::unique_ptr imu_board_; + std::unique_ptr bottom_board_; std::unique_ptr top_board_; std::unique_ptr remote_control_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 78805893..fd8442fb 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -8,6 +8,7 @@ #include "hardware/device/vt13.hpp" #include +#include #include #include #include @@ -21,6 +22,7 @@ #include #include +#include #include #include #include @@ -46,7 +48,6 @@ class DeformableInfantryOmni , command_(create_partner_component(get_component_name() + "_command", *this)) { using namespace rmcs_description; - register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); register_output( "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); @@ -58,13 +59,12 @@ class DeformableInfantryOmni bottom_board_ = std::make_unique( *this, *command_, get_parameter("serial_filter_rmcs_board").as_string()); top_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_top_board").as_string(), true); - imu_board_ = std::make_unique( - *this, vt13_, get_parameter("serial_filter_imu").as_string()); - remote_control_ = std::make_unique(*this, bottom_board_->dr16_, vt13_); + *this, *command_, vt13_, get_parameter("serial_filter_top_board").as_string()); + remote_control_ = + std::make_unique(*this, bottom_board_->dr16_, vt13_); // For command: remote-status - using Srv = std_srvs::srv::Trigger; + using Srv = std_srvs::srv::Trigger; status_service_ = create_service( "/rmcs/service/robot_status", [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { @@ -74,12 +74,9 @@ class DeformableInfantryOmni ~DeformableInfantryOmni() override = default; - void before_updating() override { top_board_->request_hard_sync_read(); } - void update() override { bottom_board_->update(); top_board_->update(); - imu_board_->update(); vt13_.update_status(); remote_control_->update(); @@ -97,7 +94,17 @@ class DeformableInfantryOmni } private: - static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; class Command : public rmcs_executor::Component { public: @@ -140,9 +147,8 @@ class DeformableInfantryOmni for (auto& motor : chassis_wheel_motors_) motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(13.0) - .enable_multi_turn_angle() - .set_reversed()); + .set_reduction_ratio(19.0) + .enable_multi_turn_angle()); for (auto& motor : chassis_joint_motors_) motor.configure( @@ -164,27 +170,17 @@ class DeformableInfantryOmni status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - status.register_output( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, - kNaN); - status.register_output( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/right_front_joint/physical_velocity", - right_front_joint_physical_velocity_, kNaN); + for (size_t i = 0; i < 4; ++i) { + status.register_output( + fmt::format( + "/chassis/{}_joint/physical_angle", DeformableInfantryOmni::kJointName[i]), + joint_physical_angle_[i], kNaN); + status.register_output( + fmt::format( + "/chassis/{}_joint/physical_velocity", + DeformableInfantryOmni::kJointName[i]), + joint_physical_velocity_[i], kNaN); + } status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); status.register_output("/chassis/radius", radius_, default_radius_); @@ -207,7 +203,7 @@ class DeformableInfantryOmni const double q3 = imu_.q3(); double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); + sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); const double standard_pitch = std::asin(sin_pitch); const double standard_roll = @@ -215,10 +211,10 @@ class DeformableInfantryOmni // Export chassis attitude using the requested convention: // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; + *chassis_imu_pitch_ = -standard_pitch; + *chassis_imu_roll_ = standard_roll; *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); + *chassis_imu_roll_rate_ = imu_.gx(); } for (auto& motor : chassis_wheel_motors_) @@ -226,14 +222,9 @@ class DeformableInfantryOmni for (auto& motor : chassis_joint_motors_) motor.update_status(); - update_joint_physical_feedback_( - 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); - update_joint_physical_feedback_( - 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); + for (size_t i = 0; i < 4; ++i) + update_joint_physical_feedback_( + i, joint_physical_angle_[i], joint_physical_velocity_[i]); update_geometry_feedback_(); if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) @@ -258,7 +249,7 @@ class DeformableInfantryOmni .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[kLeftFront].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -269,7 +260,7 @@ class DeformableInfantryOmni .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[kLeftBack].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -280,7 +271,7 @@ class DeformableInfantryOmni .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[kRightBack].generate_command(), device::CanPacket8::PaddingQuarter{}, gimbal_bullet_feeder_.generate_command(), device::CanPacket8::PaddingQuarter{}, @@ -291,7 +282,7 @@ class DeformableInfantryOmni .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[3].generate_command(), + chassis_wheel_motors_[kRightFront].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -299,7 +290,7 @@ class DeformableInfantryOmni .as_bytes(), }); builder.can2_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), }); builder.can1_transmit({ @@ -314,30 +305,42 @@ class DeformableInfantryOmni .as_bytes(), }); } else { - builder.can0_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), - }); + for (size_t i = 0; i < 4; ++i) { + switch (i) { + case kLeftFront: + builder.can0_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kLeftBack: + builder.can1_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightBack: + builder.can2_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + case kRightFront: + builder.can3_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + }); + break; + } + } } } private: static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; - static constexpr double default_radius_ = chassis_radius_base_ + rod_length_; + static constexpr double chassis_radius_base_ = 0.2341741; + static constexpr double rod_length_ = 0.150; + static constexpr double default_radius_ = chassis_radius_base_ + rod_length_; DeformableInfantryOmni& status_; Component& command_; @@ -361,9 +364,9 @@ class DeformableInfantryOmni std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; device::Supercap supercap_{status_, command_}; @@ -378,14 +381,8 @@ class DeformableInfantryOmni OutputInterface chassis_imu_roll_; OutputInterface chassis_imu_pitch_rate_; OutputInterface chassis_imu_roll_rate_; - OutputInterface left_front_joint_physical_angle_; - OutputInterface left_back_joint_physical_angle_; - OutputInterface right_back_joint_physical_angle_; - OutputInterface right_front_joint_physical_angle_; - OutputInterface left_front_joint_physical_velocity_; - OutputInterface left_back_joint_physical_velocity_; - OutputInterface right_back_joint_physical_velocity_; - OutputInterface right_front_joint_physical_velocity_; + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; OutputInterface encoder_alpha_; OutputInterface encoder_alpha_dot_; OutputInterface radius_; @@ -398,7 +395,7 @@ class DeformableInfantryOmni OutputInterface& velocity_output) { if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = kNaN; + *angle_output = kNaN; *velocity_output = kNaN; return; } @@ -408,22 +405,22 @@ class DeformableInfantryOmni }; const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; - *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); + *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); *velocity_output = to_physical_velocity(chassis_joint_motors_[index].velocity()); } void update_geometry_feedback_() { const Eigen::Vector4d alpha_rad{ - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; const Eigen::Vector4d alpha_dot_rad{ - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = kNaN; + *encoder_alpha_ = kNaN; *encoder_alpha_dot_ = kNaN; - *radius_ = default_radius_; + *radius_ = default_radius_; RCLCPP_WARN_THROTTLE( status_.get_logger(), *status_.get_clock(), 1000, "deformable joint feedback invalid, fallback chassis radius to default %.3f m", @@ -431,7 +428,7 @@ class DeformableInfantryOmni return; } - *encoder_alpha_ = alpha_rad.mean(); + *encoder_alpha_ = alpha_rad.mean(); *encoder_alpha_dot_ = alpha_dot_rad.mean(); *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); } @@ -449,29 +446,44 @@ class DeformableInfantryOmni }; if (debug_log_wheel_motor_) { + std::string wheel_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + wheel_rx_str.push_back(' '); + wheel_rx_str.push_back(wheel_rx(i)); + } RCLCPP_INFO( status_.get_logger(), "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%c %c %c %c]", - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), - wheel_rx(1), wheel_rx(2), wheel_rx(3)); + "rx=[%s]", + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); } if (debug_log_deformable_joint_motor_) { + std::string joint_rx_str; + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + joint_rx_str.push_back(' '); + joint_rx_str.push_back(joint_rx(i)); + } RCLCPP_INFO( status_.get_logger(), "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%c %c %c %c]", - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, - joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); + "rx=[%s]", + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], + joint_rx_str.c_str()); } next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); @@ -482,7 +494,7 @@ class DeformableInfantryOmni if (now < next_supercap_feedback_log_time_) return; - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); + const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); @@ -490,8 +502,7 @@ class DeformableInfantryOmni status_.get_logger(), "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', - supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, supercap_rx ? supercap_.supercap_voltage() : kNaN, supercap_rx ? supercap_.chassis_voltage() : kNaN, supercap_rx ? supercap_.chassis_power() : kNaN, @@ -511,28 +522,25 @@ class DeformableInfantryOmni dr16_.store_status(data.uart_data); } - void can0_receive_callback(const librmcs::data::CanDataView& data) override { + void process_chassis_can_receive_(size_t index, const librmcs::data::CanDataView& data) { if (data.is_extended_can_id || data.is_remote_transmission) return; if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); + chassis_wheel_motors_[index].store_status(data.can_data); + wheel_status_received_[index].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); } } + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(0, data); + } + void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x300) { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { if (data.can_data.size() == 8) latest_supercap_status_.store( device::CanPacket8{data.can_data}, std::memory_order_relaxed); @@ -542,31 +550,17 @@ class DeformableInfantryOmni } void can2_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { + if (data.can_id == 0x142) gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { + else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); - } } void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); - } + process_chassis_can_receive_(3, data); } void uart0_receive_callback(const librmcs::data::UartDataView& data) override { @@ -585,77 +579,22 @@ class DeformableInfantryOmni } }; - class ImuBoard final : private librmcs::agent::RmcsBoardLite { + class TopBoard final : private librmcs::agent::RmcsBoardLite { friend class DeformableInfantryOmni; public: - explicit ImuBoard( - DeformableInfantryOmni& status, device::Vt13& vt13, + explicit TopBoard( + DeformableInfantryOmni& status, Command& command, device::Vt13& vt13, const std::string& serial_filter = {}) : RmcsBoardLite{ serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} , tf_{status.tf_} , vt13_{vt13} - , bmi088_{1000, 0.2, 0.0} { - - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - } - - ~ImuBoard() override = default; - - void update() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); - } - - private: - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - vt13_.store_status(data.uart_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - OutputInterface gimbal_pitch_velocity_imu_; - device::Vt13& vt13_; - - device::Bmi088 bmi088_; - }; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmni; - - public: - explicit TopBoard( - DeformableInfantryOmni& status, Command& command, std::string serial_filter = {}, - bool has_external_imu_board = false) - : librmcs::agent::RmcsBoardLite( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , has_external_imu_board_(has_external_imu_board) - , tf_(status.tf_) - , bmi088_(1000, 0.2, 0.0) + , gimbal_imu_{1000, 0.2, 0.0} , gimbal_pitch_motor_(status, command, "/gimbal/pitch") , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") - , scope_motor_(status, command, "/gimbal/scope") { + , gimbal_right_friction_(status, command, "/gimbal/right_friction") { gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} @@ -670,23 +609,17 @@ class DeformableInfantryOmni gimbal_right_friction_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - if (!has_external_imu_board_) - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(-x, -y, z); - }); + gimbal_imu_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); } ~TopBoard() override = default; [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_bmi088_; + return *gimbal_yaw_velocity_imu_; } void request_hard_sync_read() { @@ -695,34 +628,21 @@ class DeformableInfantryOmni } void update() { - bmi088_.update_status(); + gimbal_imu_.update_status(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); - scope_motor_.update_status(); - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + Eigen::Quaterniond const gimbal_imu_pose{ + gimbal_imu_.q0(), gimbal_imu_.q1(), gimbal_imu_.q2(), gimbal_imu_.q3()}; + tf_->set_transform( + gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - if (!has_external_imu_board_) { - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); - - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> - // OdomImu, so use the encoder pitch from the TF tree to move the - // yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); - } + *gimbal_pitch_velocity_imu_ = gimbal_imu_.gy(); + *gimbal_yaw_velocity_imu_ = gimbal_imu_.gz(); + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); tf_->set_state( pitch_encoder_angle); } @@ -730,10 +650,9 @@ class DeformableInfantryOmni void command_update() { auto builder = start_transmit(); builder.can0_transmit({ - .can_id = 0x141, + .can_id = 0x141, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); - builder.can1_transmit({ .can_id = 0x200, .can_data = @@ -745,7 +664,6 @@ class DeformableInfantryOmni } .as_bytes(), }); - builder.can2_transmit({ .can_id = 0x200, .can_data = @@ -757,21 +675,13 @@ class DeformableInfantryOmni } .as_bytes(), }); - - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); } private: + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + vt13_.store_status(data.uart_data); + } + void uart1_receive_callback(const librmcs::data::UartDataView&) override {} void can0_receive_callback(const librmcs::data::CanDataView& data) override { @@ -795,32 +705,24 @@ class DeformableInfantryOmni gimbal_right_friction_.store_status(data.can_data); } - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); - } - void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + gimbal_imu_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + gimbal_imu_.store_gyroscope_status(data.x, data.y, data.z); } - bool has_external_imu_board_ = false; OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_encoder_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface gimbal_yaw_velocity_imu_; + device::Vt13& vt13_; - device::Bmi088 bmi088_; + device::Bmi088 gimbal_imu_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; }; auto status_service_callback(const std::shared_ptr& response) @@ -857,7 +759,6 @@ class DeformableInfantryOmni InputInterface timestamp_; device::Vt13 vt13_; - std::unique_ptr imu_board_; std::unique_ptr top_board_; std::unique_ptr bottom_board_; std::unique_ptr remote_control_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp deleted file mode 100644 index 9d6d10a7..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp +++ /dev/null @@ -1,942 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/remote_control.hpp" -#include "hardware/device/supercap.hpp" -#include "hardware/device/vt13.hpp" - -namespace rmcs_core::hardware { - -using Clock = std::chrono::steady_clock; - -class DeformableInfantryV2 - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DeformableInfantryV2() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , deformable_infantry_command_( - create_partner_component( - get_component_name() + "_command", *this)) { - using namespace rmcs_description; - - register_input("/predefined/timestamp", timestamp_); - register_output("/tf", tf_); - - tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); - - // For command: remote-status - using Srv = std_srvs::srv::Trigger; - status_service_ = create_service( - "/rmcs/service/robot_status", - [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { - status_service_callback(response); - }); - - rmcs_board_lite = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_rmcs_board").as_string()); - std::string serial_filter_imu; - get_parameter_or("serial_filter_imu", serial_filter_imu, std::string{}); - top_board_ = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_top_board").as_string(), !serial_filter_imu.empty()); - if (!serial_filter_imu.empty()) - imu_board_ = std::make_unique(*this, vt13_, serial_filter_imu); - remote_control_ = - std::make_unique(*this, rmcs_board_lite->dr16_, vt13_); - } - - ~DeformableInfantryV2() override = default; - - void before_updating() override { - top_board_->request_hard_sync_read(); - next_hard_sync_log_time_ = Clock::now() + std::chrono::seconds(1); - } - - void update() override { - rmcs_board_lite->update(); - top_board_->update(); - if (imu_board_) - imu_board_->update(); - vt13_.update_status(); - remote_control_->update(); - } - - void command_update() { - const bool even = ((cmd_tick_++ & 1u) == 0u); - rmcs_board_lite->command_update(even); - top_board_->command_update(); - } - -private: - class DeformableInfantryV2Command; - class BottomBoard; - class ImuBoard; - class TopBoard; - - class DeformableInfantryV2Command : public rmcs_executor::Component { - public: - explicit DeformableInfantryV2Command(DeformableInfantryV2& deformableInfantry) - : deformableInfantry(deformableInfantry) {} - - void update() override { deformableInfantry.command_update(); } - - DeformableInfantryV2& deformableInfantry; - }; - - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class DeformableInfantryV2; - - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - explicit BottomBoard( - DeformableInfantryV2& deformableInfantry, - DeformableInfantryV2Command& deformableInfantry_command, - const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , deformable_infantry_{deformableInfantry} - , command_{deformableInfantry_command} - , tf_{deformableInfantry.tf_} { - - deformableInfantry.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("yaw_motor_zero_point").as_int()))); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(10.0) - .enable_multi_turn_angle() - .set_reversed()); - - // V2: LK MG5010 i36 direct-drive joint motors, built-in encoder zero point - for (auto& motor : chassis_joint_motors_) - motor.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} - .set_reversed() - .enable_multi_turn_angle()); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU - // installation is re-validated on hardware. - return std::make_tuple(-y, x, z); - }); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - chassis_steer_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("left_front_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("left_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("right_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("right_front_zero_point").as_int())) - .enable_multi_turn_angle()); - - deformableInfantry.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - deformableInfantry.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); - deformableInfantry.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); - deformableInfantry.register_output( - "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, - nan_); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_velocity", - right_front_joint_physical_velocity_, nan_); - deformableInfantry.register_output("/chassis/encoder/alpha", encoder_alpha_, nan_); - deformableInfantry.register_output( - "/chassis/encoder/alpha_dot", encoder_alpha_dot_, nan_); - deformableInfantry.register_output("/chassis/radius", radius_, nan_); - - deformableInfantry.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - deformableInfantry.get_parameter_or( - "debug_log_wheel_motor", debug_log_wheel_motor_, false); - deformableInfantry.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - } - - ~BottomBoard() override = default; - - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - { - const double q0 = imu_.q0(); - const double q1 = imu_.q1(); - const double q2 = imu_.q2(); - const double q3 = imu_.q3(); - - double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); - - const double standard_pitch = std::asin(sin_pitch); - const double standard_roll = - std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); - - // Export chassis attitude using the requested convention: - // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; - *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); - } - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steer_motors_) - motor.update_status(); - for (auto& motor : chassis_joint_motors_) - motor.update_status(); - - update_joint_physical_feedback_( - 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); - update_joint_physical_feedback_( - 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); - - update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); - - dr16_.update_status(); - gimbal_yaw_motor_.update_status(); - if (supercap_status_received_.load(std::memory_order_relaxed)) - supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); - gimbal_bullet_feeder_.update_status(); - - tf_->set_state( - gimbal_yaw_motor_.angle()); - } - - void command_update(bool even) { - auto builder = start_transmit(); - if (even) { - // Steer motors: same as V1 - builder.can0_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } else { - // V2: Wheel DJI frames (wheel only, no joint packed in) - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - // V2: Joint LK motors - individual CAN frames - builder.can0_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), - }); - } - } - - private: - DeformableInfantryV2& deformable_infantry_; - - static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; - - static double to_physical_angle_(double motor_angle) { - return joint_zero_physical_angle_rad_ - motor_angle; - } - - static double to_physical_velocity_(double motor_velocity) { return -motor_velocity; } - - void update_joint_physical_feedback_( - size_t index, OutputInterface& angle_output, - OutputInterface& velocity_output) { - if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = nan_; - *velocity_output = nan_; - return; - } - - *angle_output = to_physical_angle_(chassis_joint_motors_[index].angle()); - *velocity_output = to_physical_velocity_(chassis_joint_motors_[index].velocity()); - } - - void update_geometry_feedback_() { - const Eigen::Vector4d alpha_rad{ - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; - const Eigen::Vector4d alpha_dot_rad{ - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; - - if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = nan_; - *encoder_alpha_dot_ = nan_; - *radius_ = nan_; - return; - } - - *encoder_alpha_ = alpha_rad.mean(); - *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); - } - - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%c %c %c %c]", - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), - wheel_rx(1), wheel_rx(2), wheel_rx(3)); - } - - if (debug_log_deformable_joint_motor_) { - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%c %c %c %c]", - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, - joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', - supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : nan_, - supercap_rx ? supercap_.chassis_voltage() : nan_, - supercap_rx ? supercap_.chassis_power() : nan_, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data); - } - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[0].store_status(data.can_data); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[1].store_status(data.can_data); - else if (data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { - gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { - gimbal_bullet_feeder_.store_status(data.can_data); - } else if (data.can_id == 0x205) - chassis_steer_motors_[2].store_status(data.can_data); - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[3].store_status(data.can_data); - } - - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - rmcs_executor::Component& command_; - - OutputInterface& tf_; - - device::Bmi088 imu_{1000, 0.2, 0.0}; - device::LkMotor gimbal_yaw_motor_{deformable_infantry_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_; - - device::DjiMotor chassis_wheel_motors_[4]{ - device::DjiMotor{deformable_infantry_, command_, "/chassis/left_front_wheel"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/left_back_wheel"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/right_back_wheel"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/right_front_wheel"}, - }; - device::DjiMotor chassis_steer_motors_[4]{ - device::DjiMotor{deformable_infantry_, command_, "/chassis/left_front_steering"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/left_back_steering"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/right_back_steering"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/right_front_steering"}, - }; - device::LkMotor chassis_joint_motors_[4]{ - device::LkMotor{deformable_infantry_, command_, "/chassis/left_front_joint"}, - device::LkMotor{deformable_infantry_, command_, "/chassis/left_back_joint"}, - device::LkMotor{deformable_infantry_, command_, "/chassis/right_back_joint"}, - device::LkMotor{deformable_infantry_, command_, "/chassis/right_front_joint"}, - }; - - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - Clock::time_point next_chassis_feedback_log_time_{ - Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{ - Clock::now() + std::chrono::seconds(1)}; - device::Supercap supercap_{deformable_infantry_, command_}; - std::atomic latest_supercap_status_{ - device::CanPacket8{uint64_t{0}}}; - std::atomic supercap_status_received_{false}; - device::DjiMotor gimbal_bullet_feeder_{ - deformable_infantry_, command_, "/gimbal/bullet_feeder"}; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; - OutputInterface left_front_joint_physical_angle_; - OutputInterface left_back_joint_physical_angle_; - OutputInterface right_back_joint_physical_angle_; - OutputInterface right_front_joint_physical_angle_; - OutputInterface left_front_joint_physical_velocity_; - OutputInterface left_back_joint_physical_velocity_; - OutputInterface right_back_joint_physical_velocity_; - OutputInterface right_front_joint_physical_velocity_; - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; - }; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class DeformableInfantryV2; - - explicit TopBoard( - DeformableInfantryV2& deformableInfantry, - DeformableInfantryV2Command& deformableInfantry_command, - std::string serial_filter = {}, bool has_external_imu_board = false) - : librmcs::agent::RmcsBoardLite( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , hard_sync_pending_(deformableInfantry.hard_sync_pending_) - , has_external_imu_board_(has_external_imu_board) - , tf_(deformableInfantry.tf_) - , bmi088_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") - , gimbal_left_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") - , gimbal_right_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") - , scope_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/scope") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - deformableInfantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - if (!has_external_imu_board_) - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); - - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(y, -x, z); - }); - } - - ~TopBoard() override = default; - - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request path. - } - - void update() { - bmi088_.update_status(); - - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - scope_motor_.update_status(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - if (!has_external_imu_board_) { - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); - - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> OdomImu, so use - // the encoder pitch from the TF tree to move the yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); - } - - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() { - auto builder = start_transmit(); - builder.can0_transmit({ - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - private: - void uart1_receive_callback(const librmcs::data::UartDataView&) override {} - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - else if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - else if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - std::atomic& hard_sync_pending_; - bool has_external_imu_board_ = false; - OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_encoder_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; - }; - - class ImuBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryV2; - - public: - explicit ImuBoard( - DeformableInfantryV2& deformableInfantry, device::Vt13& vt13, - const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , tf_{deformableInfantry.tf_} - , vt13_{vt13} - , bmi088_{1000, 0.2, 0.0} { - - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - } - - ~ImuBoard() override = default; - - void update() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); - } - - private: - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - vt13_.store_status(data.uart_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - OutputInterface gimbal_pitch_velocity_imu_; - device::Vt13& vt13_; - - device::Bmi088 bmi088_; - }; - - auto status_service_callback(const std::shared_ptr& response) - -> void { - response->success = true; - - auto feedback_message = std::ostringstream{}; - auto text = [&](std::format_string format, Args&&... args) { - std::println(feedback_message, format, std::forward(args)...); - }; - - text("Gimbal Status"); - text("- Yaw: {}", rmcs_board_lite->gimbal_yaw_motor_.last_raw_angle()); - text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - - text("Chassis Status"); - text("- left front wheel: {}", rmcs_board_lite->chassis_wheel_motors_[0].last_raw_angle()); - text("- left back wheel: {}", rmcs_board_lite->chassis_wheel_motors_[1].last_raw_angle()); - text("- right back wheel: {}", rmcs_board_lite->chassis_wheel_motors_[2].last_raw_angle()); - text("- right front wheel: {}", rmcs_board_lite->chassis_wheel_motors_[3].last_raw_angle()); - text("- left front steer: {}", rmcs_board_lite->chassis_steer_motors_[0].last_raw_angle()); - text("- left back steer: {}", rmcs_board_lite->chassis_steer_motors_[1].last_raw_angle()); - text("- right back steer: {}", rmcs_board_lite->chassis_steer_motors_[2].last_raw_angle()); - text("- right front steer: {}", rmcs_board_lite->chassis_steer_motors_[3].last_raw_angle()); - text("- left front joint: {}", rmcs_board_lite->chassis_joint_motors_[0].last_raw_angle()); - text("- left back joint: {}", rmcs_board_lite->chassis_joint_motors_[1].last_raw_angle()); - text("- right back joint: {}", rmcs_board_lite->chassis_joint_motors_[2].last_raw_angle()); - text("- right front joint: {}", rmcs_board_lite->chassis_joint_motors_[3].last_raw_angle()); - - response->message = feedback_message.str(); - } - - OutputInterface tf_; - InputInterface timestamp_; - device::Vt13 vt13_; - std::atomic hard_sync_pending_{false}; - size_t hard_sync_snapshot_count_ = 0; - Clock::time_point next_hard_sync_log_time_{}; - - std::shared_ptr deformable_infantry_command_; - std::unique_ptr rmcs_board_lite; - std::unique_ptr imu_board_; - std::unique_ptr top_board_; - std::unique_ptr remote_control_; - - std::shared_ptr> status_service_; - uint32_t cmd_tick_ = 0; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryV2, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index e577715f..4ac2b681 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -123,7 +123,7 @@ class Flight } .as_bytes()}) .can2_transmit( - {.can_id = 0x141, + {.can_id = 0x141, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) .can3_transmit( {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}); @@ -152,7 +152,7 @@ class Flight Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; tf_->set_transform(gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); + *gimbal_yaw_velocity_imu_ = bmi088_.gz(); *gimbal_pitch_velocity_imu_ = bmi088_.gy(); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index a5ac2792..21729d37 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -92,12 +92,12 @@ class OmniInfantry start_transmit().gpio_digital_read( librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart0Tx, { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, + .pull = librmcs::data::GpioPull::kUp, }); using namespace rmcs_description; // NOLINT(google-build-using-namespace) @@ -133,10 +133,10 @@ class OmniInfantry }; } - OmniInfantry(const OmniInfantry&) = delete; + OmniInfantry(const OmniInfantry&) = delete; OmniInfantry& operator=(const OmniInfantry&) = delete; - OmniInfantry(OmniInfantry&&) = delete; - OmniInfantry& operator=(OmniInfantry&&) = delete; + OmniInfantry(OmniInfantry&&) = delete; + OmniInfantry& operator=(OmniInfantry&&) = delete; ~OmniInfantry() override = default; @@ -163,7 +163,7 @@ class OmniInfantry }); builder.can1_transmit({ - .can_id = 0x145, + .can_id = 0x145, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), }); @@ -180,7 +180,7 @@ class OmniInfantry }); builder.can2_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_velocity_command().as_bytes(), }); @@ -225,7 +225,7 @@ class OmniInfantry tf_->set_transform( snapshot->orientation.conjugate()); - *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); + *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp index c4b873fa..8639ff9f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp @@ -135,10 +135,10 @@ class SteeringHeroLittle Eigen::Translation3d{0.06603, 0.0, 0.082}); } - SteeringHeroLittle(const SteeringHeroLittle&) = delete; + SteeringHeroLittle(const SteeringHeroLittle&) = delete; SteeringHeroLittle& operator=(const SteeringHeroLittle&) = delete; - SteeringHeroLittle(SteeringHeroLittle&&) = delete; - SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; + SteeringHeroLittle(SteeringHeroLittle&&) = delete; + SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; ~SteeringHeroLittle() override = default; @@ -292,10 +292,10 @@ class SteeringHeroLittle }); } - TopBoard(const TopBoard&) = delete; + TopBoard(const TopBoard&) = delete; TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; ~TopBoard() final = default; @@ -311,7 +311,7 @@ class SteeringHeroLittle tf_->set_transform( gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_imu_ = imu_.gz(); + *gimbal_yaw_velocity_imu_ = imu_.gz(); *gimbal_pitch_velocity_imu_ = imu_.gy(); gimbal_top_yaw_motor_.update_status(); @@ -336,7 +336,7 @@ class SteeringHeroLittle last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); - *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); + *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); } void command_update() { @@ -344,19 +344,19 @@ class SteeringHeroLittle if (std::isfinite(gimbal_pitch_motor_.control_angle())) builder.can0_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_pitch_motor_ .generate_angle_command(gimbal_pitch_motor_.control_angle()) .as_bytes(), }); else builder.can0_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), }); // Used to distinguish pitch encoder control from IMU control. builder.can0_transmit({ - .can_id = 0x141, + .can_id = 0x141, .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), }); @@ -393,7 +393,7 @@ class SteeringHeroLittle }); builder.can3_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), }); @@ -401,14 +401,14 @@ class SteeringHeroLittle librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], { .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, + .pull = librmcs::data::GpioPull::kUp, }); builder.gpio_digital_read( librmcs::spec::rmcs_board_lite::kGpioDescriptors[3], { .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, + .pull = librmcs::data::GpioPull::kUp, }); } @@ -630,10 +630,10 @@ class SteeringHeroLittle steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); } - BottomBoard(const BottomBoard&) = delete; + BottomBoard(const BottomBoard&) = delete; BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; ~BottomBoard() final = default; @@ -727,7 +727,7 @@ class SteeringHeroLittle }); builder.can3_transmit({ - .can_id = 0x141, + .can_id = 0x141, .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), }); diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 1d70f647..5b56face 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -3,8 +3,11 @@ #include #include #include +#include #include +#include +#include #include #include #include @@ -56,6 +59,7 @@ class DeformableInfantry register_input("/predefined/timestamp", timestamp_); register_input("/chassis/control_mode", chassis_mode_); + register_input("/chassis/active_suspension/active", active_suspension_active_); register_input("/chassis/angle", chassis_angle_); register_input("/chassis/supercap/voltage", supercap_voltage_); @@ -63,19 +67,11 @@ class DeformableInfantry register_input("/chassis/voltage", chassis_voltage_); - register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); - - register_input( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, false); - register_input( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, false); - register_input( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, false); - register_input( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, false); + for (size_t i = 0; i < kJointCount; ++i) { + register_input( + fmt::format("/chassis/{}_joint/physical_angle", kJointName[i]), + joint_physical_angle_[i], false); + } register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); @@ -86,6 +82,7 @@ class DeformableInfantry register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); + register_input("/auto_aim/robot_center", auto_aim_robot_center_, false); register_input("/referee/game/stage", game_stage_); @@ -100,6 +97,7 @@ class DeformableInfantry update_chassis_direction_indicator(); update_deformable_chassis_leg_arcs(); update_ctrl_ui(); + update_auto_aim_feedback(); status_ring_.update_bullet_allowance(*robot_bullet_allowance_); const double friction_wheel_speed = @@ -124,12 +122,14 @@ class DeformableInfantry private: void update_ctrl_ui() { const bool ctrl_active = keyboard_.ready() && keyboard_->ctrl; - const double reveal = ctrl_transition_.update(*timestamp_, ctrl_active); - - crosshair_circle_.set_x(static_cast(std::lround( - static_cast(crosshair_base_x_) + 45.0 * reveal))); - crosshair_circle_.set_y(static_cast(std::lround( - static_cast(crosshair_base_y_) + 20.0 * reveal))); + const double reveal = ctrl_transition_.update(*timestamp_, ctrl_active); + + crosshair_circle_.set_x( + static_cast( + std::lround(static_cast(crosshair_base_x_) + 45.0 * reveal))); + crosshair_circle_.set_y( + static_cast( + std::lround(static_cast(crosshair_base_y_) + 20.0 * reveal))); } void update_time_reminder() { @@ -137,6 +137,25 @@ class DeformableInfantry return; } + void update_auto_aim_feedback() { + if (!auto_aim_robot_center_.ready() || !auto_aim_robot_center_->allFinite()) { + target_distance_indicator_.set_visible(false); + return; + } + + const double distance = auto_aim_robot_center_->norm(); + if (!std::isfinite(distance)) { + target_distance_indicator_.set_visible(false); + return; + } + + target_distance_text_index_ ^= 1u; + auto& text = target_distance_text_[target_distance_text_index_]; + std::snprintf(text.data(), text.size(), "%.1fm", distance); + target_distance_indicator_.set_value(text.data()); + target_distance_indicator_.set_visible(true); + } + void update_chassis_direction_indicator() { auto chassis_mode = *chassis_mode_; @@ -154,27 +173,31 @@ class DeformableInfantry } void update_deformable_chassis_leg_arcs() { - if (!left_front_joint_physical_angle_.ready() || !left_back_joint_physical_angle_.ready() - || !right_back_joint_physical_angle_.ready() - || !right_front_joint_physical_angle_.ready()) { + if (!std::all_of( + joint_physical_angle_.begin(), joint_physical_angle_.end(), + [](const auto& j) { return j.ready(); })) { + deformable_chassis_leg_arcs_.set_visible(false); + return; + } + + if (!chassis_angle_.ready()) { deformable_chassis_leg_arcs_.set_visible(false); return; } - const std::array leg_angles = { - *left_front_joint_physical_angle_, - *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, - *right_front_joint_physical_angle_, - }; - deformable_chassis_leg_arcs_.update(*chassis_angle_, leg_angles); + std::array leg_angles; + for (size_t i = 0; i < kJointCount; ++i) + leg_angles[i] = *joint_physical_angle_[i]; + deformable_chassis_leg_arcs_.update( + *chassis_angle_, leg_angles, + active_suspension_active_.ready() && *active_suspension_active_); } static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; - static constexpr double friction_wheel_speed_indicator_radius_ = 430.0; + static constexpr double friction_wheel_speed_indicator_radius_ = 430.0; static constexpr uint16_t friction_wheel_speed_indicator_font_size_ = 20; - static constexpr double supercap_cutoff_voltage = 8.0; + static constexpr double supercap_cutoff_voltage = 8.0; static uint16_t friction_wheel_speed_indicator_center_x() { return static_cast(std::lround( @@ -191,6 +214,7 @@ class DeformableInfantry InputInterface timestamp_; InputInterface chassis_mode_; + InputInterface active_suspension_active_; InputInterface chassis_angle_; InputInterface supercap_voltage_; @@ -198,10 +222,15 @@ class DeformableInfantry InputInterface chassis_voltage_; - InputInterface left_front_velocity_, left_back_velocity_, right_back_velocity_, - right_front_velocity_; - InputInterface left_front_joint_physical_angle_, left_back_joint_physical_angle_, - right_back_joint_physical_angle_, right_front_joint_physical_angle_; + static constexpr size_t kJointCount = 4; + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; + + std::array, kJointCount> joint_physical_angle_; InputInterface robot_bullet_allowance_; @@ -212,6 +241,7 @@ class DeformableInfantry InputInterface mouse_; InputInterface keyboard_; + InputInterface auto_aim_robot_center_; InputInterface game_stage_; @@ -224,6 +254,10 @@ class DeformableInfantry Arc chassis_direction_indicator_; DeformableChassisLegArcs deformable_chassis_leg_arcs_; + Text target_distance_indicator_{ + Shape::Color::GREEN, 20, 2, x_center + 34, y_center + 24, "", false}; + std::array, 2> target_distance_text_{}; + size_t target_distance_text_index_ = 0; AnimatedToggle ctrl_transition_{}; uint16_t crosshair_base_x_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp index a28dc60e..e2916919 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp @@ -1,7 +1,9 @@ #include +#include #include #include +#include #include #include #include @@ -56,10 +58,11 @@ class Infantry register_input("/chassis/control_power_limit", chassis_control_power_limit_); register_input("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_); - register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); + for (size_t i = 0; i < 4; ++i) { + register_input( + fmt::format("/chassis/{}_wheel/velocity", kWheelName[i]), + wheel_velocity_[i]); + } register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); @@ -146,8 +149,11 @@ class Infantry InputInterface chassis_control_power_limit_; InputInterface supercap_charge_power_limit_; - InputInterface left_front_velocity_, left_back_velocity_, right_back_velocity_, - right_front_velocity_; + static constexpr const char* kWheelName[] = { + "left_front", "left_back", "right_back", "right_front", + }; + + std::array, 4> wheel_velocity_; InputInterface robot_bullet_allowance_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp index 80ff5e7c..0b6bd794 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp @@ -21,7 +21,7 @@ class DeformableChassisLegArcs { std::swap(min_angle_rad_, max_angle_rad_); } - void update(double chassis_angle, const std::array& leg_angles) { + void update(double chassis_angle, const std::array& leg_angles, bool active_suspension) { if (!valid_angle_range_()) { set_visible(false); return; @@ -36,7 +36,7 @@ class DeformableChassisLegArcs { last_leg_angles_[i] = leg_angles[i]; update_leg_( legs_[i], last_chassis_angle_ + leg_base_mid_angles_[i], leg_radii_near_[i], - leg_radii_far_[i], last_leg_angles_[i]); + leg_radii_far_[i], last_leg_angles_[i], active_suspension); } } @@ -71,9 +71,9 @@ class DeformableChassisLegArcs { static constexpr std::array leg_base_mid_angles_ = { front_pair_offset_deg_ * degrees_to_radians_, - std::numbers::pi_v - rear_pair_offset_deg_ * degrees_to_radians_, - std::numbers::pi_v + rear_pair_offset_deg_ * degrees_to_radians_, - -front_pair_offset_deg_ * degrees_to_radians_, + std::numbers::pi_v - rear_pair_offset_deg_* degrees_to_radians_, + std::numbers::pi_v + rear_pair_offset_deg_* degrees_to_radians_, + -front_pair_offset_deg_* degrees_to_radians_, }; static constexpr std::array leg_radii_near_ = { @@ -106,17 +106,13 @@ class DeformableChassisLegArcs { (leg_angle - min_angle_rad_) / (max_angle_rad_ - min_angle_rad_), 0.0, 1.0); } - static Shape::Color leg_color_(double normalized_extension) { - if (normalized_extension < 1.0 / 3.0) - return Shape::Color::ORANGE; - if (normalized_extension < 2.0 / 3.0) - return Shape::Color::YELLOW; - return Shape::Color::WHITE; + static Shape::Color leg_color_(bool active_suspension) { + return active_suspension ? Shape::Color::YELLOW : Shape::Color::WHITE; } void update_leg_( Arc& leg, double body_angle, uint16_t near_radius, uint16_t far_radius, - double leg_angle) const { + double leg_angle, bool active_suspension) const { const double normalized_extension = normalized_leg_extension_(leg_angle); // Min angle looks like a thin leg stretching radially outward from the center ring. const uint16_t radius = static_cast( @@ -131,7 +127,7 @@ class DeformableChassisLegArcs { leg.set_y(center_y_); leg.set_r(radius); leg.set_width(width); - leg.set_color(leg_color_(normalized_extension)); + leg.set_color(leg_color_(active_suspension)); leg.set_angle(to_referee_angle_(body_angle), half_angle); } diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp index 57167144..cea7953c 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp @@ -165,4 +165,4 @@ class Ui #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) \ No newline at end of file