diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml new file mode 100644 index 000000000..ac97a8e38 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -0,0 +1,364 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::DeformableInfantryOmni -> 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::Infantry -> 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::DeformableOmniWheelController -> 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 + # - rmcs::AutoAimComponent -> auto_aim_component + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + +deformable_infantry: + ros__parameters: + serial_filter_rmcs_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" + serial_filter_top_board: "D4-0262-9E84-E715-CADB-9894-7241" + serial_filter_imu: "AF-8217-B05F-811B-6F3E-04EA-448E-9D03-CA2C" + left_front_zero_point: 374 + left_back_zero_point: 5801 + right_back_zero_point: 7817 + right_front_zero_point: 7136 + yaw_motor_zero_point: 50642 + pitch_motor_zero_point: 6245 + 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: 8.0 + max_angle: 58.0 + active_suspension_enable: true + spin_ratio: 1.0 + + # Suspension geometry / support model. + active_suspension_mass: 22.5 + active_suspension_rod_length: 0.150 + active_suspension_Kz: 150.0 + active_suspension_D_leg: 10.0 + active_suspension_torque_limit: 80.0 + active_suspension_gravity_comp_gain: 1.0 + active_suspension_control_acceleration_limit: 6.0 + active_suspension_preload_angle_deg: 8.0 + active_suspension_entry_offset_deg: 1.5 + active_suspension_ride_height_offset_deg: 3.0 + active_suspension_hold_travel_deg: 5.0 + active_suspension_activation_velocity_threshold_deg: 15.0 + + # IMU attitude correction as suspension force bias. + 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: + upper_limit: -0.61 # -35 deg + lower_limit: 0.05 # 6 deg + + yaw_angle_kp: 30.0 + yaw_angle_ki: 0.0 + yaw_angle_kd: 0.0 + + yaw_velocity_kp: 15.0 + yaw_velocity_ki: 0.0 + yaw_velocity_kd: 0.0 + + yaw_vel_ff_gain: 0.47 + yaw_acc_ff_gain: 0.00 + + pitch_angle_kp: 25.0 + pitch_angle_ki: 0.0 + pitch_angle_kd: 0.0 + + pitch_velocity_kp: 2.2 + pitch_velocity_ki: 0.0 + pitch_velocity_kd: 0.0 + + pitch_acc_ff_gain: 0.10 + + pitch_torque_control: true + + pitch_fusion_enabled: true + pitch_fusion_alpha: 0.98 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 600.0 + - 600.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + 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.5 + ki: 0.0 + kd: 0.0 + +deformable_chassis_controller: + ros__parameters: + mass: 22.5 + moment_of_inertia: 1.0 + chassis_radius: 0.2341741 + rod_length: 0.150 + wheel_radius: 0.055 + friction_coefficient: 66.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 + measurement_velocity: /chassis/left_front_joint/physical_velocity + setpoint_angle: /chassis/left_front_joint/target_physical_angle + setpoint_velocity: /chassis/left_front_joint/target_physical_velocity + mode_input: /chassis/left_front_joint/suspension_mode + suspension_torque: /chassis/left_front_joint/suspension_torque + control: /chassis/left_front_joint/control_torque + + # Normal ADRC servo mode + dt: 0.001 + b0: -0.60 + 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 + + # Suspension ADRC servo mode + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + + # Joint-local feedforward / limit shaping + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 + +lb_joint_controller: + ros__parameters: + # Same joint-servo layout as lf_joint_controller + measurement_angle: /chassis/left_back_joint/physical_angle + measurement_velocity: /chassis/left_back_joint/physical_velocity + setpoint_angle: /chassis/left_back_joint/target_physical_angle + setpoint_velocity: /chassis/left_back_joint/target_physical_velocity + mode_input: /chassis/left_back_joint/suspension_mode + suspension_torque: /chassis/left_back_joint/suspension_torque + control: /chassis/left_back_joint/control_torque + dt: 0.001 + b0: -0.60 + 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 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 + +rb_joint_controller: + ros__parameters: + # Same joint-servo layout as lf_joint_controller + measurement_angle: /chassis/right_back_joint/physical_angle + measurement_velocity: /chassis/right_back_joint/physical_velocity + setpoint_angle: /chassis/right_back_joint/target_physical_angle + setpoint_velocity: /chassis/right_back_joint/target_physical_velocity + mode_input: /chassis/right_back_joint/suspension_mode + suspension_torque: /chassis/right_back_joint/suspension_torque + control: /chassis/right_back_joint/control_torque + dt: 0.001 + b0: -0.60 + 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 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 + +rf_joint_controller: + ros__parameters: + # Same joint-servo layout as lf_joint_controller + measurement_angle: /chassis/right_front_joint/physical_angle + measurement_velocity: /chassis/right_front_joint/physical_velocity + setpoint_angle: /chassis/right_front_joint/target_physical_angle + setpoint_velocity: /chassis/right_front_joint/target_physical_velocity + mode_input: /chassis/right_front_joint/suspension_mode + suspension_torque: /chassis/right_front_joint/suspension_torque + control: /chassis/right_front_joint/control_torque + dt: 0.001 + b0: -0.60 + 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 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.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 new file mode 100644 index 000000000..b1ec953d6 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -0,0 +1,358 @@ +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: + - /chassis/left_front_joint/torque + - /chassis/left_back_joint/torque + - /chassis/right_front_joint/torque + - /chassis/right_back_joint/torque + - /chassis/left_front_joint/suspension_mode + - /chassis/left_back_joint/suspension_mode + - /chassis/right_front_joint/suspension_mode + - /chassis/right_back_joint/suspension_mode + - /chassis/left_front_joint/suspension_torque + - /chassis/left_back_joint/suspension_torque + - /chassis/right_front_joint/suspension_torque + - /chassis/right_back_joint/suspension_torque + - /chassis/left_front_joint/control_torque + - /chassis/left_back_joint/control_torque + - /chassis/right_front_joint/control_torque + - /chassis/right_back_joint/control_torque + +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" + 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.61 # -35 deg + lower_limit: 0.05 # 6 deg + use_encoder_pitch: true + + 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: + - 600.0 + - 600.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + 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.055 + 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 + measurement_velocity: /chassis/left_front_joint/physical_velocity + setpoint_angle: /chassis/left_front_joint/target_physical_angle + setpoint_velocity: /chassis/left_front_joint/target_physical_velocity + mode_input: /chassis/left_front_joint/suspension_mode + suspension_torque: /chassis/left_front_joint/suspension_torque + control: /chassis/left_front_joint/control_torque + + # Normal ADRC servo mode + 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 + + # Suspension ADRC servo mode + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + + # Joint-local feedforward / limit shaping + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 + +lb_joint_controller: + ros__parameters: + # Same joint-servo layout as lf_joint_controller + measurement_angle: /chassis/left_back_joint/physical_angle + measurement_velocity: /chassis/left_back_joint/physical_velocity + setpoint_angle: /chassis/left_back_joint/target_physical_angle + setpoint_velocity: /chassis/left_back_joint/target_physical_velocity + mode_input: /chassis/left_back_joint/suspension_mode + suspension_torque: /chassis/left_back_joint/suspension_torque + 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 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 + +rb_joint_controller: + ros__parameters: + # Same joint-servo layout as lf_joint_controller + measurement_angle: /chassis/right_back_joint/physical_angle + measurement_velocity: /chassis/right_back_joint/physical_velocity + setpoint_angle: /chassis/right_back_joint/target_physical_angle + setpoint_velocity: /chassis/right_back_joint/target_physical_velocity + mode_input: /chassis/right_back_joint/suspension_mode + suspension_torque: /chassis/right_back_joint/suspension_torque + 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 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 + +rf_joint_controller: + ros__parameters: + # Same joint-servo layout as lf_joint_controller + measurement_angle: /chassis/right_front_joint/physical_angle + measurement_velocity: /chassis/right_front_joint/physical_velocity + setpoint_angle: /chassis/right_front_joint/target_physical_angle + setpoint_velocity: /chassis/right_front_joint/target_physical_velocity + mode_input: /chassis/right_front_joint/suspension_mode + suspension_torque: /chassis/right_front_joint/suspension_torque + 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 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 85e636c1c..99c037100 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -27,15 +27,27 @@ rmcs_executor: # - rmcs::navigation::Navigation -> rmcs_navigation + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/bottom_yaw/angle + - /gimbal/bottom_yaw/velocity + - /gimbal/pitch/angle + - /gimbal/pitch/control_torque + - /gimbal/top_yaw/angle + - /gimbal/top_yaw/velocity + # The positive direction is the one that battery exists sentry_hardware: ros__parameters: board_serial_top_board: "af-da30" board_serial_bottom_board: "d4-2184" board_serial_gimbal_board: "d4-1d2b" - bottom_yaw_motor_zero_point: 26163 - top_yaw_motor_zero_point: 4540 - pitch_motor_zero_point: 1446 + bottom_yaw_motor_zero_point: 23932 + top_yaw_motor_zero_point: 32916 + pitch_motor_zero_point: 1275 left_front_zero_point: 7131 left_back_zero_point: 3740 right_back_zero_point: 1327 @@ -57,23 +69,23 @@ gimbal_controller: upper_limit: -0.39518 lower_limit: 0.36 - top_yaw_angle_kp: 30.0 + top_yaw_angle_kp: 45.0 top_yaw_angle_ki: 0.0 top_yaw_angle_kd: 0.0 top_yaw_velocity_kp: 2.160 top_yaw_velocity_ki: 0.0 top_yaw_velocity_kd: 0.0 - bottom_yaw_angle_kp: 8.8 + bottom_yaw_angle_kp: 15.0 bottom_yaw_angle_ki: 0.0 bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 22.49 + bottom_yaw_velocity_kp: 22.5 bottom_yaw_velocity_ki: 0.0 bottom_yaw_velocity_kd: 0.0 - pitch_angle_kp: 40.0 + pitch_angle_kp: 32.0 pitch_angle_ki: 0.0 - pitch_angle_kd: 0.01 + pitch_angle_kd: 0.0 pitch_velocity_kp: 2.5 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index db9ff7c45..f314d28fc 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -4,13 +4,22 @@ + + + + + + + + + @@ -20,6 +29,7 @@ + @@ -27,5 +37,8 @@ + + + diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/README.md b/rmcs_ws/src/rmcs_core/src/controller/adrc/README.md new file mode 100644 index 000000000..3f8fa6d5d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/README.md @@ -0,0 +1,286 @@ +# ADRC V2 说明（公式 + 变量含义） + +## 1. 总体结构 + +单通道 ADRC 每周期执行顺序： + +1. TD: `r -> x1, x2` +2. ESO: `y, u(k-1) -> z1, z2, z3` +3. NLESF: `e1, e2, z3, b0 -> u` +4. 输出缩放与限幅: `u_out = clamp(kt * u, output_min, output_max)` + +其中： +- `r`: 参考输入（目标） +- `y`: 测量反馈 +- `u`: 控制器内部输出 +- `u_out`: 组件最终输出 + +--- + +## 2. TD（Tracking Differentiator） + +### 2.1 状态与作用 + +- `x1`: 平滑后的参考轨迹 +- `x2`: 平滑后的参考导数 + +TD 作用：把突变目标 `r` 变成平滑的 `(x1, x2)`，降低控制冲击。 + +### 2.2 连续形式 + +```text +x1_dot = x2 +x2_dot = fhan(x1 - r, x2, r_td, h) +``` + +### 2.3 离散更新（代码实现） + +```text +x1(k+1) = x1(k) + h * x2(k) +x2(k+1) = x2(k) + h * fh +fh = fhan(x1 - r, x2, r_td, h) +``` + +### 2.4 fhan 子公式 + +```text +d = r_td * h^2 +a0 = h * x2 +y = (x1 - r) + a0 +a1 = sqrt(d * (d + 8 * abs(y))) + +if abs(y) > d: + a = a0 + sign(y) * (a1 - d) / 2 +else: + a = a0 + y + +if abs(a) <= d: + fhan = -r_td * a / d +else: + fhan = -r_td * sign(a) +``` + +### 2.5 TD 变量物理含义 + +| 变量 | 含义 | 典型影响 | +|---|---|---| +| `r` | 原始目标输入 | 上层给定目标 | +| `x1` | 平滑目标 | 作为位置/主误差基准 | +| `x2` | 平滑目标导数 | 作为速度/辅误差基准 | +| `r_td` | TD 跟踪速度参数 | 大则快，小则更平滑 | +| `h` | TD 步长 | 通常与 `dt` 同量级 | +| `fh` | TD 内部“加速度”输出 | 驱动 `x2` 更新 | + +--- + +## 3. ESO（Extended State Observer） + +### 3.1 状态定义 + +- `z1`: 对输出 `y` 的估计 +- `z2`: 对输出一阶导的估计 +- `z3`: 对总扰动的估计（外扰 + 未建模项 + 参数误差） + +### 3.2 观测误差 + +```text +e = z1 - y +``` + +### 3.3 连续形式（LESO） + +```text +z1_dot = z2 - beta1 * e +z2_dot = z3 + b0 * u - beta2 * e +z3_dot = -beta3 * e +``` + +### 3.4 离散更新（代码实现） + +```text +z1(k+1) = z1(k) + h * (z2 - beta1 * e) +z2(k+1) = z2(k) + h * (z3 + b0 * u - beta2 * e) +z3(k+1) = z3(k) + h * (-beta3 * e) +``` + +### 3.5 带宽法自动整定 + +当 `eso_auto_beta = true`： + +```text +beta1 = 3 * w0 +beta2 = 3 * w0^2 +beta3 = w0^3 +``` + +### 3.6 ESO 变量物理含义 + +| 变量 | 含义 | 典型影响 | +|---|---|---| +| `y` | 传感器测量值 | 观测器输入 | +| `u` | 控制输入（本实现用上拍输出） | 进入系统模型输入项 | +| `b0` | 标称控制增益 | 控制量到输出变化的比例 | +| `w0` | ESO 带宽 | 大则估计快但更敏感噪声 | +| `beta1/2/3` | 观测器注入增益 | 决定误差收敛速度 | +| `z3` | 总扰动估计 | 用于控制补偿 | + +--- + +## 4. NLESF（Nonlinear Error State Feedback） + +### 4.1 误差定义 + +```text +e1 = x1 - z1 +e2 = x2 - z2 +``` + +- `e1`: 轨迹误差（主误差） +- `e2`: 导数误差（阻尼/动态误差） + +### 4.2 非线性函数 fal + +```text +fal(e, alpha, delta) = + e / delta^(1 - alpha), if |e| <= delta + |e|^alpha * sign(e), if |e| > delta +``` + +含义： +- 小误差线性化，抑制抖动； +- 大误差非线性增益，提高收敛速度。 + +### 4.3 控制律 + +先计算反馈控制项： + +```text +u0 = k1 * fal(e1, alpha1, delta) + + k2 * fal(e2, alpha2, delta) +``` + +再做扰动补偿： + +```text +u = (u0 - z3) / b0 +``` + +最后内部限幅： + +```text +u = clamp(u, u_min, u_max) +``` + +### 4.4 NLESF 变量物理含义 + +| 变量 | 含义 | 典型影响 | +|---|---|---| +| `k1` | 主误差增益 | 大则位置误差收敛更快 | +| `k2` | 导数误差增益 | 大则阻尼更强 | +| `alpha1/alpha2` | fal 指数 | 改变非线性强度 | +| `delta` | fal 线性区阈值 | 小则更“非线性” | +| `u0` | 未补偿控制量 | 仅反馈项 | +| `u` | 补偿后控制量 | 实际控制基础输出 | + +--- + +## 5. `adrc_controller_v2.cpp` 里的整合关系 + +每周期核心计算： + +```text +td_out = TD.update(reference) +eso_out = ESO.update(measurement, last_u) + +e1 = td_out.x1 - eso_out.z1 +e2 = td_out.x2 - eso_out.z2 + +nlesf_out = NLESF.compute(e1, e2, eso_out.z3, b0) +u = nlesf_out.u + +u_out = clamp(kt * u, output_min, output_max) +last_u = u_out +``` + +### error-input 模式 + +若未提供 `setpoint`/`target`，则输入 `measurement` 被当作误差 `e = r - y`，内部等效转换： + +```text +reference = 0 +measurement = -e +``` + +--- + +## 6. 参数接口（简化 YAML 友好） + +### 6.1 必要参数 + +| 参数 | 说明 | +|---|---| +| `measurement` | 测量输入通道名 | +| `control` | 控制输出通道名 | + +### 6.2 可选输入 + +| 参数 | 说明 | +|---|---| +| `setpoint` 或 `target` | 参考输入通道名 | + +### 6.3 通用参数 + +| 参数 | 默认值 | 说明 | +|---|---:|---| +| `dt` | `0.001` | 控制周期（秒） | +| `b0` | `1.0` | 标称增益 | +| `kt` | `1.0` | 输出缩放 | +| `output_min` | `-inf` | 最终输出下限 | +| `output_max` | `+inf` | 最终输出上限 | +| `init_reference` | `0.0` | TD 初始参考 | +| `init_measurement` | `0.0` | ESO 初始测量 | + +### 6.4 TD 参数 + +| 参数 | 默认值 | 说明 | +|---|---:|---| +| `td_h` | `dt` | TD 步长 | +| `td_r` | `300.0` | TD 跟踪速度 | +| `td_max_vel` | `+inf` | TD 速度限幅 | +| `td_max_acc` | `+inf` | TD 加速度限幅 | + +### 6.5 ESO 参数 + +| 参数 | 默认值 | 说明 | +|---|---:|---| +| `eso_w0` | `80.0` | ESO 带宽 | +| `eso_auto_beta` | `true` | 自动生成 beta | +| `eso_beta1` | `3*w0` | 手动 beta1 | +| `eso_beta2` | `3*w0^2` | 手动 beta2 | +| `eso_beta3` | `w0^3` | 手动 beta3 | +| `eso_z3_limit` | `1e9` | 扰动估计限幅 | + +### 6.6 NLESF 参数 + +| 参数 | 默认值 | 说明 | +|---|---:|---| +| `k1` | `50.0` | 主误差增益 | +| `k2` | `5.0` | 导数误差增益 | +| `alpha1` | `0.75` | fal 指数 1 | +| `alpha2` | `1.25` | fal 指数 2 | +| `delta` | `0.01` | fal 线性区阈值 | +| `u_min` | `-inf` | NLESF 内部下限 | +| `u_max` | `+inf` | NLESF 内部上限 | + +--- + +## 7. 变量总览速查 + +| 类别 | 变量 | +|---|---| +| 参考轨迹 | `r, x1, x2` | +| 测量与观测 | `y, z1, z2, z3, e` | +| 反馈误差 | `e1, e2` | +| 控制量 | `u0, u, u_out` | +| 参数 | `b0, w0, beta1, beta2, beta3, k1, k2, alpha1, alpha2, delta, h` | + diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/adrc_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/adrc_controller.cpp new file mode 100644 index 000000000..1cec46551 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/adrc_controller.cpp @@ -0,0 +1,202 @@ +#include +#include +#include + +#include +#include + +#include "eso.hpp" +#include "nlesf.hpp" +#include "td.hpp" + +namespace rmcs_core::controller::adrc { + +class AdrcController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + AdrcController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , td_(load_td_config()) + , eso_(load_eso_config()) + , nlesf_(load_nlesf_config()) { + register_input(get_parameter("measurement").as_string(), measurement_); + if (has_parameter("limit")) { + register_input(get_parameter("limit").as_string(), limit_); + use_dynamic_limit_ = true; + } + + if (has_parameter("setpoint")) { + register_input(get_parameter("setpoint").as_string(), setpoint_); + use_error_input_mode_ = false; + } else if (has_parameter("target")) { + register_input(get_parameter("target").as_string(), setpoint_); + use_error_input_mode_ = false; + } else { + use_error_input_mode_ = true; + } + + register_output(get_parameter("control").as_string(), control_); + + kt_ = get_parameter_or("kt", 1.0); + b0_ = get_parameter_or("b0", 1.0); + + output_min_ = get_parameter_or("output_min", -std::numeric_limits::infinity()); + output_max_ = get_parameter_or("output_max", std::numeric_limits::infinity()); + if (output_min_ > output_max_) { + std::swap(output_min_, output_max_); + } + + const double init_reference = get_parameter_or("init_reference", 0.0); + const double init_measurement = get_parameter_or("init_measurement", 0.0); + td_.reset(init_reference, 0.0); + eso_.reset(init_measurement); + + if (use_error_input_mode_) { + RCLCPP_WARN( + get_logger(), "ADRC V2 setpoint/target not found, using error-input mode: " + "measurement input is treated as error (setpoint - measurement)."); + } + } + + void update() override { + if (!refresh_limit_()) { + disable_output(); + return; + } + + const double measurement_or_error = *measurement_; + if (!std::isfinite(measurement_or_error)) { + disable_output(); + return; + } + + double reference = 0.0; + double measurement = 0.0; + + if (use_error_input_mode_) { + reference = 0.0; + measurement = -measurement_or_error; + } else { + reference = *setpoint_; + if (!std::isfinite(reference)) { + disable_output(); + return; + } + measurement = measurement_or_error; + } + + const auto td_out = td_.update(reference); + const auto eso_out = eso_.update(measurement, last_u_); + + const double e1 = td_out.x1 - eso_out.z1; + const double e2 = td_out.x2 - eso_out.z2; + + const auto nlesf_out = nlesf_.compute(e1, e2, eso_out.z3, b0_); + const double scaled_u = kt_ * nlesf_out.u; + + const double final_u = std::clamp(scaled_u, output_min_, output_max_); + *control_ = final_u; + last_u_ = final_u; + } + +private: + bool refresh_limit_() { + if (!use_dynamic_limit_) { + return true; + } + + if (!limit_.ready()) { + return false; + } + + const double raw_limit = *limit_; + if (!std::isfinite(raw_limit)) { + return false; + } + + const double effective_limit = std::max(0.0, raw_limit); + if (std::isfinite(last_limit_) && std::abs(effective_limit - last_limit_) <= 1e-9) { + return true; + } + + auto cfg = nlesf_.config(); + cfg.u_min = -effective_limit; + cfg.u_max = effective_limit; + nlesf_.set_config(cfg); + last_limit_ = effective_limit; + return true; + } + + void disable_output() { + *control_ = std::numeric_limits::quiet_NaN(); + last_u_ = 0.0; + td_.reset(); + eso_.reset(); + } + + TD::Config load_td_config() { + TD::Config cfg; + const double h = get_parameter_or("dt", 0.001); + cfg.h = get_parameter_or("td_h", h); + cfg.r = get_parameter_or("td_r", 300.0); + cfg.max_vel = get_parameter_or("td_max_vel", std::numeric_limits::infinity()); + cfg.max_acc = get_parameter_or("td_max_acc", std::numeric_limits::infinity()); + return cfg; + } + + ESO::Config load_eso_config() { + ESO::Config cfg; + cfg.h = get_parameter_or("dt", 0.001); + cfg.b0 = get_parameter_or("b0", 1.0); + cfg.w0 = get_parameter_or("eso_w0", 80.0); + cfg.auto_beta = get_parameter_or("eso_auto_beta", true); + cfg.beta1 = get_parameter_or("eso_beta1", 3.0 * cfg.w0); + cfg.beta2 = get_parameter_or("eso_beta2", 3.0 * cfg.w0 * cfg.w0); + cfg.beta3 = get_parameter_or("eso_beta3", cfg.w0 * cfg.w0 * cfg.w0); + cfg.z3_limit = get_parameter_or("eso_z3_limit", 1e9); + return cfg; + } + + NLESF::Config load_nlesf_config() { + NLESF::Config cfg; + cfg.k1 = get_parameter_or("k1", 50.0); + cfg.k2 = get_parameter_or("k2", 5.0); + cfg.alpha1 = get_parameter_or("alpha1", 0.75); + cfg.alpha2 = get_parameter_or("alpha2", 1.25); + cfg.delta = get_parameter_or("delta", 0.01); + cfg.u_min = get_parameter_or("u_min", -std::numeric_limits::infinity()); + cfg.u_max = get_parameter_or("u_max", std::numeric_limits::infinity()); + return cfg; + } + + InputInterface measurement_; + InputInterface setpoint_; + + InputInterface limit_; + + OutputInterface control_; + + TD td_; + ESO eso_; + NLESF nlesf_; + + bool use_error_input_mode_ = false; + bool use_dynamic_limit_ = false; + + double b0_ = 1.0; + double kt_ = 1.0; + double last_u_ = 0.0; + double last_limit_ = std::numeric_limits::quiet_NaN(); + + double output_min_ = -std::numeric_limits::infinity(); + double output_max_ = std::numeric_limits::infinity(); +}; + +} // namespace rmcs_core::controller::adrc + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::adrc::AdrcController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp new file mode 100644 index 000000000..e7d9d6eec --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp @@ -0,0 +1,90 @@ +#pragma once + +#include +#include + +namespace rmcs_core::controller::adrc { + +class ESO { +public: + struct Config { + double h = 0.001; + double b0 = 1.0; + double w0 = 80.0; + bool auto_beta = true; + double beta1 = 240.0; + double beta2 = 19200.0; + double beta3 = 512000.0; + double z3_limit = 1e9; + }; + + struct State { + double z1 = 0.0; + double z2 = 0.0; + double z3 = 0.0; + }; + + struct Output { + double z1 = 0.0; + double z2 = 0.0; + double z3 = 0.0; + double e = 0.0; + }; + + ESO() + : ESO(Config{}) {} + + explicit ESO(const Config& cfg) { set_config(cfg); } + + void set_config(const Config& cfg) { + cfg_ = cfg; + sanitize_config(); + if (cfg_.auto_beta) { + cfg_.beta1 = 3.0 * cfg_.w0; + cfg_.beta2 = 3.0 * cfg_.w0 * cfg_.w0; + cfg_.beta3 = cfg_.w0 * cfg_.w0 * cfg_.w0; + } + } + + const Config& config() const { return cfg_; } + const State& state() const { return state_; } + + void reset(double init_y = 0.0) { + state_.z1 = init_y; + state_.z2 = 0.0; + state_.z3 = 0.0; + } + + Output update(double y, double u) { + const double e = state_.z1 - y; + state_.z1 += cfg_.h * (state_.z2 - cfg_.beta1 * e); + state_.z2 += cfg_.h * (state_.z3 + cfg_.b0 * u - cfg_.beta2 * e); + state_.z3 += cfg_.h * (-cfg_.beta3 * e); + state_.z3 = clamp(state_.z3, -cfg_.z3_limit, cfg_.z3_limit); + + return Output{state_.z1, state_.z2, state_.z3, e}; + } + +private: + static double clamp(double x, double lo, double hi) { + return std::max(lo, std::min(x, hi)); + } + + void sanitize_config() { + constexpr double kEps = 1e-9; + cfg_.h = std::max(cfg_.h, kEps); + if (std::fabs(cfg_.b0) < kEps) { + cfg_.b0 = (cfg_.b0 >= 0.0) ? kEps : -kEps; + } + cfg_.w0 = std::max(cfg_.w0, kEps); + cfg_.beta1 = std::max(cfg_.beta1, kEps); + cfg_.beta2 = std::max(cfg_.beta2, kEps); + cfg_.beta3 = std::max(cfg_.beta3, kEps); + cfg_.z3_limit = std::max(cfg_.z3_limit, 0.0); + } + + Config cfg_; + State state_; +}; + +} // namespace rmcs_core::controller::adrc diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp new file mode 100644 index 000000000..c3bd67257 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp @@ -0,0 +1,80 @@ +#pragma once + +#include +#include +#include + +namespace rmcs_core::controller::adrc { + +class NLESF { +public: + struct Config { + double k1 = 50.0; + double k2 = 5.0; + double alpha1 = 0.75; + double alpha2 = 1.25; + double delta = 0.01; + double u_min = -std::numeric_limits::infinity(); + double u_max = std::numeric_limits::infinity(); + }; + + struct Output { + double u0 = 0.0; + double u = 0.0; + }; + + NLESF() + : NLESF(Config{}) {} + + explicit NLESF(const Config& cfg) { set_config(cfg); } + + void set_config(const Config& cfg) { + cfg_ = cfg; + sanitize_config(); + } + + const Config& config() const { return cfg_; } + + static double fal(double e, double alpha, double delta) { + const double abs_e = std::fabs(e); + if (abs_e <= delta) { + return e / std::pow(delta, 1.0 - alpha); + } + return std::pow(abs_e, alpha) * sign(e); + } + + Output compute(double e1, double e2, double z3, double b0) const { + constexpr double kEps = 1e-9; + const double safe_b0 = (std::fabs(b0) < kEps) ? ((b0 >= 0.0) ? kEps : -kEps) : b0; + + const double u0 = cfg_.k1 * fal(e1, cfg_.alpha1, cfg_.delta) + + cfg_.k2 * fal(e2, cfg_.alpha2, cfg_.delta); + const double u = clamp((u0 - z3) / safe_b0, cfg_.u_min, cfg_.u_max); + return Output{u0, u}; + } + +private: + static double sign(double x) { + if (x > 0.0) return 1.0; + if (x < 0.0) return -1.0; + return 0.0; + } + + static double clamp(double x, double lo, double hi) { + return std::max(lo, std::min(x, hi)); + } + + void sanitize_config() { + constexpr double kEps = 1e-9; + cfg_.delta = std::max(cfg_.delta, kEps); + cfg_.alpha1 = std::max(cfg_.alpha1, kEps); + cfg_.alpha2 = std::max(cfg_.alpha2, kEps); + if (cfg_.u_min > cfg_.u_max) { + std::swap(cfg_.u_min, cfg_.u_max); + } + } + + Config cfg_; +}; + +} // namespace rmcs_core::controller::adrc diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp new file mode 100644 index 000000000..6b14ce5f3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp @@ -0,0 +1,97 @@ +#pragma once + +#include +#include +#include + +namespace rmcs_core::controller::adrc { + +class TD { +public: + struct Config { + double r = 300.0; + double h = 0.003; + double max_vel = std::numeric_limits::infinity(); + double max_acc = std::numeric_limits::infinity(); + }; + + struct State { + double x1 = 0.0; + double x2 = 0.0; + }; + + struct Output { + double x1 = 0.0; + double x2 = 0.0; + double fh = 0.0; + }; + + TD() + : TD(Config{}) {} + + explicit TD(const Config& cfg) { set_config(cfg); } + + void set_config(const Config& cfg) { + cfg_ = cfg; + sanitize_config(); + } + + const Config& config() const { return cfg_; } + const State& state() const { return state_; } + + void reset(double init_x1 = 0.0, double init_x2 = 0.0) { + state_.x1 = init_x1; + state_.x2 = init_x2; + } + + Output update(double v) { + const double fh_raw = fhan(state_.x1 - v, state_.x2, cfg_.r, cfg_.h); + const double fh = clamp(fh_raw, -cfg_.max_acc, cfg_.max_acc); + + state_.x1 += cfg_.h * state_.x2; + state_.x2 += cfg_.h * fh; + state_.x2 = clamp(state_.x2, -cfg_.max_vel, cfg_.max_vel); + + return Output{state_.x1, state_.x2, fh}; + } + +private: + static double sign(double x) { + if (x > 0.0) return 1.0; + if (x < 0.0) return -1.0; + return 0.0; + } + + static double clamp(double x, double lo, double hi) { + return std::max(lo, std::min(x, hi)); + } + + static double fhan(double x1_minus_v, double x2, double r, double h) { + const double d = r * h * h; + const double a0 = h * x2; + const double y = x1_minus_v + a0; + const double a1 = std::sqrt(d * (d + 8.0 * std::fabs(y))); + + const double a = (std::fabs(y) > d) + ? (a0 + sign(y) * (a1 - d) * 0.5) + : (a0 + y); + + if (std::fabs(a) <= d) { + return -r * a / d; + } + return -r * sign(a); + } + + void sanitize_config() { + constexpr double kEps = 1e-9; + cfg_.r = std::max(cfg_.r, kEps); + cfg_.h = std::max(cfg_.h, kEps); + cfg_.max_vel = std::max(cfg_.max_vel, 0.0); + cfg_.max_acc = std::max(cfg_.max_acc, 0.0); + } + + Config cfg_; + State state_; +}; + +} // namespace rmcs_core::controller::adrc 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 new file mode 100644 index 000000000..c9c61ace5 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -0,0 +1,1018 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" +#include "deformable_joint_layer.hpp" + +namespace rmcs_core::controller::chassis { + +enum class SuspensionPhase : uint8_t { kInactive, kArming, kActive, kReleasing }; + +struct AttitudeBias { + double pitch_force = 0.0; + double roll_force = 0.0; +}; + +struct LegControlState { + SuspensionPhase phase = SuspensionPhase::kInactive; + double support_force = 0.0; + double contact_confidence = 1.0; + double filtered_contact_confidence = 1.0; + double phase_elapsed = 0.0; + bool requested_deploy = false; + bool output_active = false; + bool contact_latched = false; +}; + +struct LegCommand { + double requested_target_angle = std::numeric_limits::quiet_NaN(); + double final_target_angle = std::numeric_limits::quiet_NaN(); + double target_velocity = 0.0; + double target_acceleration = 0.0; + bool suspension_mode = false; + double suspension_torque = std::numeric_limits::quiet_NaN(); +}; + +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); + return std::clamp(kp * error + ki * integral - kd * rate, -output_limit, output_limit); + } +}; + +struct SuspensionParams { + double mass, rod_length, Kz, pitch_kp, pitch_ki, pitch_kd, roll_kp, roll_ki, roll_kd, D_leg; + double com_height, wheel_base_half_x, wheel_base_half_y; + double gravity_comp_gain, control_acceleration_limit; + double preload_angle, entry_offset, ride_height_offset, hold_travel; + double activation_velocity_threshold; + double target_physical_velocity_limit, target_physical_acceleration_limit; + double torque_limit; + double pitch_angle_diff_limit, roll_angle_diff_limit, pid_integral_limit; +}; + +// --- ChassisVelocityControl --- +struct ChassisVelocityControl { + static constexpr double kTranslationalVelocityMax = 10.0; + static constexpr double kAngularVelocityMax = 10.0; + + void configure(double spin_ratio) { + spin_ratio_ = std::clamp(spin_ratio, 0.0, 1.0); + following_velocity_controller_.output_max = kAngularVelocityMax; + following_velocity_controller_.output_min = -kAngularVelocityMax; + } + + void set_spin_forward(bool forward) { spinning_forward_ = forward; } + + Eigen::Vector2d compute_translational( + const Eigen::Vector2d& joystick_right, const rmcs_msgs::Keyboard& keyboard, + double gimbal_yaw_angle) { + Eigen::Vector2d tv = + Eigen::Rotation2Dd{gimbal_yaw_angle} + * (joystick_right + Eigen::Vector2d{keyboard.w - keyboard.s, keyboard.a - keyboard.d}); + if (tv.norm() > 1.0) + tv.normalize(); + return tv * kTranslationalVelocityMax; + } + + struct AngularResult { + double angular_velocity = 0.0; + double chassis_angle = std::numeric_limits::quiet_NaN(); + double chassis_control_angle = std::numeric_limits::quiet_NaN(); + }; + + AngularResult compute_angular( + rmcs_msgs::ChassisMode mode, double gimbal_yaw_angle, double gimbal_yaw_angle_error, + bool apply_toggle_forward) { + AngularResult result; + switch (mode) { + case rmcs_msgs::ChassisMode::AUTO: break; + case rmcs_msgs::ChassisMode::SPIN: + if (apply_toggle_forward) + spinning_forward_ = !spinning_forward_; + result.angular_velocity = std::clamp( + spin_ratio_ * (spinning_forward_ ? kAngularVelocityMax : -kAngularVelocityMax), + -kAngularVelocityMax, kAngularVelocityMax); + break; + case rmcs_msgs::ChassisMode::STEP_DOWN: + result.angular_velocity = following_velocity_controller_.update(calc_angle_err_( + result.chassis_control_angle, gimbal_yaw_angle_error, gimbal_yaw_angle, + std::numbers::pi)); + break; + case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { + double e = calc_angle_err_( + result.chassis_control_angle, gimbal_yaw_angle_error, gimbal_yaw_angle, + 2 * std::numbers::pi); + if (e > std::numbers::pi) + e -= 2 * std::numbers::pi; + result.angular_velocity = following_velocity_controller_.update(e); + break; + } + default: break; + } + result.chassis_angle = 2 * std::numbers::pi - gimbal_yaw_angle; + return result; + } + + void update_acceleration_estimate( + const Eigen::Vector2d& translational_velocity, double dt, double limit) { + if (!translational_velocity.array().isFinite().all()) { + control_acceleration_estimate_.setZero(); + last_translational_velocity_.setZero(); + last_valid_ = false; + return; + } + if (!last_valid_) { + last_translational_velocity_ = translational_velocity; + control_acceleration_estimate_.setZero(); + last_valid_ = true; + return; + } + const Eigen::Vector2d cap = Eigen::Vector2d::Constant(limit); + control_acceleration_estimate_ = + ((translational_velocity - last_translational_velocity_) / dt) + .cwiseMax(-cap) + .cwiseMin(cap); + last_translational_velocity_ = translational_velocity; + } + + void reset_acceleration_estimate() { + control_acceleration_estimate_.setZero(); + last_translational_velocity_.setZero(); + last_valid_ = false; + } + + Eigen::Vector2d control_acceleration_estimate() const { return control_acceleration_estimate_; } + bool spinning_forward() const { return spinning_forward_; } + +private: + double spin_ratio_ = 1.0; + bool spinning_forward_ = true; + pid::PidCalculator following_velocity_controller_{10.0, 0.0, 0.0}; + Eigen::Vector2d control_acceleration_estimate_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d last_translational_velocity_ = Eigen::Vector2d::Zero(); + bool last_valid_ = false; + + double + calc_angle_err_(double& cca, double yaw_error, double yaw_angle, double alignment) const { + cca = yaw_error; + if (cca < 0) + cca += 2 * std::numbers::pi; + double e = cca + yaw_angle; + if (e >= 2 * std::numbers::pi) + e -= 2 * std::numbers::pi; + while (e > alignment / 2) { + cca -= alignment; + if (cca < 0) + cca += 2 * std::numbers::pi; + e -= alignment; + } + return e; + } +}; + +// --- ActiveSuspension --- +struct ActiveSuspension { + static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr double kGravity = 9.81; + static constexpr double kMaxAttitudeRad = 30.0 * std::numbers::pi / 180.0; + static constexpr double kMinForceArmSin = 0.1; + static constexpr double kContactConfidenceEnterThreshold = 0.55; + static constexpr double kContactConfidenceExitThreshold = 0.35; + static constexpr double kContactConfidenceFilterAlpha = 0.25; + static constexpr double kMinimumArmingTime = 0.02; + static constexpr std::array kPitchSigns = {-1.0, 1.0, 1.0, -1.0}; + static constexpr std::array kRollSigns = {1.0, 1.0, -1.0, -1.0}; + + void load_params(rclcpp::Node& node, double min_angle, double max_angle) { + params_ = SuspensionParams{ + .mass = node.get_parameter_or("active_suspension_mass", 22.5), + .rod_length = node.get_parameter_or("active_suspension_rod_length", 0.150), + .Kz = node.get_parameter_or("active_suspension_Kz", 150.0), + + .pitch_kp = node.get_parameter_or("active_suspension_pitch_kp", 200.0), + .pitch_ki = node.get_parameter_or("active_suspension_pitch_ki", 0.0), + .pitch_kd = node.get_parameter_or("active_suspension_pitch_kd", 20.0), + + .roll_kp = node.get_parameter_or("active_suspension_roll_kp", 200.0), + .roll_ki = node.get_parameter_or("active_suspension_roll_ki", 0.0), + .roll_kd = node.get_parameter_or("active_suspension_roll_kd", 20.0), + + .D_leg = node.get_parameter_or("active_suspension_D_leg", 10.0), + .com_height = node.get_parameter_or("active_suspension_com_height", 0.15), + .wheel_base_half_x = node.get_parameter_or( + "active_suspension_wheel_base_half_x", 0.2341741 / std::numbers::sqrt2), + .wheel_base_half_y = node.get_parameter_or( + "active_suspension_wheel_base_half_y", 0.2341741 / std::numbers::sqrt2), + .gravity_comp_gain = node.get_parameter_or("active_suspension_gravity_comp_gain", 1.0), + .control_acceleration_limit = std::abs( + node.get_parameter_or("active_suspension_control_acceleration_limit", 6.0)), + .preload_angle = + std::abs(node.get_parameter_or("active_suspension_preload_angle_deg", 8.0)) + * std::numbers::pi / 180.0, + .entry_offset = + std::abs(node.get_parameter_or( + "active_suspension_entry_offset_deg", + node.get_parameter_or("active_suspension_enter_deploy_tolerance_deg", 1.5))) + * std::numbers::pi / 180.0, + .ride_height_offset = + std::abs(node.get_parameter_or("active_suspension_ride_height_offset_deg", 0.0)) + * std::numbers::pi / 180.0, + .hold_travel = + std::abs(node.get_parameter_or( + "active_suspension_hold_travel_deg", + node.get_parameter_or("active_suspension_exit_deploy_tolerance_deg", 3.0))) + * std::numbers::pi / 180.0, + .activation_velocity_threshold = + node.get_parameter_or("active_suspension_activation_velocity_threshold_deg", 15.0) + * std::numbers::pi / 180.0, + .target_physical_velocity_limit = + std::max( + node.get_parameter_or( + "active_suspension_target_velocity_limit_deg", + node.get_parameter_or("target_physical_velocity_limit", 180.0)), + 1e-6) + * std::numbers::pi / 180.0, + .target_physical_acceleration_limit = + std::max( + node.get_parameter_or( + "active_suspension_target_acceleration_limit_deg", + node.get_parameter_or("target_physical_acceleration_limit", 720.0)), + 1e-6) + * std::numbers::pi / 180.0, + .torque_limit = std::abs(node.get_parameter_or("active_suspension_torque_limit", 80.0)), + .pitch_angle_diff_limit = + std::abs(node.get_parameter_or( + "active_suspension_pitch_angle_diff_limit_deg", max_angle - min_angle)) + * std::numbers::pi / 180.0, + .roll_angle_diff_limit = + std::abs(node.get_parameter_or( + "active_suspension_roll_angle_diff_limit_deg", max_angle - min_angle)) + * std::numbers::pi / 180.0, + .pid_integral_limit = + std::abs(node.get_parameter_or( + "active_suspension_pid_integral_limit_deg", max_angle - min_angle)) + * std::numbers::pi / 180.0, + }; + + pitch_pid_.kp = params_.pitch_kp; + pitch_pid_.ki = params_.pitch_ki; + pitch_pid_.kd = params_.pitch_kd; + + roll_pid_.kp = params_.roll_kp; + roll_pid_.ki = params_.roll_ki; + roll_pid_.kd = params_.roll_kd; + + pitch_pid_.integral_limit = params_.pid_integral_limit; + pitch_pid_.output_limit = params_.pitch_angle_diff_limit; + roll_pid_.integral_limit = params_.pid_integral_limit; + roll_pid_.output_limit = params_.roll_angle_diff_limit; + + enabled_ = node.get_parameter_or("active_suspension_enable", false); + calib_wait_ = std::max(node.get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); + calib_sample_ = + std::max(node.get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); + } + + bool enabled() const { return enabled_; } + void set_enabled(bool v) { enabled_ = v; } + + void update( + const JointFeedbackFrame& fb, double imu_pitch, double imu_roll, double imu_pitch_rate, + double imu_roll_rate, double dt, bool requested, double min_angle_deg, double max_angle_deg, + const Eigen::Vector2d& control_accel, + std::array& current_target_physical_angles, + std::array& out_suspension_mode, + std::array& out_suspension_torque) { + static constexpr auto deg_to_rad = [](double d) { return d * std::numbers::pi / 180.0; }; + + clear_outputs_(out_suspension_mode, out_suspension_torque); + prepare_commands_(); + + if (!requested) { + reset_state_(); + current_target_physical_angles = requested_target_angles_; + publish_outputs_(out_suspension_mode, out_suspension_torque); + return; + } + + const double deploy = deg_to_rad(min_angle_deg); + const double entry = deploy + params_.entry_offset; + const double ride_height = + std::clamp(deploy + params_.ride_height_offset, deploy, deg_to_rad(max_angle_deg)); + const double support_zero_angle = deploy - params_.preload_angle; + const double release = ride_height + params_.hold_travel; + + AttitudeBias bias = + compute_attitude_bias_(imu_pitch, imu_roll, imu_pitch_rate, imu_roll_rate, dt); + if (!std::isfinite(bias.pitch_force) || !std::isfinite(bias.roll_force)) { + reset_state_(); + current_target_physical_angles.fill(deploy); + publish_outputs_(out_suspension_mode, out_suspension_torque); + return; + } + + update_leg_contact_estimates_(fb); + update_leg_states_(fb, entry, release, dt); + compute_leg_support_intents_(fb, bias, support_zero_angle, ride_height, control_accel); + current_target_physical_angles = target_angles_; + publish_outputs_(out_suspension_mode, out_suspension_torque); + } + + void update_imu_calibration( + bool symmetric_targets, double imu_pitch, double imu_roll, double dt) { + if (!symmetric_targets) { + calib_hold_ = 0.0; + calib_count_ = 0; + calib_pitch_sum_ = 0.0; + calib_roll_sum_ = 0.0; + calib_done_ = false; + return; + } + if (!std::isfinite(imu_pitch) || !std::isfinite(imu_roll)) + return; + calib_hold_ += dt; + if (calib_hold_ < calib_wait_) + return; + double end = calib_wait_ + calib_sample_; + if (calib_hold_ < end) { + calib_pitch_sum_ += imu_pitch; + calib_roll_sum_ += imu_roll; + ++calib_count_; + return; + } + if (calib_done_) + return; + calib_done_ = true; + if (calib_count_ == 0) + return; + imu_pitch_offset_ = calib_pitch_sum_ / static_cast(calib_count_); + imu_roll_offset_ = calib_roll_sum_ / static_cast(calib_count_); + } + + void reset_calibration() { + calib_hold_ = 0.0; + calib_count_ = 0; + calib_pitch_sum_ = 0.0; + calib_roll_sum_ = 0.0; + calib_done_ = false; + } + + void reset_all() { + reset_state_(); + reset_calibration(); + } + + double target_vel_limit() const { return params_.target_physical_velocity_limit; } + double target_accel_limit() const { return params_.target_physical_acceleration_limit; } + double control_accel_limit() const { return params_.control_acceleration_limit; } + +private: + SuspensionParams params_{}; + bool enabled_ = false; + AttitudePidAxis pitch_pid_, roll_pid_; + double imu_pitch_offset_ = 0.0, imu_roll_offset_ = 0.0; + double calib_wait_ = 0.0, calib_sample_ = 0.0; + double calib_hold_ = 0.0; + size_t calib_count_ = 0; + double calib_pitch_sum_ = 0.0, calib_roll_sum_ = 0.0; + bool calib_done_ = false; + std::array suspension_active_{}; + std::array leg_states_{}; + std::array leg_commands_{}; + std::array requested_target_angles_{}; + std::array target_angles_{}; + + static double deg_to_rad_(double d) { return d * std::numbers::pi / 180.0; } + + void clear_outputs_( + std::array& modes, std::array& torques) { + modes.fill(false); + torques.fill(kNaN); + } + void publish_outputs_( + std::array& modes, std::array& torques) { + for (size_t i = 0; i < kJointCount; ++i) { + modes[i] = leg_commands_[i].suspension_mode; + torques[i] = leg_commands_[i].suspension_torque; + } + } + + void reset_state_() { + suspension_active_.fill(false); + for (size_t i = 0; i < kJointCount; ++i) { + leg_states_[i] = LegControlState{}; + leg_commands_[i] = LegCommand{}; + } + } + + void prepare_commands_() { + for (size_t i = 0; i < kJointCount; ++i) { + leg_commands_[i] = LegCommand{ + .requested_target_angle = requested_target_angles_[i], + .final_target_angle = target_angles_[i], + }; + leg_states_[i].output_active = false; + leg_states_[i].support_force = 0.0; + } + } + + static LegFeedback leg_feedback_at_(const JointFeedbackFrame& fb, size_t i) { + return { + .motor_angle = fb.motor_angles[i], + .physical_angle = fb.physical_angles[i], + .physical_velocity = fb.physical_velocities[i], + .joint_torque = fb.joint_torques[i], + .eso_z2 = fb.eso_z2[i], + .eso_z3 = fb.eso_z3[i]}; + } + + double estimate_contact_(const LegFeedback& lf) const { + double c = 1.0; + if (std::isfinite(lf.eso_z3)) + c -= std::clamp(std::abs(lf.eso_z3) / 80.0, 0.0, 0.5); + if (std::isfinite(lf.joint_torque)) + c += std::clamp(std::abs(lf.joint_torque) / 20.0, 0.0, 0.3); + if (std::isfinite(lf.physical_velocity)) + c -= std::clamp(std::abs(lf.physical_velocity) / 10.0, 0.0, 0.2); + return std::clamp(c, 0.0, 1.0); + } + + bool contact_ready_(const LegControlState& s) const { + return s.contact_latched + || s.filtered_contact_confidence >= kContactConfidenceEnterThreshold; + } + + void update_leg_contact_estimates_(const JointFeedbackFrame& fb) { + for (size_t i = 0; i < kJointCount; ++i) { + auto& s = leg_states_[i]; + s.contact_confidence = estimate_contact_(leg_feedback_at_(fb, i)); + s.filtered_contact_confidence = std::clamp( + (1.0 - kContactConfidenceFilterAlpha) * s.filtered_contact_confidence + + kContactConfidenceFilterAlpha * s.contact_confidence, + 0.0, 1.0); + if (s.filtered_contact_confidence >= kContactConfidenceEnterThreshold) + s.contact_latched = true; + else if (s.filtered_contact_confidence <= kContactConfidenceExitThreshold) + s.contact_latched = false; + } + } + + void update_leg_states_(const JointFeedbackFrame& fb, double entry, double release, double dt) { + for (size_t i = 0; i < kJointCount; ++i) { + auto lf = leg_feedback_at_(fb, i); + bool rd = + std::isfinite(requested_target_angles_[i]) && requested_target_angles_[i] <= entry; + update_suspension_state_(i, lf, rd, entry, release, dt); + } + } + + void compute_leg_support_intents_( + const JointFeedbackFrame& fb, const AttitudeBias& bias, double support_zero_angle, + double ride_height, const Eigen::Vector2d& control_accel) { + for (size_t i = 0; i < kJointCount; ++i) { + auto lf = leg_feedback_at_(fb, i); + auto& s = leg_states_[i]; + if (s.phase != SuspensionPhase::kActive) + continue; + s.support_force = + compute_leg_support_force_(i, lf, bias, support_zero_angle, control_accel); + leg_commands_[i].final_target_angle = ride_height; + leg_commands_[i].suspension_mode = true; + leg_commands_[i].suspension_torque = + leg_force_to_torque_(s.support_force, lf.physical_angle); + } + } + + AttitudeBias compute_attitude_bias_( + double pitch, double roll, double pitch_rate, double roll_rate, double dt) { + double corrected_pitch = + std::clamp(pitch - imu_pitch_offset_, -kMaxAttitudeRad, kMaxAttitudeRad); + double corrected_roll = + std::clamp(roll - imu_roll_offset_, -kMaxAttitudeRad, kMaxAttitudeRad); + double pitch_force = pitch_pid_.update(-corrected_pitch, pitch_rate, dt); + double roll_force = roll_pid_.update(corrected_roll, -roll_rate, dt); + if (!std::isfinite(pitch_force) || !std::isfinite(roll_force)) + return {kNaN, kNaN}; + return {pitch_force, roll_force}; + } + + double compute_leg_support_force_( + size_t i, const LegFeedback& lf, const AttitudeBias& bias, double support_zero_angle, + const Eigen::Vector2d& control_accel) const { + double f = params_.gravity_comp_gain * params_.mass * kGravity / 4.0 + + params_.Kz * (lf.physical_angle - support_zero_angle) + + params_.D_leg * lf.physical_velocity; + f += kPitchSigns[i] * bias.pitch_force + kRollSigns[i] * bias.roll_force; + if (params_.com_height > 0.0 && params_.wheel_base_half_x > 1e-6 + && params_.wheel_base_half_y > 1e-6) { + f += kPitchSigns[i] * params_.mass * control_accel.x() * params_.com_height + / (4.0 * params_.wheel_base_half_x); + f += kRollSigns[i] * params_.mass * control_accel.y() * params_.com_height + / (4.0 * params_.wheel_base_half_y); + } + return std::max(f, 0.0); + } + + double leg_force_to_torque_(double force, double angle) const { + return std::clamp( + force * params_.rod_length * std::max(std::sin(angle), kMinForceArmSin), + -params_.torque_limit, params_.torque_limit); + } + + void update_suspension_state_( + size_t i, const LegFeedback& lf, bool rd, double entry, double release, double dt) { + auto& s = leg_states_[i]; + s.phase_elapsed += (s.requested_deploy != rd) ? 0.0 : dt; + s.requested_deploy = rd; + if (!rd || !std::isfinite(lf.physical_angle) || !std::isfinite(lf.physical_velocity)) { + s.phase = SuspensionPhase::kInactive; + suspension_active_[i] = false; + s.output_active = false; + s.phase_elapsed = 0.0; + s.contact_latched = false; + return; + } + bool eok = lf.physical_angle <= entry; + bool vok = std::abs(lf.physical_velocity) <= params_.activation_velocity_threshold; + bool cok = contact_ready_(s); + switch (s.phase) { + case SuspensionPhase::kInactive: + suspension_active_[i] = false; + s.output_active = false; + if (eok) { + s.phase = SuspensionPhase::kArming; + s.phase_elapsed = 0.0; + } + break; + case SuspensionPhase::kArming: + suspension_active_[i] = false; + s.output_active = false; + if (!eok) { + s.phase = SuspensionPhase::kInactive; + s.phase_elapsed = 0.0; + break; + } + if (vok && std::isfinite(lf.motor_angle) + && (cok || s.phase_elapsed >= kMinimumArmingTime)) { + suspension_active_[i] = true; + s.phase = SuspensionPhase::kActive; + s.output_active = true; + s.phase_elapsed = 0.0; + } + break; + case SuspensionPhase::kActive: + if (lf.physical_angle > release || (!cok && s.phase_elapsed >= kMinimumArmingTime)) { + suspension_active_[i] = false; + s.phase = SuspensionPhase::kReleasing; + s.output_active = false; + s.phase_elapsed = 0.0; + break; + } + suspension_active_[i] = true; + s.output_active = true; + break; + case SuspensionPhase::kReleasing: + suspension_active_[i] = false; + s.output_active = false; + if (!eok || s.phase_elapsed >= kMinimumArmingTime) { + s.phase = SuspensionPhase::kInactive; + s.phase_elapsed = 0.0; + } + break; + } + } +}; + +// ============================================================ +// DeformableChassis — thin orchestrator +// ============================================================ +class DeformableChassis + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableChassis() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + + const double spin_ratio = std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0); + const double min_angle = get_parameter_or("min_angle", 15.0); + const double max_angle = get_parameter_or("max_angle", 55.0); + const double vel_limit = std::max( + std::abs(get_parameter_or("target_physical_velocity_limit", 180.0)) * std::numbers::pi + / 180.0, + 1e-6); + const double accel_limit = std::max( + std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0)) + * std::numbers::pi / 180.0, + 1e-6); + + velocity_control_.configure(spin_ratio); + suspension_.load_params(*this, min_angle, max_angle); + trajectory_.init(min_angle, max_angle, vel_limit, accel_limit); + + for (size_t i = 0; i < kJointCount; ++i) + joint_offsets_[i] = + get_parameter_or(std::string(kJointNames[i]) + "_joint_offset", 0.0); + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); + register_input("/predefined/update_rate", update_rate_); + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + + auto reg_joint_input = [this](size_t i) { + const auto& name = kJointNames[i]; + auto& j = joints_[i]; + register_input(joint_path_(name, "angle"), j.angle, false); + register_input(joint_path_(name, "physical_angle"), j.physical_angle, false); + register_input(joint_path_(name, "physical_velocity"), j.physical_velocity, false); + register_input(joint_path_(name, "torque"), j.torque, false); + register_input(joint_path_(name, "encoder_angle"), j.encoder_angle, false); + register_input(joint_path_(name, "eso_z2"), j.eso_z2, false); + register_input(joint_path_(name, "eso_z3"), j.eso_z3, false); + }; + for (size_t i = 0; i < kJointCount; ++i) + reg_joint_input(i); + + 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_, kNaN); + register_output("/chassis/angle", chassis_angle_, kNaN); + register_output("/chassis/control_angle", chassis_control_angle_, kNaN); + register_output("/chassis/control_mode", mode_); + register_output("/chassis/control_velocity", chassis_control_velocity_); + + auto reg_joint_output = [this](size_t i) { + const auto& name = kJointNames[i]; + auto& j = joints_[i]; + register_output(joint_path_(name, "control_angle_error"), angle_errors_[i], kNaN); + register_output(joint_path_(name, "target_angle"), j.target_angle, kNaN); + register_output( + joint_path_(name, "target_physical_angle"), j.target_physical_angle, kNaN); + register_output( + joint_path_(name, "target_physical_velocity"), j.target_physical_velocity, kNaN); + register_output( + joint_path_(name, "target_physical_acceleration"), j.target_physical_acceleration, + kNaN); + register_output(joint_path_(name, "suspension_torque"), j.suspension_torque, kNaN); + }; + for (size_t i = 0; i < kJointCount; ++i) { + reg_joint_output(i); + register_output( + joint_path_(kJointNames[i], "suspension_mode"), suspension_modes_[i], false); + } + register_output("/chassis/processed_encoder/angle", processed_encoder_angle_, kNaN); + + *mode_ = rmcs_msgs::ChassisMode::AUTO; + chassis_control_velocity_->vector << kNaN, kNaN, kNaN; + + int offset_count = 0; + for (size_t i = 0; i < kJointCount; ++i) + if (has_parameter(std::string(kJointNames[i]) + "_joint_offset")) + ++offset_count; + if (offset_count > 0 && offset_count != static_cast(kJointCount)) + throw std::runtime_error( + "joint offsets must be configured for all four joints or removed entirely"); + joint_feedback_source_ = (offset_count == static_cast(kJointCount)) + ? JointFeedbackSource::kLegacyEncoderAngle + : JointFeedbackSource::kMotorAngle; + } + + void before_updating() override { + auto ensure = [this](auto& field, double value, const char* name) { + if (!field.ready()) { + field.make_and_bind_directly(value); + RCLCPP_WARN(get_logger(), "Failed to fetch \"%s\". Set to %.1f.", name, value); + } + }; + ensure(gimbal_yaw_angle_, 0.0, "/gimbal/yaw/angle"); + ensure(gimbal_yaw_angle_error_, 0.0, "/gimbal/yaw/control_angle_error"); + for (auto& j : joints_) + ensure(j.torque, 0.0, "joint torque"); + ensure(chassis_imu_pitch_, 0.0, "chassis imu pitch"); + ensure(chassis_imu_roll_, 0.0, "chassis imu roll"); + ensure(chassis_imu_pitch_rate_, 0.0, "chassis imu pitch_rate"); + ensure(chassis_imu_roll_rate_, 0.0, "chassis imu roll_rate"); + validate_joint_feedback_inputs_(); + } + + void update() override { + using rmcs_msgs::Switch; + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; + do { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls_(); + break; + } + update_mode_from_inputs_(switch_left, switch_right, keyboard); + update_velocity_control_(keyboard); + 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 double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr double kRadToDeg = 180.0 / std::numbers::pi; + + static std::string joint_path_(const char* name, const char* suffix) { + char b[128]; + std::snprintf(b, sizeof(b), "/chassis/%s_joint/%s", name, suffix); + return {b}; + } + + void validate_joint_feedback_inputs_() const { + bool ok = true; + for (const auto& j : joints_) { + if (joint_feedback_source_ == JointFeedbackSource::kMotorAngle) { + if (!j.angle.ready()) + ok = false; + } else { + if (!j.encoder_angle.ready()) + ok = false; + } + } + if (ok) + return; + throw std::runtime_error( + joint_feedback_source_ == JointFeedbackSource::kMotorAngle + ? "missing V2 joint feedback inputs: /chassis/*_joint/angle" + : "missing legacy joint feedback inputs: /chassis/*_joint/encoder_angle"); + } + + // --- helpers --- + bool suspension_requested_() const { + return suspension_.enabled() + && (keyboard_->ctrl + || (switch_left_.ready() && switch_right_.ready() + && *switch_left_ == rmcs_msgs::Switch::DOWN + && *switch_right_ == rmcs_msgs::Switch::UP)); + } + + // --- mode --- + void update_mode_from_inputs_( + rmcs_msgs::Switch sl, rmcs_msgs::Switch sr, const rmcs_msgs::Keyboard& kb) { + auto m = *mode_; + if (sl == rmcs_msgs::Switch::DOWN) + return; + if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && sr == rmcs_msgs::Switch::DOWN) { + m = (m == rmcs_msgs::ChassisMode::SPIN) ? rmcs_msgs::ChassisMode::STEP_DOWN + : rmcs_msgs::ChassisMode::SPIN; + } else if (!last_keyboard_.c && kb.c) { + m = (m == rmcs_msgs::ChassisMode::SPIN) ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::SPIN; + } else if (!last_keyboard_.x && kb.x) { + m = (m == rmcs_msgs::ChassisMode::LAUNCH_RAMP) ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::LAUNCH_RAMP; + } else if (!last_keyboard_.z && kb.z) { + m = (m == rmcs_msgs::ChassisMode::STEP_DOWN) ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::STEP_DOWN; + } + *mode_ = m; + } + + // --- velocity --- + void update_velocity_control_(const rmcs_msgs::Keyboard& kb) { + auto tv = velocity_control_.compute_translational(*joystick_right_, kb, *gimbal_yaw_angle_); + bool toggle = (last_keyboard_.c != kb.c && *mode_ != rmcs_msgs::ChassisMode::SPIN); + auto ar = velocity_control_.compute_angular( + *mode_, *gimbal_yaw_angle_, *gimbal_yaw_angle_error_, toggle); + double dt = update_dt_(); + velocity_control_.update_acceleration_estimate(tv, dt, suspension_.control_accel_limit()); + *chassis_angle_ = ar.chassis_angle; + *chassis_control_angle_ = ar.chassis_control_angle; + chassis_control_velocity_->vector << tv, ar.angular_velocity; + } + + double update_dt_() const { + if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) + return 1.0 / *update_rate_; + return 1e-3; + } + + // --- lift toggle --- + void update_lift_target_toggle_(rmcs_msgs::Keyboard keyboard) { + constexpr double kRotaryKnobEdgeThreshold = 0.7; + + const bool keyboard_toggle = !last_keyboard_.q && keyboard.q; + const bool rotary_knob_toggle = last_rotary_knob_ < kRotaryKnobEdgeThreshold + && *rotary_knob_ >= kRotaryKnobEdgeThreshold; + + if (apply_symmetric_target_) + trajectory_.fill_symmetric_targets(); + + if (rotary_knob_toggle || keyboard_toggle) { + trajectory_.set_target_angle( + std::abs(trajectory_.target_angle() - trajectory_.max_angle()) < 1e-6 + ? trajectory_.min_angle() + : trajectory_.max_angle()); + apply_symmetric_target_ = true; + } + + last_rotary_knob_ = *rotary_knob_; + } + + // --- reset --- + void reset_all_controls_() { + *mode_ = rmcs_msgs::ChassisMode::AUTO; + velocity_control_.reset_acceleration_estimate(); + suspension_.reset_all(); + chassis_control_velocity_->vector << kNaN, kNaN, kNaN; + *chassis_angle_ = kNaN; + *chassis_control_angle_ = kNaN; + trajectory_.reset(trajectory_.max_angle()); + *scope_motor_control_torque_ = kNaN; + for (auto& e : angle_errors_) + *e = kNaN; + for (auto& j : joints_) { + *j.target_angle = kNaN; + *j.target_physical_angle = kNaN; + *j.target_physical_velocity = kNaN; + *j.target_physical_acceleration = kNaN; + *j.suspension_torque = kNaN; + } + for (auto& m : suspension_modes_) + *m = false; + *processed_encoder_angle_ = kNaN; + } + + // --- feedback --- + JointFeedbackFrame read_joint_feedback_() const { + JointFeedbackFrame f; + f.motor_angles.fill(kNaN); + f.physical_angles.fill(kNaN); + f.physical_velocities.fill(kNaN); + f.joint_torques.fill(kNaN); + f.eso_z2.fill(kNaN); + f.eso_z3.fill(kNaN); + for (size_t i = 0; i < kJointCount; ++i) { + const auto& j = joints_[i]; + if (j.angle.ready() && std::isfinite(*j.angle)) { + f.motor_angles[i] = *j.angle; + f.physical_angles[i] = 1.090830782496456 - f.motor_angles[i]; + } + if (j.physical_angle.ready() && std::isfinite(*j.physical_angle)) + f.physical_angles[i] = *j.physical_angle; + if (j.physical_velocity.ready() && std::isfinite(*j.physical_velocity)) + f.physical_velocities[i] = *j.physical_velocity; + if (j.torque.ready() && std::isfinite(*j.torque)) + f.joint_torques[i] = *j.torque; + if (j.eso_z2.ready() && std::isfinite(*j.eso_z2)) + f.eso_z2[i] = *j.eso_z2; + if (j.eso_z3.ready() && std::isfinite(*j.eso_z3)) + f.eso_z3[i] = *j.eso_z3; + } + return f; + } + + // --- main pipeline --- + void run_joint_intent_pipeline_() { + auto feedback = read_joint_feedback_(); + + suspension_.update_imu_calibration( + trajectory_.symmetric_requested(), *chassis_imu_pitch_, *chassis_imu_roll_, + update_dt_()); + + if (!trajectory_.active() + && !trajectory_.initialize_from_feedback( + feedback.motor_angles, feedback.physical_angles)) { + reset_all_controls_(); + return; + } + + if (apply_symmetric_target_) + trajectory_.fill_symmetric_targets(); + trajectory_.refresh_deploy_targets( + suspension_requested_(), keyboard_->ctrl, trajectory_.min_angle()); + + scope_motor_control_(keyboard_->ctrl); + + auto target_physical = trajectory_.current_physical(); + std::array sus_modes, sus_torques; + sus_modes.fill(false); + sus_torques.fill(kNaN); + + suspension_.update( + feedback, *chassis_imu_pitch_, *chassis_imu_roll_, *chassis_imu_pitch_rate_, + *chassis_imu_roll_rate_, update_dt_(), suspension_requested_(), trajectory_.min_angle(), + trajectory_.max_angle(), velocity_control_.control_acceleration_estimate(), + target_physical, sus_modes, sus_torques); + + trajectory_.update_trajectory( + update_dt_(), suspension_requested_(), suspension_.target_vel_limit(), + suspension_.target_accel_limit()); + + for (size_t i = 0; i < kJointCount; ++i) { + *joints_[i].target_angle = trajectory_.target_angles()[i]; + *joints_[i].target_physical_angle = trajectory_.target_physical_angles()[i]; + *joints_[i].target_physical_velocity = trajectory_.target_velocities()[i]; + *joints_[i].target_physical_acceleration = trajectory_.target_accelerations()[i]; + *joints_[i].suspension_torque = sus_torques[i]; + *suspension_modes_[i] = static_cast(sus_modes[i]); + } + + if (apply_symmetric_target_ && trajectory_.symmetric_requested()) + trajectory_.fill_symmetric_targets(); + + double sum = 0.0; + int cnt = 0; + for (const auto& j : joints_) { + if (j.physical_angle.ready() && std::isfinite(*j.physical_angle)) { + sum += *j.physical_angle; + ++cnt; + } + } + *processed_encoder_angle_ = (cnt > 0) ? kRadToDeg * sum / cnt : kNaN; + } + + void scope_motor_control_(bool prone_override) { + if (prone_override && *mode_ != rmcs_msgs::ChassisMode::SPIN) + *scope_motor_control_torque_ = -0.3; + else + *scope_motor_control_torque_ = 0.3; + } + + // --- member variables --- + InputInterface joystick_right_, joystick_left_; + InputInterface switch_right_, switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface keyboard_; + InputInterface rotary_knob_, update_rate_; + double last_rotary_knob_ = 0.0; + 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(); + + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; + OutputInterface chassis_angle_, chassis_control_angle_; + OutputInterface mode_; + OutputInterface chassis_control_velocity_; + + ChassisVelocityControl velocity_control_; + ActiveSuspension suspension_; + JointTrajectoryPlanner trajectory_; + bool apply_symmetric_target_ = true; + + std::array joints_{}; + std::array angle_errors_; + std::array, kJointCount> suspension_modes_; + InputInterface chassis_imu_pitch_, chassis_imu_roll_, chassis_imu_pitch_rate_, + chassis_imu_roll_rate_; + OutputInterface scope_motor_control_torque_, processed_encoder_angle_; + + std::array joint_offsets_{}; // FIXME: Unused Var + JointFeedbackSource joint_feedback_source_ = JointFeedbackSource::kLegacyEncoderAngle; +}; + +} // 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 new file mode 100644 index 000000000..a629449c3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp @@ -0,0 +1,370 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include "controller/adrc/eso.hpp" +#include "controller/adrc/nlesf.hpp" +#include "controller/adrc/td.hpp" + +namespace rmcs_core::controller::chassis { + +namespace { + +double load_parameter_or(rclcpp::Node& node, const std::string& name, double fallback) { + double value = fallback; + node.get_parameter_or(name, value, fallback); + return value; +} + +bool load_parameter_or(rclcpp::Node& node, const std::string& name, bool fallback) { + bool value = fallback; + node.get_parameter_or(name, value, fallback); + return value; +} + +} // namespace + +class DeformableJointController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + // Joint controller owns only joint-local servo execution. Higher-level deploy/suspension + // intent is generated upstream by DeformableChassis via setpoint/mode/feedforward inputs. + struct ModeConfig { + 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(); + double torque_feedforward_gain = 0.0; + }; + + struct InputSnapshot { + double measurement_angle = std::numeric_limits::quiet_NaN(); + double setpoint_angle = std::numeric_limits::quiet_NaN(); + double joint_torque_feedforward = 0.0; + bool suspension_mode = false; + }; + + DeformableJointController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + register_interfaces_(); + load_mode_configs_(); + apply_mode_config_(normal_mode_config_); + } + + void update() override { + InputSnapshot inputs; + if (!read_inputs_(inputs)) { + disable_output_(); + return; + } + + update_mode_selection_(inputs.suspension_mode); + const auto& mode_config = active_mode_config_(inputs.suspension_mode); + const auto [output_min, output_max] = effective_output_limits_(mode_config); + if (std::isnan(output_min) || std::isnan(output_max) || output_min > output_max) { + disable_output_(); + return; + } + + if (!ensure_feedforward_ready_(mode_config, inputs)) { + disable_output_(); + return; + } + + initialize_if_needed_(inputs); + rmcs_core::controller::adrc::ESO::Output eso_out; + double control_torque = nan_; + if (!run_joint_servo_( + inputs, mode_config, output_min, output_max, eso_out, control_torque)) { + disable_output_(); + return; + } + + publish_control_output_(control_torque, eso_out); + } + +private: + void register_interfaces_() { + register_input(get_parameter("measurement_angle").as_string(), measurement_angle_); + register_input(get_parameter("measurement_velocity").as_string(), measurement_velocity_); + 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_input_); + } else { + setpoint_velocity_input_.make_and_bind_directly(0.0); + } + + if (has_parameter("mode_input")) { + register_input(get_parameter("mode_input").as_string(), suspension_mode_input_); + } else { + suspension_mode_input_.make_and_bind_directly(false); + } + + if (has_parameter("suspension_torque")) { + register_input( + get_parameter("suspension_torque").as_string(), joint_torque_feedforward_input_); + } else { + joint_torque_feedforward_input_.make_and_bind_directly(0.0); + } + if (has_parameter("limit")) { + register_input(get_parameter("limit").as_string(), output_limit_); + use_dynamic_limit_ = true; + } + + register_output(get_parameter("control").as_string(), control_torque_, nan_); + if (has_parameter("eso_z2_output")) { + register_output(get_parameter("eso_z2_output").as_string(), eso_z2_output_, nan_); + } + if (has_parameter("eso_z3_output")) { + register_output(get_parameter("eso_z3_output").as_string(), eso_z3_output_, nan_); + } + } + + void load_mode_configs_() { + dt_ = load_parameter_or(*this, "dt", 0.001); + b0_ = load_parameter_or(*this, "b0", 1.0); + kt_ = load_parameter_or(*this, "kt", 1.0); + + normal_mode_config_.td.h = load_parameter_or(*this, "td_h", dt_); + normal_mode_config_.td.r = load_parameter_or(*this, "td_r", 300.0); + normal_mode_config_.td.max_vel = + load_parameter_or(*this, "td_max_vel", std::numeric_limits::infinity()); + normal_mode_config_.td.max_acc = + load_parameter_or(*this, "td_max_acc", std::numeric_limits::infinity()); + + normal_mode_config_.eso.h = dt_; + normal_mode_config_.eso.b0 = b0_; + normal_mode_config_.eso.w0 = load_parameter_or(*this, "eso_w0", 80.0); + normal_mode_config_.eso.auto_beta = load_parameter_or(*this, "eso_auto_beta", true); + normal_mode_config_.eso.beta1 = + load_parameter_or(*this, "eso_beta1", 3.0 * normal_mode_config_.eso.w0); + normal_mode_config_.eso.beta2 = load_parameter_or( + *this, "eso_beta2", 3.0 * normal_mode_config_.eso.w0 * normal_mode_config_.eso.w0); + normal_mode_config_.eso.beta3 = load_parameter_or( + *this, "eso_beta3", + normal_mode_config_.eso.w0 * normal_mode_config_.eso.w0 * normal_mode_config_.eso.w0); + normal_mode_config_.eso.z3_limit = load_parameter_or(*this, "eso_z3_limit", 1e9); + + normal_mode_config_.nlesf.k1 = load_parameter_or(*this, "k1", 50.0); + normal_mode_config_.nlesf.k2 = load_parameter_or(*this, "k2", 5.0); + normal_mode_config_.nlesf.alpha1 = load_parameter_or(*this, "alpha1", 0.75); + normal_mode_config_.nlesf.alpha2 = load_parameter_or(*this, "alpha2", 1.25); + normal_mode_config_.nlesf.delta = load_parameter_or(*this, "delta", 0.01); + normal_mode_config_.nlesf.u_min = + load_parameter_or(*this, "u_min", -std::numeric_limits::infinity()); + normal_mode_config_.nlesf.u_max = + load_parameter_or(*this, "u_max", std::numeric_limits::infinity()); + normal_mode_config_.output_min = + load_parameter_or(*this, "output_min", -std::numeric_limits::infinity()); + normal_mode_config_.output_max = + load_parameter_or(*this, "output_max", std::numeric_limits::infinity()); + if (normal_mode_config_.output_min > normal_mode_config_.output_max) { + std::swap(normal_mode_config_.output_min, normal_mode_config_.output_max); + } + normal_mode_config_.torque_feedforward_gain = + load_parameter_or(*this, "torque_feedforward_gain", 0.0); + + suspension_mode_config_ = normal_mode_config_; + suspension_mode_config_.td.h = + load_parameter_or(*this, "suspension_td_h", suspension_mode_config_.td.h); + suspension_mode_config_.td.r = + load_parameter_or(*this, "suspension_td_r", suspension_mode_config_.td.r); + suspension_mode_config_.td.max_vel = + load_parameter_or(*this, "suspension_td_max_vel", suspension_mode_config_.td.max_vel); + suspension_mode_config_.td.max_acc = + load_parameter_or(*this, "suspension_td_max_acc", suspension_mode_config_.td.max_acc); + suspension_mode_config_.eso.w0 = + load_parameter_or(*this, "suspension_eso_w0", suspension_mode_config_.eso.w0); + suspension_mode_config_.eso.auto_beta = load_parameter_or( + *this, "suspension_eso_auto_beta", suspension_mode_config_.eso.auto_beta); + suspension_mode_config_.eso.beta1 = + load_parameter_or(*this, "suspension_eso_beta1", suspension_mode_config_.eso.beta1); + suspension_mode_config_.eso.beta2 = + load_parameter_or(*this, "suspension_eso_beta2", suspension_mode_config_.eso.beta2); + suspension_mode_config_.eso.beta3 = + load_parameter_or(*this, "suspension_eso_beta3", suspension_mode_config_.eso.beta3); + suspension_mode_config_.eso.z3_limit = load_parameter_or( + *this, "suspension_eso_z3_limit", suspension_mode_config_.eso.z3_limit); + suspension_mode_config_.nlesf.k1 = + load_parameter_or(*this, "suspension_k1", suspension_mode_config_.nlesf.k1); + suspension_mode_config_.nlesf.k2 = + load_parameter_or(*this, "suspension_k2", suspension_mode_config_.nlesf.k2); + suspension_mode_config_.nlesf.alpha1 = + load_parameter_or(*this, "suspension_alpha1", suspension_mode_config_.nlesf.alpha1); + suspension_mode_config_.nlesf.alpha2 = + load_parameter_or(*this, "suspension_alpha2", suspension_mode_config_.nlesf.alpha2); + suspension_mode_config_.nlesf.delta = + load_parameter_or(*this, "suspension_delta", suspension_mode_config_.nlesf.delta); + suspension_mode_config_.nlesf.u_min = + load_parameter_or(*this, "suspension_u_min", suspension_mode_config_.nlesf.u_min); + suspension_mode_config_.nlesf.u_max = + load_parameter_or(*this, "suspension_u_max", suspension_mode_config_.nlesf.u_max); + suspension_mode_config_.output_min = + load_parameter_or(*this, "suspension_output_min", normal_mode_config_.output_min); + suspension_mode_config_.output_max = + load_parameter_or(*this, "suspension_output_max", normal_mode_config_.output_max); + if (suspension_mode_config_.output_min > suspension_mode_config_.output_max) { + std::swap(suspension_mode_config_.output_min, suspension_mode_config_.output_max); + } + suspension_mode_config_.torque_feedforward_gain = + load_parameter_or(*this, "suspension_torque_feedforward_gain", 1.0); + } + + bool read_inputs_(InputSnapshot& inputs) const { + inputs.measurement_angle = *measurement_angle_; + inputs.setpoint_angle = *setpoint_angle_; + inputs.joint_torque_feedforward = *joint_torque_feedforward_input_; + inputs.suspension_mode = *suspension_mode_input_; + return std::isfinite(inputs.measurement_angle) && std::isfinite(inputs.setpoint_angle); + } + + void update_mode_selection_(bool suspension_mode) { + if (suspension_mode != last_suspension_mode_) { + apply_mode_config_(active_mode_config_(suspension_mode)); + last_suspension_mode_ = suspension_mode; + } + } + + const ModeConfig& active_mode_config_(bool suspension_mode) const { + return suspension_mode ? suspension_mode_config_ : normal_mode_config_; + } + + void apply_mode_config_(const ModeConfig& mode_config) { + td_.set_config(mode_config.td); + eso_.set_config(mode_config.eso); + nlesf_.set_config(mode_config.nlesf); + } + + std::pair effective_output_limits_(const ModeConfig& mode_config) const { + double output_min = mode_config.output_min; + double output_max = mode_config.output_max; + if (use_dynamic_limit_) { + if (!output_limit_.ready()) { + return {nan_, nan_}; + } + const double limit = *output_limit_; + if (!std::isfinite(limit)) { + return {nan_, nan_}; + } + + const double effective_limit = std::max(0.0, limit); + output_min = std::max(output_min, -effective_limit); + output_max = std::min(output_max, effective_limit); + } + return {output_min, output_max}; + } + + bool ensure_feedforward_ready_( + const ModeConfig& mode_config, const InputSnapshot& inputs) const { + return mode_config.torque_feedforward_gain == 0.0 + || std::isfinite(inputs.joint_torque_feedforward); + } + + void initialize_if_needed_(const InputSnapshot& inputs) { + if (initialized_) + return; + + reset_states_(inputs.measurement_angle, inputs.setpoint_angle); + initialized_ = true; + } + + bool run_joint_servo_( + const InputSnapshot& inputs, const ModeConfig& mode_config, double output_min, + double output_max, rmcs_core::controller::adrc::ESO::Output& eso_out, + double& control_torque) { + const auto td_out = td_.update(inputs.setpoint_angle); + 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; + + control_torque = kt_ * nlesf_.compute(e1, e2, eso_out.z3, b0_).u; + if (mode_config.torque_feedforward_gain != 0.0) { + control_torque += mode_config.torque_feedforward_gain * inputs.joint_torque_feedforward; + } + control_torque = std::clamp(control_torque, output_min, output_max); + return std::isfinite(control_torque); + } + + void reset_states_(double measurement_angle, double setpoint_angle) { + td_.reset(setpoint_angle, 0.0); + eso_.reset(measurement_angle); + last_u_ = 0.0; + } + + void publish_control_output_( + double control_torque, const rmcs_core::controller::adrc::ESO::Output& eso_out) { + *control_torque_ = control_torque; + last_u_ = control_torque; + publish_eso_state_(eso_out); + } + + void publish_eso_state_(const rmcs_core::controller::adrc::ESO::Output& eso_out) { + if (eso_z2_output_.active()) { + *eso_z2_output_ = eso_out.z2; + } + if (eso_z3_output_.active()) { + *eso_z3_output_ = eso_out.z3; + } + } + + void disable_output_() { + initialized_ = false; + last_u_ = 0.0; + *control_torque_ = nan_; + if (eso_z2_output_.active()) { + *eso_z2_output_ = nan_; + } + if (eso_z3_output_.active()) { + *eso_z3_output_ = nan_; + } + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + InputInterface measurement_angle_; + InputInterface measurement_velocity_; + InputInterface setpoint_angle_; + // Kept for graph compatibility; TD derives the actual servo-rate target locally. + InputInterface setpoint_velocity_input_; + InputInterface suspension_mode_input_; + InputInterface joint_torque_feedforward_input_; + InputInterface output_limit_; + + OutputInterface control_torque_; + OutputInterface eso_z2_output_; + OutputInterface eso_z3_output_; + + rmcs_core::controller::adrc::TD td_; + rmcs_core::controller::adrc::ESO eso_; + rmcs_core::controller::adrc::NLESF nlesf_; + + ModeConfig normal_mode_config_; + ModeConfig suspension_mode_config_; + + double dt_ = 0.001; + double b0_ = 1.0; + double kt_ = 1.0; + double last_u_ = 0.0; + bool use_dynamic_limit_ = false; + bool initialized_ = false; + bool last_suspension_mode_ = false; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::DeformableJointController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp new file mode 100644 index 000000000..beeb1d2f1 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp @@ -0,0 +1,172 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace rmcs_core::controller::chassis { + +enum class JointFeedbackSource : uint8_t { kLegacyEncoderAngle, kMotorAngle }; + +enum JointIndex : size_t { + kLeftFront = 0, + kLeftBack = 1, + kRightBack = 2, + kRightFront = 3, + kJointCount = 4 +}; + +inline constexpr std::array kJointNames{ + "left_front", "left_back", "right_back", "right_front"}; + +struct JointFeedbackFrame { + std::array motor_angles{}; + std::array physical_angles{}; + std::array physical_velocities{}; + std::array joint_torques{}; + std::array eso_z2{}; + std::array eso_z3{}; +}; + +struct LegFeedback { + double motor_angle = std::numeric_limits::quiet_NaN(); + double physical_angle = std::numeric_limits::quiet_NaN(); + double physical_velocity = std::numeric_limits::quiet_NaN(); + double joint_torque = std::numeric_limits::quiet_NaN(); + double eso_z2 = std::numeric_limits::quiet_NaN(); + double eso_z3 = std::numeric_limits::quiet_NaN(); +}; + +struct JointIO { + using In = rmcs_executor::Component::InputInterface; + using Out = rmcs_executor::Component::OutputInterface; + In angle, physical_angle, physical_velocity, torque, encoder_angle, eso_z2, eso_z3; + Out target_angle, target_physical_angle, target_physical_velocity, target_physical_acceleration; + Out suspension_torque; +}; + +struct JointTrajectoryPlanner { + static constexpr double kJointZeroPhysicalAngleRad = 1.090830782496456; + + void init(double min_angle, double max_angle, double velocity_limit, double acceleration_limit) { + min_angle_ = min_angle; + max_angle_ = max_angle; + velocity_limit_ = velocity_limit; + acceleration_limit_ = acceleration_limit; + } + + void set_target_angle(double angle) { current_target_angle_ = angle; } + double target_angle() const { return current_target_angle_; } + + bool initialize_from_feedback( + const std::array& motor_angles, + const std::array& physical_angles) { + for (size_t i = 0; i < kJointCount; ++i) + if (!std::isfinite(motor_angles[i]) || !std::isfinite(physical_angles[i])) + return false; + target_motor_state_ = motor_angles; + target_physical_state_ = physical_angles; + target_velocity_state_.fill(0.0); + target_acceleration_state_.fill(0.0); + requested_physical_ = physical_angles; + current_physical_ = physical_angles; + active_ = true; + return true; + } + + void sync_from_feedback(size_t index, double motor_angle, double physical_angle) { + target_motor_state_[index] = motor_angle; + target_physical_state_[index] = physical_angle; + target_velocity_state_[index] = 0.0; + target_acceleration_state_[index] = 0.0; + } + + bool active() const { return active_; } + void set_active(bool value) { active_ = value; } + + void fill_symmetric_targets() { per_joint_targets_.fill(current_target_angle_); } + + bool symmetric_requested() const { + for (size_t i = 1; i < kJointCount; ++i) + if (std::abs(per_joint_targets_[0] - per_joint_targets_[i]) > 1e-6) + return false; + return true; + } + + void refresh_deploy_targets(bool deploy_requested, bool /*prone_override*/, double deploy_angle) { + for (size_t i = 0; i < kJointCount; ++i) + requested_physical_[i] = per_joint_targets_[i] * std::numbers::pi / 180.0; + if (deploy_requested) + requested_physical_.fill(deploy_angle * std::numbers::pi / 180.0); + current_physical_ = requested_physical_; + } + + void update_trajectory( + double delta_time, bool use_suspension_limits, + double suspension_velocity_limit, double suspension_acceleration_limit) { + double velocity_limit = use_suspension_limits ? suspension_velocity_limit : velocity_limit_; + double acceleration_limit = + use_suspension_limits ? suspension_acceleration_limit : acceleration_limit_; + for (size_t i = 0; i < kJointCount; ++i) { + double target = current_physical_[i]; + double current_position = target_physical_state_[i]; + double current_velocity = target_velocity_state_[i]; + double error = target - current_position; + double max_velocity = std::sqrt(2.0 * acceleration_limit * std::abs(error)); + double command_velocity = std::copysign(std::min(max_velocity, velocity_limit), error); + double delta_velocity = command_velocity - current_velocity; + double command_acceleration = std::clamp(delta_velocity / delta_time, -acceleration_limit, acceleration_limit); + target_acceleration_state_[i] = command_acceleration; + target_velocity_state_[i] = + std::clamp(current_velocity + command_acceleration * delta_time, -velocity_limit, velocity_limit); + target_physical_state_[i] += target_velocity_state_[i] * delta_time; + target_motor_state_[i] = kJointZeroPhysicalAngleRad - target_physical_state_[i]; + } + } + + const std::array& target_angles() const { return target_motor_state_; } + const std::array& target_physical_angles() const { + return target_physical_state_; + } + const std::array& target_velocities() const { + return target_velocity_state_; + } + const std::array& target_accelerations() const { + return target_acceleration_state_; + } + const std::array& current_physical() const { return current_physical_; } + + void reset(double angle) { + current_target_angle_ = angle; + per_joint_targets_.fill(angle); + active_ = false; + target_motor_state_.fill(0.0); + target_physical_state_.fill(0.0); + target_velocity_state_.fill(0.0); + target_acceleration_state_.fill(0.0); + } + + double min_angle() const { return min_angle_; } + double max_angle() const { return max_angle_; } + +private: + double min_angle_ = 15.0; + double max_angle_ = 55.0; + double velocity_limit_ = 1.0; + double acceleration_limit_ = 1.0; + double current_target_angle_ = 55.0; + std::array per_joint_targets_{55.0, 55.0, 55.0, 55.0}; + bool active_ = false; + std::array target_motor_state_{}; + std::array target_physical_state_{}; + std::array target_velocity_state_{}; + std::array target_acceleration_state_{}; + std::array requested_physical_{}; + std::array current_physical_{}; +}; + +} // 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 new file mode 100644 index 000000000..7ae3c1937 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp @@ -0,0 +1,275 @@ +#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" + +namespace rmcs_core::controller::chassis { + +class DeformableOmniWheelController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableOmniWheelController() + : 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()) + , 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()) + , com_height_(get_parameter_or("com_height", 0.0)) + , chassis_radius_x_(get_parameter_or("chassis_radius_x", 0.2341741 / std::numbers::sqrt2)) + , chassis_radius_y_(get_parameter_or("chassis_radius_y", 0.2341741 / std::numbers::sqrt2)) + , translational_velocity_pid_calculator_(5.0, 0.0, 0.0) + , angular_velocity_pid_calculator_(5.0, 0.0, 0.0) + , wheel_velocity_pid_(0.6, 0.0, 0.0) { + + 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_); + + 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 { + RCLCPP_INFO( + get_logger(), "Max control torque of wheel motor: %.f", + *wheel_motor_max_control_torque_); + } + + void update() override { + if (std::isnan(chassis_control_velocity_->vector[0])) { + reset_all_controls(); + return; + } + + if (!std::isfinite(*chassis_radius_) || *chassis_radius_ <= 1e-6) { + reset_all_controls(); + return; + } + + Eigen::Vector4d wheel_velocities = { + *left_front_velocity_, *left_back_velocity_, *right_back_velocity_, + *right_front_velocity_}; + + const auto chassis_velocity = calculate_chassis_velocity(wheel_velocities); + auto chassis_control_torque = calculate_chassis_control_torque(chassis_velocity); + const auto wheel_pid_torques = + calculate_wheel_pid_torques(wheel_velocities, chassis_velocity); + chassis_control_torque.torque = constrain_chassis_control_torque( + wheel_velocities, chassis_control_torque, wheel_pid_torques); + 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]; + } + +private: + 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; + } + + 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); + Eigen::Vector3d velocity; + velocity.x() = -w1 - w2 + w3 + w4; + velocity.y() = w1 - w2 - w3 + w4; + velocity.z() = (w1 + w2 + w3 + w4) / a_plus_b; + velocity *= (-std::numbers::sqrt2 / 4 * wheel_radius_); + return velocity; + } + + ChassisControlTorque calculate_chassis_control_torque(const Eigen::Vector3d& chassis_velocity) { + ChassisControlTorque result; + + Eigen::Vector3d err = 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>()); + 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]); + + 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; + result.lambda = {-x + y, -x - y}; + + return result; + } + + Eigen::Vector4d calculate_wheel_pid_torques( + const Eigen::Vector4d& wheel_velocities, const Eigen::Vector3d& chassis_velocity) { + const auto& [x, y, z] = chassis_velocity; + const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); + Eigen::Vector4d wheel_control_velocity = { + -x + y + a_plus_b * z, + -x - y + a_plus_b * z, + +x - y + a_plus_b * z, + +x + y + a_plus_b * z, + }; + wheel_control_velocity *= -1 / (std::numbers::sqrt2 * wheel_radius_); + return wheel_velocity_pid_.update(wheel_control_velocity - wheel_velocities); + } + + Eigen::Vector2d constrain_chassis_control_torque( + const Eigen::Vector4d& wheel_velocities, const ChassisControlTorque& chassis_control_torque, + 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& [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_right = rhombus_top / std::max(std::abs(lambda_1), std::abs(lambda_2)); + + const double a = 4 * k1_; + const double b = 0; + const double c = 4 * k1_; + const double d = + lambda_1 * (w1 - w3 + 2 * k1_ * (t1 - t3)) + lambda_2 * (w2 - w4 + 2 * k1_ * (t2 - t4)); + const double e = y_sign * (2 * k1_ * wheel_pid_torques.sum() + wheel_velocities.sum()); + const double f = k1_ * wheel_pid_torques.array().pow(2).sum() + + (wheel_pid_torques.array() * wheel_velocities.array()).sum() + + k2_ * wheel_velocities.array().pow(2).sum() - no_load_power_ + - *power_limit_; + + 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 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 std::vector half_planes = { + { lambda_1 - force_to_torque * gamma_1, y_sign, rhs}, + {-lambda_1 - force_to_torque * gamma_1, -y_sign, rhs}, + { lambda_2 - force_to_torque * gamma_2, y_sign, rhs}, + {-lambda_2 - force_to_torque * gamma_2, -y_sign, rhs}, + {-lambda_1 + force_to_torque * gamma_1, y_sign, rhs}, + { lambda_1 + force_to_torque * gamma_1, -y_sign, rhs}, + {-lambda_2 + force_to_torque * gamma_2, y_sign, rhs}, + { lambda_2 + force_to_torque * gamma_2, -y_sign, rhs}, + }; + result = qcp_solver_.solve( + {1.0, 1.0}, {x_max, std::abs(y_max)}, half_planes, {a, b, c, d, e, f}); + } + + if (!result.array().isFinite().all()) { + result = qcp_solver_.solve( + {1.0, 1.0}, {x_max, std::abs(y_max)}, {rhombus_right, rhombus_top}, + {a, b, c, d, e, f}); + } + result.y() *= y_sign; + return result; + } + + 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 = { + +lambda_1 * chassis_control_torque.torque.x(), + +lambda_2 * chassis_control_torque.torque.x(), + -lambda_1 * chassis_control_torque.torque.x(), + -lambda_2 * chassis_control_torque.torque.x(), + }; + wheel_torques.array() += chassis_control_torque.torque.y(); + wheel_torques.array() += wheel_pid_torques.array(); + 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_; + const double friction_coefficient_; + const double k1_, k2_, no_load_power_; + const double com_height_; + const double chassis_radius_x_; + const double chassis_radius_y_; + + InputInterface wheel_motor_max_control_torque_; + + InputInterface left_front_velocity_; + InputInterface left_back_velocity_; + InputInterface right_back_velocity_; + InputInterface right_front_velocity_; + + InputInterface chassis_control_velocity_; + InputInterface power_limit_; + InputInterface chassis_radius_; + + pid::MatrixPidCalculator<2> translational_velocity_pid_calculator_; + pid::PidCalculator angular_velocity_pid_calculator_; + + 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 + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::DeformableOmniWheelController, 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 new file mode 100644 index 000000000..ba3b68c0c --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp @@ -0,0 +1,873 @@ +#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/qcp_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp index 2c8c8fa35..aeba601a7 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp @@ -21,6 +21,10 @@ class QcpSolver { double right, top; }; + struct HalfPlaneConstraint { + double a, b, c; // ax + by <= c + }; + struct QuadraticConstraint { double a, b, c, d, e, f; }; @@ -52,6 +56,35 @@ class QcpSolver { objective, linear_constraint, quadratic_constraint_matrixes); } + static Eigen::Vector2d solve( + const Eigen::Vector2d& objective, const BoundaryConstraint& boundary_constraint, + const std::vector& half_planes, + const QuadraticConstraint& quadratic_constraint) { + std::vector polygon = { + {boundary_constraint.x_max, boundary_constraint.y_max}, + { 0.0, boundary_constraint.y_max}, + { 0.0, -boundary_constraint.y_max}, + {boundary_constraint.x_max, -boundary_constraint.y_max}, + }; + + for (const auto& half_plane : half_planes) { + sutherland_hodgman(polygon, {half_plane.a, half_plane.b, -half_plane.c}); + if (polygon.size() < 3) + return Eigen::Vector2d::Constant(nan_); + } + + auto quadratic_constraint_matrixes = + calculate_quadratic_constraint_matrixes(quadratic_constraint); + auto quadratic_constraint_best_point = + calculate_quadratic_constraint_best_point(objective, quadratic_constraint_matrixes); + + if (is_point_inside_polygon(quadratic_constraint_best_point, polygon)) + return quadratic_constraint_best_point; + else + return calculate_best_value_at_intersections( + objective, polygon, quadratic_constraint_matrixes); + } + private: /** * Calculates the feasible region of a linear program as a polygon. Returns a non-repeating list @@ -112,11 +145,13 @@ class QcpSolver { static void sutherland_hodgman( std::vector& polygon, const auto& edge_compare, const auto& calculate_intersecting_point) { + if (polygon.empty()) + return; std::vector new_polygon; for (size_t i = polygon.size() - 1, j = 0; j < polygon.size(); i = j++) { auto& current_point = polygon[j]; - auto& prev_point = polygon[i]; + auto& prev_point = polygon[i]; if (edge_compare(current_point) == 0) { new_polygon.emplace_back(current_point); @@ -229,7 +264,7 @@ class QcpSolver { is_in_quadratic_constraint[i] = is_point_inside_quadratic_constraint(linear_constraint[i], quadratic_constraint); - double max_value = -inf_; + double max_value = -inf_; Eigen::Vector2d best_point = Eigen::Vector2d::Zero(); auto check_point = [&](Eigen::Vector2d point) { @@ -237,7 +272,7 @@ class QcpSolver { if ((value > max_value) || (value == max_value && point.x() >= best_point.x() && point.y() >= best_point.y())) { - max_value = value; + max_value = value; best_point = point; } }; @@ -250,9 +285,9 @@ class QcpSolver { const Eigen::Vector2d &x1 = linear_constraint[i], &x2 = linear_constraint[j]; Eigen::Vector2d dx = x2 - x1; - double a = 0.5 * dx.dot(q * dx); - double b = x1.dot(q * dx) + p.dot(dx); - double c = 0.5 * x1.dot(q * x1) + p.dot(x1) + r; + double a = 0.5 * dx.dot(q * dx); + double b = x1.dot(q * dx) + p.dot(dx); + double c = 0.5 * x1.dot(q * x1) + p.dot(x1) + r; double delta = b * b - 4 * a * c; if (delta < 0) @@ -276,4 +311,4 @@ class QcpSolver { static constexpr double epsilon_ = 1e-9; }; -} // namespace rmcs_core::controller::chassis \ No newline at end of file +} // 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 new file mode 100644 index 000000000..6d3fbb720 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -0,0 +1,217 @@ +#include "controller/gimbal/two_axis_gimbal_solver.hpp" +#include "controller/pid/pid_calculator.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::gimbal { + +using namespace rmcs_description; + +class DeformableInfantryGimbalController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableInfantryGimbalController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + configure_pid("yaw_angle", yaw_angle_pid_); + configure_pid("yaw_velocity", yaw_velocity_pid_); + configure_pid("pitch_angle", pitch_angle_pid_); + configure_pid("pitch_velocity", pitch_velocity_pid_); + get_parameter("pitch_torque_control", pitch_torque_control_enabled_); + get_parameter("manual_joystick_sensitivity", joystick_sensitivity_); + get_parameter("manual_mouse_sensitivity", mouse_sensitivity_); + } + + auto update() -> void override { + const auto switch_right = *input_.switch_right; + const auto switch_left = *input_.switch_left; + + using namespace rmcs_msgs; + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + return; + } + + 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() + && input_.auto_aim_control_direction->allFinite() + && !input_.auto_aim_control_direction->isZero(); + const auto angle_error = + 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 (!std::isfinite(angle_error.yaw_angle_error) + || !std::isfinite(angle_error.pitch_angle_error)) { + reset_control_outputs(); + return; + } + + const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error); + 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); + 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); + } 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(); + + auto configure_pid(const std::string& prefix, pid::PidCalculator& calculator) -> void { + get_parameter(prefix + "_integral_min", calculator.integral_min); + get_parameter(prefix + "_integral_max", calculator.integral_max); + get_parameter(prefix + "_integral_split_min", calculator.integral_split_min); + get_parameter(prefix + "_integral_split_max", calculator.integral_split_max); + get_parameter(prefix + "_output_min", calculator.output_min); + get_parameter(prefix + "_output_max", calculator.output_max); + } + + struct Input { + explicit Input(rmcs_executor::Component& component) { + component.register_input("/remote/joystick/left", joystick_left); + component.register_input("/remote/switch/right", switch_right); + component.register_input("/remote/switch/left", switch_left); + component.register_input("/remote/mouse/velocity", mouse_velocity); + component.register_input("/remote/mouse", mouse); + + component.register_input("/tf", tf); + component.register_input("/gimbal/yaw/velocity", yaw_velocity); + component.register_input("/gimbal/pitch/velocity", pitch_velocity); + component.register_input("/gimbal/yaw/velocity_imu", yaw_velocity_imu); + component.register_input("/gimbal/pitch/velocity_imu", pitch_velocity_imu); + + component.register_input("/auto_aim/should_control", auto_aim_should_control, false); + component.register_input( + "/auto_aim/control_direction", auto_aim_control_direction, false); + } + + InputInterface joystick_left; + InputInterface switch_right; + InputInterface switch_left; + InputInterface mouse_velocity; + InputInterface mouse; + + InputInterface tf; + InputInterface yaw_velocity; + InputInterface pitch_velocity; + InputInterface yaw_velocity_imu; + InputInterface pitch_velocity_imu; + + InputInterface auto_aim_should_control; + InputInterface auto_aim_control_direction; + } input_{*this}; + + struct Output { + explicit Output(rmcs_executor::Component& component) { + component.register_output("/gimbal/yaw/control_torque", yaw_control_torque, kNaN); + component.register_output( + "/gimbal/pitch/control_velocity", pitch_control_velocity, kNaN); + component.register_output("/gimbal/pitch/control_torque", pitch_control_torque, 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); + } + + OutputInterface yaw_control_torque; + OutputInterface pitch_control_velocity; + OutputInterface pitch_control_torque; + OutputInterface yaw_angle_error; + OutputInterface pitch_angle_error; + } output_{*this}; + + auto auto_aim_requested() const -> bool { + return input_.mouse->right || *input_.switch_right == rmcs_msgs::Switch::UP; + } + + auto update_auto_aim_control() -> TwoAxisGimbalSolver::AngleError { + return gimbal_solver_.update( + TwoAxisGimbalSolver::SetControlDirection{ + OdomImu::DirectionVector{*input_.auto_aim_control_direction}}); + } + + auto update_manual_control() -> TwoAxisGimbalSolver::AngleError { + if (!gimbal_solver_.enabled()) + return gimbal_solver_.update(TwoAxisGimbalSolver::SetToLevel{}); + + const auto yaw_shift = joystick_sensitivity_ * input_.joystick_left->y() + + mouse_sensitivity_ * input_.mouse_velocity->y(); + const auto pitch_shift = -joystick_sensitivity_ * input_.joystick_left->x() + + mouse_sensitivity_ * input_.mouse_velocity->x(); + return gimbal_solver_.update(TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); + } + + auto reset_control_outputs() -> void { + yaw_angle_pid_.reset(); + yaw_velocity_pid_.reset(); + pitch_angle_pid_.reset(); + pitch_velocity_pid_.reset(); + *output_.yaw_control_torque = kNaN; + *output_.pitch_control_velocity = kNaN; + *output_.pitch_control_torque = kNaN; + } + + auto reset_all_controls() -> void { + gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); + *output_.yaw_angle_error = kNaN; + *output_.pitch_angle_error = kNaN; + reset_control_outputs(); + } + + TwoAxisGimbalSolver gimbal_solver_{ + *this, get_parameter("upper_limit").as_double(), get_parameter("lower_limit").as_double(), + true}; + + pid::PidCalculator yaw_angle_pid_{ + get_parameter("yaw_angle_kp").as_double(), + get_parameter("yaw_angle_ki").as_double(), + get_parameter("yaw_angle_kd").as_double(), + }; + pid::PidCalculator yaw_velocity_pid_{ + get_parameter("yaw_velocity_kp").as_double(), + get_parameter("yaw_velocity_ki").as_double(), + get_parameter("yaw_velocity_kd").as_double(), + }; + pid::PidCalculator pitch_angle_pid_{ + get_parameter("pitch_angle_kp").as_double(), + get_parameter("pitch_angle_ki").as_double(), + get_parameter("pitch_angle_kd").as_double(), + }; + pid::PidCalculator pitch_velocity_pid_{ + get_parameter("pitch_velocity_kp").as_double(), + get_parameter("pitch_velocity_ki").as_double(), + get_parameter("pitch_velocity_kd").as_double(), + }; + + double joystick_sensitivity_ = 0.003; + double mouse_sensitivity_ = 0.5; + bool pitch_torque_control_enabled_ = false; +}; + +} // namespace rmcs_core::controller::gimbal + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::gimbal::DeformableInfantryGimbalController, rmcs_executor::Component) 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 5d937aae0..c1fc772f3 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 @@ -1,14 +1,9 @@ #pragma once #include - -#include #include -#include -#include -#include -#include +#include #include #include #include @@ -26,9 +21,12 @@ class TwoAxisGimbalSolver { }; public: - TwoAxisGimbalSolver(rmcs_executor::Component& component, double upper_limit, double lower_limit) + TwoAxisGimbalSolver( + rmcs_executor::Component& component, double upper_limit, double lower_limit, + bool use_encoder_pitch = false) : upper_limit_(std::cos(upper_limit), -std::sin(upper_limit)) - , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) { + , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) + , use_encoder_pitch_(use_encoder_pitch) { component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); component.register_input("/tf", tf_); @@ -45,7 +43,7 @@ class TwoAxisGimbalSolver { PitchLink::DirectionVector update(TwoAxisGimbalSolver& super) const override { auto odom_dir = fast_tf::cast( PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *super.tf_); - if (odom_dir->x() == 0 || odom_dir->y() == 0) + if (std::abs(odom_dir->x()) < 1e-6 && std::abs(odom_dir->y()) < 1e-6) return {}; super.control_enabled_ = true; @@ -107,7 +105,9 @@ class TwoAxisGimbalSolver { if (!control_enabled_) return {nan_, nan_}; - auto [control_direction_yaw_link, pitch] = pitch_link_to_yaw_link(control_direction); + auto [control_direction_yaw_link, pitch] = + use_encoder_pitch_ ? pitch_link_to_yaw_link_from_encoder(control_direction) + : pitch_link_to_yaw_link(control_direction); clamp_control_direction(control_direction_yaw_link); if (!control_enabled_) @@ -136,7 +136,7 @@ class TwoAxisGimbalSolver { auto yaw_axis = fast_tf::cast(yaw_axis_filtered_, *tf_); pitch = {yaw_axis->z(), yaw_axis->x()}; - pitch.normalized(); + pitch.normalize(); const auto& [x, y, z] = *dir; dir_yaw_link = {x * pitch.x() - z * pitch.y(), y, x * pitch.y() + z * pitch.x()}; @@ -144,6 +144,25 @@ class TwoAxisGimbalSolver { return result; } + auto pitch_link_to_yaw_link_from_encoder(const PitchLink::DirectionVector& dir) const + -> std::pair { + + std::pair result; + auto& [dir_yaw_link, pitch_cs] = result; + + const double encoder_pitch = *gimbal_pitch_angle_; + pitch_cs = {std::cos(encoder_pitch), -std::sin(encoder_pitch)}; + + const auto& [x, y, z] = *dir; + dir_yaw_link = { + x * pitch_cs.x() - z * pitch_cs.y(), + y, + x * pitch_cs.y() + z * pitch_cs.x(), + }; + + return result; + } + static PitchLink::DirectionVector yaw_link_to_pitch_link(const YawLink::DirectionVector& dir, const Eigen::Vector2d& pitch) { @@ -185,6 +204,7 @@ class TwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); const Eigen::Vector2d upper_limit_, lower_limit_; + bool use_encoder_pitch_ = false; rmcs_executor::Component::InputInterface gimbal_pitch_angle_; rmcs_executor::Component::InputInterface tf_; @@ -195,4 +215,4 @@ class TwoAxisGimbalSolver { OdomImu::DirectionVector control_direction_; }; -} // namespace rmcs_core::controller::gimbal \ No newline at end of file +} // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp new file mode 100644 index 000000000..fcb9a32ad --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp @@ -0,0 +1,67 @@ +// filter/alpha_beta_angle_filter.hpp +#pragma once + +#include +#include +#include +#include + +namespace rmcs_core::filter { + +class AlphaBetaAngleFilter { +public: + AlphaBetaAngleFilter(double dt, double alpha, double beta) + : dt_(dt), alpha_(alpha), beta_(beta) {} + + void reset() { + initialized_ = false; + angle_ = std::numeric_limits::quiet_NaN(); + velocity_ = 0.0; + } + + void set_gains(double alpha, double beta) { + alpha_ = alpha; + beta_ = beta; + } + + std::pair update(double measurement_rad) { + if (!std::isfinite(measurement_rad)) { + reset(); + return {std::numeric_limits::quiet_NaN(), 0.0}; + } + + if (!initialized_) { + initialized_ = true; + angle_ = wrap_angle_(measurement_rad); + velocity_ = 0.0; + return {angle_, velocity_}; + } + + const double angle_pred = wrap_angle_(angle_ + velocity_ * dt_); + const double residual = wrap_angle_(measurement_rad - angle_pred); + + angle_ = wrap_angle_(angle_pred + alpha_ * residual); + velocity_ = velocity_ + (beta_ * residual) / dt_; + + return {angle_, velocity_}; + } + + double angle() const { return angle_; } + double velocity() const { return velocity_; } + bool initialized() const { return initialized_; } + +private: + static double wrap_angle_(double x) { + return std::remainder(x, 2.0 * std::numbers::pi); + } + + double dt_; + double alpha_; + double beta_; + + bool initialized_ = false; + double angle_ = std::numeric_limits::quiet_NaN(); + double velocity_ = 0.0; +}; + +} // namespace rmcs_core::filter 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 new file mode 100644 index 000000000..3b656dd6c --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -0,0 +1,769 @@ +#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/supercap.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware { + +using Clock = std::chrono::steady_clock; + +class DeformableInfantryOmni + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableInfantryOmni() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , 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.16, 0.0, 0.15}); + + 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()); + imu_board_ = + std::make_unique(*this, get_parameter("serial_filter_imu").as_string()); + + // 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); + }); + } + + ~DeformableInfantryOmni() override = default; + + void update() override { + bottom_board_->update(); + top_board_->update(); + imu_board_->update(); + } + + void command_update() { + const bool even = ((cmd_tick_++ & 1u) == 0u); + bottom_board_->command_update(even); + top_board_->command_update(); + } + +private: + static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + + class Command : public rmcs_executor::Component { + public: + explicit Command(DeformableInfantryOmni& deformableInfantry) + : deformableInfantry(deformableInfantry) {} + + void update() override { deformableInfantry.command_update(); } + + DeformableInfantryOmni& deformableInfantry; + }; + + class BottomBoard final : private librmcs::agent::RmcsBoardLite { + friend class DeformableInfantryOmni; + + public: + explicit BottomBoard( + DeformableInfantryOmni& status, rmcs_executor::Component& command, + const std::string& serial_filter = {}) + : RmcsBoardLite{ + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} + , status_{status} + , command_{command} { + + status.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(status.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(11.0) + .enable_multi_turn_angle() + .set_reversed()); + + 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()); + + status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); + 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); + 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_, kNaN); + + status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); + status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); + status.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_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) { + 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(), + }); + builder.can2_transmit({ + .can_id = 0x142, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), + }); + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .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(), + }); + } + } + + 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; + + DeformableInfantryOmni& status_; + Component& command_; + + device::Bmi088 imu_{1000, 0.2, 0.0}; + device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; + device::Dr16 dr16_{status_}; + + device::DjiMotor chassis_wheel_motors_[4]{ + device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, + device::DjiMotor{status_, command_, "/chassis/left_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_back_wheel"}, + device::DjiMotor{status_, command_, "/chassis/right_front_wheel"}, + }; + device::LkMotor chassis_joint_motors_[4]{ + device::LkMotor{status_, command_, "/chassis/left_front_joint"}, + device::LkMotor{status_, command_, "/chassis/left_back_joint"}, + device::LkMotor{status_, command_, "/chassis/right_back_joint"}, + device::LkMotor{status_, 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_{status_, command_}; + std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; + std::atomic supercap_status_received_{false}; + device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; + + OutputInterface& tf_{status_.tf_}; + + 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_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + 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 = kNaN; + *velocity_output = kNaN; + return; + } + + const auto to_physical_angle = [](double motor_angle) { + return joint_zero_physical_angle_rad_ - motor_angle; + }; + const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; + + *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_ = kNaN; + *encoder_alpha_dot_ = kNaN; + *radius_ = kNaN; + 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( + 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)); + } + + if (debug_log_deformable_joint_motor_) { + 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)); + } + + 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( + 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 ? supercap_.supercap_voltage() : kNaN, + supercap_rx ? supercap_.chassis_voltage() : kNaN, + supercap_rx ? supercap_.chassis_power() : kNaN, + 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.data(), data.uart_data.size()); + } + + 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); + } + } + + 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) { + 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); + } + } + + 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); + } + } + + 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); + } + }; + + class ImuBoard final : private librmcs::agent::RmcsBoardLite { + friend class DeformableInfantryOmni; + + public: + explicit ImuBoard(DeformableInfantryOmni& status, const std::string& serial_filter = {}) + : RmcsBoardLite{ + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} + , tf_{status.tf_} + , 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 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::Bmi088 bmi088_; + }; + + class TopBoard final : private librmcs::agent::CBoard { + friend class DeformableInfantryOmni; + + public: + explicit TopBoard( + DeformableInfantryOmni& status, Command& command, const std::string& serial_filter = {}) + : librmcs::agent::CBoard( + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) + , tf_(status.tf_) + , bmi088_(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_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .set_encoder_zero_point( + static_cast(status.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()); + + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); + } + + ~TopBoard() override = default; + + 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_imu_ = bmi088_.gz(); + + tf_->set_state( + pitch_encoder_angle); + } + + void command_update() { + auto builder = start_transmit(); + builder.can1_transmit({ + .can_id = 0x141, + .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), + }); + + builder.can2_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 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 == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + } + + 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 == 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); + } + + OutputInterface& tf_; + OutputInterface gimbal_yaw_velocity_imu_; + + device::Bmi088 bmi088_; + 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) + -> 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: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); + text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + + text("Chassis Status"); + 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(); + } + + OutputInterface tf_; + InputInterface timestamp_; + + std::unique_ptr imu_board_; + std::unique_ptr top_board_; + std::unique_ptr bottom_board_; + + std::shared_ptr command_; + uint32_t cmd_tick_ = 0; + + std::shared_ptr> status_service_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmni, rmcs_executor::Component) 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 new file mode 100644 index 000000000..6514c58b9 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp @@ -0,0 +1,876 @@ +#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/supercap.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.16, 0.0, 0.15}); + + steers_calibrate_subscription_ = create_subscription( + "/steers/calibrate", rclcpp::QoS(1), [this](std_msgs::msg::Int32::UniquePtr msg) { + steers_calibrate_subscription_callback(std::move(msg)); + }); + + joints_calibrate_subscription_ = create_subscription( + "/joints/calibrate", rclcpp::QoS(1), [this](std_msgs::msg::Int32::UniquePtr msg) { + joints_calibrate_subscription_callback(std::move(msg)); + }); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + + rmcs_board_lite = 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()); + } + + ~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(); + } + + 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 TopBoard; + + void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + if (!rmcs_board_lite) + return; + + RCLCPP_INFO( + get_logger(), "New left front offset: %d", + rmcs_board_lite->chassis_steer_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "New left back offset: %d", + rmcs_board_lite->chassis_steer_motors_[1].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "New right back offset: %d", + rmcs_board_lite->chassis_steer_motors_[2].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "New right front offset: %d", + rmcs_board_lite->chassis_steer_motors_[3].calibrate_zero_point()); + } + + void joints_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + if (!rmcs_board_lite) + return; + + RCLCPP_INFO( + get_logger(), "New left front offset: %ld", + rmcs_board_lite->chassis_joint_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "New left back offset: %ld", + rmcs_board_lite->chassis_joint_motors_[1].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "New right back offset: %ld", + rmcs_board_lite->chassis_joint_motors_[2].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "New right front offset: %ld", + rmcs_board_lite->chassis_joint_motors_[3].calibrate_zero_point()); + } + + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + if (!rmcs_board_lite || !top_board_) + return; + + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New yaw offset: %ld", + rmcs_board_lite->gimbal_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New pitch offset: %ld", + top_board_->gimbal_pitch_motor_.calibrate_zero_point()); + } + + 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, std::string serial_filter = {}) + : librmcs::agent::RmcsBoardLite( + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) + , deformable_infantry_(deformableInfantry) + , tf_(deformableInfantry.tf_) + , imu_(1000, 0.2, 0.0) + , gimbal_yaw_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/yaw") + , dr16_(deformableInfantry) + , chassis_wheel_motors_{device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_wheel"},} + , chassis_steer_motors_{device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_steering"}} + , chassis_joint_motors_{device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_joint"}} + , next_chassis_feedback_log_time_(Clock::now() + std::chrono::seconds(1)) + , next_supercap_feedback_log_time_(Clock::now() + std::chrono::seconds(1)) + , supercap_(deformableInfantry, deformableInfantry_command) + , gimbal_bullet_feeder_( + deformableInfantry, deformableInfantry_command, "/gimbal/bullet_feeder") { + + 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_torque_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.data(), data.uart_data.size()); + } + + 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); + } + + OutputInterface& tf_; + + device::Bmi088 imu_; + device::LkMotor gimbal_yaw_motor_; + device::Dr16 dr16_; + device::DjiMotor chassis_wheel_motors_[4]; + device::DjiMotor chassis_steer_motors_[4]; + device::LkMotor chassis_joint_motors_[4]; + 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::time_point next_supercap_feedback_log_time_; + device::Supercap supercap_; + std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; + std::atomic supercap_status_received_{false}; + device::DjiMotor 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 = {}) + : librmcs::agent::RmcsBoardLite( + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) + , hard_sync_pending_(deformableInfantry.hard_sync_pending_) + , 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_); + 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(); + 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_yaw_velocity_bmi088_ = bmi088_.gz(); + *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_; + 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_; + }; + + OutputInterface tf_; + InputInterface timestamp_; + 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 top_board_; + + rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; + rclcpp::Subscription::SharedPtr joints_calibrate_subscription_; + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + + 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/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp index c11061c23..7ab8a38f5 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp @@ -23,7 +23,7 @@ namespace rmcs_core::hardware::device { class LkMotor { public: - enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10 }; + enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10, kMG5010Ei36 }; struct Config { explicit Config(Type type) @@ -69,14 +69,12 @@ class LkMotor { multi_turn_encoder_count_ = 0; last_raw_angle_ = 0; - double current_max; double torque_constant; double reduction_ratio; switch (config.motor_type) { case Type::kMG5010Ei10: raw_angle_modulus_ = 1 << 16; - current_max = 33.0; torque_constant = 0.90909; reduction_ratio = 10.0; @@ -89,25 +87,28 @@ class LkMotor { break; case Type::kMG4010Ei10: raw_angle_modulus_ = 1 << 16; - current_max = 33.0; torque_constant = 0.07; reduction_ratio = 10.0; max_torque_ = 4.5; break; case Type::kMG6012Ei8: raw_angle_modulus_ = 1 << 16; - current_max = 33.0; torque_constant = 1.09; reduction_ratio = 8.0; max_torque_ = 16.0; break; case Type::kMG4005Ei10: raw_angle_modulus_ = 1 << 16; - current_max = 33.0; torque_constant = 0.06; reduction_ratio = 10.0; max_torque_ = 2.5; break; + case Type::kMG5010Ei36: + raw_angle_modulus_ = 1 << 16; + torque_constant = 0.3; + reduction_ratio = 36.0; + max_torque_ = 25.0; + break; default: std::unreachable(); } @@ -125,7 +126,7 @@ class LkMotor { velocity_to_command_velocity_coefficient_ = sign * reduction_ratio * kRadToDeg * 100.0; status_current_to_torque_coefficient_ = - sign * (current_max / kRawCurrentMax) * torque_constant * reduction_ratio; + sign * (kProtocolCurrentMax / kRawCurrentMax) * torque_constant * reduction_ratio; torque_to_command_current_coefficient_ = 1 / status_current_to_torque_coefficient_; *max_torque_output_ = max_torque(); @@ -485,7 +486,9 @@ class LkMotor { // Limits static constexpr double kNan = std::numeric_limits::quiet_NaN(); + // LK protocol maps the raw current field range [-2048, 2048] to [-33A, 33A]. static constexpr int kRawCurrentMax = 2048; + static constexpr double kProtocolCurrentMax = 33.0; int raw_angle_modulus_; // Constants 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 new file mode 100644 index 000000000..269a5682b --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -0,0 +1,181 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" +#include "referee/app/ui/widget/crosshair_circle.hpp" +#include "referee/app/ui/widget/deformable_chassis_top_view.hpp" +#include "referee/app/ui/widget/status_ring.hpp" + +namespace rmcs_core::referee::app::ui { +using namespace std::chrono_literals; + +class DeformableInfantry + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableInfantry() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) + , status_ring_(26.5, 26.5, 600, 300) + , horizontal_center_guidelines_( + {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, + {Shape::Color::WHITE, 2, x_center + 110, y_center, x_center + 360, y_center}) + , vertical_center_guidelines_( + {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, + {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) + , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) + , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { + + double deformable_leg_min_angle_deg = 8.0; + double deformable_leg_max_angle_deg = 58.0; + get_parameter_or( + "deformable_leg_min_angle_deg", deformable_leg_min_angle_deg, + deformable_leg_min_angle_deg); + get_parameter_or( + "deformable_leg_max_angle_deg", deformable_leg_max_angle_deg, + deformable_leg_max_angle_deg); + deformable_chassis_leg_arcs_.set_angle_range( + deformable_leg_min_angle_deg, deformable_leg_max_angle_deg); + + register_input("/chassis/control_mode", chassis_mode_); + + register_input("/chassis/angle", chassis_angle_); + + register_input("/chassis/supercap/voltage", supercap_voltage_); + register_input("/chassis/supercap/enabled", supercap_enabled_); + + 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); + + register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); + + register_input("/gimbal/left_friction/control_velocity", left_friction_control_velocity_); + register_input("/gimbal/left_friction/velocity", left_friction_velocity_); + register_input("/gimbal/right_friction/velocity", right_friction_velocity_); + + register_input("/remote/mouse", mouse_); + + register_input("/referee/game/stage", game_stage_); + } + + void update() override { + update_chassis_direction_indicator(); + update_deformable_chassis_leg_arcs(); + + status_ring_.update_bullet_allowance(*robot_bullet_allowance_); + status_ring_.update_friction_wheel_speed( + std::min(*left_friction_velocity_, *right_friction_velocity_), + *left_friction_control_velocity_ > 0); + status_ring_.update_supercap(*supercap_voltage_, *supercap_enabled_); + status_ring_.update_battery_power(*chassis_voltage_); + + status_ring_.update_auto_aim_enable(mouse_->right == 1); + } + +private: + void update_time_reminder() { + if (!game_stage_.ready()) + return; + } + + void update_chassis_direction_indicator() { + auto chassis_mode = *chassis_mode_; + + auto to_referee_angle = [](double angle) { + return static_cast( + std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); + }; + chassis_direction_indicator_.set_color(chassis_direction_indicator_color(chassis_mode)); + chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); + } + + static Shape::Color chassis_direction_indicator_color(rmcs_msgs::ChassisMode mode) { + switch (mode) { + case rmcs_msgs::ChassisMode::SPIN: return Shape::Color::GREEN; + case rmcs_msgs::ChassisMode::AUTO: return Shape::Color::CYAN; + case rmcs_msgs::ChassisMode::STEP_DOWN: return Shape::Color::WHITE; + default: return Shape::Color::PINK; + } + } + + 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()) { + 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); + } + + static constexpr uint16_t screen_width = 1920, screen_height = 1080; + static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + + InputInterface chassis_mode_; + InputInterface chassis_angle_; + + InputInterface supercap_voltage_; + InputInterface supercap_enabled_; + + 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_; + + InputInterface robot_bullet_allowance_; + + InputInterface left_friction_control_velocity_; + InputInterface left_friction_velocity_; + InputInterface right_friction_velocity_; + + InputInterface mouse_; + + InputInterface game_stage_; + + CrossHairCircle crosshair_circle_; + StatusRing status_ring_; + + Line horizontal_center_guidelines_[2]; + Line vertical_center_guidelines_[2]; + + Arc chassis_direction_indicator_; + DeformableChassisLegArcs deformable_chassis_leg_arcs_; + + Integer time_reminder_; +}; + +} // namespace rmcs_core::referee::app::ui + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::DeformableInfantry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp new file mode 100644 index 000000000..21526b9b5 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::referee::app::ui { + +class CrossHairCircle { +public: + CrossHairCircle(Shape::Color color, uint16_t x, uint16_t y, bool visible = true) + : CrossHairCircle(color, x, y, 12, 2, visible) {} + + CrossHairCircle( + Shape::Color color, uint16_t x, uint16_t y, uint16_t r, uint16_t width, + bool visible = true) + : circle_(color, width, x, y, r, r, visible) {} + + void set_visible(bool value) { circle_.set_visible(value); } + void set_r(uint16_t r) { circle_.set_r(r); } + void set_width(uint16_t width) { circle_.set_width(width); } + +private: + Circle circle_; +}; + +} // namespace rmcs_core::referee::app::ui 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 new file mode 100644 index 000000000..e5ea754be --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp @@ -0,0 +1,155 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::referee::app::ui { + +class DeformableChassisLegArcs { +public: + DeformableChassisLegArcs() = default; + + void set_angle_range(double min_angle_deg, double max_angle_deg) { + min_angle_rad_ = deg_to_rad_(min_angle_deg); + max_angle_rad_ = deg_to_rad_(max_angle_deg); + if (min_angle_rad_ > max_angle_rad_) + std::swap(min_angle_rad_, max_angle_rad_); + } + + void update(double chassis_angle, const std::array& leg_angles) { + if (!valid_angle_range_()) { + set_visible(false); + return; + } + + if (std::isfinite(chassis_angle)) + last_chassis_angle_ = chassis_angle; + + set_visible(true); + for (std::size_t i = 0; i < legs_.size(); ++i) { + if (std::isfinite(leg_angles[i])) + 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]); + } + } + + void set_visible(bool value) { + for (auto& leg : legs_) + leg.set_visible(value); + } + +private: + static constexpr uint16_t center_x_ = 1920 / 2; + static constexpr uint16_t center_y_ = 1080 / 2; + + static constexpr uint16_t front_leg_radius_near_ = 102; + static constexpr uint16_t rear_leg_radius_near_ = 112; + static constexpr uint16_t front_leg_radius_far_ = 132; + static constexpr uint16_t rear_leg_radius_far_ = 142; + + static constexpr uint16_t prone_leg_width_ = 6; + static constexpr uint16_t upright_leg_width_ = 16; + + static constexpr uint16_t upright_leg_half_angle_ = 10; + static constexpr uint16_t prone_leg_half_angle_ = 6; + + static constexpr double front_pair_offset_deg_ = 28.0; + static constexpr double rear_pair_offset_deg_ = 28.0; + static constexpr double degrees_to_radians_ = std::numbers::pi_v / 180.0; + + static constexpr double default_min_angle_deg_ = 8.0; + static constexpr double default_max_angle_deg_ = 58.0; + + static constexpr double deg_to_rad_(double degrees) { + return degrees * degrees_to_radians_; + } + + 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_, + }; + + static constexpr std::array leg_radii_near_ = { + front_leg_radius_near_, + rear_leg_radius_near_, + rear_leg_radius_near_, + front_leg_radius_near_, + }; + + static constexpr std::array leg_radii_far_ = { + front_leg_radius_far_, + rear_leg_radius_far_, + rear_leg_radius_far_, + front_leg_radius_far_, + }; + + static uint16_t to_referee_angle_(double angle) { + const auto degrees = + static_cast(std::lround((2.0 * std::numbers::pi_v - angle) + / std::numbers::pi_v * 180.0)); + int wrapped = degrees % 360; + if (wrapped < 0) + wrapped += 360; + return static_cast(wrapped); + } + + bool valid_angle_range_() const { return max_angle_rad_ - min_angle_rad_ > 1e-6; } + + double normalized_leg_extension_(double leg_angle) const { + return std::clamp( + (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; + } + + void update_leg_( + Arc& leg, double body_angle, uint16_t near_radius, uint16_t far_radius, + double leg_angle) 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(std::lround( + far_radius - normalized_extension * (far_radius - near_radius))); + const uint16_t half_angle = static_cast(std::lround( + prone_leg_half_angle_ + - normalized_extension * (prone_leg_half_angle_ - upright_leg_half_angle_))); + const uint16_t width = static_cast(std::lround( + prone_leg_width_ + + normalized_extension * (upright_leg_width_ - prone_leg_width_))); + + leg.set_x(center_x_); + leg.set_y(center_y_); + leg.set_r(radius); + leg.set_width(width); + leg.set_color(leg_color_(normalized_extension)); + leg.set_angle(to_referee_angle_(body_angle), half_angle); + } + + double min_angle_rad_ = deg_to_rad_(default_min_angle_deg_); + double max_angle_rad_ = deg_to_rad_(default_max_angle_deg_); + double last_chassis_angle_ = 0.0; + std::array last_leg_angles_ = { + deg_to_rad_(default_max_angle_deg_), + deg_to_rad_(default_max_angle_deg_), + deg_to_rad_(default_max_angle_deg_), + deg_to_rad_(default_max_angle_deg_), + }; + + std::array legs_; +}; + +} // namespace rmcs_core::referee::app::ui diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index 7daf46336..c080a355c 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -1,3 +1,5 @@ +#include +#include #include #include #include @@ -19,11 +21,25 @@ class Status , public rclcpp::Node { public: Status() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , logger_(get_logger()) { register_input("/referee/serial", serial_); register_output("/referee/game/stage", game_stage_, rmcs_msgs::GameStage::UNKNOWN); + register_output("/referee/game/stage_remain_time", stage_remain_time_, 0); + register_output("/referee/game/sync_timestamp", sync_timestamp_, uint64_t{0}); + register_output( + "/referee/event/ally_big_energy_activation_status", ally_big_energy_activation_status_, + 0); + register_output( + "/referee/event/ally_small_energy_activation_status", + ally_small_energy_activation_status_, 0); + register_output( + "/referee/event/ally_fortress_occupation_status", ally_fortress_occupation_status_, 0); + register_output( + "/referee/dart/latest_hit_target_total_count", dart_latest_hit_target_total_count_, 0); register_output("/referee/id", robot_id_, rmcs_msgs::RobotId::UNKNOWN); register_output("/referee/shooter/cooling", robot_shooter_cooling_, 0); @@ -34,13 +50,49 @@ class Status register_output("/referee/chassis/output_status", chassis_output_status_, false); register_output("/referee/robots/hp", robots_hp_); + register_output("/referee/ally/hero_hp", ally_hero_hp_, 0); + register_output("/referee/ally/engineer_hp", ally_engineer_hp_, 0); + register_output("/referee/ally/infantry_1_hp", ally_infantry_1_hp_, 0); + register_output("/referee/ally/infantry_2_hp", ally_infantry_2_hp_, 0); + register_output("/referee/ally/outpost/hp", ally_outpost_hp_, 0); + register_output("/referee/ally/base/hp", ally_base_hp_, 0); + register_output("/referee/current_hp", robot_current_hp_); + register_output("/referee/position/x", robot_position_x_, 0.0); + register_output("/referee/position/y", robot_position_y_, 0.0); + register_output("/referee/position/angle", robot_position_angle_, 0.0); register_output("/referee/shooter/bullet_allowance", robot_bullet_allowance_, false); register_output( "/referee/shooter/42mm_bullet_allowance", robot_42mm_bullet_allowance_, false); + register_output( + "/referee/shooter/fortress_17mm_bullet_allowance", + robot_fortress_17mm_bullet_allowance_, 0); + register_output("/referee/remaining_gold_coin", remaining_gold_coin_, 0); register_output("/referee/shooter/initial_speed", robot_initial_speed_, false); register_output("/referee/shooter/shoot_timestamp", robot_shoot_timestamp_, false); + register_output( + "/referee/map_command/target_position_x", map_command_target_position_x_, 0.0); + register_output( + "/referee/map_command/target_position_y", map_command_target_position_y_, 0.0); + register_output("/referee/map_command/keyboard", map_command_keyboard_, 0); + register_output("/referee/map_command/target_robot_id", map_command_target_robot_id_, 0); + register_output("/referee/map_command/source", map_command_source_, 0); + register_output( + "/referee/map_command/received_timestamp", map_command_received_timestamp_, 0.0); + register_output( + "/referee/map_command/event/target_position_x", map_command_event_target_position_x_, + 0.0); + register_output( + "/referee/map_command/event/target_position_y", map_command_event_target_position_y_, + 0.0); + register_output("/referee/map_command/event/keyboard", map_command_event_keyboard_, 0); + register_output( + "/referee/map_command/event/target_robot_id", map_command_event_target_robot_id_, 0); + register_output("/referee/map_command/event/source", map_command_event_source_, 0); + register_output("/referee/map_command/event/timestamp", map_command_event_timestamp_, 0.0); + register_output("/referee/map_command/event/sequence", map_command_event_sequence_, 0); + robot_status_watchdog_.reset(5'000); } @@ -99,6 +151,10 @@ class Status update_game_status(); if (command_id == 0x0003) update_game_robot_hp(); + else if (command_id == 0x0101) + update_event_data(); + else if (command_id == 0x0105) + update_dart_info(); else if (command_id == 0x0201) update_robot_status(); else if (command_id == 0x0202) @@ -111,21 +167,48 @@ class Status update_shoot_data(); else if (command_id == 0x0208) update_bullet_allowance(); - else if (command_id == 0x020B) - update_game_robot_position(); + else if (command_id == 0x0303) + update_map_command(); } void update_game_status() { auto& data = reinterpret_cast(frame_.body.data); - *game_stage_ = static_cast(data.game_stage); + *game_stage_ = static_cast(data.game_progress); + *stage_remain_time_ = data.stage_remain_time; + *sync_timestamp_ = data.sync_timestamp; + if (*game_stage_ == rmcs_msgs::GameStage::STARTED) game_status_watchdog_.reset(30'000); else game_status_watchdog_.reset(5'000); } - void update_game_robot_hp() {} + void update_event_data() { + auto& data = reinterpret_cast(frame_.body.data); + + const uint32_t event_data = data.event_data; + *ally_small_energy_activation_status_ = (event_data >> 3) & 0x03; + *ally_big_energy_activation_status_ = (event_data >> 5) & 0x03; + *ally_fortress_occupation_status_ = (event_data >> 25) & 0x03; + } + + void update_dart_info() { + auto& data = reinterpret_cast(frame_.body.data); + + *dart_latest_hit_target_total_count_ = (data.dart_info >> 3) & 0x07; + } + + void update_game_robot_hp() { + auto& data = reinterpret_cast(frame_.body.data); + *robots_hp_ = data; + *ally_hero_hp_ = data.ally_1_robot_hp; + *ally_engineer_hp_ = data.ally_2_robot_hp; + *ally_infantry_1_hp_ = data.ally_3_robot_hp; + *ally_infantry_2_hp_ = data.ally_4_robot_hp; + *ally_outpost_hp_ = data.ally_outpost_hp; + *ally_base_hp_ = data.ally_base_hp; + } void update_robot_status() { if (*game_stage_ == rmcs_msgs::GameStage::STARTED) @@ -135,6 +218,7 @@ class Status auto& data = reinterpret_cast(frame_.body.data); + *robot_current_hp_ = data.current_hp; *robot_id_ = static_cast(data.robot_id); *robot_shooter_cooling_ = data.shooter_barrel_cooling_value; *robot_shooter_heat_limit_ = static_cast(1000) * data.shooter_barrel_heat_limit; @@ -144,18 +228,22 @@ class Status else *robot_chassis_power_limit_ = static_cast(data.chassis_power_limit); - *chassis_output_status_ = static_cast(data.power_management_chassis_output); + *chassis_output_status_ = data.power_management_status & (1u << 1); } void update_power_heat_data() { power_heat_data_watchdog_.reset(3'000); auto& data = reinterpret_cast(frame_.body.data); - *robot_chassis_power_ = data.chassis_power; *robot_buffer_energy_ = static_cast(data.buffer_energy); } - void update_robot_position() {} + void update_robot_position() { + auto& data = reinterpret_cast(frame_.body.data); + *robot_position_x_ = data.x; + *robot_position_y_ = data.y; + *robot_position_angle_ = data.angle; + } void update_hurt_data() {} @@ -169,12 +257,48 @@ class Status void update_bullet_allowance() { auto& data = reinterpret_cast(frame_.body.data); - *robot_bullet_allowance_ = data.bullet_allowance_17mm; - *robot_42mm_bullet_allowance_ = data.bullet_allowance_42mm; + *robot_bullet_allowance_ = data.projectile_allowance_17mm; + *robot_42mm_bullet_allowance_ = data.projectile_allowance_42mm; + *remaining_gold_coin_ = data.remaining_gold_coin; + *robot_fortress_17mm_bullet_allowance_ = data.projectile_allowance_fortress; } - void update_game_robot_position() {} + void update_map_command() { + if (frame_.header.data_length < sizeof(MapCommand)) { + RCLCPP_WARN( + logger_, "Map command length invalid: %u", + static_cast(frame_.header.data_length)); + return; + } + + MapCommand data; + std::memcpy(&data, frame_.body.data, sizeof(data)); + + *map_command_target_position_x_ = data.target_position_x; + *map_command_target_position_y_ = data.target_position_y; + *map_command_keyboard_ = data.cmd_keyboard; + *map_command_target_robot_id_ = data.target_robot_id; + *map_command_source_ = data.cmd_source; + const auto now = std::chrono::high_resolution_clock::now(); + *map_command_received_timestamp_ = + std::chrono::duration(now.time_since_epoch()).count(); + + if (has_last_map_command_ && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { + return; + } + + last_map_command_ = data; + has_last_map_command_ = true; + + *map_command_event_target_position_x_ = data.target_position_x; + *map_command_event_target_position_y_ = data.target_position_y; + *map_command_event_keyboard_ = data.cmd_keyboard; + *map_command_event_target_robot_id_ = data.target_robot_id; + *map_command_event_source_ = data.cmd_source; + *map_command_event_timestamp_ = *map_command_received_timestamp_; + *map_command_event_sequence_ += 1; + } // When referee system loses connection unexpectedly, // use these indicators make sure the robot safe. // Muzzle: Cooling priority with level 1 @@ -191,6 +315,12 @@ class Status rmcs_utility::TickTimer game_status_watchdog_; OutputInterface game_stage_; + OutputInterface stage_remain_time_; + OutputInterface sync_timestamp_; + OutputInterface ally_big_energy_activation_status_; + OutputInterface ally_small_energy_activation_status_; + OutputInterface ally_fortress_occupation_status_; + OutputInterface dart_latest_hit_target_total_count_; rmcs_utility::TickTimer robot_status_watchdog_; OutputInterface robot_id_; @@ -203,15 +333,43 @@ class Status OutputInterface robot_buffer_energy_; OutputInterface robots_hp_; + OutputInterface ally_hero_hp_; + OutputInterface ally_engineer_hp_; + OutputInterface ally_infantry_1_hp_; + OutputInterface ally_infantry_2_hp_; + OutputInterface ally_outpost_hp_; + OutputInterface ally_base_hp_; + OutputInterface robot_current_hp_; + OutputInterface robot_position_x_; + OutputInterface robot_position_y_; + OutputInterface robot_position_angle_; OutputInterface robot_bullet_allowance_; OutputInterface robot_42mm_bullet_allowance_; + OutputInterface robot_fortress_17mm_bullet_allowance_; + OutputInterface remaining_gold_coin_; OutputInterface robot_initial_speed_; OutputInterface robot_shoot_timestamp_; + + OutputInterface map_command_target_position_x_; + OutputInterface map_command_target_position_y_; + OutputInterface map_command_keyboard_; + OutputInterface map_command_target_robot_id_; + OutputInterface map_command_source_; + OutputInterface map_command_received_timestamp_; + OutputInterface map_command_event_target_position_x_; + OutputInterface map_command_event_target_position_y_; + OutputInterface map_command_event_keyboard_; + OutputInterface map_command_event_target_robot_id_; + OutputInterface map_command_event_source_; + OutputInterface map_command_event_timestamp_; + OutputInterface map_command_event_sequence_; + MapCommand last_map_command_{}; + bool has_last_map_command_ = false; }; } // namespace rmcs_core::referee #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::Status, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::Status, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index 5cc1129f7..de3911afc 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -6,28 +6,29 @@ namespace rmcs_core::referee::status { struct __attribute__((packed)) GameStatus { uint8_t game_type : 4; - uint8_t game_stage : 4; + uint8_t game_progress : 4; uint16_t stage_remain_time; uint64_t sync_timestamp; }; struct __attribute__((packed)) GameRobotHp { - uint16_t red_1; - uint16_t red_2; - uint16_t red_3; - uint16_t red_4; - uint16_t red_5; - uint16_t red_7; - uint16_t red_outpost; - uint16_t red_base; - uint16_t blue_1; - uint16_t blue_2; - uint16_t blue_3; - uint16_t blue_4; - uint16_t blue_5; - uint16_t blue_7; - uint16_t blue_outpost; - uint16_t blue_base; + uint16_t ally_1_robot_hp; + uint16_t ally_2_robot_hp; + uint16_t ally_3_robot_hp; + uint16_t ally_4_robot_hp; + uint16_t reserved; + uint16_t ally_7_robot_hp; + uint16_t ally_outpost_hp; + uint16_t ally_base_hp; +}; + +struct __attribute__((packed)) EventData { + uint32_t event_data; +}; + +struct __attribute__((packed)) DartInfo { + uint8_t dart_remaining_time; + uint16_t dart_info; }; struct __attribute__((packed)) RobotStatus { @@ -38,18 +39,15 @@ struct __attribute__((packed)) RobotStatus { uint16_t shooter_barrel_cooling_value; uint16_t shooter_barrel_heat_limit; uint16_t chassis_power_limit; - uint8_t power_management_gimbal_output : 1; - uint8_t power_management_chassis_output : 1; - uint8_t power_management_shooter_output : 1; + uint8_t power_management_status; }; struct __attribute__((packed)) PowerHeatData { - uint16_t chassis_voltage; - uint16_t chassis_current; - float chassis_power; + uint16_t reserved_1; + uint16_t reserved_2; + float reserved_3; uint16_t buffer_energy; - uint16_t shooter_17mm_1_barrel_heat; - uint16_t shooter_17mm_2_barrel_heat; + uint16_t shooter_17mm_barrel_heat; uint16_t shooter_42mm_barrel_heat; }; @@ -72,22 +70,19 @@ struct __attribute__((packed)) ShootData { }; struct __attribute__((packed)) BulletAllowance { - uint16_t bullet_allowance_17mm; - uint16_t bullet_allowance_42mm; + uint16_t projectile_allowance_17mm; + uint16_t projectile_allowance_42mm; uint16_t remaining_gold_coin; + uint16_t projectile_allowance_fortress; }; -struct __attribute__((packed)) GameRobotPosition { - float hero_x; - float hero_y; - float engineer_x; - float engineer_y; - float infantry_3_x; - float infantry_3_y; - float infantry_4_x; - float infantry_4_y; - float infantry_5_x; - float infantry_5_y; +struct __attribute__((packed)) MapCommand { + float target_position_x; + float target_position_y; + uint8_t cmd_keyboard; + uint8_t target_robot_id; + uint16_t cmd_source; }; +static_assert(sizeof(MapCommand) == 12); -} // namespace rmcs_core::referee::status \ No newline at end of file +} // namespace rmcs_core::referee::status