diff --git a/auth.json b/auth.json new file mode 100644 index 00000000..e69de29b diff --git a/docs/zh-cn/steering_infantry_can_id_audit.md b/docs/zh-cn/steering_infantry_can_id_audit.md new file mode 100644 index 00000000..2bed724f --- /dev/null +++ b/docs/zh-cn/steering_infantry_can_id_audit.md @@ -0,0 +1,81 @@ +# steering-infantry 电机 CAN ID 对照审计 + +## 范围 +- 文件: `rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp` +- 目标: 收集所有电机的发送报文 ID 与回调报文 ID，反推出电机 ID，并检查是否有偏差。 + +## 反推规则 +- `DjiMotor` 采用 4 路合帧发送（`device::CanPacket8`）。 +- 对于 `can_id = 0x200` 的发送帧: + - `q0 -> 0x201` + - `q1 -> 0x202` + - `q2 -> 0x203` + - `q3 -> 0x204` +- 对于 `can_id = 0x1FF / 0x1FE` 的发送帧（本项目代码中的使用方式）: + - `q0 -> 0x205` + - `q1 -> 0x206` + - `q2 -> 0x207` + - `q3 -> 0x208` +- `LkMotor` 直接使用单独 `can_id` 发送与接收（非 4 路合帧电机映射）。 + +## TopBoard 电机映射 +代码位置: +- 构造/电机定义: `121-137` +- 发送: `212-253` +- 回调: `256-279` + +| 电机 | 发送总线/ID/槽位 | 回调 ID | 反推电机 ID | 结果 | +|---|---|---|---|---| +| `/gimbal/left_friction` | CAN1, `0x200`, `q2` | `0x203` | `3` | 一致 | +| `/gimbal/right_friction` | CAN1, `0x200`, `q3` | `0x204` | `4` | 一致 | +| `/gimbal/bullet_feeder` | CAN1, `0x1FF`, `q0` | `0x205` | `5` | 一致 | +| `/gimbal/yaw` (LK) | CAN2, `0x141` | `0x141` | `0x141` | 一致 | +| `/gimbal/pitch` (LK) | CAN2, `0x142` | `0x142` | `0x142` | 一致 | + +## BottomBoard_one 电机映射 +代码位置: +- 构造/电机定义: `308-328` +- 发送: `396-434` +- 回调: `437-466` + +| 电机 | 发送总线/ID/槽位 | 回调 ID | 反推电机 ID | 结果 | +|---|---|---|---|---| +| `/chassis/right_front_wheel` (`chassis_wheel_motors_[1]`) | CAN1, `0x200`, `q0` | `0x201` | `1` | 一致 | +| `/chassis/left_front_wheel` (`chassis_wheel_motors_[0]`) | CAN1, `0x200`, `q1` | `0x202` | `2` | 一致 | +| `/chassis/left_front_steering` (`chassis_steering_motors_[0]`) | CAN1, `0x1FE`, `q1` | `0x206` | `6` | 一致 | +| `/chassis/right_front_steering` (`chassis_steering_motors_[1]`) | CAN1, `0x1FE`, `q3` | `0x208` | `8` | 一致 | +| `/chassis/climber/left_back_motor` (`chassis_back_climber_motor_[0]`) | CAN2, `0x200`, `q0` | `0x201` | `1` | 一致 | +| `/chassis/climber/right_back_motor` (`chassis_back_climber_motor_[1]`) | CAN2, `0x200`, `q1` | `0x202` | `2` | 一致 | +| `/chassis/climber/left_front_motor` (`chassis_front_climber_motor_[0]`) | CAN2, `0x200`, `q2` | `0x203` | `3` | 一致 | +| `/chassis/climber/right_front_motor` (`chassis_front_climber_motor_[1]`) | CAN2, `0x200`, `q3` | `0x204` | `4` | 一致 | + +## BottomBoard_two 电机映射 +代码位置: +- 构造/电机定义: `491-507` +- 发送: `563-591` +- 回调: `594-610` + +| 电机 | 发送总线/ID/槽位 | 回调 ID | 反推电机 ID | 结果 | +|---|---|---|---|---| +| `/chassis/right_back_wheel` (`chassis_wheel_motors2_[1]`) | CAN1, `0x200`, `q2` | `0x203` | `3` | 一致 | +| `/chassis/left_back_wheel` (`chassis_wheel_motors2_[0]`) | CAN1, `0x200`, `q3` | `0x204` | `4` | 一致 | +| `/chassis/left_back_steering` (`chassis_steering_motors2_[0]`) | CAN1, `0x1FE`, `q0` | `0x205` | `5` | 一致 | +| `/chassis/right_back_steering` (`chassis_steering_motors2_[1]`) | CAN1, `0x1FE`, `q2` | `0x207` | `7` | 一致 | + +## 总结 +- 结论 1: 所有电机都能在代码中找到“发送 -> 回调”的闭环映射，未发现直接冲突或漏接收。 +- 结论 2: 从代码反推，当前电机 ID 规划为: + - TopBoard: 摩擦轮 `3/4`，拨弹 `5`，LK 云台 `0x141/0x142` + - BottomBoard_one: 轮 `1/2`，前转向 `6/8`，攀爬 `1/2/3/4`（CAN2） + - BottomBoard_two: 后轮 `3/4`，后转向 `5/7` + +## 偏差/问题反馈 +- 问题 1（注释已提示）: 文件头标注“电机ID没更改”，说明该映射可能为占位或沿用旧车配置，不能等同于当前实车真值。 +- 问题 2（可维护性）: 多处左右轮顺序与 ID 顺序是“交叉映射”（例如右前轮是 ID1、左前轮是 ID2），逻辑上可行，但后续维护容易误改。 +- 问题 3（非 ID 但会影响状态）: `TopBoard::update()` 中调用了 `gimbal_pitch_motor_.update_status()`，但未调用 `gimbal_yaw_motor_.update_status()`，可能导致 yaw 角度状态不更新。 + +## 建议 +- 用实车实测确认最终 ID: + 1. 在各 `can*_receive_callback` 临时打印所有 `can_id`。 + 2. 单电机动作，建立“物理电机 <-> can_id”表。 + 3. 以实测表覆盖本文件映射，再同步更新发送槽位与回调分发。 diff --git a/librmcs-firmware-c_board-3.0.1-0.dev.4.gbaf538b.dfu b/librmcs-firmware-c_board-3.0.1-0.dev.4.gbaf538b.dfu new file mode 100644 index 00000000..c3e952e1 Binary files /dev/null and b/librmcs-firmware-c_board-3.0.1-0.dev.4.gbaf538b.dfu differ diff --git a/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml new file mode 100644 index 00000000..8f464574 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/climbable-infantry.yaml @@ -0,0 +1,266 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::ClimbableInfantry -> Infantry_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::UIClimbableInfantry -> referee_ui_infantry + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + + # - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller + # - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_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::shooting::ShootingRecorder -> shooting_recorder + - 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::SteeringWheelStatus -> steering_wheel_status + - rmcs_core::controller::chassis::ClimbableInfantryChassisController -> chassis_controller + - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller + - rmcs_core::controller::chassis::ClimbableInfantryChassisClimberController -> climber_controller + + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer + # - rmcs_auto_aim::AutoAimController -> auto_aim_controller + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + +Infantry_hardware: + ros__parameters: + board_serial_top_board: "D4-632C-37BC-6A17-ACA2-E3FE-91E9" + board_serial_bottom_board: "AF-EEF5-24BA-6675-F1B1-C797-50AF-869C-870E" + yaw_motor_zero_point: 7570 + pitch_motor_zero_point: 24243 + # top_yaw_motor_zero_point: 58744 + # viewer_motor_zero_point: 3030 + # external_imu_port: /dev/ttyUSB0 + # bullet_feeder_motor_zero_point: 15111 + left_front_zero_point: 5806 + right_front_zero_point: 5092 + left_back_zero_point: 3759 + right_back_zero_point: 3103 + + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/yaw/velocity + - /gimbal/yaw/control_velocity + # - /gimbal/yaw/control_torque + # - /gimbal/left_friction/velocity + # - /gimbal/right_friction/velocity + # - /chassis/pitch_imu + # - /gimbal/pitch/velocity_imu + # - /gimbal/pitch/angle + # - /gimbal/yaw/angle + # - /chassis/climber/right_back_motor/control_torque + # - /chassis/climber/left_back_motor/control_torque + # - /gimbal/putter/velocity + # - /gimbal/first_left_friction/velocity + # - /gimbal/first_second_friction/velocity + # - /gimbal/second_left_friction/velocity + # - /gimbal/second_right_friction/velocity + # - /gimbal/first_left_friction/control_torque + # - /gimbal/first_second_friction/control_torque + # - /gimbal/second_left_friction/control_torque + # - /gimbal/second_right_friction/control_torque + # - /chassis/left_front_steering/angle + # - /chassis/left_back_steering/angle + # - /chassis/right_back_steering/angle + # - /chassis/right_front_steering/angle + # - /chassis/left_front_steering/control_torque + # - /chassis/left_back_steering/control_torque + # - /chassis/right_back_steering/control_torque + # - /chassis/right_front_steering/control_torque + # - /gimbal/pitch/control_torque + # - /gimbal/pitch/velocity + # - /gimbal/pitch/control_velocity + # - /gimbal/pitch/control_angle_error + # - /gimbal/bullet_feeder/phase_zero + # - /gimbal/bullet_feeder/alignment_error + # - /chassis/climber/left_back_motor/torque + # - /chassis/climber/right_back_motor/torque + # - /chassis/climber/left_back_motor/velocity + # - /chassis/climber/right_back_motor/velocity + # - /chassis/climber/right_back_motor/control_torque + # - /chassis/climber/left_back_motor/control_torque + +climber_controller: + ros__parameters: + front_climber_velocity: 60.0 + back_climber_velocity: 20.0 #13.0 + auto_climb_dash_velocity: 4.0 #3.0 + auto_climb_support_retract_velocity: 40.0 #47.0 + sync_coefficient: 0.8 + first_stair_approach_pitch: 0.590 + second_stair_approach_pitch: 0.320 + front_kp: 1.0 + front_ki: 0.0 + front_kd: 0.5 + back_kp: 1.0 + back_ki: 0.0 + back_kd: 0.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.54 + lower_limit: 0.274 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 15.0 #5.0 + ki: 0.0 + kd: 0.0 #1.5 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + kp: 40.0 # + ki: 0.00 + kd: 0.8 # + +yaw_angle_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 20.0 #20.00 + ki: 0.0 + kd: 0.9 #0.9 + +yaw_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 2.5 #2.5 + ki: 0.00 + kd: 0.6 #0.6 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 590.0 + - 590.0 + friction_soft_start_stop_time: 0.3 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 5000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 9.0 + shot_frequency: 60.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 + +shooting_recorder: + ros__parameters: + friction_wheel_count: 2 + log_mode: 1 # 1: trigger, 2: timing + +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 #0.003436926 #0.0016 + ki: 0.00 + kd: 0.009373434 #0.009373434 #0.001 + +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 #0.0016 + ki: 0.00 + kd: 0.009373434 #0.009373434 #0.001 + +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: 0.7 + ki: 0.0 + kd: 0.0 + +steering_wheel_status: + ros__parameters: + vehicle_radius: 0.28284271247462 + wheel_radius: 0.055 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.28284271247462 + wheel_radius: 0.055 + friction_coefficient: 0.6 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 11.37 + +auto_aim_controller: + ros__parameters: + # capture + use_video: false # If true, use video stream instead of camera. + video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" + exposure_time: 1 + invert_image: false + # identifier + armor_model_path: "/models/mlp.onnx" + # pnp + fx: 1.722231837421459e+03 + fy: 1.724876404292754e+03 + cx: 7.013056440882832e+02 + cy: 5.645821718351237e+02 + k1: -0.064232403853946 + k2: -0.087667493884102 + k3: 0.792381808294582 + # tracker + armor_predict_duration: 500 + # controller + gimbal_predict_duration: 100 + yaw_error: 0. + pitch_error: 0. + shoot_velocity: 28.0 + predict_sec: 0.095 + # etc + buff_predict_duration: 200 + buff_model_path: "/models/buff_nocolor_v6.onnx" + omni_exposure: 1000.0 + record_fps: 120 + debug: false # Setup in actual using.Debug mode is used when referee is not ready + debug_color: 0 # 0 For blue while 1 for red. mine + debug_robot_id: 4 + debug_buff_mode: false + record: false + raw_img_pub: false # Set false in actual use + image_viewer_type: 2 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml new file mode 100644 index 00000000..d58c5745 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -0,0 +1,285 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::SteeringHero -> hero_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::Interaction -> referee_interaction + # - rmcs_core::referee::command::interaction::Ui -> referee_ui + # - rmcs_core::referee::app::ui::Hero -> referee_ui_hero + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller + - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + + # - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller + # - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller + + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller + - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder + + - rmcs_core::controller::chassis::ChassisController -> chassis_controller + - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller + - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller + + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer + # - rmcs_auto_aim::AutoAimController -> auto_aim_controller + + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + +hero_hardware: + ros__parameters: + board_serial_top_board: "D4-2BCA-2E47-76CD-23BC-0B78-684B" + board_serial_bottom_board_one: "D4-7973-19A9-EA40-4A3E-306F-10F9" + board_serial_bottom_board_two: "D4-3674-7174-8768-879E-E44A-3931" + bottom_yaw_motor_zero_point: 52508 + pitch_motor_zero_point: 40651 #35701 + top_yaw_motor_zero_point: 58744 + viewer_motor_zero_point: 3030 + external_imu_port: /dev/ttyUSB0 + bullet_feeder_motor_zero_point: 15111 + left_front_zero_point: 4516 + right_front_zero_point: 7190 + left_back_zero_point: 3841 + right_back_zero_point: 5159 + # left_front_zero_point: 3473 + # left_back_zero_point: 6124 + # right_back_zero_point: 6157 + # right_front_zero_point: 2723 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/putter/velocity + - /gimbal/first_left_friction/velocity + - /gimbal/first_second_friction/velocity + - /gimbal/second_left_friction/velocity + - /gimbal/second_right_friction/velocity + - /gimbal/first_left_friction/control_torque + - /gimbal/first_second_friction/control_torque + - /gimbal/second_left_friction/control_torque + - /gimbal/second_right_friction/control_torque + # - /chassis/left_front_steering/angle + # - /chassis/left_back_steering/angle + # - /chassis/right_back_steering/angle + # - /chassis/right_front_steering/angle + # - /chassis/left_front_steering/control_torque + # - /chassis/left_back_steering/control_torque + # - /chassis/right_back_steering/control_torque + # - /chassis/right_front_steering/control_torque + # - /gimbal/pitch/control_torque + # - /gimbal/pitch/velocity + # - /gimbal/pitch/control_velocity + # - /gimbal/pitch/control_angle_error + # - /gimbal/bullet_feeder/phase_zero + # - /gimbal/bullet_feeder/alignment_error + # - /chassis/climber/left_back_motor/torque + # - /chassis/climber/right_back_motor/torque + # - /chassis/climber/left_back_motor/velocity + # - /chassis/climber/right_back_motor/velocity + # - /chassis/climber/right_back_motor/control_torque + # - /chassis/climber/left_back_motor/control_torque + +climber_controller: + ros__parameters: + front_climber_velocity: 60.0 + back_climber_velocity: 20.0 + auto_climb_dash_velocity: 3.0 + auto_climb_support_retract_velocity: 30.0 + sync_coefficient: 0.3 + climb_timeout_limit: 1000 + first_stair_approach_pitch: 0.485 + second_stair_approach_pitch: 0.35 + back_climber_burst_velocity: 15.0 + back_climber_burst_duration: 300 + front_kp: 1.0 + front_ki: 0.0 + front_kd: 0.5 + back_kp: 1.0 + back_ki: 0.0 + back_kd: 0.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.6266 + lower_limit: 0.4629 + +dual_yaw_controller: + ros__parameters: + top_yaw_angle_kp: 10.0 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 50.0 + top_yaw_velocity_ki: 0.004 + top_yaw_velocity_kd: 0.0 + bottom_yaw_angle_kp: 5.0 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 10.0 + bottom_yaw_velocity_ki: 0.0 + bottom_yaw_velocity_kd: 50.0 + +# dual_yaw_controller: +# ros__parameters: +# top_yaw_angle_kp: 24.5 +# top_yaw_angle_ki: 0.0 +# top_yaw_angle_kd: 0.0 +# top_yaw_velocity_kp: 77.4 +# top_yaw_velocity_ki: 0.004 +# top_yaw_velocity_kd: 1.0 +# bottom_yaw_angle_kp: 8.6 +# bottom_yaw_angle_ki: 0.0 +# bottom_yaw_angle_kd: 0.0 +# bottom_yaw_velocity_kp: 25.85 +# bottom_yaw_velocity_ki: 0.0 +# bottom_yaw_velocity_kd: 50.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 30.0 #28.00 + ki: 0.0 + kd: 0.6 #0.6 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + kp: 15.0 #45.00 + ki: 0.00 + kd: 2.0 #1.00 + +gimbal_player_viewer_controller: + ros__parameters: + upper_limit: 0.0101 + lower_limit: 0.6191 + +viewer_angle_pid_controller: + ros__parameters: + measurement: /gimbal/player_viewer/control_angle_error + control: /gimbal/player_viewer/control_velocity + kp: 17.00 + ki: 0.00 + kd: 2.00 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/first_left_friction + - /gimbal/first_right_friction + - /gimbal/second_left_friction + - /gimbal/second_right_friction + friction_velocities: + - 375.00 + - 375.00 + - 520.00 + - 520.00 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 100000 + reserved_heat: 0 + +shooting_recorder: + ros__parameters: + friction_wheel_count: 4 + aim_velocity: 11.8 + log_mode: 1 # 1: trigger, 2: timing + +first_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_left_friction/velocity + setpoint: /gimbal/first_left_friction/control_velocity + control: /gimbal/first_left_friction/control_torque + kp: 0.0016 + ki: 0.00 + kd: 0.001 + +first_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_right_friction/velocity + setpoint: /gimbal/first_right_friction/control_velocity + control: /gimbal/first_right_friction/control_torque + kp: 0.0016 + ki: 0.00 + kd: 0.001 + +second_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_left_friction/velocity + setpoint: /gimbal/second_left_friction/control_velocity + control: /gimbal/second_left_friction/control_torque + kp: 0.001 + ki: 0.00 + kd: 0.001 + +second_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_right_friction/velocity + setpoint: /gimbal/second_right_friction/control_velocity + control: /gimbal/second_right_friction/control_torque + kp: 0.001 + ki: 0.00 + kd: 0.00 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.28284271247462 + wheel_radius: 0.055 + friction_coefficient: 0.6 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 11.37 + +auto_aim_controller: + ros__parameters: + # capture + use_video: false # If true, use video stream instead of camera. + video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" + exposure_time: 1 + invert_image: false + # identifier + armor_model_path: "/models/mlp.onnx" + # pnp + fx: 1.722231837421459e+03 + fy: 1.724876404292754e+03 + cx: 7.013056440882832e+02 + cy: 5.645821718351237e+02 + k1: -0.064232403853946 + k2: -0.087667493884102 + k3: 0.792381808294582 + # tracker + armor_predict_duration: 500 + # controller + gimbal_predict_duration: 100 + yaw_error: 0. + pitch_error: 0. + shoot_velocity: 28.0 + predict_sec: 0.095 + # etc + buff_predict_duration: 200 + buff_model_path: "/models/buff_nocolor_v6.onnx" + omni_exposure: 1000.0 + record_fps: 120 + debug: false # Setup in actual using.Debug mode is used when referee is not ready + debug_color: 0 # 0 For blue while 1 for red. mine + debug_robot_id: 4 + debug_buff_mode: false + record: false + raw_img_pub: false # Set false in actual use + image_viewer_type: 2 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index dd3c8579..36e7e570 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -17,8 +17,8 @@ include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.0.0/librmcs-sdk-src-3.0.0.zip - URL_HASH SHA256=b39f51c21baacdcbf3f0176119b8850137a108b88a67e12395d37d89e5ef53e8 + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0/librmcs-sdk-src-3.1.0.zip + URL_HASH SHA256=07107e251745ddb23f7b3e39edec5d6910be1a197025d167ec9849c5c80dd954 DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs new file mode 160000 index 00000000..eff14b71 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/librmcs @@ -0,0 +1 @@ +Subproject commit eff14b71e334baaaa462a83c7e6d834886b10168 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index f7847151..0bd9315c 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,29 +1,146 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Steering wheel controller. + + + Steering wheel controller. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Gimbal player viewer + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + the recorder of Hero + + + Test plugin. + + + Recorder for friction wheel PID tuning. + + + Test plugin. + + + Feedforward pid controller. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index da70a96f..f183e349 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -19,7 +19,7 @@ class ChassisController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , following_velocity_controller_(7.0, 0.0, 0.0) { + , following_velocity_controller_(9.0 , 0.0, 1000.0) { following_velocity_controller_.output_max = angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; @@ -34,6 +34,7 @@ class ChassisController register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, nan); register_output("/chassis/angle", chassis_angle_, nan); register_output("/chassis/control_angle", chassis_control_angle_, nan); @@ -58,8 +59,8 @@ class ChassisController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -74,14 +75,14 @@ class ChassisController if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.c && keyboard.c) { if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::AUTO; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.x && keyboard.x) { @@ -100,8 +101,8 @@ class ChassisController } while (false); last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; } void reset_all_controls() { @@ -112,12 +113,16 @@ class ChassisController void update_velocity_control() { auto translational_velocity = update_translational_velocity_control(); - auto angular_velocity = update_angular_velocity_control(); + auto angular_velocity = update_angular_velocity_control(); chassis_control_velocity_->vector << translational_velocity, angular_velocity; } Eigen::Vector2d update_translational_velocity_control() { + if (!std::isnan(*climbing_forward_velocity_)) { + return Eigen::Vector2d{*climbing_forward_velocity_, 0.0}; + } + auto keyboard = *keyboard_; Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; @@ -133,7 +138,13 @@ class ChassisController } double update_angular_velocity_control() { - double angular_velocity = 0.0; + if (!std::isnan(*climbing_forward_velocity_)) { + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = nan; + return 0.0; + } + + double angular_velocity = 0.0; double chassis_control_angle = nan; switch (*mode_) { @@ -171,7 +182,7 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; } - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; return angular_velocity; @@ -202,7 +213,7 @@ class ChassisController // Maximum control velocities static constexpr double translational_velocity_max = 10.0; - static constexpr double angular_velocity_max = 16.0; + static constexpr double angular_velocity_max = 16.0; InputInterface joystick_right_; InputInterface joystick_left_; @@ -214,10 +225,11 @@ class ChassisController InputInterface rotary_knob_; 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(); + 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_; + InputInterface climbing_forward_velocity_; OutputInterface chassis_angle_, chassis_control_angle_; OutputInterface mode_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 2d084f8b..cf4cb1a8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -33,6 +33,9 @@ class ChassisPowerController register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); + register_input( + "/chassis/climber/left_front_motor/velocity", + chassis_climber_left_front_motor_velocity_); register_output("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_, 0.0); register_output("/chassis/control_power_limit", chassis_control_power_limit_, 0.0); @@ -51,9 +54,9 @@ class ChassisPowerController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; - auto rotary_knob = *rotary_knob_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob = *rotary_knob_; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { @@ -63,7 +66,7 @@ class ChassisPowerController update_virtual_buffer_energy(); - boost_mode_ = keyboard.shift || rotary_knob < -0.9; + boost_mode_ = keyboard.shift || rotary_knob < -0.9 || rotary_knob > 0.9 || keyboard.g; update_control_power_limit(); } @@ -74,8 +77,8 @@ class ChassisPowerController // charging_power_limit = constexpr double buffer_energy_control_line = 120; // = referee + excess - constexpr double buffer_energy_base_line = 30; // = referee - constexpr double buffer_energy_dead_line = 0; // = 0 + constexpr double buffer_energy_base_line = 30; // = referee + constexpr double buffer_energy_dead_line = 0; // = 0 *supercap_charge_power_limit_ = *chassis_power_limit_referee_ @@ -91,8 +94,8 @@ class ChassisPowerController } void reset_power_control() { - virtual_buffer_energy_ = virtual_buffer_energy_limit_; - boost_mode_ = false; + virtual_buffer_energy_ = virtual_buffer_energy_limit_; + boost_mode_ = false; *chassis_control_power_limit_ = 0.0; } @@ -106,9 +109,10 @@ class ChassisPowerController void update_control_power_limit() { double power_limit; - + double chassis_climber_left_front_motor = *chassis_climber_left_front_motor_velocity_; if (boost_mode_ && *supercap_enabled_) - power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP + power_limit = (*mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP) + || (chassis_climber_left_front_motor > 0.5) ? inf_ : *chassis_power_limit_referee_ + 80.0; else @@ -117,8 +121,8 @@ class ChassisPowerController // chassis_control_power_limit = constexpr double supercap_voltage_control_line = 12.5; // = supercap - constexpr double supercap_voltage_base_line = 12.0; // = referee - power_limit = *chassis_power_limit_referee_ + constexpr double supercap_voltage_base_line = 12.0; // = referee + power_limit = *chassis_power_limit_referee_ + (power_limit - *chassis_power_limit_referee_) * std::clamp( (*supercap_voltage_ - supercap_voltage_base_line) @@ -126,7 +130,7 @@ class ChassisPowerController 0.0, 1.0); // Maximum excess power when virtual buffer energy is full. - constexpr double excess_power_limit = 15; + constexpr double excess_power_limit = 0; // 15 power_limit += excess_power_limit; power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; @@ -160,6 +164,8 @@ class ChassisPowerController InputInterface chassis_power_limit_referee_; InputInterface chassis_buffer_energy_referee_; + InputInterface chassis_climber_left_front_motor_velocity_; + bool boost_mode_ = false; OutputInterface supercap_charge_power_limit_; double chassis_power_limit_expected_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp new file mode 100644 index 00000000..652ac130 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_climber_controller.cpp @@ -0,0 +1,571 @@ +#include "controller/pid/matrix_pid_calculator.hpp" +#include "rmcs_msgs/keyboard.hpp" +#include "rmcs_msgs/switch.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +enum class AutoClimbState { + IDLE, + ALIGN, + APPROACH, + SUPPORT_DEPLOY, + DASH, + SUPPORT_RETRACT, + FORERAKED +}; + +class ClimbableInfantryChassisClimberController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ClimbableInfantryChassisClimberController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) + , front_velocity_pid_calculator_( + get_parameter("front_kp").as_double(), get_parameter("front_ki").as_double(), + get_parameter("front_kd").as_double()) + , back_velocity_pid_calculator_( + get_parameter("back_kp").as_double(), get_parameter("back_ki").as_double(), + get_parameter("back_kd").as_double()) { + + track_velocity_max_ = get_parameter("front_climber_velocity").as_double(); + climber_back_control_velocity_abs_ = get_parameter("back_climber_velocity").as_double(); + auto_climb_dash_velocity_ = get_parameter("auto_climb_dash_velocity").as_double(); + auto_climb_support_retract_velocity_abs_ = + get_parameter("auto_climb_support_retract_velocity").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + first_stair_approach_pitch_ = get_parameter("first_stair_approach_pitch").as_double(); + second_stair_approach_pitch_ = get_parameter("second_stair_approach_pitch").as_double(); + + register_output( + "/chassis/climber/left_front_motor/control_torque", climber_front_left_control_torque_, + nan_); + register_output( + "/chassis/climber/right_front_motor/control_torque", + climber_front_right_control_torque_, nan_); + register_output( + "/chassis/climber/left_back_motor/control_torque", climber_back_left_control_torque_, + nan_); + register_output( + "/chassis/climber/right_back_motor/control_torque", climber_back_right_control_torque_, + nan_); + register_output("/chassis/climbing_forward_velocity", climbing_forward_velocity_, nan_); + + register_input("/chassis/climber/left_front_motor/velocity", climber_front_left_velocity_); + register_input( + "/chassis/climber/right_front_motor/velocity", climber_front_right_velocity_); + register_input("/chassis/climber/left_back_motor/velocity", climber_back_left_velocity_); + register_input("/chassis/climber/right_back_motor/velocity", climber_back_right_velocity_); + + register_input("/chassis/climber/left_back_motor/torque", climber_back_left_torque_); + register_input("/chassis/climber/right_back_motor/torque", climber_back_right_torque_); + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob_switch", rotary_knob_switch_); + register_input("/chassis/pitch_imu", chassis_pitch_imu_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_); + register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + } + + void update() override { + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob_switch = *rotary_knob_switch_; + + // RCLCPP_INFO(logger_, "%lf", *chassis_pitch_imu_); + + bool rotary_knob_to_down = + (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN); + bool rotary_knob_from_down = + (last_rotary_knob_switch_ == Switch::DOWN && rotary_knob_switch != Switch::DOWN); + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + } else { + handle_auto_climb_requests( + (!last_keyboard_.g && keyboard.g) || rotary_knob_to_down, rotary_knob_from_down, + rotary_knob_switch); + + if (auto_climb_state_ != AutoClimbState::IDLE) { + stop_manual_support(); + apply_auto_climb_control(update_auto_climb_control()); + } else { + apply_auto_climb_control(update_manual_support_control(keyboard)); + apply_manual_support_zero_output(); + } + } + + last_switch_left_ = switch_left; + last_switch_right_ = switch_right; + last_keyboard_ = keyboard; + last_rotary_knob_switch_ = rotary_knob_switch; + } + +private: + struct AutoClimbControl { + double front_track_velocity = nan_; + double back_climber_velocity = nan_; + double override_chassis_vx = nan_; + }; + + void handle_auto_climb_requests( + bool start_requested, bool abort_by_rotary, rmcs_msgs::Switch rotary_knob_switch) { + + if (start_requested) { + if (auto_climb_state_ == AutoClimbState::IDLE) { + start_auto_climb( + rotary_knob_switch == rmcs_msgs::Switch::UP ? "Rotary Knob" : "Keyboard G"); + } else { + abort_auto_climb("toggled again"); + } + } else if (abort_by_rotary && auto_climb_state_ != AutoClimbState::IDLE) { + abort_auto_climb("rotary knob left UP"); + } + } + + void start_auto_climb(const char* source) { + stop_manual_support(); + auto_climb_continue_ = true; + auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + enter_auto_climb_state(AutoClimbState::ALIGN); + + RCLCPP_INFO(logger_, "Auto climb started by %s. Entering ALIGN.", source); + } + + void abort_auto_climb(const char* reason) { + reset_all_controls(); + RCLCPP_INFO(logger_, "Auto climb aborted (%s).", reason); + } + + AutoClimbControl update_auto_climb_control() { + + if (auto_climb_state_ == AutoClimbState::IDLE) { + detect_is_foreraked(); + if (auto_climb_state_ == AutoClimbState::IDLE) { + return {}; + } + } + + auto_climb_timer_++; + + switch (auto_climb_state_) { + case AutoClimbState::IDLE: return {}; + case AutoClimbState::ALIGN: return update_auto_climb_align(); + case AutoClimbState::APPROACH: return update_auto_climb_approach(); + case AutoClimbState::SUPPORT_DEPLOY: return update_auto_climb_support_deploy(); + case AutoClimbState::DASH: return update_auto_climb_dash(); + case AutoClimbState::SUPPORT_RETRACT: return update_auto_climb_support_retract(); + case AutoClimbState::FORERAKED: return update_forerake_defend(); + } + + return {}; + } + + AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) { + AutoClimbControl control; + + if (keyboard.b) { + stop_manual_support(); + control.back_climber_velocity = climber_back_control_velocity_abs_; + return control; + } + + if (last_keyboard_.b) { + manual_support_retracting_ = true; + manual_support_retract_block_count_ = 0; + manual_support_zero_output_ = false; + RCLCPP_INFO(logger_, "Manual support retract started."); + } + + if (!manual_support_retracting_) { + return control; + } + + control.back_climber_velocity = -auto_climb_support_retract_velocity_abs_; + + if (is_back_climber_blocked()) { + manual_support_retract_block_count_++; + } else { + manual_support_retract_block_count_ = 0; + } + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "MANUAL_SUPPORT_RETRACT: blocked_ticks=%d", + manual_support_retract_block_count_); + + if (manual_support_retract_block_count_ >= kManualSupportRetractConfirmTicks) { + stop_manual_support(); + manual_support_zero_output_ = true; + RCLCPP_INFO(logger_, "Manual support retract completed."); + return {}; + } + + return control; + } + + AutoClimbControl update_auto_climb_align() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = 0.0, + .override_chassis_vx = 0.0, + }; + + double gimbal_yaw_angle_error = *gimbal_yaw_angle_error_; + if (gimbal_yaw_angle_error < 0) + gimbal_yaw_angle_error += 2 * std::numbers::pi; + + double err = gimbal_yaw_angle_error + *gimbal_yaw_angle_; + while (err >= std::numbers::pi) + err -= 2 * std::numbers::pi; + while (err < -std::numbers::pi) + err += 2 * std::numbers::pi; + + double yaw_velocity = *gimbal_yaw_velocity_imu_; + bool is_aligned = std::abs(err) < kAutoClimbAlignThreshold; + bool is_stable = std::abs(yaw_velocity) < kAutoClimbAlignVelocityThreshold; + + if (is_aligned && is_stable) { + auto_climb_align_stable_count_++; + } else { + auto_climb_align_stable_count_ = 0; + } + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "ALIGN: err=%.3f, yaw_velocity=%.3f, stable_ticks=%d", err, + yaw_velocity, auto_climb_align_stable_count_); + + if (auto_climb_align_stable_count_ >= kAutoClimbAlignConfirmTicks) { + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO(logger_, "Chassis aligned. Entering APPROACH."); + } + + return control; + } + + AutoClimbControl update_auto_climb_approach() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = 0.0, + .override_chassis_vx = kAutoClimbApproachVelocity, + }; + + double pitch = *chassis_pitch_imu_; + double target_pitch = auto_climb_stair_index_ == 0 ? first_stair_approach_pitch_ + : second_stair_approach_pitch_; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "APPROACH (step %d): pitch=%.3f, target>%.3f", + auto_climb_stair_index_ + 1, pitch, target_pitch); + + if (pitch > target_pitch) { + enter_auto_climb_state(AutoClimbState::SUPPORT_DEPLOY); + RCLCPP_INFO( + logger_, "Auto climb entering SUPPORT_DEPLOY (step %d).", + auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_support_deploy() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = 0.5, + }; + + if (is_back_climber_blocked()) { + auto_climb_support_block_count_++; + } else { + auto_climb_support_block_count_ = 0; + } + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_DEPLOY (step %d): blocked_ticks=%d", + auto_climb_stair_index_ + 1, auto_climb_support_block_count_); + + if (auto_climb_support_block_count_ >= kAutoClimbSupportConfirmTicks) { + enter_auto_climb_state(AutoClimbState::DASH); + RCLCPP_INFO( + logger_, "Auto climb entering DASH (step %d).", auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_dash() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_dash_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + bool is_leveled = std::abs(pitch) < kAutoClimbLeveledPitchThreshold + && auto_climb_timer_ > kAutoClimbDashMinTicks; + bool timeout = auto_climb_timer_ > kAutoClimbDashTimeoutTicks; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "DASH (step %d): pitch=%.3f, timer=%d", + auto_climb_stair_index_ + 1, pitch, auto_climb_timer_); + + if (is_leveled || timeout) { + enter_auto_climb_state(AutoClimbState::SUPPORT_RETRACT); + + if (timeout) { + RCLCPP_WARN( + logger_, "Auto climb DASH timeout on step %d. Entering SUPPORT_RETRACT.", + auto_climb_stair_index_ + 1); + } else { + RCLCPP_INFO( + logger_, "Auto climb reached platform on step %d.", + auto_climb_stair_index_ + 1); + } + } + + return control; + } + + AutoClimbControl update_auto_climb_support_retract() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = -auto_climb_support_retract_velocity_abs_, + .override_chassis_vx = auto_climb_dash_velocity_, + }; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_RETRACT (step %d): timer=%d", + auto_climb_stair_index_ + 1, auto_climb_timer_); + + if (auto_climb_timer_ > kAutoClimbSupportRetractTicks) { + bool has_next_stair = + auto_climb_continue_ && (auto_climb_stair_index_ + 1 < kAutoClimbMaxStairs); + + if (has_next_stair) { + auto_climb_stair_index_++; + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO( + logger_, "Auto climb continuing to step %d.", auto_climb_stair_index_ + 1); + } else { + int finished_steps = auto_climb_stair_index_ + 1; + stop_auto_climb(); + RCLCPP_INFO(logger_, "Auto climb completed (finished %d steps).", finished_steps); + } + } + + return control; + } + + AutoClimbControl update_forerake_defend() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = 0.0, + .override_chassis_vx = nan_, + }; + + double pitch = *chassis_pitch_imu_; + bool is_leveled = std::abs(pitch) < kAutoClimbLeveledPitchThreshold + && auto_climb_timer_ > kAutoClimbDashMinTicks; + bool timeout = auto_climb_timer_ > kAutoClimbForerakedTimeoutTicks; + + if (is_leveled || timeout) { + enter_auto_climb_state(AutoClimbState::IDLE); + } + + return control; + } + + void apply_auto_climb_control(const AutoClimbControl& control) { + *climbing_forward_velocity_ = control.override_chassis_vx; + + dual_motor_sync_control( + control.front_track_velocity, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_velocity_pid_calculator_, + *climber_front_left_control_torque_, *climber_front_right_control_torque_); + + dual_motor_sync_control( + control.back_climber_velocity, *climber_back_left_velocity_, + *climber_back_right_velocity_, back_velocity_pid_calculator_, + *climber_back_left_control_torque_, *climber_back_right_control_torque_); + } + + void reset_all_controls() { + *climber_front_left_control_torque_ = nan_; + *climber_front_right_control_torque_ = nan_; + *climber_back_left_control_torque_ = nan_; + *climber_back_right_control_torque_ = nan_; + *climbing_forward_velocity_ = nan_; + stop_manual_support(); + stop_auto_climb(); + } + + void apply_manual_support_zero_output() { + if (!manual_support_zero_output_) { + return; + } + + *climber_back_left_control_torque_ = 0.0; + *climber_back_right_control_torque_ = 0.0; + } + + void stop_manual_support() { + manual_support_retracting_ = false; + manual_support_retract_block_count_ = 0; + manual_support_zero_output_ = false; + } + + void stop_auto_climb() { + auto_climb_state_ = AutoClimbState::IDLE; + auto_climb_timer_ = 0; + auto_climb_stair_index_ = 0; + auto_climb_continue_ = false; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + void enter_auto_climb_state(AutoClimbState state) { + if (state == auto_climb_state_) { + return; + } + auto_climb_state_ = state; + auto_climb_timer_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + bool is_back_climber_blocked() const { + return (std::abs(*climber_back_left_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_left_velocity_) < kBackClimberBlockedVelocityThreshold) + || (std::abs(*climber_back_right_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_right_velocity_) < kBackClimberBlockedVelocityThreshold); + } + + void dual_motor_sync_control( + double setpoint, double left_velocity, double right_velocity, + pid::MatrixPidCalculator<2>& pid_calculator, double& left_torque_out, + double& right_torque_out) { + + if (std::isnan(setpoint)) { + left_torque_out = nan_; + right_torque_out = nan_; + return; + } + + Eigen::Vector2d setpoint_error{setpoint - left_velocity, setpoint - right_velocity}; + Eigen::Vector2d relative_velocity{ + left_velocity - right_velocity, right_velocity - left_velocity}; + + Eigen::Vector2d control_error = setpoint_error - sync_coefficient_ * relative_velocity; + auto control_torques = pid_calculator.update(control_error); + + left_torque_out = control_torques[0]; + right_torque_out = control_torques[1]; + } + + void detect_is_foreraked() { + if (auto_climb_state_ != AutoClimbState::IDLE) { + return; + } + + double pitch = *chassis_pitch_imu_; + bool is_foreraked = pitch < kForerakedetectPitchThreshold; + if (is_foreraked) { + RCLCPP_INFO(get_logger(), "Enter foreraked"); + enter_auto_climb_state(AutoClimbState::FORERAKED); + } + } + + rclcpp::Logger logger_; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double kAutoClimbApproachVelocity = 1.5; + static constexpr double kAutoClimbAlignThreshold = 0.10; + static constexpr double kAutoClimbAlignVelocityThreshold = 0.2; + static constexpr double kAutoClimbLeveledPitchThreshold = 0.1; + static constexpr double kForerakedetectPitchThreshold = -0.05; + static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; + static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; + static constexpr int kAutoClimbAlignConfirmTicks = 50; + static constexpr int kAutoClimbSupportConfirmTicks = 100; + static constexpr int kAutoClimbDashMinTicks = 500; + static constexpr int kAutoClimbDashTimeoutTicks = 3000; + static constexpr int kAutoClimbForerakedTimeoutTicks = 2000; + static constexpr int kAutoClimbSupportRetractTicks = 1500; + static constexpr int kAutoClimbMaxStairs = 2; + static constexpr int kManualSupportRetractConfirmTicks = 50; + + double sync_coefficient_; + double first_stair_approach_pitch_; + double second_stair_approach_pitch_; + + double track_velocity_max_; + double climber_back_control_velocity_abs_; + double auto_climb_dash_velocity_; + double auto_climb_support_retract_velocity_abs_; + + AutoClimbState auto_climb_state_ = AutoClimbState::IDLE; + int auto_climb_timer_ = 0; + int auto_climb_stair_index_ = 0; + int auto_climb_align_stable_count_ = 0; + int auto_climb_support_block_count_ = 0; + bool auto_climb_continue_ = false; + bool manual_support_retracting_ = false; + int manual_support_retract_block_count_ = 0; + bool manual_support_zero_output_ = false; + + OutputInterface climber_front_left_control_torque_; + OutputInterface climber_front_right_control_torque_; + OutputInterface climber_back_left_control_torque_; + OutputInterface climber_back_right_control_torque_; + OutputInterface climbing_forward_velocity_; + + InputInterface climber_front_left_velocity_; + InputInterface climber_front_right_velocity_; + InputInterface climber_back_left_velocity_; + InputInterface climber_back_right_velocity_; + + InputInterface climber_back_left_torque_; + InputInterface climber_back_right_torque_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_switch_; + + InputInterface chassis_pitch_imu_; + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_, gimbal_yaw_velocity_imu_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + pid::MatrixPidCalculator<2> front_velocity_pid_calculator_, back_velocity_pid_calculator_; +}; +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::ClimbableInfantryChassisClimberController, + rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_controller.cpp new file mode 100644 index 00000000..13d101ef --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climbable_infantry_chassis_controller.cpp @@ -0,0 +1,287 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::chassis { + +class ClimbableInfantryChassisController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ClimbableInfantryChassisController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , following_velocity_controller_(10.0, 0.0, 1.2) { + following_velocity_controller_.output_max = angular_velocity_max; + following_velocity_controller_.output_min = -angular_velocity_max; + + 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("/gimbal/yaw/angle", gimbal_yaw_angle_, false); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/chassis/velocity", chassis_velocity_, false); + register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, false); + + register_output("/chassis/angle", chassis_angle_, nan); + register_output("/chassis/control_angle", chassis_control_angle_, nan); + + register_output("/chassis/control_mode", mode_); + register_output("/chassis/control_velocity", chassis_control_velocity_); + } + + void before_updating() override { + if (!gimbal_yaw_angle_.ready()) { + gimbal_yaw_angle_.make_and_bind_directly(0.0); + RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + } + if (!gimbal_yaw_angle_error_.ready()) { + gimbal_yaw_angle_error_.make_and_bind_directly(0.0); + RCLCPP_WARN( + get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + } + chassis_velocity_feedback_ready_ = chassis_velocity_.ready(); + if (!chassis_velocity_feedback_ready_) { + chassis_velocity_.make_and_bind_directly(0.0, 0.0, 0.0); + } + } + + void update() override { + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto mode = *mode_; + + do { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + break; + } + if (switch_left != Switch::DOWN) { + if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { + if (mode == rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } else if (!last_keyboard_.c && keyboard.c) { + if (mode == rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } else if (!last_keyboard_.x && keyboard.x) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::FRONT; + + } else if (!last_keyboard_.z && keyboard.z) { + if (mode == rmcs_msgs::ChassisMode::STEP_DOWN) { + mode = rmcs_msgs::ChassisMode::AUTO; + } else { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::BACK; + } + } + *mode_ = mode; + } + + update_velocity_control(); + } while (false); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + + void reset_all_controls() { + *mode_ = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::BACK; + + *chassis_control_velocity_ = {nan, nan, nan}; + } + + void update_velocity_control() { + auto translational_velocity = update_translational_velocity_control(); + auto angular_velocity = update_angular_velocity_control(translational_velocity); + + chassis_control_velocity_->vector << translational_velocity, angular_velocity; + } + + Eigen::Vector2d update_translational_velocity_control() { + if (!std::isnan(*climbing_forward_velocity_)) { + return Eigen::Vector2d{*climbing_forward_velocity_, 0.0}; + } + + auto keyboard = *keyboard_; + Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; + + Eigen::Vector2d translational_velocity = + Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move); + + if (translational_velocity.norm() > 1.0) + translational_velocity.normalize(); + + translational_velocity *= translational_velocity_max; + + return translational_velocity; + } + + double update_angular_velocity_control(const Eigen::Vector2d& translational_velocity) { + double angular_velocity = 0.0; + double chassis_control_angle = nan; + + if (!std::isnan(*climbing_forward_velocity_)) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + angular_velocity = following_velocity_controller_.update(err); + + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + return angular_velocity; + } + + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: { + angular_velocity = + update_following_angular_velocity(StepDownFacing::BACK, chassis_control_angle); + + // Keep AUTO rear-following gentle at low translation speed and fully enabled at max. + const double measured_translational_speed = + chassis_velocity_feedback_ready_ ? chassis_velocity_->vector.head<2>().norm() + : translational_velocity.norm(); + angular_velocity *= + std::clamp(measured_translational_speed / translational_velocity_max, 0.0, 0.3); + } break; + case rmcs_msgs::ChassisMode::SPIN: { + angular_velocity = + 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); + } break; + case rmcs_msgs::ChassisMode::STEP_DOWN: { + angular_velocity = + update_following_angular_velocity(step_down_facing_, chassis_control_angle); + } break; + case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + // err: [0, 2pi) -> signed + // In launch ramp mode, only one direction can be used for alignment. + // TODO: Dynamically determine the split angle based on chassis velocity. + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + angular_velocity = following_velocity_controller_.update(err); + } break; + } + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + + return angular_velocity; + } + + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { + chassis_control_angle = normalize_positive_angle(*gimbal_yaw_angle_error_); + + // err = setpoint - measurement + // ^ ^ + // |gimbal_yaw_angle_error |chassis_angle + // ^ + // |(2pi - gimbal_yaw_angle) + double err = normalize_positive_angle(chassis_control_angle + *gimbal_yaw_angle_); + + return err; + } + +private: + enum class StepDownFacing { FRONT, BACK }; + + double update_following_angular_velocity( + StepDownFacing target_facing, double& chassis_control_angle) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + if (target_facing == StepDownFacing::BACK) { + chassis_control_angle = + normalize_positive_angle(chassis_control_angle + std::numbers::pi); + err = normalize_positive_angle(err + std::numbers::pi); + } + + err = normalize_signed_angle(err); + return following_velocity_controller_.update(err); + } + + static double normalize_positive_angle(double angle) { + constexpr double full_turn = 2 * std::numbers::pi; + while (angle >= full_turn) + angle -= full_turn; + while (angle < 0.0) + angle += full_turn; + return angle; + } + + static double normalize_signed_angle(double angle) { + angle = normalize_positive_angle(angle); + if (angle > std::numbers::pi) + angle -= 2 * std::numbers::pi; + return angle; + } + + static constexpr double nan = std::numeric_limits::quiet_NaN(); + // Maximum control velocities + static constexpr double translational_velocity_max = 10.0; + static constexpr double angular_velocity_max = 16.0; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface keyboard_; + InputInterface rotary_knob_; + + 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_; + InputInterface chassis_velocity_; + InputInterface climbing_forward_velocity_; + OutputInterface chassis_angle_, chassis_control_angle_; + + OutputInterface mode_; + bool spinning_forward_ = true; + bool chassis_velocity_feedback_ready_ = false; + StepDownFacing step_down_facing_ = StepDownFacing::BACK; + pid::PidCalculator following_velocity_controller_; + + OutputInterface chassis_control_velocity_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::ClimbableInfantryChassisController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp new file mode 100644 index 00000000..8450b386 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp @@ -0,0 +1,104 @@ +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +class SteeringWheelStatus + : public rmcs_executor::Component + , public rclcpp::Node { +public: + explicit SteeringWheelStatus() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , vehicle_radius_(get_parameter("vehicle_radius").as_double()) + , wheel_radius_(get_parameter("wheel_radius").as_double()) { + 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_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_output("/chassis/velocity", chassis_velocity_, 0.0, 0.0, 0.0); + } + + void update() override { + auto steering_status = calculate_steering_status(); + auto wheel_velocities = calculate_wheel_velocities(); + chassis_velocity_->vector = calculate_chassis_velocity(steering_status, wheel_velocities); + } + +private: + struct SteeringStatus { + Eigen::Vector4d angle, cos_angle, sin_angle; + }; + + SteeringStatus calculate_steering_status() const { + SteeringStatus steering_status; + + steering_status.angle = { + *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(); + + return steering_status; + } + + Eigen::Vector4d calculate_wheel_velocities() const { + return { + *left_front_wheel_velocity_, // + *left_back_wheel_velocity_, // + *right_back_wheel_velocity_, // + *right_front_wheel_velocity_ // + }; + } + + Eigen::Vector3d calculate_chassis_velocity( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities) const { + Eigen::Vector3d velocity; + double one_quarter_r = wheel_radius_ / 4.0; + velocity.x() = one_quarter_r * wheel_velocities.dot(steering_status.cos_angle); + velocity.y() = one_quarter_r * wheel_velocities.dot(steering_status.sin_angle); + velocity.z() = -one_quarter_r / vehicle_radius_ + * (-wheel_velocities[0] * steering_status.sin_angle[0] + + wheel_velocities[1] * steering_status.cos_angle[1] + + wheel_velocities[2] * steering_status.sin_angle[2] + - wheel_velocities[3] * steering_status.cos_angle[3]); + return velocity; + } + + const double vehicle_radius_; + const double wheel_radius_; + + InputInterface left_front_steering_angle_; + InputInterface left_back_steering_angle_; + InputInterface right_back_steering_angle_; + InputInterface right_front_steering_angle_; + + InputInterface left_front_wheel_velocity_; + InputInterface left_back_wheel_velocity_; + InputInterface right_back_wheel_velocity_; + InputInterface right_front_wheel_velocity_; + + OutputInterface chassis_velocity_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::SteeringWheelStatus, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp index 80d7a72e..90ae2576 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp @@ -44,9 +44,9 @@ class DualYawController register_output("/gimbal/top_yaw/control_torque", top_yaw_control_torque_, 0.0); register_output("/gimbal/bottom_yaw/control_torque", bottom_yaw_control_torque_, 0.0); - - register_output("/gimbal/top_yaw/control_angle", top_yaw_control_torque_, 0.0); - register_output("/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_torque_, 0.0); + register_output("/gimbal/top_yaw/control_angle", top_yaw_control_angle_, nan_); + register_output( + "/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_angle_shift_, nan_); status_component_ = create_partner_component(get_component_name() + "_status"); diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index f20bd98f..40aba208 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -67,6 +67,7 @@ class HeroGimbalController } *gimbal_mode_ = *switch_right_ == Switch::UP ? GimbalMode::ENCODER : gimbal_mode_keyboard_; + // *gimbal_mode_ = gimbal_mode_keyboard_; if (*gimbal_mode_ == GimbalMode::IMU) { auto angle_error = update_imu_control(); @@ -121,7 +122,7 @@ class HeroGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); return imu_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); @@ -138,7 +139,7 @@ class HeroGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_yaw_sensitivity * mouse_velocity_->y(); double pitch_shift = -joystick_sensitivity * joystick_left_->x() - - mouse_pitch_sensitivity * mouse_velocity_->x(); + + mouse_pitch_sensitivity * mouse_velocity_->x(); return encoder_gimbal_solver.update( PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index d27704d5..dd37c17a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp @@ -67,7 +67,7 @@ class SimpleGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp new file mode 100644 index 00000000..366f66b6 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp @@ -0,0 +1,166 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace rmcs_core::controller::pid { + +class FrictionWheelPidRecorder + : public rmcs_executor::Component + , public rclcpp::Node { +public: + FrictionWheelPidRecorder() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + const auto wheels = + declare_parameter>("wheels", std::vector{}); + if (wheels.empty()) + throw std::runtime_error("Parameter \"wheels\" must not be empty"); + + min_setpoint_abs_ = declare_parameter("min_setpoint_abs", 10.0); + + const auto flush_every_n_samples = declare_parameter("flush_every_n_samples", 100); + if (flush_every_n_samples <= 0) + throw std::runtime_error("Parameter \"flush_every_n_samples\" must be positive"); + flush_every_n_samples_ = static_cast(flush_every_n_samples); + + const auto output_dir = std::filesystem::path{ + declare_parameter("output_dir", "/tmp/friction_pid_logs")}; + const auto output_name = declare_parameter("output_name", ""); + + wheel_count_ = wheels.size(); + wheel_ids_ = std::make_unique(wheel_count_); + setpoints_ = std::make_unique[]>(wheel_count_); + measurements_ = std::make_unique[]>(wheel_count_); + control_torques_ = std::make_unique[]>(wheel_count_); + run_ids_ = std::make_unique(wheel_count_); + sample_indices_ = std::make_unique(wheel_count_); + previously_enabled_ = std::make_unique(wheel_count_); + + for (size_t i = 0; i < wheel_count_; ++i) { + wheel_ids_[i] = wheel_id_from_topic(wheels[i]); + register_input(wheels[i] + "/control_velocity", setpoints_[i]); + register_input(wheels[i] + "/velocity", measurements_[i]); + register_input(wheels[i] + "/control_torque", control_torques_[i]); + + run_ids_[i] = 0; + sample_indices_[i] = 0; + previously_enabled_[i] = false; + } + + std::filesystem::create_directories(output_dir); + log_file_path_ = output_dir / resolve_output_name(output_name); + log_stream_.open(log_file_path_); + if (!log_stream_.is_open()) + throw std::runtime_error( + "Failed to open recorder output file: " + log_file_path_.string()); + + log_stream_.setf(std::ios::fixed); + log_stream_.precision(9); + log_stream_ + << "timestamp_us,wheel_id,run_id,sample_idx,setpoint_velocity,measured_velocity," + "control_torque,enabled\n"; + + RCLCPP_INFO(get_logger(), "FrictionWheelPidRecorder writing to %s", log_file_path_.c_str()); + } + + ~FrictionWheelPidRecorder() override { + if (log_stream_.is_open()) { + log_stream_.flush(); + log_stream_.close(); + } + } + + void update() override { + const auto timestamp_us = now_timestamp_us(); + + for (size_t i = 0; i < wheel_count_; ++i) { + const bool enabled = should_record(i); + if (enabled && !previously_enabled_[i]) { + ++run_ids_[i]; + sample_indices_[i] = 0; + } + + if (enabled) { + log_stream_ << timestamp_us << ',' << wheel_ids_[i] << ',' << run_ids_[i] << ',' + << sample_indices_[i]++ << ',' << *setpoints_[i] << ',' + << *measurements_[i] << ',' << *control_torques_[i] << ",1\n"; + if (++unflushed_samples_ >= flush_every_n_samples_) { + log_stream_.flush(); + unflushed_samples_ = 0; + } + } + + previously_enabled_[i] = enabled; + } + } + +private: + static std::string wheel_id_from_topic(std::string_view topic) { + auto pos = topic.find_last_of('/'); + if (pos == std::string_view::npos || pos + 1 >= topic.size()) + return std::string(topic); + return std::string(topic.substr(pos + 1)); + } + + static uint64_t now_timestamp_us() { + const auto now = std::chrono::steady_clock::now().time_since_epoch(); + return static_cast( + std::chrono::duration_cast(now).count()); + } + + static std::string resolve_output_name(const std::string& configured_name) { + if (!configured_name.empty()) + return configured_name; + + const auto now = std::chrono::system_clock::now().time_since_epoch(); + const auto ms = std::chrono::duration_cast(now).count(); + return "friction_wheel_pid_" + std::to_string(ms) + ".csv"; + } + + bool should_record(size_t index) const { + if (!setpoints_[index].ready() || !measurements_[index].ready() + || !control_torques_[index].ready()) + return false; + + const auto setpoint = *setpoints_[index]; + const auto measurement = *measurements_[index]; + const auto control_torque = *control_torques_[index]; + + return std::isfinite(setpoint) && std::isfinite(measurement) + && std::isfinite(control_torque) && std::abs(setpoint) >= min_setpoint_abs_; + } + + size_t wheel_count_ = 0; + double min_setpoint_abs_ = 10.0; + size_t flush_every_n_samples_ = 100; + size_t unflushed_samples_ = 0; + + std::filesystem::path log_file_path_; + std::ofstream log_stream_; + + std::unique_ptr wheel_ids_; + std::unique_ptr[]> setpoints_; + std::unique_ptr[]> measurements_; + std::unique_ptr[]> control_torques_; + std::unique_ptr run_ids_; + std::unique_ptr sample_indices_; + std::unique_ptr previously_enabled_; +}; + +} // namespace rmcs_core::controller::pid + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::pid::FrictionWheelPidRecorder, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp index bf3076e3..95d75cb9 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp @@ -31,14 +31,15 @@ class BulletFeederController42mm register_input( "/gimbal/control_bullet_allowance/limited_by_heat", control_bullet_allowance_limited_by_heat_); + register_input("/gimbal/bullet_fired", bullet_fired_); - bullet_feeder_velocity_pid_.kp = 50.0; - bullet_feeder_velocity_pid_.ki = 10.0; - bullet_feeder_velocity_pid_.kd = 0.0; + bullet_feeder_velocity_pid_.kp = 50.0; + bullet_feeder_velocity_pid_.ki = 10.0; + bullet_feeder_velocity_pid_.kd = 0.0; bullet_feeder_velocity_pid_.integral_max = 60.0; bullet_feeder_velocity_pid_.integral_min = 0.0; - bullet_feeder_angle_pid_.kp = 50.0; + bullet_feeder_angle_pid_.kp = 60.0; bullet_feeder_angle_pid_.ki = 0.0; bullet_feeder_angle_pid_.kd = 2.0; @@ -51,9 +52,9 @@ class BulletFeederController42mm void update() override { const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto mouse = *mouse_; - const auto keyboard = *keyboard_; + const auto switch_left = *switch_left_; + const auto mouse = *mouse_; + const auto keyboard = *keyboard_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -61,7 +62,6 @@ class BulletFeederController42mm reset_all_controls(); return; } - overdrive_mode_ = keyboard.f; if (keyboard.ctrl && !last_keyboard_.r && keyboard.r) low_latency_mode_ = !low_latency_mode_; @@ -84,8 +84,8 @@ class BulletFeederController42mm } else { if (!*friction_ready_ || std::isnan(bullet_feeder_control_angle_)) { bullet_feeder_control_angle_ = *bullet_feeder_angle_; - shoot_stage_ = ShootStage::PRELOADED; - bullet_fed_count_ = static_cast( + shoot_stage_ = ShootStage::PRELOADED; + bullet_fed_count_ = static_cast( (*bullet_feeder_angle_ - bullet_feeder_compressed_zero_point_ - 0.1) / bullet_feeder_angle_per_bullet_); } @@ -135,24 +135,24 @@ class BulletFeederController42mm } last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_mouse_ = mouse; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_mouse_ = mouse; + last_keyboard_ = keyboard; } private: void reset_all_controls() { last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - last_mouse_ = rmcs_msgs::Mouse::zero(); - last_keyboard_ = rmcs_msgs::Keyboard::zero(); + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_mouse_ = rmcs_msgs::Mouse::zero(); + last_keyboard_ = rmcs_msgs::Keyboard::zero(); overdrive_mode_ = low_latency_mode_ = false; - shoot_stage_ = ShootStage::PRELOADED; + shoot_stage_ = ShootStage::PRELOADED; bullet_fed_count_ = std::numeric_limits::min(); - bullet_feeder_control_angle_ = nan_; + bullet_feeder_control_angle_ = nan_; bullet_feeder_angle_pid_.output_max = inf_; bullet_feeder_velocity_pid_.reset(); @@ -160,13 +160,13 @@ class BulletFeederController42mm *bullet_feeder_control_torque_ = nan_; bullet_feeder_faulty_count_ = 0; - bullet_feeder_cool_down_ = 0; + bullet_feeder_cool_down_ = 0; } void set_preloading() { RCLCPP_INFO(get_logger(), "PRELOADING"); bullet_fed_count_++; - shoot_stage_ = ShootStage::PRELOADING; + shoot_stage_ = ShootStage::PRELOADING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 0.5) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 1.0; @@ -179,7 +179,7 @@ class BulletFeederController42mm void set_compressing() { RCLCPP_INFO(get_logger(), "COMPRESSING"); - shoot_stage_ = ShootStage::COMPRESSING; + shoot_stage_ = ShootStage::COMPRESSING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 1) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 0.8; @@ -192,7 +192,7 @@ class BulletFeederController42mm void set_shooting() { RCLCPP_INFO(get_logger(), "SHOOTING"); - shoot_stage_ = ShootStage::SHOOTING; + shoot_stage_ = ShootStage::SHOOTING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 1.2) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 1.0; @@ -215,7 +215,7 @@ class BulletFeederController42mm void enter_jam_protection() { bullet_feeder_control_angle_ = nan_; - bullet_feeder_cool_down_ = 1000; + bullet_feeder_cool_down_ = 1000; bullet_feeder_angle_pid_.reset(); bullet_feeder_velocity_pid_.reset(); RCLCPP_INFO(get_logger(), "Jammed!"); @@ -225,9 +225,10 @@ class BulletFeederController42mm static constexpr double inf_ = std::numeric_limits::infinity(); static constexpr double bullet_feeder_compressed_zero_point_ = 0.58; - static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; + static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; InputInterface friction_ready_; + InputInterface bullet_fired_; InputInterface switch_right_; InputInterface switch_left_; @@ -235,9 +236,9 @@ class BulletFeederController42mm InputInterface keyboard_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); bool overdrive_mode_ = false, low_latency_mode_ = false; @@ -247,8 +248,8 @@ class BulletFeederController42mm InputInterface control_bullet_allowance_limited_by_heat_; enum class ShootStage { PRELOADING, PRELOADED, COMPRESSING, COMPRESSED, SHOOTING }; - ShootStage shoot_stage_ = ShootStage::PRELOADED; - int bullet_fed_count_ = std::numeric_limits::min(); + ShootStage shoot_stage_ = ShootStage::PRELOADED; + int bullet_fed_count_ = std::numeric_limits::min(); double bullet_feeder_control_angle_ = nan_; pid::PidCalculator bullet_feeder_velocity_pid_; @@ -256,7 +257,7 @@ class BulletFeederController42mm OutputInterface bullet_feeder_control_torque_; int bullet_feeder_faulty_count_ = 0; - int bullet_feeder_cool_down_ = 0; + int bullet_feeder_cool_down_ = 0; OutputInterface shoot_mode_; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index a83dc5f4..4e22c13e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -89,6 +89,10 @@ class FrictionWheelController last_switch_right_ = switch_right; last_switch_left_ = switch_left; last_keyboard_ = keyboard; + // RCLCPP_INFO(logger_, "Bullet Fired: {%d}", *bullet_fired_); + } + if (!friction_enabled_) { + reset_all_controls(); } } @@ -150,8 +154,8 @@ class FrictionWheelController bool detect_friction_faulty() { for (size_t i = 0; i < friction_count_; i++) { - if (*friction_velocities_[i] < *friction_control_velocities_[i] * 0.5) - return true; + if (abs(*friction_velocities_[i]) < abs(*friction_control_velocities_[i] * 0.5)) + return false; } return false; } diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index 3de0960a..6cbbda2b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -1,5 +1,7 @@ #include +#include +#include #include #include @@ -26,10 +28,12 @@ class HeatController } void update() override { + shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); - if (*bullet_fired_) - shooter_heat_ += heat_per_shot + 10; + if (*bullet_fired_) { + shooter_heat_ += heat_per_shot; + } *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp index 5500b15c..6d8fbb4d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp @@ -20,7 +20,7 @@ class ShootingRecorder register_input("/referee/shooter/initial_speed", initial_speed_); register_input("/referee/shooter/shoot_timestamp", shoot_timestamp_); - register_input("/friction_wheels/temperature", fractional_temperature_); + // register_input("/friction_wheels/temperature", fractional_temperature_); if (friction_wheel_count_ == 2) { const auto topic = std::array{ @@ -42,7 +42,7 @@ class ShootingRecorder using namespace std::chrono; auto now = high_resolution_clock::now(); - auto ms = duration_cast(now.time_since_epoch()).count(); + auto ms = duration_cast(now.time_since_epoch()).count(); auto file = fmt::format("/robot_shoot/{}.log", ms); log_stream_.open(file); @@ -65,14 +65,14 @@ class ShootingRecorder break; } - auto log_text = std::string{}; + auto log_text = std::string{}; auto timestamp = timestamp_to_string(*shoot_timestamp_); if (friction_wheel_count_ == 2) { log_text = fmt::format( - "{},{},{},{},{}", timestamp, *initial_speed_, // - *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], - *fractional_temperature_); + "{},{},{},{}", timestamp, *initial_speed_, // + *friction_wheels_velocity_[0], *friction_wheels_velocity_[1]); + // *fractional_temperature_); } else if (friction_wheel_count_ == 4) { log_text = fmt::format( "{},{},{},{},{},{},{}", timestamp, *initial_speed_, // @@ -108,14 +108,14 @@ class ShootingRecorder private: static std::string timestamp_to_string(double timestamp) { - auto time = static_cast(timestamp); + auto time = static_cast(timestamp); auto local_time = std::localtime(&time); char buffer[100]; std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", local_time); double fractional_seconds = timestamp - std::floor(timestamp); - int milliseconds = static_cast(fractional_seconds * 1000); + int milliseconds = static_cast(fractional_seconds * 1000); char result[150]; std::snprintf(result, sizeof(result), "%s.%03d", buffer, milliseconds); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp new file mode 100644 index 00000000..35948421 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/climbable-infantry.cpp @@ -0,0 +1,626 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" + +namespace rmcs_core::hardware { + +class ClimbableInfantry + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ClimbableInfantry() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_component_( + create_partner_component( + get_component_name() + "_command", *this)) { + + register_output("/tf", tf_); + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + + top_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); + bottom_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.06603, 0.0, 0.082}); + } + + ClimbableInfantry(const ClimbableInfantry&) = delete; + ClimbableInfantry& operator=(const ClimbableInfantry&) = delete; + ClimbableInfantry(ClimbableInfantry&&) = delete; + ClimbableInfantry& operator=(ClimbableInfantry&&) = delete; + + ~ClimbableInfantry() override = default; + + void update() override { + top_board_->update(); + bottom_board_->update(); + } + + void command_update() { + top_board_->command_update(); + bottom_board_->command_update(); + + } + +private: + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New yaw offset: %ld", + bottom_board_->gimbal_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New pitch offset: %ld", + top_board_->gimbal_pitch_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New bullet feeder offset: %ld", + static_cast(bottom_board_->gimbal_bullet_feeder_.calibrate_zero_point())); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left front steering offset: %d", + bottom_board_->chassis_front_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right front steering offset: %d", + bottom_board_->chassis_front_steering_motors_[1].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left back steering offset: %d", + bottom_board_->chassis_back_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right back steering offset: %d", + bottom_board_->chassis_back_steering_motors_[1].calibrate_zero_point()); + } + + class ClimbableInfantryCommand : public rmcs_executor::Component { + public: + explicit ClimbableInfantryCommand(ClimbableInfantry& infantry_) + : infantry_(infantry_) {} + + void update() override { infantry_.command_update(); } + + ClimbableInfantry& infantry_; + }; + std::shared_ptr command_component_; + + class TopBoard final : private librmcs::agent::CBoard { + public: + friend class ClimbableInfantry; + explicit TopBoard( + ClimbableInfantry& climbable_infantry, + ClimbableInfantryCommand& climbable_infantry_command, + std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , logger_(climbable_infantry.get_logger()) + , tf_(climbable_infantry.tf_) + , imu_(1000, 0.2, 0.0) + , gimbal_pitch_motor_(climbable_infantry, climbable_infantry_command, "/gimbal/pitch") + , gimbal_left_friction_( + climbable_infantry, climbable_infantry_command, "/gimbal/left_friction") + , gimbal_right_friction_( + climbable_infantry, climbable_infantry_command, "/gimbal/right_friction") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("pitch_motor_zero_point").as_int())) + .enable_multi_turn_angle()); + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .set_reversed()); + climbable_infantry.register_output( + "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + climbable_infantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + imu_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, y, z); }); + } + + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() final = default; + + double yaw_imu_velocity() const { return imu_.gz(); } + + void update() { + imu_.update_status(); + const Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_yaw_velocity_imu_ = imu_.gz(); + *gimbal_pitch_velocity_imu_ = imu_.gy(); + + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + tf_->set_state( + gimbal_pitch_motor_.angle()); + } + + void command_update() { + auto builder = start_transmit(); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_right_friction_.generate_command(), + gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x143, + .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), + }); + } + + private: + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x201) { + gimbal_right_friction_.store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_left_friction_.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; + auto can_id = data.can_id; + if (can_id == 0x143) { + gimbal_pitch_motor_.store_status(data.can_data); + } + } + + 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); + } + + rclcpp::Logger logger_; + + OutputInterface& tf_; + + device::Bmi088 imu_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + }; + + class BottomBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class ClimbableInfantry; + explicit BottomBoard( + ClimbableInfantry& climbable_infantry, + ClimbableInfantryCommand& climbable_infantry_command, + std::string_view board_serial = {}) + : librmcs::agent::RmcsBoardLite(board_serial) + , logger_(climbable_infantry.get_logger()) + , tf_(climbable_infantry.tf_) + , imu_(1000, 0.2, 0.0) + , dr16_(climbable_infantry) + , gimbal_yaw_motor_(climbable_infantry, climbable_infantry_command, "/gimbal/yaw") + , supercap_(climbable_infantry, climbable_infantry_command) + , gimbal_bullet_feeder_( + climbable_infantry, climbable_infantry_command, "/gimbal/bullet_feeder") + , chassis_front_climber_motor_( + {climbable_infantry, climbable_infantry_command, + "/chassis/climber/left_front_motor"}, + {climbable_infantry, climbable_infantry_command, + "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {climbable_infantry, climbable_infantry_command, + "/chassis/climber/left_back_motor"}, + {climbable_infantry, climbable_infantry_command, + "/chassis/climber/right_back_motor"}) + , chassis_front_steering_motors_( + {climbable_infantry, climbable_infantry_command, "/chassis/left_front_steering"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_front_steering"}) + , chassis_front_wheel_motors_( + {climbable_infantry, climbable_infantry_command, "/chassis/left_front_wheel"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_front_wheel"}) + , chassis_back_steering_motors_( + {climbable_infantry, climbable_infantry_command, "/chassis/left_back_steering"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_back_steering"}) + , chassis_back_wheel_motors_( + {climbable_infantry, climbable_infantry_command, "/chassis/left_back_wheel"}, + {climbable_infantry, climbable_infantry_command, "/chassis/right_back_wheel"}) { + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("yaw_motor_zero_point").as_int()))); + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .enable_multi_turn_angle()); + + chassis_front_steering_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("left_front_zero_point").as_int())) + .set_reversed()); + chassis_front_steering_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("right_front_zero_point").as_int())) + .set_reversed()); + chassis_back_steering_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("left_back_zero_point").as_int())) + .set_reversed()); + chassis_back_steering_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + climbable_infantry.get_parameter("right_back_zero_point").as_int())) + .set_reversed()); + + chassis_front_wheel_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_front_wheel_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_back_wheel_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_back_wheel_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + + chassis_front_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + chassis_front_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + chassis_back_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle() + .set_reduction_ratio(19.) + .set_reversed()); + chassis_back_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + + climbable_infantry.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; + }; + + climbable_infantry.register_output( + "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + climbable_infantry.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); + } + + BottomBoard(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() final = default; + + void update() { + imu_.update_status(); + dr16_.update_status(); + + *chassis_yaw_velocity_imu_ = imu_.gz(); + *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); + + chassis_front_climber_motor_[0].update_status(); + chassis_front_climber_motor_[1].update_status(); + chassis_back_climber_motor_[0].update_status(); + chassis_back_climber_motor_[1].update_status(); + + gimbal_yaw_motor_.update_status(); + supercap_.update_status(); + gimbal_bullet_feeder_.update_status(); + + tf_->set_state( + gimbal_yaw_motor_.angle()); + + for (auto& motor : chassis_front_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_front_steering_motors_) + motor.update_status(); + for (auto& motor : chassis_back_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_back_steering_motors_) + motor.update_status(); + } + + void command_update() { + auto builder = start_transmit(); + + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_back_wheel_motors_[0].generate_command(), + chassis_back_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can0_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_back_steering_motors_[0].generate_command(), + chassis_back_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_wheel_motors_[0].generate_command(), + chassis_front_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_front_steering_motors_[0].generate_command(), + chassis_front_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_back_climber_motor_[0].generate_command(), + chassis_back_climber_motor_[1].generate_command(), + chassis_front_climber_motor_[0].generate_command(), + chassis_front_climber_motor_[1].generate_command(), + } + .as_bytes(), + }); + + + } + + private: + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + + if (can_id == 0x201) { + chassis_back_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_back_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_back_steering_motors_[0].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_back_steering_motors_[1].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; + auto can_id = data.can_id; + + if (can_id == 0x201) { + chassis_front_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_front_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_front_steering_motors_[0].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_front_steering_motors_[1].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; + auto can_id = data.can_id; + + if (can_id == 0x201) { + gimbal_bullet_feeder_.store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_yaw_motor_.store_status(data.can_data); + } + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + + if (can_id == 0x201) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_front_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_front_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); + } + } + + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + const auto* uart_data = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, + data.uart_data.size()); + // if (!data.uart_data.empty()) { + // RCLCPP_INFO( + // logger_, "ref uart0 recv: size=%zu first=0x%02X", + // data.uart_data.size(), + // std::to_integer(data.uart_data[0])); + // } + + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), 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); + } + + rclcpp::Logger logger_; + + OutputInterface& tf_; + + device::Bmi088 imu_; + device::Dr16 dr16_; + + device::LkMotor gimbal_yaw_motor_; + + device::Supercap supercap_; + device::DjiMotor gimbal_bullet_feeder_; + + device::DjiMotor chassis_front_climber_motor_[2]; + device::DjiMotor chassis_back_climber_motor_[2]; + device::DjiMotor chassis_front_steering_motors_[2]; + device::DjiMotor chassis_front_wheel_motors_[2]; + device::DjiMotor chassis_back_steering_motors_[2]; + device::DjiMotor chassis_back_wheel_motors_[2]; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; + }; + + OutputInterface tf_; + InputInterface robot_chassis_power_limit_; + + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + + std::shared_ptr top_board_; + std::shared_ptr bottom_board_; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::ClimbableInfantry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp index d30c1ce0..0f16692d 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -1,14 +1,15 @@ +#include #include #include #include #include #include #include -#include #include #include #include +#include #include #include #include @@ -18,10 +19,8 @@ #include #include #include -#include #include -#include "hardware/device/benewake.hpp" #include "hardware/device/bmi088.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" @@ -36,16 +35,14 @@ class SteeringHero , public rclcpp::Node { public: SteeringHero() - : Node{ + : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) , command_component_( create_partner_component( get_component_name() + "_command", *this)) { register_output("/tf", tf_); - tf_->set_transform( - Eigen::Translation3d{0.16, 0.0, 0.15}); gimbal_calibrate_subscription_ = create_subscription( "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { @@ -54,10 +51,15 @@ class SteeringHero top_board_ = std::make_unique( *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); - temperature_logging_timer_.reset(1000); + bottom_board_one_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board_one").as_string()); + + bottom_board_two_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board_two").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.06603, 0.0, 0.082}); } SteeringHero(const SteeringHero&) = delete; @@ -69,30 +71,21 @@ class SteeringHero void update() override { top_board_->update(); - bottom_board_->update(); - - if (temperature_logging_timer_.tick()) { - temperature_logging_timer_.reset(1000); - RCLCPP_INFO( - get_logger(), - "Temperature: pitch: %.1f, top_yaw: %.1f, bottom_yaw: %.1f, feeder: %.1f", - top_board_->gimbal_pitch_motor_.temperature(), - top_board_->gimbal_top_yaw_motor_.temperature(), - bottom_board_->gimbal_bottom_yaw_motor_.temperature(), - top_board_->gimbal_bullet_feeder_.temperature()); - } + bottom_board_one_->update(); + bottom_board_two_->update(); } void command_update() { top_board_->command_update(); - bottom_board_->command_update(); + bottom_board_one_->command_update(); + bottom_board_two_->command_update(); } private: void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { RCLCPP_INFO( get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", - bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); + bottom_board_two_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[gimbal calibration] New pitch offset: %ld", top_board_->gimbal_pitch_motor_.calibrate_zero_point()); @@ -100,20 +93,20 @@ class SteeringHero get_logger(), "[gimbal calibration] New top yaw offset: %ld", top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); RCLCPP_INFO( - get_logger(), "[gimbal calibration] New viewer offset: %ld", - top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); + get_logger(), "[gimbal calibration] New bullet feeder offset: %ld", + top_board_->gimbal_bullet_feeder_.calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[chassis calibration] left front steering offset: %d", - bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); + bottom_board_one_->chassis_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right front steering offset: %d", + bottom_board_one_->chassis_steering_motors_[1].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[chassis calibration] left back steering offset: %d", - bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); + bottom_board_two_->chassis_steering_motors2_[0].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[chassis calibration] right back steering offset: %d", - bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right front steering offset: %d", - bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); + bottom_board_two_->chassis_steering_motors2_[1].calibrate_zero_point()); } class SteeringHeroCommand : public rmcs_executor::Component { @@ -123,7 +116,6 @@ class SteeringHero void update() override { hero_.command_update(); } - private: SteeringHero& hero_; }; std::shared_ptr command_component_; @@ -132,68 +124,75 @@ class SteeringHero public: friend class SteeringHero; explicit TopBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, + SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, std::string_view board_serial = {}) : librmcs::agent::CBoard(board_serial) - , tf_(hero.tf_) + , logger_(steering_hero.get_logger()) + , tf_(steering_hero.tf_) , imu_(1000, 0.2, 0.0) - , benewake_(hero, "/gimbal/auto_aim/laser_distance") - , gimbal_top_yaw_motor_( - hero, hero_command, "/gimbal/top_yaw", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast( - hero.get_parameter("top_yaw_motor_zero_point").as_int()))) - , gimbal_pitch_motor_( - hero, hero_command, "/gimbal/pitch", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("pitch_motor_zero_point").as_int()))) + , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") + , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") , gimbal_friction_wheels_( - {hero, hero_command, "/gimbal/second_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( - 1.)}, - {hero, hero_command, "/gimbal/first_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( - 1.)}, - {hero, hero_command, "/gimbal/first_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()}, - {hero, hero_command, "/gimbal/second_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()}) - , gimbal_bullet_feeder_( - hero, hero_command, "/gimbal/bullet_feeder", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_reversed() - .enable_multi_turn_angle()) - , gimbal_scope_motor_( - hero, hero_command, "/gimbal/scope", - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}) - , gimbal_player_viewer_motor_( - hero, hero_command, "/gimbal/player_viewer", - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("viewer_motor_zero_point").as_int())) - .set_reversed()) { + {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, + {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}) + , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") + , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") { + + gimbal_top_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10}.set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("top_yaw_motor_zero_point").as_int()))); + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10}.set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("pitch_motor_zero_point").as_int()))); + gimbal_friction_wheels_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_friction_wheels_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_bullet_feeder_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bullet_feeder_motor_zero_point").as_int())) + .set_reversed() + .enable_multi_turn_angle()); + putter_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .enable_multi_turn_angle()); + + steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + steering_hero.register_output( + "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); + steering_hero.register_output( + "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); + steering_hero.register_output( + "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); imu_.set_coordinate_mapping([](double x, double y, double z) { // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise use a - // matrix. + // The rotation angle must be an exact multiple of 90 degrees, otherwise + // use a matrix. - // Eigen::AngleAxisd pitch_link_to_imu_link{ - // std::numbers::pi, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; + // Eigen::AngleAxisd pitch_link_to_bmi088_link{ + // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; + // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * Eigen::Vector3d{1, 2, 3}; // std::cout << mapping << std::endl; - return std::make_tuple(-x, -y, z); }); - - hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); } TopBoard(const TopBoard&) = delete; @@ -202,16 +201,13 @@ class SteeringHero TopBoard& operator=(TopBoard&&) = delete; ~TopBoard() final = default; - void update() { imu_.update_status(); - const Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; tf_->set_transform( gimbal_imu_pose.conjugate()); - benewake_.update_status(); - *gimbal_yaw_velocity_imu_ = imu_.gz(); *gimbal_pitch_velocity_imu_ = imu_.gy(); @@ -221,16 +217,15 @@ class SteeringHero tf_->set_state( gimbal_pitch_motor_.angle()); - gimbal_player_viewer_motor_.update_status(); - tf_->set_state( - gimbal_player_viewer_motor_.angle()); - - gimbal_scope_motor_.update_status(); - for (auto& motor : gimbal_friction_wheels_) motor.update_status(); gimbal_bullet_feeder_.update_status(); + putter_motor_.update_status(); + + if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) + *camera_capturer_trigger_ = true; + last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; } void command_update() { @@ -240,26 +235,19 @@ class SteeringHero .can_id = 0x200, .can_data = device::CanPacket8{ - gimbal_friction_wheels_[0].generate_command(), + gimbal_friction_wheels_[3].generate_command(), gimbal_friction_wheels_[1].generate_command(), gimbal_friction_wheels_[2].generate_command(), - gimbal_friction_wheels_[3].generate_command(), + gimbal_friction_wheels_[0].generate_command(), } .as_bytes(), }); builder.can1_transmit({ - .can_id = 0x141, - .can_data = gimbal_bullet_feeder_ - .generate_torque_command(gimbal_bullet_feeder_.control_torque()) - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, + .can_id = 0x1FF, .can_data = device::CanPacket8{ - gimbal_scope_motor_.generate_command(), + putter_motor_.generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -267,12 +255,9 @@ class SteeringHero .as_bytes(), }); - builder.can2_transmit({ - .can_id = 0x143, - .can_data = - gimbal_player_viewer_motor_ - .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) - .as_bytes(), + builder.can1_transmit({ + .can_id = 0x141, + .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), }); builder.can2_transmit({ @@ -284,22 +269,28 @@ class SteeringHero .can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); + + builder.gpio_digital_read({ + .channel = 1, + .falling_edge = true, + }); } private: void can1_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - auto can_id = data.can_id; - if (can_id == 0x201) { + if (can_id == 0x204) { gimbal_friction_wheels_[0].store_status(data.can_data); } else if (can_id == 0x202) { gimbal_friction_wheels_[1].store_status(data.can_data); } else if (can_id == 0x203) { gimbal_friction_wheels_[2].store_status(data.can_data); - } else if (can_id == 0x204) { + } else if (can_id == 0x201) { gimbal_friction_wheels_[3].store_status(data.can_data); + } else if (can_id == 0x205) { + putter_motor_.store_status(data.can_data); } else if (can_id == 0x141) { gimbal_bullet_feeder_.store_status(data.can_data); } @@ -308,21 +299,22 @@ class SteeringHero void can2_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - auto can_id = data.can_id; + if (can_id == 0x141) { gimbal_top_yaw_motor_.store_status(data.can_data); } else if (can_id == 0x142) { gimbal_pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x143) { - gimbal_player_viewer_motor_.store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_scope_motor_.store_status(data.can_data); } } - void uart2_receive_callback(const librmcs::data::UartDataView& data) override { - benewake_.store_status(data.uart_data.data(), data.uart_data.size()); + void gpio_digital_read_result_callback( + const librmcs::data::GpioDigitalDataView& data) override { + *photoelectric_sensor_status_ = false; + if (data.channel == 1) { + *photoelectric_sensor_status_ = true; + RCLCPP_INFO(logger_, "trigger!"); + } } void accelerometer_receive_callback( @@ -334,82 +326,257 @@ class SteeringHero imu_.store_gyroscope_status(data.x, data.y, data.z); } - OutputInterface& tf_; + void putter_receive_callback(bool status) { *photoelectric_sensor_status_ = status; } - device::Bmi088 imu_; - device::Benewake benewake_; + rclcpp::Logger logger_; + OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; + std::time_t last_camera_capturer_trigger_timestamp_{0}; + device::Bmi088 imu_; device::LkMotor gimbal_top_yaw_motor_; device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_friction_wheels_[4]; device::LkMotor gimbal_bullet_feeder_; + device::DjiMotor putter_motor_; - device::DjiMotor gimbal_scope_motor_; - device::LkMotor gimbal_player_viewer_motor_; + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface photoelectric_sensor_status_; + OutputInterface camera_capturer_trigger_; + OutputInterface camera_capturer_trigger_timestamp_; + std::atomic photoelectric_sensor_status_atomic{false}; }; - class BottomBoard final : private librmcs::agent::CBoard { + class BottomBoard_one final : private librmcs::agent::CBoard { public: friend class SteeringHero; - explicit BottomBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, + explicit BottomBoard_one( + SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, std::string_view board_serial = {}) : librmcs::agent::CBoard(board_serial) + , logger_(steering_hero.get_logger()) , imu_(1000, 0.2, 0.0) - , tf_(hero.tf_) - , dr16_(hero) + , chassis_front_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) , chassis_steering_motors_( - {hero, hero_command, "/chassis/left_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_front_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/left_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_front_zero_point").as_int())) - .set_reversed()}) + {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_front_steering"}) , chassis_wheel_motors_( - {hero, hero_command, "/chassis/left_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/left_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}) - , supercap_(hero, hero_command) - , gimbal_bottom_yaw_motor_( - hero, hero_command, "/gimbal/bottom_yaw", - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} - .set_reversed() - .set_encoder_zero_point( - static_cast( - hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))) { - - hero.register_output("/referee/serial", referee_serial_); + {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}) { + // + + chassis_steering_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_front_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_front_zero_point").as_int())) + .set_reversed()); + chassis_wheel_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + + chassis_front_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + chassis_front_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + chassis_back_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + chassis_back_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + + steering_hero.register_output( + "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); + } + BottomBoard_one(const BottomBoard_one&) = delete; + BottomBoard_one& operator=(const BottomBoard_one&) = delete; + BottomBoard_one(BottomBoard_one&&) = delete; + BottomBoard_one& operator=(BottomBoard_one&&) = delete; + + ~BottomBoard_one() final = default; + + void update() { + imu_.update_status(); + + *chassis_yaw_velocity_imu_ = imu_.gz(); + *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); + + chassis_front_climber_motor_[0].update_status(); + chassis_front_climber_motor_[1].update_status(); + chassis_back_climber_motor_[0].update_status(); + chassis_back_climber_motor_[1].update_status(); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_steering_motors_) + motor.update_status(); + } + + void command_update() { + auto builder = start_transmit(); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[1].generate_command(), + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_back_climber_motor_[0].generate_command(), + chassis_back_climber_motor_[1].generate_command(), + chassis_front_climber_motor_[0].generate_command(), + chassis_front_climber_motor_[1].generate_command(), + } + .as_bytes(), + }); + } + + private: + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x201) { + chassis_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_steering_motors_[0].store_status(data.can_data); + } else if (can_id == 0x208) { + chassis_steering_motors_[1].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; + auto can_id = data.can_id; + + if (can_id == 0x203) { + chassis_front_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_front_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x201) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } + } + + 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); + } + + rclcpp::Logger logger_; + + device::Bmi088 imu_; + device::DjiMotor chassis_front_climber_motor_[2]; + device::DjiMotor chassis_back_climber_motor_[2]; + device::DjiMotor chassis_steering_motors_[2]; + device::DjiMotor chassis_wheel_motors_[2]; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + }; + + class BottomBoard_two final : private librmcs::agent::CBoard { + public: + friend class SteeringHero; + explicit BottomBoard_two( + SteeringHero& steering_hero, SteeringHeroCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , logger_(steering_hero.get_logger()) + , imu_(1000, 0.2, 0.0) + , tf_(steering_hero.tf_) + , dr16_(steering_hero) + , supercap_(steering_hero, steering_hero_command) + , chassis_steering_motors2_( + {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) + , chassis_wheel_motors2_( + {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) + , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { + chassis_steering_motors2_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_back_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors2_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_back_zero_point").as_int())) + .set_reversed()); + chassis_wheel_motors2_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors2_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + gimbal_bottom_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} + .set_reversed() + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); + steering_hero.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); @@ -420,49 +587,47 @@ class SteeringHero }); return size; }; - - hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - - hero.register_output( + steering_hero.register_output( "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); - hero.register_output( + steering_hero.register_output( "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); } - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; + BottomBoard_two(const BottomBoard_two&) = delete; + BottomBoard_two& operator=(const BottomBoard_two&) = delete; + BottomBoard_two(BottomBoard_two&&) = delete; + BottomBoard_two& operator=(BottomBoard_two&&) = delete; - ~BottomBoard() final = default; + ~BottomBoard_two() final = default; void update() { imu_.update_status(); dr16_.update_status(); supercap_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - - for (auto& motor : chassis_wheel_motors_) + for (auto& motor : chassis_wheel_motors2_) motor.update_status(); - for (auto& motor : chassis_steering_motors_) + for (auto& motor : chassis_steering_motors2_) motor.update_status(); + gimbal_bottom_yaw_motor_.update_status(); + tf_->set_state( gimbal_bottom_yaw_motor_.angle()); } void command_update() { + auto builder = start_transmit(); builder.can1_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors2_[1].generate_command(), + chassis_wheel_motors2_[0].generate_command(), } .as_bytes(), }); @@ -471,26 +636,14 @@ class SteeringHero .can_id = 0x1FE, .can_data = device::CanPacket8{ + chassis_steering_motors2_[0].generate_command(), device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors2_[1].generate_command(), supercap_.generate_command(), } .as_bytes(), }); - builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steering_motors_[0].generate_command(), - chassis_steering_motors_[1].generate_command(), - chassis_steering_motors_[2].generate_command(), - chassis_steering_motors_[3].generate_command(), - } - .as_bytes(), - }); - builder.can2_transmit({ .can_id = 0x141, .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), @@ -501,16 +654,16 @@ class SteeringHero void can1_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - auto can_id = data.can_id; - if (can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(data.can_data); + + if (can_id == 0x203) { + chassis_wheel_motors2_[1].store_status(data.can_data); } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); + chassis_wheel_motors2_[0].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_steering_motors2_[0].store_status(data.can_data); + } else if (can_id == 0x207) { + chassis_steering_motors2_[1].store_status(data.can_data); } else if (can_id == 0x300) { supercap_.store_status(data.can_data); } @@ -519,21 +672,15 @@ class SteeringHero void can2_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; - auto can_id = data.can_id; - if (can_id == 0x205) { - chassis_steering_motors_[0].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[1].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[3].store_status(data.can_data); - } else if (can_id == 0x141) { + + if (can_id == 0x141) { gimbal_bottom_yaw_motor_.store_status(data.can_data); } } + rclcpp::Logger logger_; + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { const auto* uart_data = data.uart_data.data(); referee_ring_buffer_receive_.emplace_back_n( @@ -557,17 +704,14 @@ class SteeringHero device::Bmi088 imu_; OutputInterface& tf_; - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface powermeter_control_enabled_; OutputInterface powermeter_charge_power_limit_; device::Dr16 dr16_; - - device::DjiMotor chassis_steering_motors_[4]; - device::DjiMotor chassis_wheel_motors_[4]; device::Supercap supercap_; + device::DjiMotor chassis_steering_motors2_[2]; + device::DjiMotor chassis_wheel_motors2_[2]; device::LkMotor gimbal_bottom_yaw_motor_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; @@ -578,10 +722,9 @@ class SteeringHero rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - std::unique_ptr top_board_; - std::unique_ptr bottom_board_; - - rmcs_utility::TickTimer temperature_logging_timer_; + std::shared_ptr top_board_; + std::shared_ptr bottom_board_one_; + std::shared_ptr bottom_board_two_; }; } // namespace rmcs_core::hardware diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/ui-climbable-infantry.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/ui-climbable-infantry.cpp new file mode 100644 index 00000000..7693cae4 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/ui-climbable-infantry.cpp @@ -0,0 +1,210 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" +#include "referee/app/ui/widget/status_ring.hpp" + +namespace rmcs_core::referee::app::ui { +using namespace std::chrono_literals; + +class UIClimbableInfantry + : public rmcs_executor::Component + , public rclcpp::Node { +public: + UIClimbableInfantry() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , aiming_mark_(Shape::Color::WHITE, 2, x_center - 12, y_center - 37, 10, 10) + , status_ring_(26.5, 26.5, 600, 300) + , chassis_power_number_(Shape::Color::WHITE, 20, 2, x_center - 40, 860, 0) + , yaw_indicator_guidelines_( + {Shape::Color::WHITE, 2, x_center - 32, 830, x_center + 32, 830}, + {Shape::Color::WHITE, 2, x_center, 830, x_center, 820}) + , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) + ,front_climber_motor_direction_indicator_{{ + Shape::Color::CYAN, 60, x_center, y_center, 0, 0, 64, 64 + }, { + Shape::Color::CYAN, 60, x_center, y_center, 0, 0, 64, 64 + }} + , chassis_control_power_limit_indicator_(Shape::Color::WHITE, 20, 2, x_center + 10, 820, 0) + , supercap_control_power_limit_indicator_(Shape::Color::WHITE, 20, 2, x_center + 10, 790, 0) + , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { + + chassis_control_direction_indicator_.set_x(x_center); + chassis_control_direction_indicator_.set_y(y_center); + + register_input("/chassis/control_mode", chassis_mode_); + + register_input("/chassis/angle", chassis_angle_); + register_input("/chassis/control_angle", chassis_control_angle_); + + register_input("/chassis/supercap/voltage", supercap_voltage_); + register_input("/chassis/supercap/enabled", supercap_enabled_); + + register_input("/chassis/voltage", chassis_voltage_); + register_input("/chassis/power", chassis_power_); + register_input("/chassis/control_power_limit", chassis_control_power_limit_); + register_input("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_); + + register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); + register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); + register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); + register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); + + 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("/remote/keyboard", keyboard_); + + register_input("/referee/game/stage", game_stage_); + + // register_input("/auto_aim/ui_target", auto_aim_target_, false); + } + + void update() override { + update_chassis_direction_indicator(); + + chassis_control_power_limit_indicator_.set_value(*chassis_control_power_limit_); + supercap_control_power_limit_indicator_.set_value(*supercap_charge_power_limit_); + + chassis_power_number_.set_value(*chassis_power_); + + 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 keyboard = *keyboard_; + + auto to_referee_angle = [](double angle) { + return static_cast( + std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); + }; + + int climber_motor_angle; + int half_intervel_angle = 30; + chassis_direction_indicator_.set_color( + chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN + : Shape::Color::PINK); + chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); + if (keyboard.g) { + front_climber_motor_direction_indicator_[0].set_color(Shape::Color::ORANGE); + front_climber_motor_direction_indicator_[1].set_color(Shape::Color::ORANGE); + front_climber_motor_direction_indicator_[0].set_width(90); + front_climber_motor_direction_indicator_[1].set_width(90); + front_climber_motor_direction_indicator_[0].set_r(100); + front_climber_motor_direction_indicator_[1].set_r(100); + } else { + front_climber_motor_direction_indicator_[0].set_color(Shape::Color::CYAN); + front_climber_motor_direction_indicator_[1].set_color(Shape::Color::CYAN); + front_climber_motor_direction_indicator_[0].set_width(30); + front_climber_motor_direction_indicator_[1].set_width(30); + front_climber_motor_direction_indicator_[0].set_r(80); + front_climber_motor_direction_indicator_[1].set_r(80); + } + climber_motor_angle = to_referee_angle(*chassis_angle_); + front_climber_motor_direction_indicator_[1].set_angle( + climber_motor_angle + half_intervel_angle, 5); + if (climber_motor_angle - half_intervel_angle < 0) { + climber_motor_angle += 360; + } + front_climber_motor_direction_indicator_[0].set_angle( + climber_motor_angle - half_intervel_angle, 5); + + bool chassis_control_direction_indicator_visible = false; + if (!std::isnan(*chassis_control_angle_)) { + if (chassis_mode == rmcs_msgs::ChassisMode::STEP_DOWN) { + chassis_control_direction_indicator_visible = true; + chassis_control_direction_indicator_.set_color(Shape::Color::CYAN); + chassis_control_direction_indicator_.set_width(8); + chassis_control_direction_indicator_.set_r(92); + chassis_control_direction_indicator_.set_angle( + to_referee_angle(*chassis_control_angle_), 30); + } else if (chassis_mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP) { + chassis_control_direction_indicator_visible = true; + chassis_control_direction_indicator_.set_color(Shape::Color::CYAN); + chassis_control_direction_indicator_.set_width(28); + chassis_control_direction_indicator_.set_r(102); + chassis_control_direction_indicator_.set_angle( + to_referee_angle(*chassis_control_angle_), 4); + } + } + chassis_control_direction_indicator_.set_visible( + chassis_control_direction_indicator_visible); + front_climber_motor_direction_indicator_[0].set_visible(true); + front_climber_motor_direction_indicator_[1].set_visible(true); + } + + 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_, chassis_control_angle_; + + InputInterface supercap_voltage_; + InputInterface supercap_enabled_; + + InputInterface chassis_voltage_; + InputInterface chassis_power_; + InputInterface chassis_control_power_limit_; + InputInterface supercap_charge_power_limit_; + + InputInterface left_front_velocity_, left_back_velocity_, right_back_velocity_, + right_front_velocity_; + + InputInterface robot_bullet_allowance_; + + InputInterface left_friction_control_velocity_; + InputInterface left_friction_velocity_; + InputInterface right_friction_velocity_; + + InputInterface mouse_; + InputInterface keyboard_; + + InputInterface game_stage_; + + // InputInterface> auto_aim_target_; + + Circle aiming_mark_; + StatusRing status_ring_; + + Float chassis_power_number_; + Line yaw_indicator_guidelines_[2]; + + Arc chassis_direction_indicator_, chassis_control_direction_indicator_; + Arc front_climber_motor_direction_indicator_[2]; + + Float chassis_control_power_limit_indicator_, supercap_control_power_limit_indicator_; + + Integer time_reminder_; +}; + +} // namespace rmcs_core::referee::app::ui + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::UIClimbableInfantry, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/third_party/tinympc b/rmcs_ws/src/rmcs_core/third_party/tinympc new file mode 160000 index 00000000..6e710a2e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/third_party/tinympc @@ -0,0 +1 @@ +Subproject commit 6e710a2e5813ce261117299b84577294ebc02be0 diff --git a/tools/pid_tuning/README.md b/tools/pid_tuning/README.md new file mode 100644 index 00000000..eab5cd21 --- /dev/null +++ b/tools/pid_tuning/README.md @@ -0,0 +1,134 @@ +# Friction Wheel PID Tuning + +This directory contains friction wheel PID tuning utilities and configuration snippets. + +## Recorder + +The recorder plugin class is: + +```text +rmcs_core::controller::pid::FrictionWheelPidRecorder +``` + +It records only when: + +- `control_velocity` is finite +- `velocity` is finite +- `control_torque` is finite +- `abs(control_velocity) >= min_setpoint_abs` + +Raw CSV columns: + +```text +timestamp_us,wheel_id,run_id,sample_idx,setpoint_velocity,measured_velocity,control_torque,enabled +``` + +Default output directory: + +```text +/tmp/friction_pid_logs +``` + +See `friction_wheel_pid_recorder.example.yaml` for a minimal configuration snippet. + +## Identification + +`identify_plant.py` estimates a discrete first-order-plus-dead-time model from the recorder CSV: + +```text +y[k+1] = a * y[k] + b * u[k-d] + c +``` + +Where: + +- `u` is `control_torque` +- `y` is `measured_velocity` +- `d` is the pure delay in samples + +The script scans candidate delays, fits each candidate with least squares, and writes: + +- `identification_summary.csv` +- one `*_model.json` per `wheel_id/run_id` +- one `*_fit.csv` per `wheel_id/run_id` + +Example: + +```bash +python3 tools/pid_tuning/identify_plant.py /tmp/friction_pid_logs/friction_wheel_pid_xxx.csv \ + --output-dir /tmp/friction_pid_identify +``` + +Common filters: + +```bash +python3 tools/pid_tuning/identify_plant.py /tmp/friction_pid_logs/friction_wheel_pid_xxx.csv \ + --wheel-id first_left_friction \ + --run-id 1 +``` + +Key options: + +- `--sample-time-ms`: fallback sample interval when timestamps are unavailable +- `--max-delay-ms`: maximum delay search range +- `--min-samples`: minimum segment length +- `--allow-unphysical`: export the best fit even if `a` or `b` is not physically plausible + +## Tuning + +`tune_pid.py` tunes only `kp` and `kd`. `ki` is always fixed to `0` so the offline controller matches the current friction wheel constraint. + +The simulated controller uses the same discrete update rule as `PidCalculator`: + +```text +u[k] = clamp(kp * e[k] + kd * (e[k] - e[k-1])) +``` + +Where: + +- `e[k] = setpoint_velocity[k] - measured_velocity[k]` +- `ki = 0` +- plant model comes from `identify_plant.py` + +Supported optimization methods: + +- `hybrid` (default): GA first, then GP-like Bayesian optimization refinement +- `ga`: genetic algorithm only +- `bo`: GP-like Bayesian optimization only + +Trace-driven tuning with the original recorded setpoint profile: + +```bash +python3 tools/pid_tuning/tune_pid.py /tmp/friction_pid_identify/first_left_friction_run_1_model.json \ + --trace-csv /tmp/friction_pid_logs/friction_wheel_pid_xxx.csv \ + --output-dir /tmp/friction_pid_tuning +``` + +Synthetic startup step tuning: + +```bash +python3 tools/pid_tuning/tune_pid.py /tmp/friction_pid_identify/first_left_friction_run_1_model.json \ + --step-setpoint 5000 \ + --output-limit 12 \ + --method hybrid +``` + +Batch tuning from an identification summary: + +```bash +python3 tools/pid_tuning/tune_pid.py /tmp/friction_pid_identify/identification_summary.csv \ + --trace-csv /tmp/friction_pid_logs/friction_wheel_pid_xxx.csv +``` + +Outputs: + +- `tuning_summary.csv` +- one `tuning_result.json` per `wheel_id/run_id` +- one `candidates.csv` per `wheel_id/run_id` +- one `best_response.csv` per `wheel_id/run_id` + +Common options: + +- `--seed-kp/--seed-kd`: baseline gains used for comparison +- `--kp-min/--kp-max`, `--kd-min/--kd-max`: search bounds +- `--output-limit` or `--output-min/--output-max`: controller output clamp +- `--trace-scope wheel`: reuse all runs of the same wheel instead of exact `run_id` diff --git a/tools/pid_tuning/friction_wheel_pid_recorder.example.yaml b/tools/pid_tuning/friction_wheel_pid_recorder.example.yaml new file mode 100644 index 00000000..8a709112 --- /dev/null +++ b/tools/pid_tuning/friction_wheel_pid_recorder.example.yaml @@ -0,0 +1,15 @@ +rmcs_executor: + ros__parameters: + components: + - rmcs_core::controller::pid::FrictionWheelPidRecorder -> friction_wheel_pid_recorder + +friction_wheel_pid_recorder: + ros__parameters: + wheels: + - /gimbal/first_left_friction + - /gimbal/first_right_friction + - /gimbal/second_left_friction + - /gimbal/second_right_friction + output_dir: /tmp/friction_pid_logs + min_setpoint_abs: 10.0 + flush_every_n_samples: 100 diff --git a/tools/pid_tuning/identify_plant.py b/tools/pid_tuning/identify_plant.py new file mode 100644 index 00000000..2f50af2e --- /dev/null +++ b/tools/pid_tuning/identify_plant.py @@ -0,0 +1,724 @@ +#!/usr/bin/env python3 + +from __future__ import annotations + +import argparse +import csv +import json +import math +import re +import sys +from dataclasses import dataclass +from datetime import datetime, timezone +from pathlib import Path +from typing import Iterable + +import numpy as np + + +REQUIRED_COLUMNS = { + "timestamp_us", + "wheel_id", + "run_id", + "sample_idx", + "setpoint_velocity", + "measured_velocity", + "control_torque", +} + + +@dataclass(frozen=True) +class Sample: + timestamp_us: int + wheel_id: str + run_id: int + sample_idx: int + setpoint_velocity: float + measured_velocity: float + control_torque: float + + +@dataclass(frozen=True) +class Segment: + wheel_id: str + run_id: int + timestamp_us: np.ndarray + sample_idx: np.ndarray + setpoint_velocity: np.ndarray + measured_velocity: np.ndarray + control_torque: np.ndarray + dropped_rows: int + inferred_dt_s: float + sample_jitter_ratio: float + missing_samples: int + + +@dataclass(frozen=True) +class CandidateResult: + delay_samples: int + a: float + b: float + c: float + rollout_rmse: float + one_step_rmse: float + mae: float + fit_percent: float + physically_plausible: bool + predicted_velocity: np.ndarray + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Estimate a first-order-plus-dead-time plant model for friction wheel velocity loops " + "from recorder CSV." + ) + ) + parser.add_argument("input_csv", help="CSV produced by FrictionWheelPidRecorder") + parser.add_argument( + "--output-dir", + help="Directory for summary/model files. Default: /tmp/friction_pid_identify/", + ) + parser.add_argument("--wheel-id", help="Only process one wheel_id") + parser.add_argument("--run-id", type=int, help="Only process one run_id") + parser.add_argument( + "--sample-time-ms", + type=float, + default=1.0, + help="Fallback sample time in milliseconds when timestamps are missing or invalid", + ) + parser.add_argument( + "--max-delay-ms", + type=float, + default=25.0, + help="Maximum pure delay to scan in milliseconds", + ) + parser.add_argument( + "--max-delay-samples", + type=int, + help="Maximum pure delay to scan in samples. Overrides --max-delay-ms", + ) + parser.add_argument( + "--min-samples", + type=int, + default=80, + help="Minimum samples required for one segment", + ) + parser.add_argument( + "--min-speed-span", + type=float, + default=30.0, + help="Minimum measured velocity span required to consider a segment sufficiently excited", + ) + parser.add_argument( + "--min-torque-span", + type=float, + default=0.05, + help="Minimum control torque span required to consider a segment sufficiently excited", + ) + parser.add_argument( + "--allow-unphysical", + action="store_true", + help="Allow selecting candidates outside the expected 00 range if needed", + ) + parser.add_argument( + "--quiet", + action="store_true", + help="Only print output paths and fatal errors", + ) + return parser.parse_args() + + +def parse_float(value: str) -> float: + number = float(value) + if not math.isfinite(number): + raise ValueError("non-finite number") + return number + + +def parse_int(value: str) -> int: + return int(value) + + +def default_output_dir() -> Path: + stamp = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ") + return Path("/tmp/friction_pid_identify") / stamp + + +def log(message: str, quiet: bool = False) -> None: + if not quiet: + print(message) + + +def load_samples(input_csv: Path, wheel_id: str | None, run_id: int | None) -> list[Sample]: + with input_csv.open("r", newline="") as handle: + reader = csv.DictReader(handle) + if reader.fieldnames is None: + raise ValueError("input CSV has no header") + + missing = REQUIRED_COLUMNS.difference(reader.fieldnames) + if missing: + raise ValueError(f"input CSV is missing columns: {', '.join(sorted(missing))}") + + samples: list[Sample] = [] + for row_index, row in enumerate(reader, start=2): + try: + enabled_value = row.get("enabled", "1").strip() + if enabled_value not in {"", "1", "true", "True", "TRUE"}: + continue + + sample = Sample( + timestamp_us=parse_int(row["timestamp_us"]), + wheel_id=row["wheel_id"].strip(), + run_id=parse_int(row["run_id"]), + sample_idx=parse_int(row["sample_idx"]), + setpoint_velocity=parse_float(row["setpoint_velocity"]), + measured_velocity=parse_float(row["measured_velocity"]), + control_torque=parse_float(row["control_torque"]), + ) + except Exception as exc: + raise ValueError(f"failed to parse row {row_index}: {exc}") from exc + + if wheel_id is not None and sample.wheel_id != wheel_id: + continue + if run_id is not None and sample.run_id != run_id: + continue + samples.append(sample) + + return samples + + +def group_samples(samples: Iterable[Sample]) -> dict[tuple[str, int], list[Sample]]: + grouped: dict[tuple[str, int], list[Sample]] = {} + for sample in samples: + grouped.setdefault((sample.wheel_id, sample.run_id), []).append(sample) + return grouped + + +def build_segment( + wheel_id: str, + run_id: int, + rows: list[Sample], + fallback_dt_s: float, +) -> Segment: + ordered = sorted(rows, key=lambda item: (item.sample_idx, item.timestamp_us)) + + filtered: list[Sample] = [] + dropped_rows = 0 + last_sample_idx: int | None = None + last_timestamp_us: int | None = None + for row in ordered: + if last_sample_idx is not None and row.sample_idx <= last_sample_idx: + dropped_rows += 1 + continue + if last_timestamp_us is not None and row.timestamp_us <= last_timestamp_us: + dropped_rows += 1 + continue + filtered.append(row) + last_sample_idx = row.sample_idx + last_timestamp_us = row.timestamp_us + + if not filtered: + raise ValueError(f"{wheel_id}/run{run_id}: no valid rows remain after ordering checks") + + timestamp_us = np.asarray([row.timestamp_us for row in filtered], dtype=np.int64) + sample_idx = np.asarray([row.sample_idx for row in filtered], dtype=np.int64) + setpoint_velocity = np.asarray([row.setpoint_velocity for row in filtered], dtype=np.float64) + measured_velocity = np.asarray([row.measured_velocity for row in filtered], dtype=np.float64) + control_torque = np.asarray([row.control_torque for row in filtered], dtype=np.float64) + + inferred_dt_s, sample_jitter_ratio = infer_sample_time(timestamp_us, fallback_dt_s) + missing_samples = count_missing_samples(sample_idx) + + return Segment( + wheel_id=wheel_id, + run_id=run_id, + timestamp_us=timestamp_us, + sample_idx=sample_idx, + setpoint_velocity=setpoint_velocity, + measured_velocity=measured_velocity, + control_torque=control_torque, + dropped_rows=dropped_rows, + inferred_dt_s=inferred_dt_s, + sample_jitter_ratio=sample_jitter_ratio, + missing_samples=missing_samples, + ) + + +def infer_sample_time(timestamp_us: np.ndarray, fallback_dt_s: float) -> tuple[float, float]: + if timestamp_us.size < 2: + return fallback_dt_s, 0.0 + + dt_us = np.diff(timestamp_us).astype(np.float64) + dt_us = dt_us[dt_us > 0.0] + if dt_us.size == 0: + return fallback_dt_s, 0.0 + + median_dt_us = float(np.median(dt_us)) + if not math.isfinite(median_dt_us) or median_dt_us <= 0.0: + return fallback_dt_s, 0.0 + + mad_us = float(np.median(np.abs(dt_us - median_dt_us))) + jitter_ratio = 0.0 if median_dt_us == 0.0 else mad_us / median_dt_us + return median_dt_us / 1e6, jitter_ratio + + +def count_missing_samples(sample_idx: np.ndarray) -> int: + if sample_idx.size < 2: + return 0 + diffs = np.diff(sample_idx) + return int(np.sum(np.clip(diffs - 1, 0, None))) + + +def excitation_ok(segment: Segment, min_speed_span: float, min_torque_span: float) -> tuple[bool, str]: + speed_span = percentile_span(segment.measured_velocity) + torque_span = percentile_span(segment.control_torque) + if speed_span < min_speed_span: + return False, f"measured velocity span too small ({speed_span:.3f} < {min_speed_span:.3f})" + if torque_span < min_torque_span: + return False, f"control torque span too small ({torque_span:.3f} < {min_torque_span:.3f})" + return True, "" + + +def percentile_span(values: np.ndarray) -> float: + if values.size == 0: + return 0.0 + return float(np.percentile(values, 95) - np.percentile(values, 5)) + + +def estimate_segment( + segment: Segment, + max_delay_samples: int, + allow_unphysical: bool, +) -> CandidateResult: + if segment.measured_velocity.size < 3: + raise ValueError(f"{segment.wheel_id}/run{segment.run_id}: too few samples to fit") + + best_result: CandidateResult | None = None + best_unphysical: CandidateResult | None = None + + for delay_samples in range(max_delay_samples + 1): + candidate = fit_delay_candidate(segment.measured_velocity, segment.control_torque, delay_samples) + if candidate is None: + continue + + if candidate.physically_plausible: + if best_result is None or compare_candidates(candidate, best_result): + best_result = candidate + elif best_unphysical is None or compare_candidates(candidate, best_unphysical): + best_unphysical = candidate + + if best_result is not None: + return best_result + if allow_unphysical and best_unphysical is not None: + return best_unphysical + if best_unphysical is not None: + raise ValueError( + f"{segment.wheel_id}/run{segment.run_id}: only unphysical candidates found; " + "rerun with --allow-unphysical to inspect them" + ) + raise ValueError(f"{segment.wheel_id}/run{segment.run_id}: no candidate could be fitted") + + +def fit_delay_candidate(y: np.ndarray, u: np.ndarray, delay_samples: int) -> CandidateResult | None: + sample_count = y.size + if sample_count - delay_samples < 3: + return None + + k = np.arange(delay_samples, sample_count - 1, dtype=np.int64) + if k.size < 3: + return None + + x = np.column_stack((y[k], u[k - delay_samples], np.ones(k.size, dtype=np.float64))) + target = y[k + 1] + + theta, *_ = np.linalg.lstsq(x, target, rcond=None) + a, b, c = (float(theta[0]), float(theta[1]), float(theta[2])) + if not all(math.isfinite(value) for value in (a, b, c)): + return None + + one_step = x @ theta + one_step_rmse = rmse(target, one_step) + + predicted = np.zeros_like(y) + predicted[0] = y[0] + for index in range(sample_count - 1): + delayed_u = u[index - delay_samples] if index >= delay_samples else 0.0 + predicted[index + 1] = a * predicted[index] + b * delayed_u + c + + actual_tail = y[1:] + predicted_tail = predicted[1:] + if actual_tail.size == 0: + return None + + rollout_rmse = rmse(actual_tail, predicted_tail) + mae = float(np.mean(np.abs(actual_tail - predicted_tail))) + fit_percent = normalized_fit_percent(actual_tail, predicted_tail) + physically_plausible = (0.0 < a < 1.0) and (b > 0.0) + + return CandidateResult( + delay_samples=delay_samples, + a=a, + b=b, + c=c, + rollout_rmse=rollout_rmse, + one_step_rmse=one_step_rmse, + mae=mae, + fit_percent=fit_percent, + physically_plausible=physically_plausible, + predicted_velocity=predicted, + ) + + +def compare_candidates(left: CandidateResult, right: CandidateResult) -> bool: + if left.rollout_rmse < right.rollout_rmse - 1e-9: + return True + if math.isclose(left.rollout_rmse, right.rollout_rmse, rel_tol=0.0, abs_tol=1e-9): + if left.fit_percent > right.fit_percent + 1e-9: + return True + if math.isclose(left.fit_percent, right.fit_percent, rel_tol=0.0, abs_tol=1e-9): + return left.delay_samples < right.delay_samples + return False + + +def rmse(actual: np.ndarray, predicted: np.ndarray) -> float: + return float(np.sqrt(np.mean(np.square(actual - predicted)))) + + +def normalized_fit_percent(actual: np.ndarray, predicted: np.ndarray) -> float: + centered = actual - np.mean(actual) + denominator = float(np.linalg.norm(centered)) + if denominator <= 0.0: + return 0.0 + numerator = float(np.linalg.norm(actual - predicted)) + return 100.0 * max(0.0, 1.0 - numerator / denominator) + + +def continuous_params(candidate: CandidateResult, dt_s: float) -> tuple[float | None, float | None, float | None]: + a = candidate.a + if not (0.0 < a < 1.0): + return None, None, None + + one_minus_a = 1.0 - a + if one_minus_a <= 0.0: + return None, None, None + + time_constant_s = -dt_s / math.log(a) + dc_gain = candidate.b / one_minus_a + velocity_bias = candidate.c / one_minus_a + return time_constant_s, dc_gain, velocity_bias + + +def sanitize_token(value: str) -> str: + sanitized = re.sub(r"[^A-Za-z0-9_.-]+", "_", value.strip()) + return sanitized or "unknown" + + +def write_prediction_csv( + output_path: Path, + segment: Segment, + candidate: CandidateResult, +) -> None: + with output_path.open("w", newline="") as handle: + writer = csv.writer(handle) + writer.writerow( + [ + "timestamp_us", + "sample_idx", + "wheel_id", + "run_id", + "setpoint_velocity", + "measured_velocity", + "predicted_velocity", + "control_torque", + "residual", + ] + ) + residual = segment.measured_velocity - candidate.predicted_velocity + for index in range(segment.measured_velocity.size): + writer.writerow( + [ + int(segment.timestamp_us[index]), + int(segment.sample_idx[index]), + segment.wheel_id, + segment.run_id, + float(segment.setpoint_velocity[index]), + float(segment.measured_velocity[index]), + float(candidate.predicted_velocity[index]), + float(segment.control_torque[index]), + float(residual[index]), + ] + ) + + +def write_model_json( + output_path: Path, + segment: Segment, + candidate: CandidateResult, + time_constant_s: float | None, + dc_gain: float | None, + velocity_bias: float | None, + excitation_message: str, +) -> None: + payload = { + "wheel_id": segment.wheel_id, + "run_id": segment.run_id, + "sample_count": int(segment.measured_velocity.size), + "dropped_rows": segment.dropped_rows, + "missing_samples": segment.missing_samples, + "inferred_dt_s": segment.inferred_dt_s, + "sample_jitter_ratio": segment.sample_jitter_ratio, + "excitation_warning": excitation_message or None, + "model_type": "first_order_plus_dead_time_discrete", + "discrete": { + "a": candidate.a, + "b": candidate.b, + "c": candidate.c, + "delay_samples": candidate.delay_samples, + }, + "continuous": { + "time_constant_s": time_constant_s, + "dc_gain": dc_gain, + "velocity_bias": velocity_bias, + "delay_s": candidate.delay_samples * segment.inferred_dt_s, + }, + "fit": { + "rollout_rmse": candidate.rollout_rmse, + "one_step_rmse": candidate.one_step_rmse, + "mae": candidate.mae, + "fit_percent": candidate.fit_percent, + "physically_plausible": candidate.physically_plausible, + }, + } + + output_path.write_text(json.dumps(payload, indent=2, sort_keys=True) + "\n") + + +def write_summary_csv(output_path: Path, rows: list[dict[str, object]]) -> None: + fieldnames = [ + "wheel_id", + "run_id", + "status", + "message", + "sample_count", + "dropped_rows", + "missing_samples", + "inferred_dt_s", + "sample_jitter_ratio", + "speed_span", + "torque_span", + "delay_samples", + "delay_s", + "a", + "b", + "c", + "time_constant_s", + "dc_gain", + "velocity_bias", + "rollout_rmse", + "one_step_rmse", + "mae", + "fit_percent", + "model_json", + "prediction_csv", + ] + + with output_path.open("w", newline="") as handle: + writer = csv.DictWriter(handle, fieldnames=fieldnames) + writer.writeheader() + for row in rows: + writer.writerow(row) + + +def main() -> int: + args = parse_args() + + input_csv = Path(args.input_csv) + if not input_csv.is_file(): + print(f"input CSV does not exist: {input_csv}", file=sys.stderr) + return 2 + + fallback_dt_s = args.sample_time_ms / 1000.0 + if fallback_dt_s <= 0.0: + print("--sample-time-ms must be positive", file=sys.stderr) + return 2 + + output_dir = Path(args.output_dir) if args.output_dir else default_output_dir() + output_dir.mkdir(parents=True, exist_ok=True) + + try: + samples = load_samples(input_csv, args.wheel_id, args.run_id) + except Exception as exc: + print(f"failed to load CSV: {exc}", file=sys.stderr) + return 2 + + if not samples: + print("no samples matched the selected wheel/run filters", file=sys.stderr) + return 2 + + grouped = group_samples(samples) + summary_rows: list[dict[str, object]] = [] + success_count = 0 + + for (wheel_id, run_id), rows in sorted(grouped.items()): + base_summary = { + "wheel_id": wheel_id, + "run_id": run_id, + "model_json": "", + "prediction_csv": "", + } + + try: + segment = build_segment(wheel_id, run_id, rows, fallback_dt_s) + except Exception as exc: + summary_rows.append( + { + **base_summary, + "status": "error", + "message": str(exc), + } + ) + continue + + if segment.measured_velocity.size < args.min_samples: + summary_rows.append( + { + **base_summary, + "status": "skipped", + "message": ( + f"too few samples ({segment.measured_velocity.size} < {args.min_samples})" + ), + "sample_count": int(segment.measured_velocity.size), + "dropped_rows": segment.dropped_rows, + "missing_samples": segment.missing_samples, + "inferred_dt_s": segment.inferred_dt_s, + "sample_jitter_ratio": segment.sample_jitter_ratio, + "speed_span": percentile_span(segment.measured_velocity), + "torque_span": percentile_span(segment.control_torque), + } + ) + continue + + excitation_good, excitation_message = excitation_ok( + segment, + min_speed_span=args.min_speed_span, + min_torque_span=args.min_torque_span, + ) + if not excitation_good: + summary_rows.append( + { + **base_summary, + "status": "skipped", + "message": excitation_message, + "sample_count": int(segment.measured_velocity.size), + "dropped_rows": segment.dropped_rows, + "missing_samples": segment.missing_samples, + "inferred_dt_s": segment.inferred_dt_s, + "sample_jitter_ratio": segment.sample_jitter_ratio, + "speed_span": percentile_span(segment.measured_velocity), + "torque_span": percentile_span(segment.control_torque), + } + ) + continue + + if args.max_delay_samples is not None: + max_delay_samples = max(0, args.max_delay_samples) + else: + max_delay_samples = max(0, int(round((args.max_delay_ms / 1000.0) / segment.inferred_dt_s))) + + try: + candidate = estimate_segment( + segment=segment, + max_delay_samples=max_delay_samples, + allow_unphysical=args.allow_unphysical, + ) + except Exception as exc: + summary_rows.append( + { + **base_summary, + "status": "error", + "message": str(exc), + "sample_count": int(segment.measured_velocity.size), + "dropped_rows": segment.dropped_rows, + "missing_samples": segment.missing_samples, + "inferred_dt_s": segment.inferred_dt_s, + "sample_jitter_ratio": segment.sample_jitter_ratio, + "speed_span": percentile_span(segment.measured_velocity), + "torque_span": percentile_span(segment.control_torque), + } + ) + continue + + time_constant_s, dc_gain, velocity_bias = continuous_params(candidate, segment.inferred_dt_s) + + wheel_token = sanitize_token(wheel_id) + run_token = f"run_{run_id}" + model_json_path = output_dir / f"{wheel_token}_{run_token}_model.json" + prediction_csv_path = output_dir / f"{wheel_token}_{run_token}_fit.csv" + + write_model_json( + model_json_path, + segment, + candidate, + time_constant_s, + dc_gain, + velocity_bias, + excitation_message="" if excitation_good else excitation_message, + ) + write_prediction_csv(prediction_csv_path, segment, candidate) + + speed_span = percentile_span(segment.measured_velocity) + torque_span = percentile_span(segment.control_torque) + summary_rows.append( + { + **base_summary, + "status": "ok", + "message": "" if candidate.physically_plausible else "selected unphysical candidate", + "sample_count": int(segment.measured_velocity.size), + "dropped_rows": segment.dropped_rows, + "missing_samples": segment.missing_samples, + "inferred_dt_s": segment.inferred_dt_s, + "sample_jitter_ratio": segment.sample_jitter_ratio, + "speed_span": speed_span, + "torque_span": torque_span, + "delay_samples": candidate.delay_samples, + "delay_s": candidate.delay_samples * segment.inferred_dt_s, + "a": candidate.a, + "b": candidate.b, + "c": candidate.c, + "time_constant_s": time_constant_s, + "dc_gain": dc_gain, + "velocity_bias": velocity_bias, + "rollout_rmse": candidate.rollout_rmse, + "one_step_rmse": candidate.one_step_rmse, + "mae": candidate.mae, + "fit_percent": candidate.fit_percent, + "model_json": str(model_json_path), + "prediction_csv": str(prediction_csv_path), + } + ) + + success_count += 1 + log( + ( + f"[ok] {wheel_id}/run{run_id}: delay={candidate.delay_samples} samples, " + f"tau={time_constant_s if time_constant_s is not None else float('nan'):.6f}s, " + f"gain={dc_gain if dc_gain is not None else float('nan'):.6f}, " + f"fit={candidate.fit_percent:.2f}%" + ), + quiet=args.quiet, + ) + + summary_path = output_dir / "identification_summary.csv" + write_summary_csv(summary_path, summary_rows) + + if success_count == 0: + print(f"no segment was successfully identified; summary: {summary_path}", file=sys.stderr) + return 1 + + log(f"summary: {summary_path}", quiet=args.quiet) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/pid_tuning/tune_pid.py b/tools/pid_tuning/tune_pid.py new file mode 100644 index 00000000..6bd61a3a --- /dev/null +++ b/tools/pid_tuning/tune_pid.py @@ -0,0 +1,1280 @@ +#!/usr/bin/env python3 + +from __future__ import annotations + +import argparse +import csv +import json +import math +import random +import sys +from dataclasses import dataclass +from datetime import datetime, timezone +from pathlib import Path +from typing import Iterable + +import numpy as np + +from identify_plant import build_segment, group_samples, load_samples, sanitize_token + + +CURRENT_DEFAULT_KP = 0.003436926 +CURRENT_DEFAULT_KD = 0.009373434 + + +@dataclass(frozen=True) +class PlantModel: + wheel_id: str + run_id: int + dt_s: float + a: float + b: float + c: float + delay_samples: int + source_path: Path + fit_percent: float | None + + +@dataclass(frozen=True) +class TraceData: + wheel_id: str + run_id: int + dt_s: float + setpoint: np.ndarray + initial_velocity: float + observed_velocity: np.ndarray | None + observed_control: np.ndarray | None + source_name: str + + +@dataclass(frozen=True) +class SimulationResult: + velocity: np.ndarray + control: np.ndarray + diverged: bool + + +@dataclass(frozen=True) +class Metrics: + cost: float + mae_norm: float + rmse_norm: float + itae_norm: float + overshoot_norm: float + steady_error_norm: float + final_error_norm: float + settle_ratio: float + control_rms_norm: float + control_tv_norm: float + saturation_ratio: float + + +@dataclass(frozen=True) +class EvaluationRecord: + kp: float + kd: float + cost: float + mae_norm: float + rmse_norm: float + itae_norm: float + overshoot_norm: float + steady_error_norm: float + final_error_norm: float + settle_ratio: float + control_rms_norm: float + control_tv_norm: float + saturation_ratio: float + method: str + evaluation_index: int + + +@dataclass(frozen=True) +class TuningOutcome: + model: PlantModel + trace_count: int + output_min: float + output_max: float + best: EvaluationRecord + baseline: EvaluationRecord + best_simulation: SimulationResult + best_trace: TraceData + output_dir: Path + + +class ObjectiveEvaluator: + def __init__( + self, + model: PlantModel, + traces: list[TraceData], + output_min: float, + output_max: float, + settle_band_ratio: float, + quiet: bool, + ) -> None: + self.model = model + self.traces = traces + self.output_min = output_min + self.output_max = output_max + self.settle_band_ratio = settle_band_ratio + self.quiet = quiet + self.cache: dict[tuple[float, float], EvaluationRecord] = {} + self.records: list[EvaluationRecord] = [] + self.best_record: EvaluationRecord | None = None + + def evaluate(self, kp: float, kd: float, method: str) -> EvaluationRecord: + kp = float(kp) + kd = float(kd) + key = (round(kp, 15), round(kd, 15)) + cached = self.cache.get(key) + if cached is not None: + return cached + + trace_metrics: list[Metrics] = [] + for trace in self.traces: + simulation = simulate_closed_loop( + model=self.model, + trace=trace, + kp=kp, + kd=kd, + output_min=self.output_min, + output_max=self.output_max, + ) + metrics = compute_metrics( + trace=trace, + simulation=simulation, + settle_band_ratio=self.settle_band_ratio, + output_min=self.output_min, + output_max=self.output_max, + ) + trace_metrics.append(metrics) + + record = EvaluationRecord( + kp=kp, + kd=kd, + cost=mean_metric(trace_metrics, "cost"), + mae_norm=mean_metric(trace_metrics, "mae_norm"), + rmse_norm=mean_metric(trace_metrics, "rmse_norm"), + itae_norm=mean_metric(trace_metrics, "itae_norm"), + overshoot_norm=mean_metric(trace_metrics, "overshoot_norm"), + steady_error_norm=mean_metric(trace_metrics, "steady_error_norm"), + final_error_norm=mean_metric(trace_metrics, "final_error_norm"), + settle_ratio=mean_metric(trace_metrics, "settle_ratio"), + control_rms_norm=mean_metric(trace_metrics, "control_rms_norm"), + control_tv_norm=mean_metric(trace_metrics, "control_tv_norm"), + saturation_ratio=mean_metric(trace_metrics, "saturation_ratio"), + method=method, + evaluation_index=len(self.records), + ) + + self.records.append(record) + self.cache[key] = record + if self.best_record is None or record.cost < self.best_record.cost: + self.best_record = record + if not self.quiet: + print( + ( + f"[best] {self.model.wheel_id}/run{self.model.run_id}: " + f"kp={record.kp:.9g}, kd={record.kd:.9g}, cost={record.cost:.6f}" + ) + ) + return record + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Tune friction wheel velocity PID gains offline from identified plant models. " + "The discrete control law matches rmcs_core::controller::pid::PidCalculator " + "with ki fixed to 0." + ) + ) + parser.add_argument( + "model_input", + help="Either one *_model.json from identify_plant.py or identification_summary.csv", + ) + parser.add_argument( + "--trace-csv", + help=( + "Recorder CSV from FrictionWheelPidRecorder. If provided, the original setpoint trace " + "is reused for closed-loop evaluation." + ), + ) + parser.add_argument("--wheel-id", help="Only tune one wheel_id") + parser.add_argument("--run-id", type=int, help="Only tune one run_id") + parser.add_argument( + "--trace-scope", + choices=("exact", "wheel"), + default="exact", + help="How to match trace segments to a model when --trace-csv is provided", + ) + parser.add_argument( + "--sample-time-ms", + type=float, + default=1.0, + help="Fallback sample interval for trace CSV parsing when timestamps are invalid", + ) + parser.add_argument( + "--method", + choices=("hybrid", "ga", "bo"), + default="hybrid", + help="Optimization method", + ) + parser.add_argument("--kp-min", type=float, default=1e-6, help="Lower bound for kp") + parser.add_argument("--kp-max", type=float, default=1.0, help="Upper bound for kp") + parser.add_argument("--kd-min", type=float, default=1e-8, help="Lower bound for kd") + parser.add_argument("--kd-max", type=float, default=1.0, help="Upper bound for kd") + parser.add_argument( + "--seed-kp", + type=float, + default=CURRENT_DEFAULT_KP, + help="Initial/reference kp to compare against", + ) + parser.add_argument( + "--seed-kd", + type=float, + default=CURRENT_DEFAULT_KD, + help="Initial/reference kd to compare against", + ) + parser.add_argument( + "--ga-population", + type=int, + default=36, + help="GA population size", + ) + parser.add_argument( + "--ga-generations", + type=int, + default=20, + help="GA generation count", + ) + parser.add_argument( + "--bo-initial", + type=int, + default=12, + help="BO random warmup samples", + ) + parser.add_argument( + "--bo-iterations", + type=int, + default=24, + help="BO surrogate iterations", + ) + parser.add_argument( + "--random-seed", + type=int, + default=42, + help="Random seed for deterministic tuning runs", + ) + parser.add_argument( + "--output-limit", + type=float, + help="Symmetric control limit. Equivalent to --output-min=-x --output-max=x", + ) + parser.add_argument("--output-min", type=float, help="Lower control clamp") + parser.add_argument("--output-max", type=float, help="Upper control clamp") + parser.add_argument( + "--infer-output-percentile", + type=float, + default=99.5, + help="Percentile used to infer |control_torque| limit from --trace-csv", + ) + parser.add_argument( + "--estimated-output-factor", + type=float, + default=2.5, + help="Fallback multiplier when output limit is estimated from target speed and plant gain", + ) + parser.add_argument( + "--step-setpoint", + type=float, + help="Target speed for synthetic step evaluation when --trace-csv is not provided", + ) + parser.add_argument( + "--step-duration-ms", + type=float, + default=1200.0, + help="Synthetic step hold duration in milliseconds", + ) + parser.add_argument( + "--pre-roll-ms", + type=float, + default=30.0, + help="Synthetic zero-input lead-in before the step", + ) + parser.add_argument( + "--settle-band-ratio", + type=float, + default=0.05, + help="Relative settling band used by the objective", + ) + parser.add_argument( + "--output-dir", + help="Directory for tuning outputs. Default: /tmp/friction_pid_tuning/", + ) + parser.add_argument( + "--quiet", + action="store_true", + help="Suppress progress output", + ) + return parser.parse_args() + + +def default_output_dir() -> Path: + stamp = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ") + return Path("/tmp/friction_pid_tuning") / stamp + + +def load_models(model_input: Path, wheel_id: str | None, run_id: int | None) -> list[PlantModel]: + if not model_input.is_file(): + raise ValueError(f"model input does not exist: {model_input}") + + if model_input.suffix.lower() == ".json": + models = [load_model_json(model_input)] + else: + models = load_models_from_summary(model_input) + + filtered = [ + model + for model in models + if (wheel_id is None or model.wheel_id == wheel_id) + and (run_id is None or model.run_id == run_id) + ] + if not filtered: + raise ValueError("no model matched the selected wheel/run filters") + return filtered + + +def load_model_json(path: Path) -> PlantModel: + payload = json.loads(path.read_text()) + discrete = payload["discrete"] + fit = payload.get("fit", {}) + dt_s = float(payload["inferred_dt_s"]) + if not math.isfinite(dt_s) or dt_s <= 0.0: + raise ValueError(f"{path}: invalid inferred_dt_s") + + return PlantModel( + wheel_id=str(payload["wheel_id"]), + run_id=int(payload["run_id"]), + dt_s=dt_s, + a=float(discrete["a"]), + b=float(discrete["b"]), + c=float(discrete["c"]), + delay_samples=int(discrete["delay_samples"]), + source_path=path, + fit_percent=float(fit["fit_percent"]) if "fit_percent" in fit else None, + ) + + +def load_models_from_summary(path: Path) -> list[PlantModel]: + models: list[PlantModel] = [] + with path.open("r", newline="") as handle: + reader = csv.DictReader(handle) + for row in reader: + if row.get("status") != "ok": + continue + model_json = (row.get("model_json") or "").strip() + if not model_json: + continue + model_path = Path(model_json) + if not model_path.is_absolute(): + model_path = (path.parent / model_path).resolve() + models.append(load_model_json(model_path)) + + if not models: + raise ValueError(f"{path}: no usable model_json rows found in summary") + return models + + +def load_trace_map(trace_csv: Path, fallback_dt_s: float) -> dict[tuple[str, int], TraceData]: + samples = load_samples(trace_csv, None, None) + grouped = group_samples(samples) + trace_map: dict[tuple[str, int], TraceData] = {} + for (wheel_id, run_id), rows in grouped.items(): + segment = build_segment(wheel_id, run_id, rows, fallback_dt_s) + trace_map[(wheel_id, run_id)] = TraceData( + wheel_id=wheel_id, + run_id=run_id, + dt_s=segment.inferred_dt_s, + setpoint=segment.setpoint_velocity.copy(), + initial_velocity=float(segment.measured_velocity[0]) if segment.measured_velocity.size else 0.0, + observed_velocity=segment.measured_velocity.copy(), + observed_control=segment.control_torque.copy(), + source_name=f"{trace_csv}:{wheel_id}/run{run_id}", + ) + return trace_map + + +def select_traces( + model: PlantModel, + trace_map: dict[tuple[str, int], TraceData] | None, + trace_scope: str, + step_setpoint: float | None, + step_duration_ms: float, + pre_roll_ms: float, +) -> list[TraceData]: + if trace_map: + exact = trace_map.get((model.wheel_id, model.run_id)) + if exact is not None: + return [exact] + if trace_scope == "wheel": + same_wheel = [ + trace + for trace in trace_map.values() + if trace.wheel_id == model.wheel_id + ] + if same_wheel: + return sorted(same_wheel, key=lambda trace: trace.run_id) + raise ValueError( + f"no trace matched {model.wheel_id}/run{model.run_id}; " + "provide the corresponding recorder CSV or switch to synthetic step mode" + ) + + if step_setpoint is None: + raise ValueError( + f"{model.wheel_id}/run{model.run_id}: --step-setpoint is required when --trace-csv is absent" + ) + return [make_step_trace(model, step_setpoint, step_duration_ms, pre_roll_ms)] + + +def make_step_trace( + model: PlantModel, + step_setpoint: float, + step_duration_ms: float, + pre_roll_ms: float, +) -> TraceData: + pre_samples = max(1, int(round((pre_roll_ms / 1000.0) / model.dt_s))) + hold_samples = max(10, int(round((step_duration_ms / 1000.0) / model.dt_s))) + total = pre_samples + hold_samples + setpoint = np.zeros(total, dtype=np.float64) + setpoint[pre_samples:] = step_setpoint + + return TraceData( + wheel_id=model.wheel_id, + run_id=model.run_id, + dt_s=model.dt_s, + setpoint=setpoint, + initial_velocity=0.0, + observed_velocity=None, + observed_control=None, + source_name=f"synthetic_step:{model.wheel_id}/run{model.run_id}", + ) + + +def infer_output_bounds( + model: PlantModel, + traces: list[TraceData], + output_limit: float | None, + output_min: float | None, + output_max: float | None, + infer_output_percentile: float, + estimated_output_factor: float, +) -> tuple[float, float]: + if output_limit is not None: + limit = abs(output_limit) + if limit <= 0.0: + raise ValueError("--output-limit must be positive") + return -limit, limit + + if output_min is not None or output_max is not None: + if output_min is None or output_max is None: + raise ValueError("--output-min and --output-max must be set together") + if output_min >= output_max: + raise ValueError("--output-min must be smaller than --output-max") + return float(output_min), float(output_max) + + observed_controls = [ + trace.observed_control + for trace in traces + if trace.observed_control is not None and trace.observed_control.size > 0 + ] + if observed_controls: + merged = np.concatenate(observed_controls) + limit = float(np.percentile(np.abs(merged), infer_output_percentile)) + limit = max(limit, 1e-6) + return -limit, limit + + dc_gain = plant_dc_gain(model) + target_mag = max(trace_target_magnitude(trace) for trace in traces) + if dc_gain is None or abs(dc_gain) < 1e-9: + estimated_limit = 10.0 + else: + estimated_hold = target_mag / abs(dc_gain) + estimated_limit = max(estimated_output_factor * estimated_hold, 1.0) + return -estimated_limit, estimated_limit + + +def plant_dc_gain(model: PlantModel) -> float | None: + denominator = 1.0 - model.a + if abs(denominator) < 1e-12: + return None + return model.b / denominator + + +def trace_target_magnitude(trace: TraceData) -> float: + if trace.setpoint.size == 0: + return 1.0 + return max(float(np.percentile(np.abs(trace.setpoint), 95)), 1.0) + + +def simulate_closed_loop( + model: PlantModel, + trace: TraceData, + kp: float, + kd: float, + output_min: float, + output_max: float, +) -> SimulationResult: + sample_count = trace.setpoint.size + velocity = np.zeros(sample_count, dtype=np.float64) + control = np.zeros(sample_count, dtype=np.float64) + velocity[0] = trace.initial_velocity + + diverged = False + last_err = math.nan + divergence_limit = max(50.0 * trace_target_magnitude(trace), 1e6) + + for index in range(sample_count): + err = float(trace.setpoint[index] - velocity[index]) + command = kp * err + if not math.isnan(last_err): + command += kd * (err - last_err) + last_err = err + command = min(max(command, output_min), output_max) + control[index] = command + + if index + 1 >= sample_count: + continue + + delayed_u = control[index - model.delay_samples] if index >= model.delay_samples else 0.0 + next_velocity = model.a * velocity[index] + model.b * delayed_u + model.c + if not math.isfinite(next_velocity) or abs(next_velocity) > divergence_limit: + diverged = True + velocity[index + 1 :] = divergence_limit * np.sign(next_velocity if math.isfinite(next_velocity) else 1.0) + break + velocity[index + 1] = next_velocity + + return SimulationResult(velocity=velocity, control=control, diverged=diverged) + + +def compute_metrics( + trace: TraceData, + simulation: SimulationResult, + settle_band_ratio: float, + output_min: float, + output_max: float, +) -> Metrics: + setpoint = trace.setpoint + velocity = simulation.velocity + control = simulation.control + scale = trace_target_magnitude(trace) + error = setpoint - velocity + normalized_error = error / scale + + mae_norm = float(np.mean(np.abs(normalized_error))) + rmse_norm = float(np.sqrt(np.mean(np.square(normalized_error)))) + + time_vector = np.linspace(0.0, 1.0, setpoint.size, endpoint=True) + itae_norm = float(np.mean(time_vector * np.abs(normalized_error))) + + active_mask = np.abs(setpoint) >= 0.1 * scale + if not np.any(active_mask): + active_mask = np.ones(setpoint.size, dtype=bool) + active_indices = np.where(active_mask)[0] + active_start = int(active_indices[0]) + + plateau_mask = active_mask & ( + np.abs(np.diff(setpoint, prepend=setpoint[0])) <= 0.005 * scale + ) + if np.count_nonzero(plateau_mask) < max(5, setpoint.size // 20): + plateau_mask = np.zeros(setpoint.size, dtype=bool) + plateau_mask[max(active_start, int(setpoint.size * 0.8)) :] = True + plateau_indices = np.where(plateau_mask)[0] + + if plateau_indices.size > 0: + steady_error_norm = float(np.mean(np.abs(normalized_error[plateau_indices]))) + overshoot_norm = ( + float(np.max(np.maximum(0.0, np.abs(velocity[plateau_indices]) - np.abs(setpoint[plateau_indices])))) + / scale + ) + else: + steady_error_norm = float(np.mean(np.abs(normalized_error[-max(1, setpoint.size // 10) :]))) + overshoot_norm = ( + float(np.max(np.maximum(0.0, np.abs(velocity[active_mask]) - np.abs(setpoint[active_mask])))) + / scale + ) + + band = np.maximum(settle_band_ratio * np.abs(setpoint), settle_band_ratio * scale) + outside = np.where(active_mask & (np.abs(error) > band))[0] + if outside.size == 0: + settle_ratio = 0.0 + else: + denominator = max(1, (setpoint.size - 1) - active_start) + settle_ratio = float((outside[-1] - active_start) / denominator) + + final_error_norm = float(abs(error[-1]) / scale) + + finite_limits = math.isfinite(output_min) and math.isfinite(output_max) + if finite_limits: + control_scale = max(abs(output_min), abs(output_max), 1.0) + saturation_margin = 0.01 * control_scale + saturation_ratio = float( + np.mean((control <= output_min + saturation_margin) | (control >= output_max - saturation_margin)) + ) + else: + control_scale = max(float(np.percentile(np.abs(control), 95)), 1.0) + saturation_ratio = 0.0 + + control_rms_norm = float(np.sqrt(np.mean(np.square(control / control_scale)))) + if control.size >= 2: + control_tv_norm = float(np.mean(np.abs(np.diff(control))) / control_scale) + else: + control_tv_norm = 0.0 + + divergence_penalty = 50.0 if simulation.diverged else 0.0 + cost = ( + 4.0 * mae_norm + + 2.5 * rmse_norm + + 2.0 * itae_norm + + 3.5 * overshoot_norm + + 2.5 * steady_error_norm + + 2.0 * final_error_norm + + 1.2 * settle_ratio + + 0.25 * control_rms_norm + + 0.6 * control_tv_norm + + 1.5 * saturation_ratio + + divergence_penalty + ) + + return Metrics( + cost=cost, + mae_norm=mae_norm, + rmse_norm=rmse_norm, + itae_norm=itae_norm, + overshoot_norm=overshoot_norm, + steady_error_norm=steady_error_norm, + final_error_norm=final_error_norm, + settle_ratio=settle_ratio, + control_rms_norm=control_rms_norm, + control_tv_norm=control_tv_norm, + saturation_ratio=saturation_ratio, + ) + + +def mean_metric(metrics: Iterable[Metrics], field_name: str) -> float: + values = [getattr(metric, field_name) for metric in metrics] + return float(sum(values) / len(values)) + + +def encode_params(kp: float, kd: float) -> np.ndarray: + return np.asarray([math.log10(kp), math.log10(kd)], dtype=np.float64) + + +def decode_params(encoded: np.ndarray) -> tuple[float, float]: + return 10.0 ** float(encoded[0]), 10.0 ** float(encoded[1]) + + +def sample_encoded(bounds: tuple[np.ndarray, np.ndarray], rng: random.Random) -> np.ndarray: + lower, upper = bounds + return np.asarray( + [rng.uniform(float(lower[0]), float(upper[0])), rng.uniform(float(lower[1]), float(upper[1]))], + dtype=np.float64, + ) + + +def clamp_encoded(encoded: np.ndarray, bounds: tuple[np.ndarray, np.ndarray]) -> np.ndarray: + lower, upper = bounds + return np.minimum(np.maximum(encoded, lower), upper) + + +def build_seed_points( + bounds: tuple[np.ndarray, np.ndarray], + seed_kp: float, + seed_kd: float, + model: PlantModel, + traces: list[TraceData], + output_min: float, + output_max: float, +) -> list[np.ndarray]: + lower, upper = bounds + seeds: list[np.ndarray] = [] + + def append_point(kp: float, kd: float) -> None: + kp = min(max(kp, 10.0 ** float(lower[0])), 10.0 ** float(upper[0])) + kd = min(max(kd, 10.0 ** float(lower[1])), 10.0 ** float(upper[1])) + encoded = encode_params(kp, kd) + for existing in seeds: + if np.allclose(existing, encoded, atol=1e-12, rtol=0.0): + return + seeds.append(encoded) + + append_point(seed_kp, seed_kd) + append_point(seed_kp * 0.5, seed_kd * 0.5) + append_point(seed_kp * 2.0, seed_kd * 2.0) + append_point(seed_kp * 0.5, seed_kd * 2.0) + append_point(seed_kp * 2.0, max(seed_kd * 0.5, 1e-12)) + + dc_gain = plant_dc_gain(model) + target_mag = max(trace_target_magnitude(trace) for trace in traces) + output_limit = max(abs(output_min), abs(output_max), 1.0) + if dc_gain is not None and abs(dc_gain) > 1e-9: + proportional_guess = min(max(0.35 * output_limit / target_mag, 1e-12), 10.0 ** float(upper[0])) + derivative_guess = min(max(proportional_guess * max(1.0, model.delay_samples), 1e-12), 10.0 ** float(upper[1])) + append_point(proportional_guess, derivative_guess) + append_point(0.75 * proportional_guess, 0.5 * derivative_guess) + + append_point(10.0 ** float(lower[0]), 10.0 ** float(lower[1])) + return seeds + + +def run_ga( + evaluator: ObjectiveEvaluator, + bounds: tuple[np.ndarray, np.ndarray], + population_size: int, + generations: int, + seed_points: list[np.ndarray], + rng: random.Random, + quiet: bool, +) -> None: + lower, upper = bounds + population: list[np.ndarray] = [point.copy() for point in seed_points[:population_size]] + while len(population) < population_size: + population.append(sample_encoded(bounds, rng)) + + for generation in range(generations): + scored: list[tuple[EvaluationRecord, np.ndarray]] = [] + for encoded in population: + kp, kd = decode_params(encoded) + scored.append((evaluator.evaluate(kp, kd, method="ga"), encoded)) + scored.sort(key=lambda item: item[0].cost) + + if not quiet: + best_record = scored[0][0] + print( + ( + f"[ga] generation={generation + 1}/{generations} " + f"best_cost={best_record.cost:.6f} " + f"kp={best_record.kp:.9g} kd={best_record.kd:.9g}" + ) + ) + + elite_count = max(2, population_size // 5) + survivors = [encoded.copy() for _, encoded in scored[:elite_count]] + + next_population: list[np.ndarray] = survivors[:] + mutation_sigma = max(0.03, 0.18 * (1.0 - generation / max(1, generations))) + tournament_pool = [encoded for _, encoded in scored[: max(elite_count * 2, 4)]] + + while len(next_population) < population_size: + parent_a = rng.choice(tournament_pool) + parent_b = rng.choice(tournament_pool) + alpha = rng.uniform(0.2, 0.8) + child = alpha * parent_a + (1.0 - alpha) * parent_b + if rng.random() < 0.85: + child = child + np.asarray( + [rng.gauss(0.0, mutation_sigma), rng.gauss(0.0, mutation_sigma)], + dtype=np.float64, + ) + if rng.random() < 0.12: + child = sample_encoded(bounds, rng) + child = clamp_encoded(child, bounds) + next_population.append(child) + + population = next_population + + # Evaluate the final population once more so the final offspring enter the archive. + for encoded in population: + kp, kd = decode_params(encoded) + evaluator.evaluate(kp, kd, method="ga") + + +def kernel_rbf(x1: np.ndarray, x2: np.ndarray, length_scale: float) -> np.ndarray: + diff = x1[:, None, :] - x2[None, :, :] + dist_sq = np.sum(np.square(diff), axis=2) + return np.exp(-0.5 * dist_sq / (length_scale * length_scale)) + + +def normalize_encoded(encoded: np.ndarray, bounds: tuple[np.ndarray, np.ndarray]) -> np.ndarray: + lower, upper = bounds + return (encoded - lower) / np.maximum(upper - lower, 1e-12) + + +def run_bo( + evaluator: ObjectiveEvaluator, + bounds: tuple[np.ndarray, np.ndarray], + initial_samples: int, + iterations: int, + seed_points: list[np.ndarray], + rng: random.Random, + quiet: bool, +) -> None: + archive_encoded: list[np.ndarray] = [] + archive_costs: list[float] = [] + + def register_point(encoded: np.ndarray, method: str) -> None: + kp, kd = decode_params(encoded) + record = evaluator.evaluate(kp, kd, method=method) + archive_encoded.append(encoded.copy()) + archive_costs.append(record.cost) + + seen: set[tuple[float, float]] = set() + + def add_if_new(encoded: np.ndarray, method: str) -> None: + key = (round(float(encoded[0]), 12), round(float(encoded[1]), 12)) + if key in seen: + return + seen.add(key) + register_point(encoded, method) + + for encoded in seed_points[:initial_samples]: + add_if_new(clamp_encoded(encoded, bounds), "bo") + + while len(archive_encoded) < initial_samples: + add_if_new(sample_encoded(bounds, rng), "bo") + + for iteration in range(iterations): + x_train = np.asarray(archive_encoded, dtype=np.float64) + y_train = np.asarray(archive_costs, dtype=np.float64) + x_norm = normalize_encoded(x_train, bounds) + + length_scale = 0.22 + jitter = 1e-6 + k_xx = kernel_rbf(x_norm, x_norm, length_scale) + jitter * np.eye(x_norm.shape[0]) + centered = y_train - np.mean(y_train) + alpha = np.linalg.solve(k_xx, centered) + inverse_k = np.linalg.inv(k_xx) + + candidate_pool = [sample_encoded(bounds, rng) for _ in range(2048)] + if evaluator.best_record is not None: + best_encoded = encode_params(evaluator.best_record.kp, evaluator.best_record.kd) + for _ in range(256): + local = best_encoded + np.asarray( + [rng.gauss(0.0, 0.06), rng.gauss(0.0, 0.06)], + dtype=np.float64, + ) + candidate_pool.append(clamp_encoded(local, bounds)) + + candidate_matrix = np.asarray(candidate_pool, dtype=np.float64) + candidate_norm = normalize_encoded(candidate_matrix, bounds) + + k_star = kernel_rbf(x_norm, candidate_norm, length_scale) + mean = np.mean(y_train) + k_star.T @ alpha + variance = np.maximum(0.0, 1.0 - np.sum((k_star.T @ inverse_k) * k_star.T, axis=1)) + sigma = np.sqrt(variance) + beta = max(0.35, 1.75 - 1.2 * (iteration / max(1, iterations))) + acquisition = mean - beta * sigma + + ordering = np.argsort(acquisition) + chosen: np.ndarray | None = None + for idx in ordering: + encoded = candidate_matrix[int(idx)] + key = (round(float(encoded[0]), 12), round(float(encoded[1]), 12)) + if key not in seen: + chosen = encoded + break + if chosen is None: + chosen = sample_encoded(bounds, rng) + + add_if_new(chosen, "bo") + + if not quiet and evaluator.best_record is not None: + best = evaluator.best_record + print( + ( + f"[bo] iteration={iteration + 1}/{iterations} " + f"best_cost={best.cost:.6f} " + f"kp={best.kp:.9g} kd={best.kd:.9g}" + ) + ) + + +def tune_one_model( + model: PlantModel, + traces: list[TraceData], + args: argparse.Namespace, + rng: random.Random, + output_dir: Path, +) -> TuningOutcome: + output_min, output_max = infer_output_bounds( + model=model, + traces=traces, + output_limit=args.output_limit, + output_min=args.output_min, + output_max=args.output_max, + infer_output_percentile=args.infer_output_percentile, + estimated_output_factor=args.estimated_output_factor, + ) + + if args.kp_min <= 0.0 or args.kd_min <= 0.0: + raise ValueError("--kp-min and --kd-min must be strictly positive for log-scale search") + if args.kp_min >= args.kp_max or args.kd_min >= args.kd_max: + raise ValueError("kp/kd bounds must satisfy min < max") + + bounds = ( + np.asarray([math.log10(args.kp_min), math.log10(args.kd_min)], dtype=np.float64), + np.asarray([math.log10(args.kp_max), math.log10(args.kd_max)], dtype=np.float64), + ) + + evaluator = ObjectiveEvaluator( + model=model, + traces=traces, + output_min=output_min, + output_max=output_max, + settle_band_ratio=args.settle_band_ratio, + quiet=args.quiet, + ) + + seeds = build_seed_points( + bounds=bounds, + seed_kp=args.seed_kp, + seed_kd=args.seed_kd, + model=model, + traces=traces, + output_min=output_min, + output_max=output_max, + ) + + baseline = evaluator.evaluate(args.seed_kp, args.seed_kd, method="baseline") + + if args.method in {"ga", "hybrid"}: + run_ga( + evaluator=evaluator, + bounds=bounds, + population_size=args.ga_population, + generations=args.ga_generations, + seed_points=seeds, + rng=rng, + quiet=args.quiet, + ) + + if args.method in {"bo", "hybrid"}: + if args.method == "bo": + bo_seeds = seeds + else: + sorted_records = sorted(evaluator.records, key=lambda record: record.cost) + bo_seeds = [encode_params(record.kp, record.kd) for record in sorted_records[: max(8, args.bo_initial)]] + bo_seeds.extend(seeds) + + run_bo( + evaluator=evaluator, + bounds=bounds, + initial_samples=args.bo_initial, + iterations=args.bo_iterations, + seed_points=bo_seeds, + rng=rng, + quiet=args.quiet, + ) + + if evaluator.best_record is None: + raise RuntimeError(f"{model.wheel_id}/run{model.run_id}: no candidate was evaluated") + + best = evaluator.best_record + best_trace = traces[0] + best_simulation = simulate_closed_loop( + model=model, + trace=best_trace, + kp=best.kp, + kd=best.kd, + output_min=output_min, + output_max=output_max, + ) + + tune_output_dir = output_dir / f"{sanitize_token(model.wheel_id)}_run_{model.run_id}" + tune_output_dir.mkdir(parents=True, exist_ok=True) + + write_candidates_csv(tune_output_dir / "candidates.csv", evaluator.records) + write_response_csv( + tune_output_dir / "best_response.csv", + trace=best_trace, + simulation=best_simulation, + output_min=output_min, + output_max=output_max, + ) + write_tuning_json( + tune_output_dir / "tuning_result.json", + model=model, + trace_count=len(traces), + output_min=output_min, + output_max=output_max, + baseline=baseline, + best=best, + method=args.method, + ) + + return TuningOutcome( + model=model, + trace_count=len(traces), + output_min=output_min, + output_max=output_max, + best=best, + baseline=baseline, + best_simulation=best_simulation, + best_trace=best_trace, + output_dir=tune_output_dir, + ) + + +def write_candidates_csv(output_path: Path, records: list[EvaluationRecord]) -> None: + fieldnames = [ + "evaluation_index", + "method", + "kp", + "kd", + "cost", + "mae_norm", + "rmse_norm", + "itae_norm", + "overshoot_norm", + "steady_error_norm", + "final_error_norm", + "settle_ratio", + "control_rms_norm", + "control_tv_norm", + "saturation_ratio", + ] + ordered = sorted(records, key=lambda record: (record.cost, record.evaluation_index)) + with output_path.open("w", newline="") as handle: + writer = csv.DictWriter(handle, fieldnames=fieldnames) + writer.writeheader() + for record in ordered: + writer.writerow( + { + "evaluation_index": record.evaluation_index, + "method": record.method, + "kp": record.kp, + "kd": record.kd, + "cost": record.cost, + "mae_norm": record.mae_norm, + "rmse_norm": record.rmse_norm, + "itae_norm": record.itae_norm, + "overshoot_norm": record.overshoot_norm, + "steady_error_norm": record.steady_error_norm, + "final_error_norm": record.final_error_norm, + "settle_ratio": record.settle_ratio, + "control_rms_norm": record.control_rms_norm, + "control_tv_norm": record.control_tv_norm, + "saturation_ratio": record.saturation_ratio, + } + ) + + +def write_response_csv( + output_path: Path, + trace: TraceData, + simulation: SimulationResult, + output_min: float, + output_max: float, +) -> None: + saturation_margin = 0.01 * max(abs(output_min), abs(output_max), 1.0) + with output_path.open("w", newline="") as handle: + writer = csv.writer(handle) + writer.writerow( + [ + "sample_idx", + "time_s", + "setpoint_velocity", + "simulated_velocity", + "simulated_control", + "saturated", + "observed_velocity", + "observed_control", + ] + ) + for index in range(trace.setpoint.size): + simulated_control = float(simulation.control[index]) + saturated = int( + (simulated_control <= output_min + saturation_margin) + or (simulated_control >= output_max - saturation_margin) + ) + writer.writerow( + [ + index, + index * trace.dt_s, + float(trace.setpoint[index]), + float(simulation.velocity[index]), + simulated_control, + saturated, + "" if trace.observed_velocity is None else float(trace.observed_velocity[index]), + "" if trace.observed_control is None else float(trace.observed_control[index]), + ] + ) + + +def write_tuning_json( + output_path: Path, + model: PlantModel, + trace_count: int, + output_min: float, + output_max: float, + baseline: EvaluationRecord, + best: EvaluationRecord, + method: str, +) -> None: + payload = { + "wheel_id": model.wheel_id, + "run_id": model.run_id, + "trace_count": trace_count, + "method": method, + "ki": 0.0, + "output_min": output_min, + "output_max": output_max, + "plant_model": { + "source_path": str(model.source_path), + "dt_s": model.dt_s, + "a": model.a, + "b": model.b, + "c": model.c, + "delay_samples": model.delay_samples, + "fit_percent": model.fit_percent, + }, + "baseline": record_to_dict(baseline), + "best": record_to_dict(best), + "improvement": { + "absolute_cost_reduction": baseline.cost - best.cost, + "relative_cost_reduction": 0.0 if baseline.cost == 0.0 else (baseline.cost - best.cost) / baseline.cost, + }, + } + output_path.write_text(json.dumps(payload, indent=2, sort_keys=True) + "\n") + + +def record_to_dict(record: EvaluationRecord) -> dict[str, float | str | int]: + return { + "kp": record.kp, + "ki": 0.0, + "kd": record.kd, + "cost": record.cost, + "mae_norm": record.mae_norm, + "rmse_norm": record.rmse_norm, + "itae_norm": record.itae_norm, + "overshoot_norm": record.overshoot_norm, + "steady_error_norm": record.steady_error_norm, + "final_error_norm": record.final_error_norm, + "settle_ratio": record.settle_ratio, + "control_rms_norm": record.control_rms_norm, + "control_tv_norm": record.control_tv_norm, + "saturation_ratio": record.saturation_ratio, + "method": record.method, + "evaluation_index": record.evaluation_index, + } + + +def write_summary_csv(output_path: Path, outcomes: list[TuningOutcome]) -> None: + fieldnames = [ + "wheel_id", + "run_id", + "trace_count", + "method", + "seed_kp", + "seed_kd", + "best_kp", + "best_kd", + "baseline_cost", + "best_cost", + "absolute_cost_reduction", + "relative_cost_reduction", + "output_min", + "output_max", + "result_dir", + ] + with output_path.open("w", newline="") as handle: + writer = csv.DictWriter(handle, fieldnames=fieldnames) + writer.writeheader() + for outcome in outcomes: + baseline = outcome.baseline + best = outcome.best + writer.writerow( + { + "wheel_id": outcome.model.wheel_id, + "run_id": outcome.model.run_id, + "trace_count": outcome.trace_count, + "method": best.method, + "seed_kp": baseline.kp, + "seed_kd": baseline.kd, + "best_kp": best.kp, + "best_kd": best.kd, + "baseline_cost": baseline.cost, + "best_cost": best.cost, + "absolute_cost_reduction": baseline.cost - best.cost, + "relative_cost_reduction": 0.0 if baseline.cost == 0.0 else (baseline.cost - best.cost) / baseline.cost, + "output_min": outcome.output_min, + "output_max": outcome.output_max, + "result_dir": str(outcome.output_dir), + } + ) + + +def main() -> int: + args = parse_args() + model_input = Path(args.model_input) + output_dir = Path(args.output_dir) if args.output_dir else default_output_dir() + output_dir.mkdir(parents=True, exist_ok=True) + + if args.sample_time_ms <= 0.0: + print("--sample-time-ms must be positive", file=sys.stderr) + return 2 + if args.step_duration_ms <= 0.0 or args.pre_roll_ms < 0.0: + print("--step-duration-ms must be positive and --pre-roll-ms must be non-negative", file=sys.stderr) + return 2 + + try: + models = load_models(model_input, args.wheel_id, args.run_id) + except Exception as exc: + print(f"failed to load model input: {exc}", file=sys.stderr) + return 2 + + trace_map = None + if args.trace_csv: + try: + trace_map = load_trace_map(Path(args.trace_csv), args.sample_time_ms / 1000.0) + except Exception as exc: + print(f"failed to load trace CSV: {exc}", file=sys.stderr) + return 2 + + rng = random.Random(args.random_seed) + outcomes: list[TuningOutcome] = [] + + for model in models: + try: + traces = select_traces( + model=model, + trace_map=trace_map, + trace_scope=args.trace_scope, + step_setpoint=args.step_setpoint, + step_duration_ms=args.step_duration_ms, + pre_roll_ms=args.pre_roll_ms, + ) + outcome = tune_one_model( + model=model, + traces=traces, + args=args, + rng=rng, + output_dir=output_dir, + ) + except Exception as exc: + print(f"[error] {model.wheel_id}/run{model.run_id}: {exc}", file=sys.stderr) + continue + + outcomes.append(outcome) + if not args.quiet: + print( + ( + f"[ok] {model.wheel_id}/run{model.run_id}: " + f"kp={outcome.best.kp:.9g}, kd={outcome.best.kd:.9g}, " + f"cost={outcome.best.cost:.6f}, " + f"baseline={outcome.baseline.cost:.6f}, " + f"dir={outcome.output_dir}" + ) + ) + + if not outcomes: + print("no model was successfully tuned", file=sys.stderr) + return 1 + + summary_path = output_dir / "tuning_summary.csv" + write_summary_csv(summary_path, outcomes) + if not args.quiet: + print(f"summary: {summary_path}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main())