diff --git a/.gitmodules b/.gitmodules index de2c6a1cb..70f46ddc6 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "rmcs_ws/src/rmcs_auto_aim_v2"] path = rmcs_ws/src/rmcs_auto_aim_v2 url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git +[submodule "rmcs_ws/src/rmcs_rl"] + path = rmcs_ws/src/rmcs_rl + url = https://github.com/Alliance-Algorithm/rmcs_rl.git diff --git a/README.md b/README.md index 75dd7ff67..5366e4503 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,8 @@ RoboMaster Control System based on ROS2. 快速开始: [quick-start](https://github.com/Alliance-Algorithm/RMCS/wiki/Quick-Start) +轮腿跳跃策略的模型导出、控制合同与离线验证见 [WheelBipe jump-v4 部署](docs/WHEELBIPE_JUMP_DEPLOYMENT.md)。 + ## Development ### Pre-requirements: diff --git a/rmcs_ws/src/rmcs_bringup/config/contracts/wheel_leg_calibration110.json b/rmcs_ws/src/rmcs_bringup/config/contracts/wheel_leg_calibration110.json new file mode 100644 index 000000000..f3233b1ec --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/contracts/wheel_leg_calibration110.json @@ -0,0 +1,166 @@ +{ + "schema": "rmcs-wheel-leg-calibration110-v1", + "authorization": "User correction 2026-10-30", + "training_commit": "fd73050348e0b20a529e7f93b4044582d5d46e1c", + "source_sha256": { + "tools/full_robot_v202.py": "4fb90fc546b33d2c898d7e450f8a26570e28e67abc442fcb978c99a2e4f13f89", + "model/wheel_leg_v2.0.2/training/model_spec.json": "7f62e0b47546a047219c078bdaa44fc2d82553af04b36b72ec1a94be29a22e18", + "model/wheel_leg_v2.0.2/training/gas_spring_model.json": "69d9afbbf32f8835214c5a191d32a6576872b3578ed9a4c2b4cdd099f022c9f1" + }, + "model_spec_sha256": "7f62e0b47546a047219c078bdaa44fc2d82553af04b36b72ec1a94be29a22e18", + "encoder_zero_inner_knee_deg": 110.0, + "mechanical_inner_knee_domain_deg": [ + 40.0, + 110.0 + ], + "coordinate": "q_model = -q_api + offset; left delta=q_LK-q_LH, right delta=q_RH-q_RK", + "model_offset_rad": [ + 1.6, + 3.0282761689183353, + -1.6, + -3.0283102010168 + ], + "retained_hip_zero_model_rad": [ + 1.6, + -1.6 + ], + "hinge_min": [ + -0.14089326779459427, + -0.1408473212590866 + ], + "hinge_max": [ + 1.4282761689183354, + 1.4283102010167998 + ], + "hinge_margin_rad": 0.03, + "calibration_scope": "User-specified knee zero; existing absolute hip reference retained", + "gas_geometry_extension": { + "left": [ + [ + 1.4036218628388806, + 108.75, + 0.016531185774092738, + -0.041249833676382686, + 0.04125004460895004 + ], + [ + 1.408556209911497, + 109.0, + 0.01632778511246641, + -0.04119240565617699, + 0.041192616590464026 + ], + [ + 1.4134888007530308, + 109.25, + 0.016124741920915765, + -0.04113424493518686, + 0.041134455872180675 + ], + [ + 1.418419648726353, + 109.5, + 0.01592205905427349, + -0.04107535478389657, + 0.04107556573884602 + ], + [ + 1.4233487670611564, + 109.75, + 0.015719739352581828, + -0.0410157381254137, + 0.04101594906239753 + ], + [ + 1.4282761689183354, + 110.0, + 0.015517785643440356, + -0.04095539822173111, + 0.040955609167264595 + ], + [ + 1.4332018673155118, + 110.25, + 0.015316200743265444, + -0.04089433808011056, + 0.04089454898704209 + ], + [ + 1.4381258751718267, + 110.5, + 0.015114987456153313, + -0.04083256082720654, + 0.040832771748420926 + ] + ], + "right": [ + [ + 1.403655904215249, + 108.75, + 0.016531088950099575, + 0.04125050588151254, + -0.04125029861691106 + ], + [ + 1.4085902491532705, + 109.0, + 0.01632768712693127, + 0.04119307624654064, + -0.04119286898765447 + ], + [ + 1.4135228380054787, + 109.25, + 0.016124642776482977, + 0.041134913973605365, + -0.04113470673453735 + ], + [ + 1.4184536841179425, + 109.5, + 0.015921958753102407, + 0.04107602215912079, + -0.04107581491458104 + ], + [ + 1.4233828007362028, + 109.75, + 0.01571963789735205, + 0.04101640392535699, + -0.041016196699227736 + ], + [ + 1.4283102010167998, + 110.0, + 0.015517683036690835, + 0.040956062366274866, + -0.04095585515393491 + ], + [ + 1.4332358979798345, + 110.25, + 0.015316096987625677, + 0.04089500068595678, + -0.040894793466338314 + ], + [ + 1.4381599045371354, + 110.5, + 0.0151148825540693, + 0.04083322173504597, + -0.040833014520947314 + ] + ] + }, + "gas_geometry_beta_domain_deg": [ + 39.5, + 110.5 + ], + "gas_force_curve": "Inherited measured curve and declared endpoint trend; no refit", + "max_closure_gap_m": 1.2418291577181471e-12, + "max_velocity_mapping_residual": 2.974277783300774e-11, + "policy_target_domain_changed": false, + "standing_observation_domain_changed": false, + "hardware_zero_command_sent": false, + "hardware_tested": false +} diff --git a/rmcs_ws/src/rmcs_bringup/config/contracts/wheel_leg_prone_motion_v4.json b/rmcs_ws/src/rmcs_bringup/config/contracts/wheel_leg_prone_motion_v4.json new file mode 100644 index 000000000..1b8493a75 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/contracts/wheel_leg_prone_motion_v4.json @@ -0,0 +1,174 @@ +{ + "schema": "rmcs-prone-motion-deployment-code-v1", + "training_code_version": "wheel-leg-prone-motion-v4", + "training_commit": "fd73050348e0b20a529e7f93b4044582d5d46e1c", + "source_sha256": { + "contracts/wheel_leg_prone_motion_v4.json": "edf820720e51be460481398befbabf4fede3896e871f8d94d578c729ce0df582", + "contracts/wheel_leg_prone_motion_control_100hz_v1.json": "679f972b201ed639e28fa9b8babbd3fecd3fc2fc3206bfc3ca973558a6033ed7", + "contracts/standing_geometry107_v1.json": "7b313c807aefe51058567b1b2b8ce5300be85ccc8d9ec091d0aa7ae1708b9216", + "src/wheeled_tasks/chassis/standing_local_odometry.py": "844110c2a2a9e29e03e2a662f07e19646ab78abe6951c72da1df9c2e2f1d3ae2", + "src/wheeled_tasks/chassis/standing_sensor_uncertainty.py": "37fd7efd29483e20e5de457f106d5c2b48d99be2e01325de781cdd4c695d2cc2", + "src/wheeled_algo/encoder_leg_geometry.py": "e20380b4b2a0124d2aa3a256b01b3cafbb454b4881bc6c413825cf16a2386e60", + "src/wheeled_algo/standing_displacement.py": "2ea38ea52067b3aef34df7906b0fb5b16d0bb4688535174fa1ad37a11629872c", + "src/wheeled_algo/rolling_velocity.py": "dfb4889af2bf73f38b5e9590fcf0ccb16d0c98caffcd777225ba7afc5676e65d", + "src/wheeled_tasks/chassis/scut_observation.py": "fbb3980fb75bd947fb4121892fe5a22d8f3c0d002e5ef3e9d637a12921c7e2b5", + "src/wheeled_tasks/chassis/v5_control.py": "141e21d88513e5f8cccdb6cdbf19abcd66f433c6e0ccb3cd7e93d5afc0c7c7fe", + "scripts/train_prone_motion.py": "2219e25944ca8f07ab0d6d54ce5186539a513ea77a1dcce34111e5b4a8782e80", + "src/wheeled_algo/scratch_standing.py": "6b886b410e94e13c9412c531e796beac33ace867ad57194f4b60b72016af145d", + "src/wheeled_algo/paired_recovery.py": "3c45173dbe3a90c34434f84a7489368f022b84cc97657dd4decc4d7cc888ae90", + "src/wheeled_tasks/chassis/paired_leg_targets.py": "7d711b122cdedfd0c42e384546921a9e181dc99f05c0a039eb441caa2d794ba4", + "src/wheeled_tasks/chassis/full_vehicle_training.py": "dd8872308ca31068079260ca116746924e79a2d1f433a0959916517550ecac13", + "src/wheeled_tasks/chassis/gas_spring_compensation.py": "337eea09412ca59e3bd256133e7ffc13ee8cb4840d88f952f9671c1ff835b2d9", + "model/wheel_leg_v2.0.2/training/encoder_geometry.json": "96d656672cade2cfc07c80f2f427b8f6ff5aae70aa2c0e35fcaab738f9805dd6", + "model/wheel_leg_v2.0.2/training/model_spec.json": "7f62e0b47546a047219c078bdaa44fc2d82553af04b36b72ec1a94be29a22e18", + "model/wheel_leg_v2.0.2/training/manifest.json": "58e1e3874f1ed21609f61cf28c48d7ee6b9716f76661a73dc337ef2aa0fb3295", + "model/wheel_leg_v2.0.2/training/recipe.json": "038ae83a0cdf3b638644bdffdffa0fe652c7dcf26da67f72ef8d9466d5fe127c", + "model/wheel_leg_v2.0.2/training/gas_spring_model.json": "69d9afbbf32f8835214c5a191d32a6576872b3578ed9a4c2b4cdd099f022c9f1", + "model/wheel_leg_v2.0.2/training/gimbal_motion.json": "04ca3c88e63b7c30061505f790609f75e781378c0aa0c648620a6ff604cd7e6b", + "model/wheel_leg_v2.0.2/training/wheel_model.json": "f89af15231f944c15b4b988bd3bf77ad85c42f2fa27c2dfc970c1f8c7246346e", + "model/wheel_leg_v2.0.2/training/mechanism.json": "1c8ee2a8b65fc55ad7c4a3f825a6ba281142c59e9d272c8f4003e656ab60aae5", + "model/wheel_leg_v2.0.2/training/pose_bank.json": "21b55479e743d2090eb984b7cdad3df56ba41a0982a47a7ad504a28c8363c7ff" + }, + "observation": { + "schema": "wheel-leg-local-standing-odometry-v1", + "actor_dim": 35, + "displacement_slots": [ + 14, + 15 + ], + "validity_slot": 31, + "scale_per_m": 20.0, + "limit_m": 1.0, + "filter_seconds": 0.05, + "capture_seconds": 0.2, + "command_threshold": [ + 0.001, + 0.001, + 0.001 + ], + "capture_gyro_max_rad_s": 0.5, + "capture_leg_speed_max_rad_s": 3.0, + "capture_wheel_speed_max_rad_s": 2.5, + "minimum_encoder_stance_height_m": 0.2, + "upright_gravity_z_max": -0.9659258262890683, + "severe_gravity_z": -0.5, + "wheel_increment_margin_rad": 0.02, + "wheel_increment_speed_allowance": 3.0, + "geometry_table_sha256": "96d656672cade2cfc07c80f2f427b8f6ff5aae70aa2c0e35fcaab738f9805dd6", + "source": "same_policy_encoder_IMU_stream_after_sensor_delay_and_noise", + "wheel_input": "driver_continuous_output_shaft_angles; subtract_float64_request_origin_before_float32", + "reference": "capture_once_after_contiguous_sensor_standing; preserve_during_height_change_and_drift", + "reset": "episode_or_disabled_or_motion_request_or_severe_tilt", + "invalid": "zero_features_and_validity; retain_reference; wheel_jump_fault_until_next_request", + "control_outputs": false, + "privileged_position_or_contact": false, + "geometry_model_spec_sha256": "7f62e0b47546a047219c078bdaa44fc2d82553af04b36b72ec1a94be29a22e18", + "geometry_file": "model/wheel_leg_v2.0.2/training/encoder_geometry.json" + }, + "observation_contract_sha256": "6062baf37967d853446a95317aae4a0b315c4267da7387ab7ae020f37a021e35", + "timing": { + "physics_dt": 0.001, + "policy_dt": 0.01, + "pc_control_dt": 0.001, + "decimation": 10 + }, + "model_spec_sha256": "7f62e0b47546a047219c078bdaa44fc2d82553af04b36b72ec1a94be29a22e18", + "leg_winding_version": "paired_branch107_v1", + "control_profile": "full_vehicle_v202", + "nominal_policy_position": [ + -0.42, + 0.13722330866417465, + 0.42, + -0.13721605817245947, + 0.0, + 0.0 + ], + "full_vehicle": { + "recipe": { + "file": "model/wheel_leg_v2.0.2/training/recipe.json", + "sha256": "038ae83a0cdf3b638644bdffdffa0fe652c7dcf26da67f72ef8d9466d5fe127c" + }, + "asset_manifest_sha256": "58e1e3874f1ed21609f61cf28c48d7ee6b9716f76661a73dc337ef2aa0fb3295", + "mass_kg": 21.44174, + "gimbal_mode": "independent_servo", + "gimbal_policy_outputs": false + }, + "action_order": [ + "left_hip_joint", + "left_knee_joint", + "right_hip_joint", + "right_knee_joint", + "left_wheel", + "right_wheel" + ], + "action_scale": [ + 0.25, + 0.25, + 0.25, + 0.25, + 10.0, + 10.0 + ], + "raw_action_limit": [ + 16.0, + 16.0, + 16.0, + 16.0, + 9.0, + 9.0 + ], + "actuators": { + "leg": { + "model": "DM-J8009-2EC", + "kp": 160.0, + "kd": 2.5, + "effort_limit": 40.0, + "armature": 0.0, + "source_note": "Selected recorded PC PD; MIT gains zero. No identified drive response or torque-speed envelope; engine armature is an explicit unfit prior." + }, + "wheel": { + "model": "M3508", + "gear_ratio": 15.62, + "gearbox_efficiency": 1.0, + "kd": 0.5, + "armature": 0.0, + "effort_limit": 4.5, + "curve_side": "wheel_output", + "curve_kind": "software_cap_only_not_identified", + "torque_reference": "wheel_output", + "scope": "new_gearbox_airborne_identified_candidate", + "source_note": "Both new-gearbox CSVs use 24V, full ratio15.62, output velocity P0.5 and software cap4.5Nm. Wheel-specific inertia/loss/aggregate delay and measured no-load ceilings are bound by identified_wheel_response. Rolloff shape at high loaded torque remains a prior; no second ratio multiplication." + } + }, + "gas_spring": { + "enabled": true, + "model_file": "model/wheel_leg_v2.0.2/training/gas_spring_model.json", + "model_sha256": "69d9afbbf32f8835214c5a191d32a6576872b3578ed9a4c2b4cdd099f022c9f1", + "gain": 1.0, + "force_extrapolation": "positive_c1_endpoint_trend_v1", + "physical_curve_outside_geometry": "hold_geometry_endpoint", + "relative_soft_limits": { + "minimum": -0.140638855, + "maximum": 1.3889088544, + "margin": 0.03, + "projection": "outward_relative_torque_v1" + } + }, + "commands": { + "forward_m_s": 2.0, + "yaw_rad_s": 10.471975511965978, + "height_m": [ + 0.23, + 0.42 + ] + }, + "normalization": { + "mode": "embedded", + "metadata_key": "rmcs_obs_normalization", + "frozen_training_statistics": true, + "expression": "RSL-RL eval normalizer before MLP; no second external standardization" + }, + "artifact_bound": false, + "behavior_qualified": false, + "hardware_tested": false +} diff --git a/rmcs_ws/src/rmcs_bringup/config/contracts/wheel_leg_training_pair_20261103.json b/rmcs_ws/src/rmcs_bringup/config/contracts/wheel_leg_training_pair_20261103.json new file mode 100644 index 000000000..3ef588dd2 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/contracts/wheel_leg_training_pair_20261103.json @@ -0,0 +1,526 @@ +{ + "schema": "rmcs-wheel-leg-training-pair-v1", + "date": "2026-11-03", + "hardware_tested": false, + "behavior_qualified": false, + "prone": { + "run": "/home/noir/Documents/workspace/isaac_wheeled_rl_train/runs/prone_motion/v4_standard_getup_standing_bridge_seed107_env256_20261103", + "source": { + "schema": "scratch-committed-source-v1", + "commit": "e9e424219c58ac17b0a40c1a4d9c321d067ef9fd", + "source_count": 578, + "all_source_bytes_match_commit": true, + "archive": "/home/noir/Documents/workspace/isaac_wheeled_rl_train/runs/prone_motion/v4_standard_getup_standing_bridge_seed107_env256_20261103/committed_source.tar", + "archive_sha256": "48c208f34260c66573fb037dbc416843d0038227331fc9e73e4e42e6a4f82a40" + }, + "task_contract_sha256": "c075b2c262ffa3ba96913d251a15488fb9ff2edd69ba4086408150f33c45be5c", + "observation": { + "schema": "wheel-leg-local-standing-odometry-v2", + "actor_dim": 35, + "displacement_slots": [ + 14, + 15 + ], + "validity_slot": 31, + "scale_per_m": 5.0, + "limit_m": 0.2, + "filter_seconds": 0.05, + "capture_seconds": 0.05, + "command_threshold": [ + 0.001, + 0.001, + 0.001 + ], + "capture_gyro_max_rad_s": 0.5, + "capture_leg_speed_max_rad_s": 3.0, + "capture_wheel_speed_max_rad_s": 2.5, + "minimum_encoder_stance_height_m": 0.2, + "upright_gravity_z_max": -0.9659258262890683, + "severe_gravity_z": -0.5, + "wheel_increment_margin_rad": 0.02, + "wheel_increment_speed_allowance": 3.0, + "geometry_table_sha256": "c30136fdae9b504b0033629c4b95e187cbcb841b7307f05be3d0d6f9ce6850b9", + "source": "same_policy_encoder_IMU_stream_after_sensor_delay_and_noise", + "wheel_input": "driver_continuous_output_shaft_angles; subtract_float64_request_origin_before_float32", + "reference": "capture_once_after_sensor_standing_geometry; preserve_during_height_change_and_drift", + "reset": "episode_or_disabled_or_motion_request_or_severe_tilt", + "invalid": "zero_features_and_validity; retain_reference; wheel_jump_fault_until_next_request", + "control_outputs": false, + "privileged_position_or_contact": false, + "capture_velocity_gate": false, + "normalization": "fixed_physical_scale_for_displacement_and_binary_validity", + "geometry_model_spec_sha256": "8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812", + "geometry_file": "model/wheel_leg_v2.0.2/friction_v1/training/encoder_geometry.json" + }, + "observation_contract_sha256": "6613ab1b2a538fc31558b668117dbcb77bed4f84e93133d9c087c5752ec6c967", + "policy_hz": 100, + "pd_hz": 200, + "model_spec_sha256": "8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812", + "gas_spring_feedforward": { + "enabled": true, + "model_file": "model/wheel_leg_v2.0.2/friction_v1/training/gas_spring_model.json", + "model_sha256": "698f67ccc7d6364b73f1b426ed3e6386ffc9e11912ef95a89d716eb7ee0e06b9", + "gain": 1.0, + "force_extrapolation": "positive_c1_endpoint_trend_v1", + "physical_curve_outside_geometry": "hold_geometry_endpoint", + "relative_soft_limits": { + "minimum": -0.14084732125908495, + "maximum": 1.4282761689185115, + "margin": 0.03, + "projection": "outward_relative_torque_v1" + } + }, + "action_order": [ + "L_joint1", + "LL_joint1", + "R_joint1", + "RR_joint1", + "L_joint3", + "R_joint3" + ], + "action_scale": [ + 0.25, + 0.25, + 0.25, + 0.25, + 10.0, + 10.0 + ], + "leg_kp": 160.0, + "leg_kd": 2.5 + }, + "wheelbipe": { + "run": "/home/kaiser/workspace/wheelbipe-v202-port/runs/full_flat_1024_20000_200hz_balanced_seed119_011/attempts/0001-03d234624906/train", + "source_commit": "a603a7c9a5b4fa6d94e7b6846c944a42cdba8391", + "source_snapshot_sha256": "58e406a68713b23688c96b90b083c77c7125b8fbe9b42ebab284b0210f79e6a2", + "native": { + "actor_dim": 35, + "critic_dim": 78, + "action_dim": 6, + "policy_action_order": [ + "L_joint1", + "R_joint1", + "LL_joint1", + "RR_joint1", + "L_joint3", + "R_joint3" + ], + "default_joint_pos": [ + -0.41999998688697815, + 0.41999998688697815, + 0.13722330331802368, + -0.13721606135368347, + 0.4000000059604645, + -0.4000000059604645, + -0.39988231658935547, + 0.3998713195323944, + 0.0, + -0.4584108591079712, + 0.0, + 0.4584149122238159, + 0.24258989095687866, + -0.242586150765419, + 0.017636841163039207, + 0.017636636272072792, + -0.39989104866981506, + 0.39989104866981506 + ], + "leg_action_scale": 0.5, + "wheel_action_scale": 10.0, + "policy_dt_s": 0.02, + "physics_dt_s": 0.005, + "leg_kp_Nm_per_rad": 60.0, + "leg_kd_Nm_per_rad_s": 2.0, + "paired_target_signed_delta_bounds_rad": [ + [ + -0.1408932677945942, + 1.4282761689185115 + ], + [ + -0.14084732125908495, + 1.4283102010168576 + ] + ], + "training_profile": "foundation-v2", + "training_profile_sha256": "90d7640dc423a56b18d59b1064694a56ca22dd6c3ba1140790996bdfc9312ad4", + "gas_cancellation_feedforward": false, + "physical_measured_gas_spring": true, + "spring_model_sha256": "5682982f82c35c3864771d4822e6d6d3932cea2800464b255a1de6d4bd7fec41", + "wheel_model_sha256": "22e02a731dbe3c00cb89f04ea695d1d80a3f11cb2a9574f3dac6eb680b1f7657", + "friction_model_sha256": "90d299995f24ba38e6c1ed67e32dd40b9b2fa26e041f2f814db1e177ac040b52" + }, + "implementation_files_sha256": { + "src/wheeled_algo/__init__.py": "e1e768e75e9ee328148962a363c76542d8b8e9f58ae24a8abd897cc44a3de9d1", + "src/wheeled_algo/algorithms/__init__.py": "03c6c47784197b6c0952e17bc6a9fbee878c09d4a241f429e3b8e9b50c6e24ed", + "src/wheeled_algo/algorithms/dreamwaq.py": "78a995b99af560100a5098edbfc6136292241cf169e9b7c1a179f449c0167665", + "src/wheeled_algo/algorithms/him.py": "45ac72a1d56dd8a276ac05944f0b23e544a2902be667f0f1a8ec7d516ca01c7f", + "src/wheeled_algo/algorithms/np3o.py": "b49ef444dca31a45917cade8356d96c77050b58689e3e51740bb6deabb9897cd", + "src/wheeled_algo/algorithms/ppo_base.py": "57dacd53afc1c323a0882e34b4fa23436c32cd810d4f5cd73b476dbb49c636cf", + "src/wheeled_algo/algorithms/recurrent.py": "8dc4d081ae79d53b98476c37b7dd61d6ceacb6f24c2ec2b144a371ff789ff993", + "src/wheeled_algo/anchored_ppo.py": "164724128d176261e7cce2a77c7012c008b3e5b488047682818b82d8013b1fcd", + "src/wheeled_algo/chassis_export.py": "cfb38118a32a97839f29abe82c69ae1c6caab9ad9aa708d648a177282d452838", + "src/wheeled_algo/command_residual.py": "f94ee68f4820fafd5e4fdabf7c63cf83693d26033344f2bb9239784575883976", + "src/wheeled_algo/continuous_rollout.py": "843b7d38727d25302e77684ac7589960ef47cacc689b0fc366f4b9febc91d0c0", + "src/wheeled_algo/critic_input_extension.py": "a23ef15711eed0803e413f84945ad9b2ad64f3493c5d896be49f58baffc95618", + "src/wheeled_algo/directional_motion.py": "5053590e98ebb3416d3fab376fa319b5f9c11c340dc1aa718a81afb947964534", + "src/wheeled_algo/directional_runtime.py": "e657ab616ffcc8f8242fb8471bc22da1386402fbdd9f1b35d29734b26b6103c7", + "src/wheeled_algo/durable_checkpoint.py": "748da8a80dd1695b6bca30063edf582b43a4dfcfae9cf074a29e6772ea42621f", + "src/wheeled_algo/encoder_leg_geometry.py": "e20380b4b2a0124d2aa3a256b01b3cafbb454b4881bc6c413825cf16a2386e60", + "src/wheeled_algo/entry_checkpoint.py": "13f7fd83fe01f89019d5e6572c083ecc91d5c69bc49dbc69fc4197936523004a", + "src/wheeled_algo/entry_optimizer.py": "544d6f564eff4086869de55012d35c4e9418ceafff7103511a2edb4d21d6244e", + "src/wheeled_algo/evaluation_command_feedback.py": "7d263d327543eda73233391618bcd07f4b2d1d811e3118cb1626043ec4baf2d9", + "src/wheeled_algo/evaluation_early_roll.py": "261b8ac63a1b367a868c64753de57939571c8ae94c35dca657a8e7d58b1f5a7c", + "src/wheeled_algo/evaluation_gyro_filter.py": "e225a9cfd09165ba5fc593a55697fa51c7b80966bc735b02253402102c09bd6d", + "src/wheeled_algo/evaluation_position_filter.py": "77dff9e601f070436cffef4490316752bce7a4a1c3b7bb17007067770dcfc58c", + "src/wheeled_algo/evaluation_quiet_output.py": "15cb74455a9c8e6dd2e86a0f3e8b40643f20d3e0ec1b2c719e030f108380544e", + "src/wheeled_algo/evaluation_recovery_noise.py": "3b2b38743e1aa5b396a201bce15b648d95fa56a66ebba93a8f1dfddbd764cc32", + "src/wheeled_algo/evaluation_recovery_step.py": "7eedbed0cd9c69517b262ec82d829f7be52e109c9bc7d26315dea4619d8a6b9f", + "src/wheeled_algo/experiments/__init__.py": "4929c9b7890edc53641e1690c32739038f6bb32eb84c85206078cdedcb0588eb", + "src/wheeled_algo/experiments/ppo_hist.py": "aed5c084791f58c3f527d88d904d59145dd83995914cad45ffdb179fc6bd84eb", + "src/wheeled_algo/experiments/registry.py": "8e67c633f0d1c40e7e4ebc1b12212a72c343fc5875ddba780437de76e5e45743", + "src/wheeled_algo/exploration_init.py": "b7bb7c71714adb65aa8758bcf1eff474428514df522ad2acb35548dc756b8b89", + "src/wheeled_algo/fallen_output_checkpoint.py": "876086bf545c987f629512935f1eca8f9dd7cd0ad3c3629d751b8f04c11070f0", + "src/wheeled_algo/flat_actor_trainability.py": "22134182e03b5926f248c7d502431c88c7f47d391040d20b812226a220a1ed30", + "src/wheeled_algo/flat_optimizer_rates.py": "05a20b3d84ddcf88901230355cc95beaad651a13222dc34e8fbfc94e136ceaae", + "src/wheeled_algo/function_root_checkpoint.py": "bd426da2dbf2fd00306b2af3d5a54c1f857ece77625b20f8ed14158cdcc55408", + "src/wheeled_algo/gap_entry_handoff.py": "1efcf412ce01c0019618b47d2c6f025a8fc1c883e173f83584da6114818e3d39", + "src/wheeled_algo/gap_entry_preservation.py": "2594881617d1991e270f62f58f6a0002dcec137deb9f2162db467e63d75a865f", + "src/wheeled_algo/gap_recovery_exploration.py": "4769cb8f9279bf42ad0b2b9f7f17986ec11451ffe080110a8c4023786013df63", + "src/wheeled_algo/gap_robust_actor.py": "664d58d22b386c36bc1cf4df647950c5950b0bd9034511ca98521411777225f2", + "src/wheeled_algo/getup_symmetry.py": "5f22abfeaa358d6ec298b385b9e698f10026cfca3d835992a8897c57a2ffcffa", + "src/wheeled_algo/global_curriculum.py": "09b9e84405a682ba2f59782f88dbfbd85aa0c889d153ff7c53a6607eb5e28fdc", + "src/wheeled_algo/guarded_feedback_residual.py": "833c3a05a0427f9ca223f259cdfeade9780188b64fbe1197196c7056e2806b5f", + "src/wheeled_algo/intent_speed_reference.py": "2f1fd8225b147ce432378054fc347e07df0649cbf0222a36cf5336de085fa816", + "src/wheeled_algo/leg_velocity_separated.py": "de49d66cef1982b3ddb056df1564ca59c0fd106c0e7d3f93168cf6d7e2001314", + "src/wheeled_algo/low_demand_feedback.py": "d6770af0b3d0604ccccd7cbb8a73e5992e6108ec5e3a46006da0569b512593d1", + "src/wheeled_algo/managed_training.py": "00867eeb466d5856482324c7b49bd676a9a6f8e2cc62c96fc61663d1a92ca5d6", + "src/wheeled_algo/modules/__init__.py": "8786eff8a3a1014fc80f2c082816f3dc6e6d0a750ece776c110ef364d1ca66f9", + "src/wheeled_algo/modules/actor_critic_ext.py": "c84c7acb63c7501b9c73c44712f9f907a588649fc26cbb02173f861bd5315ca3", + "src/wheeled_algo/motion_grip.py": "dc9cfd04e211d7cf006df023c6b5071c2fe0682edcded5751fc425c517da3584", + "src/wheeled_algo/motion_output_checkpoint.py": "e9ad7204d8cb58c5939db9ced0938b0a2c56d46dc224bad1e7c8e15131a1f9b3", + "src/wheeled_algo/motion_state_residual.py": "f154db3c4430e6eb4845cbedeb7260517b9684c41c9e17bd02db08abe1d748e3", + "src/wheeled_algo/motion_target_damping.py": "c7cf2e69550eff2a49f49e5725e39ad0b1e28a4f6e6afcfd5c73c39b0716d755", + "src/wheeled_algo/motion_trust_region.py": "6bedca4817f40b1899952e537fca0629d990d5ff1895e0259565b6d247d45325", + "src/wheeled_algo/native_ppo_audit.py": "1858ac27fc271d7c781defa0db0ca7092d124bb142e17a492464d62d1556742a", + "src/wheeled_algo/onnx_actor_import.py": "e5c57dd0482dc3e0584b8872a779e939fc46a4d46ae3e24495421343b1e7ffe8", + "src/wheeled_algo/paired_recovery.py": "3c45173dbe3a90c34434f84a7489368f022b84cc97657dd4decc4d7cc888ae90", + "src/wheeled_algo/periodic_entry_actor.py": "279c85baafe45e460eafe0942c57294ba5ae4d1e09797a37c2012a4f99613a4a", + "src/wheeled_algo/prone_motion_resume.py": "8dfcd1391c0acd3a6433749a1a27b112d26786b7ab83aa304c92c07c772decd0", + "src/wheeled_algo/quiet_full_checkpoint.py": "978c1b2536e00d56c84217978f93e69cd03aebf6414b3ca01a01b95fa56cf981", + "src/wheeled_algo/quiet_output_actor.py": "fd48fabcfdf3d6bdb980b369b17fe7ec82c317449ef20c72f46b74a2d0bde410", + "src/wheeled_algo/quiet_output_checkpoint.py": "86bc2adf76f99f67d28164207a348f0475ab7a5281f7d952ad1a4339aae09b60", + "src/wheeled_algo/quiet_stance_authority.py": "16e03653c85228a349b4eecc5dd241e19b0098452eb28af85ae9861bd48c3d5c", + "src/wheeled_algo/recovery_coordinates.py": "4e7b7911feea4b9cdf201ca7f2ec9456a10fa731fc9cf15a3ccc3ba7c6b6adcf", + "src/wheeled_algo/recovery_exploration.py": "11bbc8ad85d24013a4687ee1f4464525c877bf92a13cc2b9a076c414409281d9", + "src/wheeled_algo/recovery_mastery.py": "ba05bc6ff36c4f90c8651af7af3004d37fcf8e4eeb90048b5a1a4f5bc2b20c06", + "src/wheeled_algo/recovery_rollout.py": "e845984d413b36faa56ab67c451c5c41d368adacd5391f05014283445a12e23a", + "src/wheeled_algo/reverse_reference_scope.py": "ed1752edbb105f44d548398919c9eb6cdadfe9945eed64bde468f9b3383fbf48", + "src/wheeled_algo/robust_getup.py": "f789979c8f03d2d3e2d9c80c214215bc91c172b0e7bfd2825ba25593812ee486", + "src/wheeled_algo/rolling_velocity.py": "dfb4889af2bf73f38b5e9590fcf0ccb16d0c98caffcd777225ba7afc5676e65d", + "src/wheeled_algo/runners/__init__.py": "573f311fce4800d928b5f4c17ceb8550b2973e95d4862b93b4af5d2ebed99936", + "src/wheeled_algo/runners/branch_runners.py": "338dbee9278879faece78c8221a4e698a93ec17be41f8b5ef6df2e8ac8fa6c3e", + "src/wheeled_algo/runners/on_policy_runner_ext.py": "4c88ebccfc6a74a21845f17c8f1f6fd66bd814dcf54bd28cb6c1f9618c509d5b", + "src/wheeled_algo/scratch_standing.py": "6b886b410e94e13c9412c531e796beac33ace867ad57194f4b60b72016af145d", + "src/wheeled_algo/separated_feedback.py": "8c986223613bb8c07c758464a70d54664e6f01c49ae23482408123d5dafdd886", + "src/wheeled_algo/single_frame_inference.py": "d2165bf7aefe98bd220cd8d1cb5af2ab1e1af9ea6fdbea06ba50bc25762105a4", + "src/wheeled_algo/speed_reference.py": "5f82f4ede749108426899ba1641b55cdf69fea72e052a2b00e0b4497c7eb52fe", + "src/wheeled_algo/standing_acceptance.py": "3464c56aae4fcfe66a93cec946a93ba660bab2de667a861c0d61059dd633868b", + "src/wheeled_algo/standing_displacement.py": "2ea38ea52067b3aef34df7906b0fb5b16d0bb4688535174fa1ad37a11629872c", + "src/wheeled_algo/standing_displacement_contract.py": "db5147a407bb8ba267fbf28dc9f21dc1f0c8c73f4cad3dd0be13ca0e1276f159", + "src/wheeled_algo/standing_load_initialization.py": "3cb4154b943f90f2f9000d6e302791219c3755a3bacc8047e19cafd78cf44f50", + "src/wheeled_algo/standing_reference_lifecycle.py": "cabcdebb8631904e0512ee53a38f5f6de519715a72c1b1ada7f7acbd28820acc", + "src/wheeled_algo/static_feedback.py": "d1d3bd834faeee925cd5b7fb2032882f4e20fb86ece9c6553f19572828bab2f4", + "src/wheeled_algo/static_grip.py": "84abe3195548a454e1169b495d0faedc28e7f11b38c25d4f918d00bf4ce5fab3", + "src/wheeled_algo/trainctl_shared/__init__.py": "4e0fc6c60f33ceea56b2318cc49990af3e88e105e0b42307dc3fe7ab52c4f01f", + "src/wheeled_algo/trainctl_shared/session_router.py": "c6bbae18c533a856414a67502553891d90ce7a187b00e54e21f091da2c5da4f7", + "src/wheeled_algo/trainctl_shared/training_cli.py": "47f2c4e228fae5c1b84281aa699b8b6d4137dfb35c84988b8c2c9a8ccb7ecbcd", + "src/wheeled_algo/trainctl_shared/training_control.py": "b596ecd3803c1f54671297c839c6a205f09bac415ec0d662c186072dc8a47c8d", + "src/wheeled_algo/training_control.py": "bd46ea60fb9d82d82818ed7283ebf500ce6c53f9798de2d91d801cdbf6c4e20f", + "src/wheeled_algo/transient_leg_feedback.py": "97a1a2eb50092b5568909738201af1dd4d8837e6bf044cc671cc6e81e2e708fa", + "src/wheeled_algo/transient_motion.py": "a6269f1bf529c340a6243fbab5c3ba98f621e540bcaa8c960ea8af5cc2b2e4f5", + "src/wheeled_algo/unified_getup.py": "a6db8de139db9cd9d780d0ba4ffd708832569ea1e56cf08547daa364d8e0de85", + "src/wheeled_algo/unified_getup_ablation.py": "446da211221a5d68c2dd6057585be5c22255c3b4205159ace9524f04c552b856", + "src/wheeled_algo/upright_target_limit.py": "fceb8f710ba2025f2eeee50bce8db160f96413f78a86aa86d5d5ceaa08f8c42c", + "src/wheeled_algo/utils/__init__.py": "a721f3f6570dcdc8dd72edb3dd6d56d75c32a649f7b035e6d3e020855c868b3c", + "src/wheeled_algo/utils/exporter.py": "df0d1952ecbbae2268f910ae5d02764e8d9ac23c9cd1950b7b55afa5d74d1915", + "src/wheeled_algo/v40_export.py": "12a469b9ce9e40c980140bc8821762409c8a420b052adde0ea5da72e6f345251", + "src/wheeled_algo/v40_job.py": "b255ab1597e064aaae23e8a2139a9eff1cef3437c24004c996ccd9c6ba94312b", + "src/wheeled_algo/v40_metrics.py": "88f8c8aa56ef4368f16018e4790e6ee721378303cae9b652a92a3d5881c21948", + "src/wheeled_algo/wheel_authority.py": "7f396abd18bc3c5d54210e1e76642443d90b2476f160b553bad29ed753690cc4", + "src/wheeled_algo/wheel_velocity_increment.py": "750530e7d6905aa30a2809936282028276a57ec908e83bc77a5848149ded76af", + "src/wheeled_tasks/__init__.py": "52498a20849a2cdc4076122a104379d2b3d7341868a0fb80d403394899545c32", + "src/wheeled_tasks/agents/__init__.py": "c94a1dae79dbe70f6719756bee59c6a356d93565a0ac89cd28d7c53cd846076e", + "src/wheeled_tasks/agents/rsl_rl_ppo_cfg.py": "b9b26b48d848e7f28360535a6e7b01debaa27a10a45f70bfdb00cfa1d75636b8", + "src/wheeled_tasks/agents/v40_ppo_cfg.py": "3445aa0fdbf0fc4986be9b139d3c2c1ffa22782daf53cee6580c966fb9db0d5f", + "src/wheeled_tasks/chassis/__init__.py": "018e3cdee84902478a0ebebdcc830df26f5ef4c9525ba302b95b2e7c43e06039", + "src/wheeled_tasks/chassis/actuator_response.py": "3e96d4eb929c21a17be120b304e886aa626629e3bc1b2259e04c764bbbffbe57", + "src/wheeled_tasks/chassis/adaptive_commands.py": "54351b0afe58de2ffbec10dcd0d1c16bf2377e209b90fd042b6510eb4f87310e", + "src/wheeled_tasks/chassis/arc_rehearsal.py": "bc0e11c0907a6e5e02821294d9cdddf5a54fe13b19e543d10c22b2d32273ea7a", + "src/wheeled_tasks/chassis/command_transport.py": "ec04fbc44b785ed4a9f8f688c76aa0e5abf28744c43fe7c6bafda50ed411a93c", + "src/wheeled_tasks/chassis/contact_domain.py": "4b205a0cc8ad0f57eaeb37f08b7ba7fb27b5fe9cfbdef6afaae3851e1725cbd7", + "src/wheeled_tasks/chassis/contact_material.py": "991ba7efa3d2ee1ed4be00d19ff5630615cff1c60eaf162f00e258304ff273eb", + "src/wheeled_tasks/chassis/continuation.py": "c7681427dafa24d4beaee3eaee406aa76e2835f5cff1b584f533bc33a786716c", + "src/wheeled_tasks/chassis/continuous_encoders.py": "987fd14fdc0799bbeaed7cd2fd10acaedd0b0ce695634f324ee0958795c28cfb", + "src/wheeled_tasks/chassis/control_contract.py": "8877e6c4faeb70515bf98b06a6ff875513ad1de1889a29dc7356ff75b80e99a6", + "src/wheeled_tasks/chassis/delay_buffer.py": "2679ba18940a1b7c448fe59454642ec3c0d4e763be59d286d39116210df628b3", + "src/wheeled_tasks/chassis/diagnostic_curriculum.py": "2013a17f59c1662fdc258cfae8bca6b70d70500a6b3b6742ed576fc50e544e06", + "src/wheeled_tasks/chassis/directional_motion_plan.py": "c0183e859741cb7fd2a623fb73c09ab81cd441fa4a384b3d0dddf3aa1fb2505d", + "src/wheeled_tasks/chassis/dynamics.py": "f394eb6a5a00c1321c4c528374c4dd3e158af99695753d5ea9d0fd71fcd668bf", + "src/wheeled_tasks/chassis/encoder_spring_feedforward.py": "da89ba6ccd4aae8eab0af4539194e3fc44a58a5262d9ad5b1d67d022ce677ea0", + "src/wheeled_tasks/chassis/env.py": "3bb1559043df136914378aab2bd2cdd95dee95c2a57f9b54e06286eb2e3aea4f", + "src/wheeled_tasks/chassis/episode_metrics.py": "26438717a12b3ab37b7279cbcae09dda91c549438c7a6090e7d6876c7a046511", + "src/wheeled_tasks/chassis/eval_env.py": "0e40a0398351cc8bb66faf5d6db939056282867a376fec79af68379a446ed53a", + "src/wheeled_tasks/chassis/evaluation.py": "63e9adef92a14266024c93edd56734c3c739c9b2a487b05ee505a85bff57a895", + "src/wheeled_tasks/chassis/evaluation_interventions.py": "08259f0c514db3a3df224841aecd67f6c4633e217b32b46a80f0b75e12f07c0b", + "src/wheeled_tasks/chassis/fallen_pose_bank.py": "ee9db60ec42fc31dbe304ec93a3eaaf783fddb15166724eee7126495ba945feb", + "src/wheeled_tasks/chassis/feedback_transport.py": "9fe9d4372bfeaf6efbc3902cd0e7abbbddb68cb20b7bdcf679bef134a1242613", + "src/wheeled_tasks/chassis/feedback_transport_config.py": "4e260eeb6156189359f407f32ccc1695cf3536a7e2efba4f8bac1329addc301d", + "src/wheeled_tasks/chassis/flat_base_continuation.py": "edf5fbaf6f648eb7a9a4da42dedf57a515b6bd0d2fdd5b1482078ead5c5ad7ca", + "src/wheeled_tasks/chassis/flat_clock_transition.py": "66c6ddeb90d6772dcc4dc6c83b8efdcfcd7a881a34d59db059ff0ccbe40b1e04", + "src/wheeled_tasks/chassis/flat_command_reference.py": "14090b8e27df5dd0b5adf35150d374ddbc0e17103bb91231c63856b6c138dc03", + "src/wheeled_tasks/chassis/flat_policy_rate.py": "296d7ecb547c684d9f2ec2f2a5372a0c8d33b4cd5d8f62898d4b16987bb14393", + "src/wheeled_tasks/chassis/followup_continuity_reward.py": "aee67757fbb84986e37938dca4abb5d91dee24c7e43a3d4c74df8f105d43f687", + "src/wheeled_tasks/chassis/full_curriculum.py": "22476c6ffde546eff248993e6bd3e2b70c14c7e6619c22917e7bcbc835d19f9e", + "src/wheeled_tasks/chassis/full_robot_v202.py": "98a97da9c6daaac206f3d25165e87b59592e80e83ad733aaa30030c5d0410d2e", + "src/wheeled_tasks/chassis/full_tasks.py": "63d9e1e110c92c5094cb90d91c1e5dce4550ea3b601e46cce5b7b8c723972a4d", + "src/wheeled_tasks/chassis/full_vehicle_training.py": "a54d40ac6db3910bf37ae367bbe44acc74ac7c7bf91417bd03008fa58646aa41", + "src/wheeled_tasks/chassis/gap_robustness.py": "5778a89c1cf9159b4454c8d526330b880811803a9c75d6ef6bdb0e0f4f135b74", + "src/wheeled_tasks/chassis/gas_spring.py": "b147ff71a3df841eaefe96042ac379e11dd3dcf776645dbdbc4efdb6c16ad502", + "src/wheeled_tasks/chassis/gas_spring_compensation.py": "47fe59b4dd385c74702ef0d52ae5d81ddaeecaa57eb4d7f3f0d00c259c50e14b", + "src/wheeled_tasks/chassis/getup_feedback.py": "ffb6ab575b1f2f6984068b33603bb5da74a29a607f5f34ffd812ebae54019473", + "src/wheeled_tasks/chassis/getup_ground.py": "5f600ee86ae008cf74fb6d92a7beac3b9e627cb8630c14dc2d6980a0bbcafe8d", + "src/wheeled_tasks/chassis/getup_reward.py": "f83a6101e915bdfc891af55946c9327e7530c7b6df819090c469c16d4f74ced2", + "src/wheeled_tasks/chassis/gimbal_motion.py": "8f2b133bb66b0c3331fd5ff63c57023d70a2e022d60ea951afa12e1c0f01973f", + "src/wheeled_tasks/chassis/global_curriculum.py": "bb8b16fa88b256de66ab1384bed6d8feae21665a04b19d0a815a4ac2428963da", + "src/wheeled_tasks/chassis/graded_locomotion_revision.py": "2ba7f990ac283aa9ca44fbc10efb7f6d8263cfd33abf7355f138936fb0e277e1", + "src/wheeled_tasks/chassis/grounded_standing_reset.py": "ca74b8409a707bb1a0ed4ff2fd2b62c4e4ad7546fafae9d116f66a18138f9a0b", + "src/wheeled_tasks/chassis/heading_command.py": "cfa95c65c98041c447ac28e0e5682052f3f1c27017a5fd4cd7317095976d8db3", + "src/wheeled_tasks/chassis/identified_gas_spring.py": "5f1e6fe5b57518dc71bcb5a32345f37514a9459d7cd0bdaeee926fd5b107f4e4", + "src/wheeled_tasks/chassis/identified_wheel_response.py": "775451a040d28c52b775c12d9c8cd35660a597168ff1beda92a54aa9126858c8", + "src/wheeled_tasks/chassis/imu_mount.py": "49cb2dae1ab2f18ae456c871c1633382b6413d217d89405ca8b917dd77a4429d", + "src/wheeled_tasks/chassis/integrated_completed_speed_revision.py": "174c9568d41571995f14f7294bda2c22acfd74047873f89805761103298c747f", + "src/wheeled_tasks/chassis/integrated_curriculum.py": "b0f6277dbf52bdc1b77335af0e6a3788851152d8219245a7013885b2403599c2", + "src/wheeled_tasks/chassis/integrated_lineage.py": "ef4eebbe73280e647bf9fb22ef31958984f8e8aa71cda1b1cd961a58b8b9cb92", + "src/wheeled_tasks/chassis/integrated_preflight.py": "ed179e1b22e33d2a48776453af62b78aecd4cab58efc9814b592284752d659b4", + "src/wheeled_tasks/chassis/integrated_production.py": "44d3db3c9ac225d476aef982a02ca276a5a44c6727db78c1e5c88eb7fc3b0bec", + "src/wheeled_tasks/chassis/integrated_runtime.py": "41bbf92cce4fbb68e400beb226d99948e1461a68c4746921dc95d85eb4403255", + "src/wheeled_tasks/chassis/integrated_sampling.py": "c844c42778c3f03135775465c5a79c32caa780fd881ad245aba4b71d712f97c4", + "src/wheeled_tasks/chassis/integrated_training.py": "1b1cb81ef68716f2a20f7878f36df2e486a41ae6655fcac509aa851de8a0b1ba", + "src/wheeled_tasks/chassis/isolated_origins.py": "393ce58ce7e6768ab9fb9cedfe79b98569925f5f89edd5bbefb7e581955598a7", + "src/wheeled_tasks/chassis/jump_landing.py": "5373e8a7aa2a2a9f1cf7427df27687e095e764353b8734b3401783f718e6f71d", + "src/wheeled_tasks/chassis/jump_landing_revision.py": "9a906a996f41c97d2e1cf8c86c200577a2a21e652e5433522f0248099e480ff4", + "src/wheeled_tasks/chassis/learning_audit.py": "989c2a4b30c2b60d89b7f6f2ab93f1024324524e389b2c8a3f2bd02be5bab2bc", + "src/wheeled_tasks/chassis/leg_motor_friction.py": "09473ff4559826416685ba09495af758a3288afd2e4bc90879feeb58be2e12d6", + "src/wheeled_tasks/chassis/local_birth_revision.py": "651ef066b4997608aa8d8212d0f1093d947cb3d8eae82e761356ad666bdfaf3e", + "src/wheeled_tasks/chassis/local_command_revision.py": "5793a4f3823f72ab42f0a9a095d3a2902dbb75d5bbd4bdc0c492129a327c08ac", + "src/wheeled_tasks/chassis/local_entropy_revision.py": "7d11174fc93ea24a11da4610a0b499784571013c7766eac902ac0402aa110a75", + "src/wheeled_tasks/chassis/local_evaluation.py": "83b4f209f13a15803d32e584cf70622ce6a8ce929ab3ee34719c5cf1f4d78dd5", + "src/wheeled_tasks/chassis/local_return_revision.py": "8071f232032b6cdf6bb338d986315becfad24bbb43e6f137b6abd97d39e85593", + "src/wheeled_tasks/chassis/local_scratch.py": "a311ef788feb87add30fe0fd3107b0da83452adeb9da8096ffbf1b75eee60908", + "src/wheeled_tasks/chassis/local_value_revision.py": "1161fcc31da47cfbb2ea965f0d2862cffb71482f9aa9cce6147742a6e9e55822", + "src/wheeled_tasks/chassis/locomotion_difficulty.py": "b59d7e6e29084da6abafd40b1d8689c40e98cf669633edf76c9e56948bc32250", + "src/wheeled_tasks/chassis/locomotion_quality_revision.py": "a896c95ca34fb039ba8addf7e570380216f6d347ca0ecc64f0acd4479f2491fc", + "src/wheeled_tasks/chassis/locomotion_revision.py": "37d0dc94ff2ab70dbaf78b571167138861982390d6c26428cfefff38384dbd9d", + "src/wheeled_tasks/chassis/mechanism_observer.py": "d3162d7be3dd685dde6bd0187fb03fc93245bd019c54757b3e9dcc5355be13d1", + "src/wheeled_tasks/chassis/motion_limits.py": "af66bba4c0d677925ac5ed807cef4128babe53e19d722abf7e75b24337b9b546", + "src/wheeled_tasks/chassis/motion_rehearsal.py": "d831474a22a352ae9f991d0c6b76ba44643ed57d57137b8404267e4ce53f60da", + "src/wheeled_tasks/chassis/multi_pose_recovery.py": "4ef1bb1bb0282b2f7ef461d3e2bb27301c28bba0f5697a891b0cd686405e7e9b", + "src/wheeled_tasks/chassis/nonstandard_start.py": "2864376b8b9882170304e206333e13d372e9a474cb34d3efd8b3757b1ce172a9", + "src/wheeled_tasks/chassis/nonstandard_start_revision.py": "07de45f381499fc776dd1147e3eb1e03e88a41c2b3be9f416b93a5b534d2a6c6", + "src/wheeled_tasks/chassis/ordinary_large_stair.py": "8f827d016cdb0b655ef49a052ea6d1a12c4857f18978880c5d8743a570e782a8", + "src/wheeled_tasks/chassis/ordinary_posture_curriculum.py": "85f7a9f8a7d78988482f0051438da98f71acef343dd34fe9bd73d5f362d25ce8", + "src/wheeled_tasks/chassis/ordinary_posture_revision.py": "644fc5e33dac17ab5e6cd182c11cfe2ffb32b4c4bc555d682b187dd26f077a66", + "src/wheeled_tasks/chassis/paired_feedback.py": "492172df17e5f05428a8b75418bd10cc53d8e1dde39f3a148cca54e013fb993b", + "src/wheeled_tasks/chassis/paired_leg_targets.py": "7d711b122cdedfd0c42e384546921a9e181dc99f05c0a039eb441caa2d794ba4", + "src/wheeled_tasks/chassis/pc_communication.py": "64c00e4bb1fc3e580c30bc1ad71f193408db52a4d1cec3b3a92a158e7649249f", + "src/wheeled_tasks/chassis/performance_curriculum.py": "bb8af22fb772cf9cc9b9fe06d492c77de6b9b732040330bf2ffc60f96749a976", + "src/wheeled_tasks/chassis/physical_history.py": "ebe733835990b6d2cd9a62b7ec0b8221c09ac104fcd08c869e8578476ba57b22", + "src/wheeled_tasks/chassis/physical_standing_reward.py": "486cf949ac345351e1555fe8a889aea1339eeb31d68159e8afa4a0826fffb6ba", + "src/wheeled_tasks/chassis/playback_command.py": "506d51e0ca2442d55da264e58d8c91a6a2dc3a7ed713f1e675cfec1839019eaf", + "src/wheeled_tasks/chassis/policy_transfer.py": "afcdc141d4232396079c4d93b590d08e2793502904ca324cc9ed5d5d24e31082", + "src/wheeled_tasks/chassis/precise_standing_reward.py": "2a714c3d78e547339e4998453bcc4a9b998549596fd15b0963893b53ee30709e", + "src/wheeled_tasks/chassis/preflight.py": "a9302aa7bffd051a7a202885a64422bdfa5cfe9828613ebd619709a77e9c3bf9", + "src/wheeled_tasks/chassis/prone_motion.py": "1f805c18a3e497eb7a43b43a6602bd49a8cf90d30ccc4db65e85b25a1a875fc5", + "src/wheeled_tasks/chassis/prone_motion_cli.py": "bcdd9e995a9dcab1b17845453c98d620406a543c6357ef9568eb71a095b85829", + "src/wheeled_tasks/chassis/prone_motion_env.py": "66d1e4f4a772f62876f8fd902b867d3094bc1eac312f7c215d46eed3867f95d4", + "src/wheeled_tasks/chassis/prone_motion_performance.py": "8f56a0fa2285537c6e5fc9eb0e4fde3dcb32d436bee9339d7e5af3d0d0d98ab3", + "src/wheeled_tasks/chassis/prone_motion_sampling.py": "ff5ae71abe65c1c32b0ef3af79cfd0b7b41b2f7d68c9c2872d3dbacc4f67652e", + "src/wheeled_tasks/chassis/quiet_standing.py": "307ceb8de5487ecbadca251727edc9335532303ba561b89540094b78ebd91fe3", + "src/wheeled_tasks/chassis/ramp_flight.py": "3936f040d272ed1b09c845f546b4b60146f7a47e6a09f46672d44643f4d51123", + "src/wheeled_tasks/chassis/randomization_study.py": "80ec179c8af89f85f0439c0fd15e78815860a78c98ec2ab99eb6de6af187dec1", + "src/wheeled_tasks/chassis/recovery.py": "49fa07c4f29621aa60c9c603b5458f63b2013ae7326cdbd50d921e3855142842", + "src/wheeled_tasks/chassis/recovery_curriculum.py": "6617b7667dc703447b10c0ccd701424f833df51cf828f54e67b63e875b8e2a78", + "src/wheeled_tasks/chassis/recovery_entropy_revision.py": "23f4372c2a23188decfd2b4a19baf32ba43d71ae65f16b95a4e47897b73665f6", + "src/wheeled_tasks/chassis/recovery_impact.py": "05c8c1af506cd3460d9b9a188bb6f97010c08767e9816b0823eb5b2cc612f62b", + "src/wheeled_tasks/chassis/recovery_mastery.py": "a389580179613a615a597852ff28186f7ccec1b668671a37ab0f0b4f819ee2b7", + "src/wheeled_tasks/chassis/recovery_observer.py": "7671a7528fc19a3872ac1e1ec54fd763b999f054660861aac964ecadc60282c6", + "src/wheeled_tasks/chassis/recovery_prefix_curriculum.py": "e25a2a89db76fde2dff680a3c76af283989b6377e2a64896de1568a050e40e2c", + "src/wheeled_tasks/chassis/recovery_prefix_revision.py": "6716d014d686ff69d660ac4cc02d9b7b2122fbd272ab37d361372363b08bf129", + "src/wheeled_tasks/chassis/recovery_quality_revision.py": "6ac2ce49d57c1061ed03a519e7290904fb831d33dca6f64cbc351a0b9d9b4972", + "src/wheeled_tasks/chassis/recovery_revision.py": "bfaae477e29291a75672a2834dbef83407fbf4b7b987d80d41f863ad4de8260f", + "src/wheeled_tasks/chassis/recovery_script_revision.py": "4d59036e4e0f5e7662f56e94976a373de14a1f1f6a562368b51c517f8e1d28f0", + "src/wheeled_tasks/chassis/recovery_support_reward.py": "d0257576ee5d25657d19238f29c7aa8cb9de8c12a92195ae507ef52f17da9914", + "src/wheeled_tasks/chassis/recovery_training.py": "dcae1f616e7d06b8fcd504471b6f7dcdc33b1f24865ecf870a1d475cca702448", + "src/wheeled_tasks/chassis/references.py": "80d5ad36501714996df1d33958013ab302d72758838d48e8648100828dc1a14a", + "src/wheeled_tasks/chassis/release_stop.py": "3399e440e9021257f444e3ab8a527d168ae6e9663b062a3623f01c2df758109c", + "src/wheeled_tasks/chassis/remediation.py": "91ca961cbbce3ba418ea38cb756dee6f5dc8b7e9c583553b9ab16a4154334d52", + "src/wheeled_tasks/chassis/response_migration.py": "345294ac1a1d3d86580919231cb82b3235271028be1a6ee29fa0531cc9bfc40a", + "src/wheeled_tasks/chassis/rewards.py": "f4dc968cb323e50ad894b1715a9865f1202e4663c401e2f68bf64398a1b7818d", + "src/wheeled_tasks/chassis/robust_getup.py": "5ecc6d6c98bdbec050f78455efe6a5b9e842ba5fc70c0d48a751f3a206c57f99", + "src/wheeled_tasks/chassis/robust_getup_env.py": "f1a63f3db58dc7bbbcfcd7b34db2207ffa4d973ccd2536f219ec383c58c7c54e", + "src/wheeled_tasks/chassis/robustness.py": "b6d73543e0f9873c664e79d6ac6e0ca1f931864ef29abeaa4cab893a9c142339", + "src/wheeled_tasks/chassis/root_kinematics.py": "0c63be06fc026b0687aa4e9d9fd7dabb009f7c1ab24446244b672928368f610b", + "src/wheeled_tasks/chassis/scratch_standing_env.py": "83026f8b0602a7b5a33b0a1515af3f2079ac43a4c37d9ed68b46465df90f02e9", + "src/wheeled_tasks/chassis/scut_base_tracking.py": "2acce09e0d31184eb191faf592215a67a445573729fbaf0406701c84e9e3bfea", + "src/wheeled_tasks/chassis/scut_curriculum_revision.py": "eb3759d600c722345c4e8ffe082b9c8f13dc1e401bf0fd25d2ecadfae5c8b44e", + "src/wheeled_tasks/chassis/scut_jump_revision.py": "984643e343a0202b5b0c525f4992aad767222e51f8185945ddbca1d686c44350", + "src/wheeled_tasks/chassis/scut_observation.py": "fbb3980fb75bd947fb4121892fe5a22d8f3c0d002e5ef3e9d637a12921c7e2b5", + "src/wheeled_tasks/chassis/scut_reuse_curriculum.py": "fa0b5ab7a399157b4fb87eb97f89739c1c6d14ba047d54aee8f1047eff702e13", + "src/wheeled_tasks/chassis/scut_terrain_revision.py": "d61a107418694d5a261c18700a847d245e8166f7feb295c6b4d25c379234dc54", + "src/wheeled_tasks/chassis/side_fall_study.py": "72aed14a51668fad673c211bf3d33658d10b17e1cca680fd51b85414f6c4206d", + "src/wheeled_tasks/chassis/signal_delay.py": "64bc34d73e297400e5d96bcdb9b8f4a325747d470ce5884ee1e5834576f5b725", + "src/wheeled_tasks/chassis/skill_commands.py": "df86738911abbc52e683368c809c9a09977f781d5eb642484bbaf27f20c275ab", + "src/wheeled_tasks/chassis/skill_curriculum.py": "3efc8e18b671e3f5177b4544488b8655a19f46c90695db80d8d0b1fb4c2de446", + "src/wheeled_tasks/chassis/slow_recovery.py": "9e9046d2d047c4f044c9548aec7d090f43c33ca0193a0f0551a349638a480f34", + "src/wheeled_tasks/chassis/small_stair_revision.py": "6ff1c050195323efe851c38da35a7c3cb8271f9a2696f854cc9058ce21532478", + "src/wheeled_tasks/chassis/standard_prone.py": "5638ce549ba457734d0217baefd62ae79093c00ee54132e3002713ac36d6ddc8", + "src/wheeled_tasks/chassis/standard_prone_env.py": "ba9d334bfb92a3f811ccf05148e0b5f3d527e76c8e408ba893f54cf69fd25b90", + "src/wheeled_tasks/chassis/standing009_curriculum.py": "2a3876748d15f9e25b14eeadbb94021ef2485a7a612ef21fa3176918b17ba763", + "src/wheeled_tasks/chassis/standing009_env.py": "882cce59800770923f3e69836386bd8b0540a8a6cdc94858388a433537d51c6b", + "src/wheeled_tasks/chassis/standing009_retention.py": "509da4baf2041fda79b653cfc86f06ebd2f61b959968fafc9aef40bb6572b988", + "src/wheeled_tasks/chassis/standing_contact_continuity.py": "38c7961c8df2dc158c338fcd50edc839cc627e018cadc2c6303c4b06e926bce3", + "src/wheeled_tasks/chassis/standing_contact_monitor.py": "932d9ca3edc3ee19d610c28af3725a52831704c59b9cf2dff84b5e3d05161e79", + "src/wheeled_tasks/chassis/standing_contact_reward.py": "423c1024267637f105d86e83f5c73c25c9da1893aa3adad075adb67716fc30b8", + "src/wheeled_tasks/chassis/standing_failure_termination.py": "516ff33f5504eaeda0549287dddde58c6004b87fb09b1addc53463dae8a96f0e", + "src/wheeled_tasks/chassis/standing_geometry107.py": "4ba929cd6898b335b23e4d2e2a68724ded593d516302df9077e9bfdfc3a507af", + "src/wheeled_tasks/chassis/standing_leg_target_rate.py": "fa3c3f4a136e2bfc11069e32d036844f6374a6ed9541b1f4cf77cf4b56860ed4", + "src/wheeled_tasks/chassis/standing_local_odometry.py": "a3eacbfb96ed0afc1747df219c294a7cbba2589b423864f4c459bb0b930d4682", + "src/wheeled_tasks/chassis/standing_recovery_reference.py": "3811d59d19dcce02c730f30c1dd3897e0241f7a191c0135318248fb46cc561c3", + "src/wheeled_tasks/chassis/standing_sensor_uncertainty.py": "37fd7efd29483e20e5de457f106d5c2b48d99be2e01325de781cdd4c695d2cc2", + "src/wheeled_tasks/chassis/standing_sensor_winding.py": "bfe05f94b466ad72f26178c0482f7429186df08927fe489c43463044d65bc9ac", + "src/wheeled_tasks/chassis/standing_support.py": "049d4d4611404a722f0a8005fb59236955b5953d8f9cad72fe10ac5f8f09d032", + "src/wheeled_tasks/chassis/state_snapshot.py": "e988163a3921ddf83b596412e5ee593a5d6a6a56498c087357b7eb0b93c8e7ff", + "src/wheeled_tasks/chassis/step_assist.py": "3f5a4a2d68d99ac98c3f4296ec581d70cde515202be757853d2bf4c73d82c748", + "src/wheeled_tasks/chassis/step_climb.py": "e050a749879e1649b7f8508a9fa1ed67e328953ba6fe06debc86401ee91c8694", + "src/wheeled_tasks/chassis/step_jump.py": "b4c6c2dbbcc4eed71a6e31f06120cf0fe8e9abb16a5f46a107ebeb35d8b7ff8d", + "src/wheeled_tasks/chassis/step_sensors.py": "6ceea4b0096e84b8622adee9312d349bf69fa313bcce2341aaa495ca2169e145", + "src/wheeled_tasks/chassis/step_trigger.py": "8419ddb18dc9f75049ca6b56e6907790ee4bbe451d1ba5eedbf210fc74f00188", + "src/wheeled_tasks/chassis/task.py": "124caab37c1e220f286ff473e151822cbfa9d980dc3c55db650038cf334303e1", + "src/wheeled_tasks/chassis/task_campaign.py": "8472e457a3e50fdb2d06331f95b34eb60e82b74f35aee8f8939049b180c6ac38", + "src/wheeled_tasks/chassis/teleop.py": "38b25aff36216b1f3fee0fa32717042f725587ce5259f21373eaf9685aa84fc5", + "src/wheeled_tasks/chassis/tensor_cuda_graph.py": "bca28b331ae2d9e6a805801d116396ec0971ebc4acdab6b8e4233bb841d66574", + "src/wheeled_tasks/chassis/terrain_rehearsal_revision.py": "07d47967b39a6c8df8df3182a65d955262a7c8f969fd73718c45579c1268c52f", + "src/wheeled_tasks/chassis/terrain_runout.py": "ac84d5ac7232250cce281beb09cb16046ba31cc36e9da612a0a7c763d64e0908", + "src/wheeled_tasks/chassis/terrain_runout_revision.py": "3877edd3c9a623a5bad9e98c00f5ca170e9accd073626b3ba7599925f3b89147", + "src/wheeled_tasks/chassis/terrain_runout_runtime.py": "4db4183cffd3fb65ef47f3c9f79c09af3e26c0dd0fb2ae88cc90687e16c9ef59", + "src/wheeled_tasks/chassis/terrain_schedule_revision.py": "fec047ed08df8e12b29598ea8e8479292c6acf7b395a0087d02453e0b77c0093", + "src/wheeled_tasks/chassis/torque_monitor.py": "0b28995db19a6f46e2401165ea7f154af8ea947f426fcfd374b7f4d1774555f0", + "src/wheeled_tasks/chassis/training_gas_spring.py": "04b9befb981753966e9f64ae173dd960e5ac0a55702a951dc44b2b93fe8f1cc7", + "src/wheeled_tasks/chassis/training_schedule.py": "6d0bd9fe99d7e4af0b793ceb0f4f3734f6233d0d3a83d3d686ab6be584bd23e8", + "src/wheeled_tasks/chassis/training_timing.py": "f28bf42c951a2f9ad05ced79c9c9c64121b1c62e239fd8a9eb540969a73264a9", + "src/wheeled_tasks/chassis/training_wheel_response.py": "24435566a554cf1e004f45f103428ff42554a3131bf82ec8215f8d08618a92b4", + "src/wheeled_tasks/chassis/transient_campaign_plan.py": "c609e2ed09c3fc9ee24e46b38310188e7523714bb8f1daa97a4206096318a3e2", + "src/wheeled_tasks/chassis/transient_dwell_plan.py": "3ef19fb2375ec290125ed4e162d2216cf43acb36232adbc21cd62a5fc5ae6279", + "src/wheeled_tasks/chassis/transient_leg_feedback_plan.py": "e3e49c2a1044010019a7db4ad31baf41dba6cc9ff0e0fcbda7e35b78cb71957a", + "src/wheeled_tasks/chassis/transient_response.py": "276892bc536912d2c43bedc9ca619527a48db2d626b9c3b673142faa0b6de3a5", + "src/wheeled_tasks/chassis/transient_response_plan.py": "656969785bb46b9e704c35355bacec40ac291598f16a5b4db7ed21158509bb26", + "src/wheeled_tasks/chassis/unified_getup.py": "c4e2d57a8604812fa0a1f35884517c01214c4da038e9c65085197904d31f5035", + "src/wheeled_tasks/chassis/unified_getup_env.py": "d7c78417e6e1743e24ae02f8eaed5023618a7b0a4f4a5052d14f06c582519ce0", + "src/wheeled_tasks/chassis/upright_locomotion.py": "f99755a0078036731bf8c1a673658d3df74fc6d5ad368d7f341c09698bead549", + "src/wheeled_tasks/chassis/upright_locomotion_env.py": "d08d613d790cdb54e8675d353b841902a6e27de5bf09d8e45e64e3f10c6cafae", + "src/wheeled_tasks/chassis/usb_transport.py": "04487b7c4c9010b5e8ffdbde87b819140b615e8ddde0b84bbd3454b73f406d8c", + "src/wheeled_tasks/chassis/v5_control.py": "141e21d88513e5f8cccdb6cdbf19abcd66f433c6e0ccb3cd7e93d5afc0c7c7fe", + "src/wheeled_tasks/direct/__init__.py": "4349e745e864cb63a38b992e982e5e47fd1b1d93edc214bb7793d99be93ab657", + "src/wheeled_tasks/direct/v40_serial/__init__.py": "065d4430332b2887ac1e2f749e3c83e3f06077156c98b4efd27221d0604ee756", + "src/wheeled_tasks/direct/v40_serial/env.py": "66e26b470a128c7504d3df17111cd9cc0cf3183a0daec1888b702dacea6f0ca0", + "src/wheeled_tasks/direct/v40_serial/env_cfg.py": "551dbc78197d8c9e1c20f8d9456bfb07ff1cb84e8491ee1c50928790b3a51a9e", + "src/wheeled_tasks/direct/wheeled_biped/__init__.py": "8662decdb342a928a803bef340c5d7422d5d6f3a0e82f3b4a80c76591a3c3be8", + "src/wheeled_tasks/direct/wheeled_biped/env.py": "dfac055af7926ed1051bf57f50014292665ba6dc7565ebd6e0372b371d0c2dd7", + "src/wheeled_tasks/direct/wheeled_biped/env_cfg.py": "b79cb340eb5e4be65cb2a93b6fd72232121f121e7b1614809fcfeb2854d17710", + "src/wheeled_tasks/direct/wheeled_biped/state_machines/__init__.py": "34d4be971ef8bbb1ffa3738176afc8b4f010b6ce98e8849663ae6163c2e5d317", + "src/wheeled_tasks/manager/__init__.py": "1d5144e72325c5755bdfb7cff7acd5cf1f23f240e40e734878ea25d69e7db8d9", + "src/wheeled_tasks/manager/mdp/__init__.py": "0c223c316b2545c87d1443d3bef74194b06ad159e5378bd883a2e92b9ce6e5d4", + "src/wheeled_tasks/manager/mdp/commands.py": "f561220cb3f5041d1425443191601831bdb40e4a93050110efe4fcf28fec7364", + "src/wheeled_tasks/manager/mdp/curriculums.py": "9068051bd64868812c42a1906c4f6f1b9d4aec8a8e131a5a57f4a132e60e5d3e", + "src/wheeled_tasks/manager/mdp/delay.py": "1ac59b60bfd8c8f36ddfa4a1d0fc394e14a875394c78b0072cd454bd54139d6c", + "src/wheeled_tasks/manager/mdp/events.py": "3be91ffb3e76c3418bbb9a3a206110820a5940cad7ea7600e1ca6d71b830942f", + "src/wheeled_tasks/manager/mdp/jump_rewards.py": "8cc2c97fe2b87b692dc54d070a6d8db3c02e3d2a35f0532c72404d29650a60c2", + "src/wheeled_tasks/manager/mdp/terrain.py": "f8610b4b00f44a19681c5e9e96c3ff75a0e1c14d96f9ad6acdaf99a6bbad96f1", + "src/wheeled_tasks/manager/mdp/terrain_cmd.py": "8d9dc86a795e5471f5b2ea09d42270714c0aa043bd86c457c148abde2a90ad57", + "src/wheeled_tasks/v40/__init__.py": "0c3d36963f42a18fca9a65bb3a9f26774d34efd1b7ee2a0758b613395fb722a6", + "src/wheeled_tasks/v40/contract.py": "7f16335d383ad4f392a5b4dbee98b012f59b83be78b718a5c976ad2be903944f", + "src/wheeled_tasks/v40/core.py": "86975086b43808865cc2710cd77b4cd8eae95ab84ca9f18309c023110bceb326", + "src/wheeled_tasks/wheelbipe_port/__init__.py": "32d06b64f21811e762e36d6b8f7616bdfb29ee38d13c6ad064b4ef7945855da2", + "src/wheeled_tasks/wheelbipe_port/config.py": "3a2b11627ecea35fd37a6dc5b75966352c5c312c691abdd3425d5f9f4815b636", + "src/wheeled_tasks/wheelbipe_port/environment.py": "07515846d17c66eb3eef8cbadaa0594383ca31cc36b2e9e72c50808411e761f9", + "src/wheeled_tasks/wheelbipe_port/model.py": "59edbfc733384ebfcab6ba1912c599e3298e8a0fff1d4bad1f1e41b21a2149b2", + "src/wheeled_tasks/wheelbipe_port/training_contract.py": "c5c2697fb3aa76a0314980e3d2e2fe74489f8c4246823f80bbe3ad956fd26c63", + "src/wheeled_tasks/wheelbipe_port/training_profile.py": "68203e18f775b1b30297b4983d7d1cd4d4390bdd29b31fe6882f33564b7713ba", + "src/wheeled_world/__init__.py": "ce657a3ab4d5d2f176ddf24bd125f6ba84e3444d7bedbd600fdd17f29b116b53", + "src/wheeled_world/actuators/__init__.py": "b94cdeab330a4a165bfc3e379d95c08451ea75e0e0e8c38a917acfd67d2881bb", + "src/wheeled_world/actuators/m3508_curve.py": "6fbb9f25d877c113e1e7c4d71d8c8336f3ac436840975348ca286f41ce61f255", + "src/wheeled_world/assets/__init__.py": "fd52316c70e94c05fd5d348cd944647616ba131f3de07c6a6f5cb11047575c77", + "src/wheeled_world/assets/v40.py": "6970bdbe4fda5ca0fad4578d6a62f5a2e99d7b134812fded7712e05e69e7ec78", + "src/wheeled_world/assets/wheelbipe_v33.py": "6d53bbfae88775095a9eb34df68860c24620eebd379bf47532c03cf8fd084bef", + "src/wheeled_world/terrains/__init__.py": "a7c20fc69638e75ba721bf0eb86ce4966fe8890a8fcb0d8ee32b95875000bd57", + "scripts/train_wheelbipe_port.py": "925afe4dfe63f61bde7f1d87253cbf34062f3d7a30bf67ccc854db31c2755ee2", + "scripts/export_wheelbipe_port_onnx.py": "75b98a8361b467016b353181a2c31611db6903acc442a56fdc05f42bd18eb3bd" + }, + "env_yaml_sha256": "5dc69eb22af2480ffaf5cfa0cfd36d700820a811046bcbf8c71076734e589467", + "agent_yaml_sha256": "9963e14144376c6412a82a44038fa409e0c0011b059c1a5ae2d9b65d55303983", + "observed_completed_updates": 230, + "observation": { + "schema": "wheelbipe-v202-foundation-v2", + "actor_dim": 35, + "policy_order": [ + "L_joint1", + "R_joint1", + "LL_joint1", + "RR_joint1", + "L_joint3", + "R_joint3" + ], + "layout": [ + "command_xyz3", + "height_command1_times5", + "gyro3_times0.5", + "projected_gravity3", + "raw_relative_leg_position4", + "muted_wheel_position2", + "joint_velocity6_times0.1", + "previous_unclipped_policy_action6", + "normal_mode7" + ], + "input_clips": { + "command": 100.0, + "height": [ + 0.0, + 1.0 + ], + "gyro": 100.0, + "gravity": 100.0, + "position": 100.0, + "velocity": 200.0, + "previous_action": 100.0 + }, + "mode": [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "normalization": "none", + "history_length": 1, + "policy_hz": 50 + }, + "observation_contract_sha256": "bd8397a8cb057c5d7859ba79ffbc1310c7c75d2a9ddcfdb7ea844d02db001e69", + "policy_hz": 50, + "pd_hz": 200, + "gas_spring_compensation_enabled": false, + "physical_gas_spring_required": true, + "model_spec_sha256": "8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812" + } +} diff --git a/rmcs_ws/src/rmcs_bringup/config/contracts/wheelbipe_8500_scripted_20261103.json b/rmcs_ws/src/rmcs_bringup/config/contracts/wheelbipe_8500_scripted_20261103.json new file mode 100644 index 000000000..5e9406e87 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/contracts/wheelbipe_8500_scripted_20261103.json @@ -0,0 +1,823 @@ +{ + "schema": "rmcs-wheelbipe-near8000-scripted-deployment-v1", + "status": "integrated_offline_verified_script_recovery_failed", + "hardware_tested": false, + "behavior_qualified": false, + "live_training_modified": false, + "campaign": "full_flat_1024_20000_200hz_balanced_seed119_011", + "attempt": "0001-03d234624906", + "source_commit": "a603a7c9a5b4fa6d94e7b6846c944a42cdba8391", + "source_snapshot_sha256": "58e406a68713b23688c96b90b083c77c7125b8fbe9b42ebab284b0210f79e6a2", + "asset_manifest_sha256": "01b3f0e8714ad30236a1b45be9f61e2986a19f81504fc96ea5fd97bde5cbe2e0", + "candidates": { + "7500": { + "completed_updates": 7501, + "checkpoint_sha256": "61c7c73086cc21ec049acde6d21ab7de2160522913d43683a12a1aa607226a08", + "evaluation_host": "kaiser", + "envs": 32, + "duration_s": 8, + "measurement_window_s": [ + 4, + 8 + ], + "cases": [ + { + "case": "stand_low", + "command": [ + 0.0, + 0.0, + 0.23 + ], + "resets": 0, + "mean_vx_m_s": 0.04084836691617966, + "mean_abs_vx_m_s": 0.04084836691617966, + "height_mae_m": 0.0038164069410413504, + "tilt_mean_deg": 0.26697465777397156, + "gap_peak_m": 8.553011139156297e-05 + }, + { + "case": "stand_mid", + "command": [ + 0.0, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": 0.012776381336152554, + "mean_abs_vx_m_s": 0.012776381336152554, + "height_mae_m": 0.008983264677226543, + "tilt_mean_deg": 0.35669082403182983, + "gap_peak_m": 3.354763248353265e-05 + }, + { + "case": "stand_high", + "command": [ + 0.0, + 0.0, + 0.42 + ], + "resets": 0, + "mean_vx_m_s": 0.07881119847297668, + "mean_abs_vx_m_s": 0.07881119847297668, + "height_mae_m": 0.010239928960800171, + "tilt_mean_deg": 0.4565146565437317, + "gap_peak_m": 1.7815533283283003e-05 + }, + { + "case": "forward", + "command": [ + 0.5, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": 0.4165031313896179, + "mean_abs_vx_m_s": 0.4165031313896179, + "height_mae_m": 0.010927912779152393, + "tilt_mean_deg": 0.5472559928894043, + "gap_peak_m": 3.672889215522446e-05 + }, + { + "case": "reverse", + "command": [ + -0.5, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": -0.4051765203475952, + "mean_abs_vx_m_s": 0.4051765203475952, + "height_mae_m": 0.00848840270191431, + "tilt_mean_deg": 0.3050115406513214, + "gap_peak_m": 3.131757694063708e-05 + } + ] + }, + "8000": { + "completed_updates": 8001, + "checkpoint_sha256": "0e817ac58eecd4d3649956a2e1dac69d44fbe239b7d58d3b37fda25c203f3824", + "evaluation_host": "local", + "envs": 32, + "duration_s": 8, + "measurement_window_s": [ + 4, + 8 + ], + "cases": [ + { + "case": "stand_low", + "command": [ + 0.0, + 0.0, + 0.23 + ], + "resets": 0, + "mean_vx_m_s": -0.02515212446451187, + "mean_abs_vx_m_s": 0.02515212446451187, + "height_mae_m": 0.003913540858775377, + "tilt_mean_deg": 0.279194712638855, + "gap_peak_m": 8.295888255815953e-05 + }, + { + "case": "stand_mid", + "command": [ + 0.0, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": -0.007931123487651348, + "mean_abs_vx_m_s": 0.008557259105145931, + "height_mae_m": 0.009320233017206192, + "tilt_mean_deg": 0.43359634280204773, + "gap_peak_m": 3.1848754588281736e-05 + }, + { + "case": "stand_high", + "command": [ + 0.0, + 0.0, + 0.42 + ], + "resets": 0, + "mean_vx_m_s": 0.07282265275716782, + "mean_abs_vx_m_s": 0.07282265275716782, + "height_mae_m": 0.00822633970528841, + "tilt_mean_deg": 0.3373720943927765, + "gap_peak_m": 2.5025066861417145e-05 + }, + { + "case": "forward", + "command": [ + 0.5, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": 0.3650336265563965, + "mean_abs_vx_m_s": 0.3650336265563965, + "height_mae_m": 0.012426191009581089, + "tilt_mean_deg": 0.3889496624469757, + "gap_peak_m": 3.0609699024353176e-05 + }, + { + "case": "reverse", + "command": [ + -0.5, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": -0.4007728397846222, + "mean_abs_vx_m_s": 0.4007728397846222, + "height_mae_m": 0.009436123073101044, + "tilt_mean_deg": 0.4254956841468811, + "gap_peak_m": 3.2754869607742876e-05 + } + ] + }, + "8500": { + "completed_updates": 8501, + "checkpoint_sha256": "fb36b4a9e5bd7f85f7bc3b4e28532b77edb7516fa1c46ed029a2a4d72fdd4a3f", + "evaluation_host": "kaiser", + "envs": 32, + "duration_s": 8, + "measurement_window_s": [ + 4, + 8 + ], + "cases": [ + { + "case": "stand_low", + "command": [ + 0.0, + 0.0, + 0.23 + ], + "resets": 0, + "mean_vx_m_s": -0.006370065733790398, + "mean_abs_vx_m_s": 0.007246531546115875, + "height_mae_m": 0.0038434844464063644, + "tilt_mean_deg": 0.6022952795028687, + "gap_peak_m": 8.290469122584909e-05 + }, + { + "case": "stand_mid", + "command": [ + 0.0, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": 0.0064705596305429935, + "mean_abs_vx_m_s": 0.0064705596305429935, + "height_mae_m": 0.0057403575628995895, + "tilt_mean_deg": 0.28168031573295593, + "gap_peak_m": 2.9872706363676116e-05 + }, + { + "case": "stand_high", + "command": [ + 0.0, + 0.0, + 0.42 + ], + "resets": 0, + "mean_vx_m_s": 0.0710696130990982, + "mean_abs_vx_m_s": 0.0710696130990982, + "height_mae_m": 0.009796342812478542, + "tilt_mean_deg": 0.4804050326347351, + "gap_peak_m": 2.0404037059051916e-05 + }, + { + "case": "forward", + "command": [ + 0.5, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": 0.397012859582901, + "mean_abs_vx_m_s": 0.397012859582901, + "height_mae_m": 0.00835499633103609, + "tilt_mean_deg": 0.2647896409034729, + "gap_peak_m": 2.9273724067024887e-05 + }, + { + "case": "reverse", + "command": [ + -0.5, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": -0.4029821753501892, + "mean_abs_vx_m_s": 0.4029821753501892, + "height_mae_m": 0.005120600573718548, + "tilt_mean_deg": 0.26965320110321045, + "gap_peak_m": 3.135736915282905e-05 + } + ] + } + }, + "selected_checkpoint": "model_8500.pt", + "selected_completed_updates": 8501, + "selection_reason": "Lower low/mid standing speed and most height errors than the nearby candidates in nominal seed111 tests; 7500 tracks forward speed better. This is a deployment candidate, not a statistically established optimum.", + "script_origin": { + "repository_commit": "2e883162762f0d658a7cc2e066ccbd00698c1d01", + "files": [ + "rmcs_ws/src/rmcs_core/src/controller/wheelleg/self_righting_core.hpp", + "rmcs_ws/src/rmcs_core/src/controller/wheelleg/recovery_geometry_v202.hpp" + ], + "full_fallen_start_qualified": false + }, + "configuration": "wheel-leg-infantry-wheelbipe-recovery.yaml", + "clocks": { + "physics_dt": 0.005, + "policy_hz": 50, + "decimation": 4, + "feedback_pd_hz": 200, + "rmcs_executor_hz": 1000, + "solver_iterations": [ + 64, + 32 + ] + }, + "qualification_limits": [ + "Nominal flat diagnostics only, no robustness or full capability acceptance", + "Kaiser training accumulated a 1.4104231595993042 m excluded-anchor distance peak; not repaired or used to stop training", + "No hardware/CAN test", + "Low-prone, roof-down and both side starts triggered script fault5 (trajectory not reached) before policy handoff", + "The scripted-start configuration is an integrated research candidate; fallen-start deployment is not qualified" + ], + "evidence_directory": "reports/wheelbipe8000_scripted_20261103", + "selected_extended_evaluation": { + "source": "reports/wheelbipe8000_scripted_20261103/eval8500_20s_no_timeout/receipt.json", + "host": "local", + "envs": 4, + "seed": 111, + "duration_s": 20, + "measurement_window_s": [ + 10, + 20 + ], + "episode_budget_s": 21, + "cases": [ + { + "case": "stand_low", + "command": [ + 0.0, + 0.0, + 0.23 + ], + "resets": 0, + "mean_vx_m_s": -0.007050737738609314, + "mean_abs_vx_m_s": 0.007621185854077339, + "height_mae_m": 0.0038683474995195866, + "tilt_mean_deg": 0.6044012904167175, + "gap_peak_m": 8.196041744668037e-05 + }, + { + "case": "stand_mid", + "command": [ + 0.0, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": 0.00667093601077795, + "mean_abs_vx_m_s": 0.00667093601077795, + "height_mae_m": 0.005745765753090382, + "tilt_mean_deg": 0.2819359004497528, + "gap_peak_m": 2.712214336497709e-05 + }, + { + "case": "stand_high", + "command": [ + 0.0, + 0.0, + 0.42 + ], + "resets": 0, + "mean_vx_m_s": 0.07099682092666626, + "mean_abs_vx_m_s": 0.07099682092666626, + "height_mae_m": 0.009793665260076523, + "tilt_mean_deg": 0.4805068075656891, + "gap_peak_m": 1.841204721131362e-05 + }, + { + "case": "forward", + "command": [ + 0.5, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": 0.3971460461616516, + "mean_abs_vx_m_s": 0.3971460461616516, + "height_mae_m": 0.008355526253581047, + "tilt_mean_deg": 0.2647579610347748, + "gap_peak_m": 2.919928010669537e-05 + }, + { + "case": "reverse", + "command": [ + -0.5, + 0.0, + 0.305 + ], + "resets": 0, + "mean_vx_m_s": -0.4028686583042145, + "mean_abs_vx_m_s": 0.4028686583042145, + "height_mae_m": 0.005121039692312479, + "tilt_mean_deg": 0.2700353264808655, + "gap_peak_m": 3.130850018351339e-05 + } + ] + }, + "onnx_cpp_parity": { + "status": "pass", + "samples": 1039, + "native_observation_frames": 15, + "max_absolute_error": 1.9073486328125e-06, + "hardware_tested": false + }, + "regression_parity": { + "status": "passed", + "hardware_io": false, + "behavior_qualified": false, + "prone_frames": 4778, + "prone_frame_max_absolute_error": 1.7881393432617188e-07, + "wheelbipe_frames": 256, + "wheelbipe_frame_max_absolute_error": 0.0, + "gas_samples": 1201, + "gas_max_absolute_error_Nm": 3.039894025391732e-08, + "profiles": { + "prone": { + "config_sha256": "dc36ceb032166ea2fc8760645deb71b0544721b0b8db73ff924308848a7d3276", + "model_sha256": "5e57f89ddb39efe36b1c5ce353606d2d3a289e65438b117ef25e0b379f3aa9c6", + "policy_samples": 239, + "policy_max_absolute_error": 2.384185791015625e-07, + "wrong_profile_model_rejected": true + }, + "wheelbipe": { + "config_sha256": "44c679b559c766e13648f74f8ca2a17451f6603be6e173cf4ee7b7e474d10608", + "model_sha256": "4abb1556fe69a1f11dcb546585d210f200f4b0ba4fca2a0c195098a29879b4e9", + "policy_samples": 256, + "policy_max_absolute_error": 9.5367431640625e-07, + "wrong_profile_model_rejected": true + } + } + }, + "native_recovery_evaluation": { + "status": "diagnostics_complete_not_qualified", + "cases": [ + { + "start": "stand", + "initial_condition": "pose bank geometry placement; no passive settle proof", + "handoff_s": 5.64, + "fault": 0, + "resets": 0, + "peak_gap_m": 5.260527541395277e-05, + "final_phase": 5, + "duration_s": 30.0, + "tail_mean_vx_height_tilt": [ + 0.00683005486925443, + 0.2992509517272313, + 0.28228739309310913 + ], + "pose_binding": { + "name": "stand", + "pose_bank_gimbal_q": { + "M_joint1": 0.0, + "M_joint2": 0.0 + }, + "actual_fixed_gimbal_q": [ + 0.0, + 0.0 + ], + "minimum_collision_clearance_m": 0.0 + } + }, + { + "start": "floor_knee103_103_orbit1.6_r0_p0", + "initial_condition": "pose bank geometry placement; no passive settle proof", + "handoff_s": null, + "fault": 5, + "resets": 0, + "peak_gap_m": 4.261145295458846e-05, + "final_phase": 255, + "duration_s": 5.62, + "pose_binding": { + "name": "floor_knee103_103_orbit1.6_r0_p0", + "pose_bank_gimbal_q": { + "M_joint1": 0.0, + "M_joint2": 0.0 + }, + "actual_fixed_gimbal_q": [ + 0.0, + 0.0 + ], + "minimum_collision_clearance_m": 0.008000000000000007 + } + }, + { + "start": "recovery_roof_down_front_front_00", + "initial_condition": "pose bank geometry placement; no passive settle proof", + "handoff_s": null, + "fault": 5, + "resets": 0, + "peak_gap_m": 8.794435416348279e-05, + "final_phase": 255, + "duration_s": 5.66, + "pose_binding": { + "name": "recovery_roof_down_front_front_00", + "pose_bank_gimbal_q": { + "M_joint1": -0.44605698598908283, + "M_joint2": 0.0 + }, + "actual_fixed_gimbal_q": [ + 0.0, + 0.0 + ], + "minimum_collision_clearance_m": 0.0022029110862565315 + } + }, + { + "start": "recovery_left_side_front_front_00", + "initial_condition": "pose bank geometry placement; no passive settle proof", + "handoff_s": null, + "fault": 5, + "resets": 0, + "peak_gap_m": 0.00023705627245362848, + "final_phase": 255, + "duration_s": 5.58, + "pose_binding": { + "name": "recovery_left_side_front_front_00", + "pose_bank_gimbal_q": { + "M_joint1": 0.6601212629069932, + "M_joint2": 0.0 + }, + "actual_fixed_gimbal_q": [ + 0.0, + 0.0 + ], + "minimum_collision_clearance_m": 0.0020000000000000018 + } + }, + { + "start": "recovery_right_side_front_front_00", + "initial_condition": "pose bank geometry placement; no passive settle proof", + "handoff_s": null, + "fault": 5, + "resets": 0, + "peak_gap_m": 0.00022669436293654144, + "final_phase": 255, + "duration_s": 5.62, + "pose_binding": { + "name": "recovery_right_side_front_front_00", + "pose_bank_gimbal_q": { + "M_joint1": -0.3924365117217459, + "M_joint2": 0.0 + }, + "actual_fixed_gimbal_q": [ + 0.0, + 0.0 + ], + "minimum_collision_clearance_m": 0.0020000000000000018 + } + } + ], + "physics_dt": 0.005, + "policy_hz": 50, + "feedback_pd_hz": 200, + "solver_iterations": [ + 64, + 32 + ], + "gap_scope": "Excluded-anchor distance at policy observation boundaries", + "script_library_sha256": "8f950362ecb8b4c21084b4dac6e4a3de46fd643631f39563cc46c6effcdf56cf", + "native_cases_scope": "One geometric pose-bank motor/base placement per case, with gimbal projected to the frozen zero-gimbal asset; no passive-settle or seed-sweep proof", + "intermediate_simulator_failure": { + "run": "native_recovery002", + "exit_code": 134, + "message": "carb.tasking::Mutex::lock Recursion not allowed after three completed cases; side cases rerun in separate processes" + }, + "first_invalid_harness": "native_recovery used policy fall termination during script; superseded by 002 and independent side cases", + "superseded_endpoint_reset_eval": "eval8500_20s hit the existing 20s episode timeout at its last frame; all five 4-lane evaluations repeated with a 21s episode budget" + }, + "capability_status": [ + { + "id": "initial_recovery", + "requirement": "Recover from initial front/back/left/right/upside-down and asymmetric leg poses; stabilize and continue the requested task", + "qualified": false, + "status": "failed_examples", + "reason": "All four selected fallen placements ended with script fault5, no actor handoff" + }, + { + "id": "moving_fall_recovery", + "requirement": "Detect and recover quickly after a real fall during forward/reverse/turning/spinning motion; resume the request without scripted assistance", + "qualified": false, + "status": "not_accepted", + "reason": "Only partial nominal diagnostics; no complete requirement assessment" + }, + { + "id": "standing", + "requirement": "Stable quiet low/mid/high standing after actual recovery completion, with continuous effective dual-wheel support allowing<=2ms pulses, small true XY drift and no sustained outward drift", + "qualified": false, + "status": "partial_nominal_diagnostics", + "reason": "0 resets in finite nominal standing windows; high standing speed drift remains about 0.071m/s; contact/XY acceptance not assessed" + }, + { + "id": "low_speed_straight", + "requirement": "Track low positive and negative speeds and the straight path smoothly", + "qualified": false, + "status": "partial_nominal_diagnostics", + "reason": "Commands +/-0.5m/s yield about +0.397/-0.403m/s; no full straight-path acceptance" + }, + { + "id": "high_speed_straight", + "requirement": "Track high forward and reverse speeds while retaining straight-path accuracy and balance", + "qualified": false, + "status": "not_accepted", + "reason": "Only partial nominal diagnostics; no complete requirement assessment" + }, + { + "id": "stationary_spin", + "requirement": "Target100rpm, at least80rpm, both directions, near the same ground location without falling or vibration", + "qualified": false, + "status": "not_accepted", + "reason": "Only partial nominal diagnostics; no complete requirement assessment" + }, + { + "id": "four_direction_arcs", + "requirement": "Forward-left, forward-right, reverse-left and reverse-right simultaneous translation and turning, including transitions", + "qualified": false, + "status": "not_accepted", + "reason": "Only partial nominal diagnostics; no complete requirement assessment" + }, + { + "id": "standing_height_change", + "requirement": "Smooth standing height changes without meaningful wheel lift, excessive drift or loss of balance; apply the post-completion<=2ms contact tolerance", + "qualified": false, + "status": "not_accepted", + "reason": "Only partial nominal diagnostics; no complete requirement assessment" + }, + { + "id": "moving_height_change", + "requirement": "Change height during forward/reverse/turning motion while maintaining tracking and balance", + "qualified": false, + "status": "not_accepted", + "reason": "Only partial nominal diagnostics; no complete requirement assessment" + }, + { + "id": "spinning_height_change", + "requirement": "Change height during both spin directions while maintaining yaw, location and balance", + "qualified": false, + "status": "not_accepted", + "reason": "Only partial nominal diagnostics; no complete requirement assessment" + }, + { + "id": "high_speed_braking", + "requirement": "Abruptly command zero from high forward and reverse speeds; stop promptly, smoothly and upright with small overshoot and little rebound", + "qualified": false, + "status": "not_accepted", + "reason": "Only partial nominal diagnostics; no complete requirement assessment" + }, + { + "id": "smoothness", + "requirement": "All tasks must avoid high-frequency leg/wheel excitation, contact chatter and dangerous measured joint speed", + "qualified": false, + "status": "not_accepted", + "reason": "Only partial nominal diagnostics; no complete requirement assessment" + }, + { + "id": "robustness", + "requirement": "Confirm nominal and randomized held-out dynamics, actuator/sensor/timing/friction domains, seeds and disturbances; preserve all failures", + "qualified": false, + "status": "not_accepted", + "reason": "Only partial nominal diagnostics; no complete requirement assessment" + } + ], + "training_still_running": { + "unix_time": 1793714338.3695393, + "pid": 1320791, + "start_ticks": 23335213, + "process_state": "S", + "argv": "/home/kaiser/wheelbipe-sim51/env/bin/python -u /home/kaiser/workspace/wheelbipe-v202-port/a603a7c9/scripts/train_wheelbipe_port.py --mode train --task flat --num-envs 1024 --iterations 20000 --save-interval 500 --seed 119 --device cuda:0 --headless --training-profile foundation-v2 --physics-hz 200 --leg-armature .015795 --leg-kp 60 --leg-kd 2 --leg-target-projection-penalty 1 --solver-position-iterations 64 --solver-velocity-iterations 32 --no-external-forces-every-iteration --randomization-profile requested --randomization-strength 1 --output /home/kaiser/workspace/wheelbipe-v202-port/runs/full_flat_1024_20000_200hz_balanced_seed119_011/attempts/0001-03d234624906/train --managed-control-dir /home/kaiser/workspace/wheelbipe-v202-port/runs/full_flat_1024_20000_200hz_balanced_seed119_011/attempts/0001-03d234624906/control ", + "cwd": "/home/kaiser/workspace/wheelbipe-v202-port/a603a7c9", + "completed_updates": 9480, + "training_status": "training", + "checkpoint_sha256": "fb36b4a9e5bd7f85f7bc3b4e28532b77edb7516fa1c46ed029a2a4d72fdd4a3f", + "max_constraint_gap_m": 1.4104231595993042, + "peak_diagnostic": { + "lane": 994, + "pin": 5, + "physical_call": 749120, + "sample_scope": "policy_observation_boundary", + "episode_time_s": 5.5200000000000005, + "initialized": true, + "gaps_m": [ + 0.13621333241462708, + 0.3538719415664673, + 0.1651465743780136, + 0.2716774344444275, + 0.6148353815078735, + 1.4104231595993042 + ], + "root_state": [ + -62.5504264831543, + -54.298561096191406, + 0.6415430307388306, + -0.44883695244789124, + -0.6553512215614319, + -0.5993499159812927, + -0.09919567406177521, + 2.7656021118164062, + -3.6029481887817383, + 3.8944549560546875, + 233.80902099609375, + 117.84221649169922, + -30.71749496459961 + ], + "joint_pos": [ + -4.041479587554932, + 1.434624195098877, + -1.7222093343734741, + 1.4573698043823242, + 0.6970804929733276, + -0.7672196626663208, + -0.15380236506462097, + -2.3536856174468994, + -4.789806842803955, + -0.9418294429779053, + -5.270866870880127, + 1.0145152807235718, + -5.359323501586914, + -2.438453197479248, + 0.06972765922546387, + 0.06972894817590714, + -0.4300231337547302, + 2.191075325012207 + ], + "joint_vel": [ + 70.7160415649414, + 99.79187774658203, + -114.09512329101562, + -96.76628875732422, + -100.02099609375, + -43.05046081542969, + -658.90380859375, + -6.427330017089844, + -15.308927536010742, + 37.756370544433594, + 2.852876663208008, + 34.85694122314453, + -104.21798706054688, + 138.13558959960938, + 77.8165283203125, + 258.44586181640625, + 32.450157165527344, + 100.46217346191406 + ], + "previous_motor_effort_Nm": [ + 40.0, + -40.0, + -4.499999523162842, + 40.0, + 40.0, + -0.0 + ] + }, + "live_training_modified": false, + "signals_sent_to_learner": 0 + }, + "geometry_binding": { + "canonical_model_spec_sha256": "8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812", + "physics_model_spec_sha256": "388dbdc69cbc7021ea02e553caaed098a05e237a6867b45ee7df172858050b29", + "fixed_zero_gimbal_only_spec_difference": true, + "native_pose_binding_audit": { + "status": "complete", + "fixed_gimbal_projection": true, + "all_numerical_geometry_except_gimbal_joint_release_identical": true, + "poses": [ + { + "name": "stand", + "pose_bank_gimbal_q": { + "M_joint1": 0.0, + "M_joint2": 0.0 + }, + "actual_fixed_gimbal_q": [ + 0.0, + 0.0 + ], + "minimum_collision_clearance_m": 0.0 + }, + { + "name": "floor_knee103_103_orbit1.6_r0_p0", + "pose_bank_gimbal_q": { + "M_joint1": 0.0, + "M_joint2": 0.0 + }, + "actual_fixed_gimbal_q": [ + 0.0, + 0.0 + ], + "minimum_collision_clearance_m": 0.008000000000000007 + }, + { + "name": "recovery_left_side_front_front_00", + "pose_bank_gimbal_q": { + "M_joint1": 0.6601212629069932, + "M_joint2": 0.0 + }, + "actual_fixed_gimbal_q": [ + 0.0, + 0.0 + ], + "minimum_collision_clearance_m": 0.0020000000000000018 + }, + { + "name": "recovery_right_side_front_front_00", + "pose_bank_gimbal_q": { + "M_joint1": -0.3924365117217459, + "M_joint2": 0.0 + }, + "actual_fixed_gimbal_q": [ + 0.0, + 0.0 + ], + "minimum_collision_clearance_m": 0.0020000000000000018 + }, + { + "name": "recovery_roof_down_front_front_00", + "pose_bank_gimbal_q": { + "M_joint1": -0.44605698598908283, + "M_joint2": 0.0 + }, + "actual_fixed_gimbal_q": [ + 0.0, + 0.0 + ], + "minimum_collision_clearance_m": 0.0022029110862565315 + } + ] + } + }, + "verification": { + "build": "build-rmcs: rmcs_core rmcs_rl rmcs_bringup passed", + "ctest": { + "rmcs_core": { + "passed": 11, + "failed": 0 + }, + "rmcs_rl": { + "passed": 2, + "failed": 0 + } + }, + "formatter": "clang-format passed", + "hardware_io": false + }, + "onnx_sha256": "f4baaa4daa4611a9d438ff75bf75dced4df81ceb28b22a5a1e41ca07425b0f85" +} diff --git a/rmcs_ws/src/rmcs_bringup/config/contracts/wheelbipe_jump_v4.json b/rmcs_ws/src/rmcs_bringup/config/contracts/wheelbipe_jump_v4.json new file mode 100644 index 000000000..a99a3c05e --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/contracts/wheelbipe_jump_v4.json @@ -0,0 +1,102 @@ +{ + "schema": "wheelbipe-jump-v4-deployment-v1", + "training_source_commit": "0f5cbd3805c2dab0c0595fe43ae61ad34d6f5a29", + "training_profile": "foundation-jump-v4", + "source_files_sha256": { + "contracts/wheelbipe_foundation_jump_v4.json": "c0f89adf0e83aa11557e2a29a3183309a9cca227f8d66848e7926a61c60865e7", + "model/wheel_leg_v2.0.2/friction_v1/isaac/gas_spring_model.json": "13db9ada6fa0e111a51406fc950ca0b970d1fd3de0d9ceeac07b22053a952aa6", + "model/wheel_leg_v2.0.2/friction_v1/isaac/manifest.json": "6f6c8ec5b4ec6b4345670a272a1e162111b369013a8f07dd1ed17254a5edb9b2", + "model/wheel_leg_v2.0.2/friction_v1/isaac/model_spec.json": "67f824bc29b05647e381f8584528d686955c2cfd99fd783ba40f382279bf2284", + "model/gas_spring_identified_symmetric_20261028/model.json": "eb0c709e8915c14a4cb852551a6079af89a6f4b8a9d135751fdf86cc98c1baf0" + }, + "model_spec_sha256": "67f824bc29b05647e381f8584528d686955c2cfd99fd783ba40f382279bf2284", + "gas_model_sha256": "13db9ada6fa0e111a51406fc950ca0b970d1fd3de0d9ceeac07b22053a952aa6", + "policy_hz": 50, + "feedback_pd_hz": 200, + "control": { + "leg_kp_nm_per_rad": 80.0, + "leg_kd_nm_s_per_rad": 1.5, + "virtual_pair_overtravel_rad": 0.2, + "virtual_pair_overtravel_scope": "jump-request", + "tuck_spring_compensation": { + "mode": "model", + "gain": 0.75, + "cap_nm": 16.0, + "error_on_rad": 0.02, + "error_full_rad": 0.1, + "inward_pd_full_nm": 2.0 + } + }, + "request": { + "standing_height_m": 0.305, + "crouch_height_m": 0.25, + "release_height_m": 0.39, + "crouch_seconds": 0.35, + "apex_range_m": [ + 0.42, + 0.54 + ], + "nominal_request_delay_s": 0.8, + "outcome_seconds": 4.5, + "moving_vx_max_m_s": 0.5 + }, + "nominal_position": [ + -0.42, + 0.13722330866417465, + 0.42, + -0.13721605817245947, + 0.0, + 0.0 + ], + "winding_bounds_rad": [ + [ + -0.1408932677945942, + 1.4282761689185115 + ], + [ + -0.14084732125908495, + 1.4283102010168576 + ] + ], + "real_state_soft_limits_rad": [ + -0.14084732125908495, + 1.4282761689185115 + ], + "real_state_margin_rad": 0.03, + "leg_torque_cap_nm": 40.0, + "wheel_torque_cap_nm": 4.5, + "observation_version": "wheelbipe-v202-foundation-jump-v4", + "observation": { + "size": 35, + "action_size": 6, + "normalization": "none", + "joint_select": [ + 0, + 2, + 1, + 3, + 4, + 5 + ], + "order": [ + "command", + "height", + "gyro", + "gravity", + "position", + "velocity", + "previous_action", + "auxiliary" + ], + "jump_context": "ordinary=[1,0,0,0,0,0,0]; request=[0,0,0,0,1,apex,elapsed_s]", + "command_inputs": "external request and elapsed time only; no contact or measured apex" + }, + "landing_buffer": { + "seconds": 0.3, + "scope": "training reward only; actor controls deployment landing" + }, + "behavior_qualified": false, + "hardware_tested": false, + "observation_contract_sha256": "f907a791b3d47f0f5e6a17a095dc51841f8085cadf1bbfda9ba273ade70efedd", + "deployment_contract_sha256": "8ce02510cc775eb5c8ab1118aa2830d6eb8a5c2cd4c4d30a967b5c329c092102" +} diff --git a/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-gimbal.yaml b/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-gimbal.yaml new file mode 100644 index 000000000..d2beafc36 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-gimbal.yaml @@ -0,0 +1,225 @@ +# 轮腿步兵「仅云台 + 自瞄 + 发弹」台架配置。 +# +# 启动:ros2 launch rmcs_bringup rmcs.launch.py robot:=wheel-leg-infantry-gimbal +# +# 与 wheel-leg-infantry-wheelbipe.yaml 的差异: +# * 不加载 RL 状态估计 / 观测 / 策略、底盘控制器与 mirror,因此没有任何组件输出 +# /wheel_leg/enable_request 与四个 /wheel_leg/*_joint/control_torque; +# * wheel_leg.leg_drive_enabled: false 是硬件硬门:四个 DM 腿关节只收 disable 与反馈轮询帧, +# 两个 M3508 驱动轮转矩恒为 0,永远不会被使能; +# * 云台(yaw LK4010 + pitch MG4010)与摩擦轮 / 拨弹轮不受该门影响。云台在遥控器在线 +# 且左右拨杆不是双下时使能,之后由遥控器摇杆/鼠标或自瞄驱动。 +# +# 仍需注意: +# * 下板(底盘板)必须在线:yaw LK4010 走它的 CAN3,DR16 与裁判系统串口也都挂在它上面。 +# 本配置只是不使能腿和轮子,不是“只接上板”。 +# * 发弹热量门来自裁判系统。裁判系统未接时 /referee/shooter/heat_limit 为 0, +# HeatController 的 allowance 恒为 0,拨弹轮不会转;这是既有安全逻辑,不是本配置的问题。 +# * 自瞄姿态基准:WheelLeg 硬件目前不发布 /gimbal/auto_aim/imu_snapshot 与 +# /gimbal/auto_aim/exposure_signal,所以采集端按自由采集运行、每帧姿态是单位四元数 +# (等价于假定云台水平且 yaw=0)。台架上打静止目标够用;要打移动目标必须先给 +# WheelLeg 补这两个输出,否则预测量会偏。 +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::WheelLeg -> wheel_leg + + - rmcs_core::referee::Status -> referee_status + + - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::controller::gimbal::PitchGravityFeedforward -> pitch_gravity_ff + + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller + + - rmcs::AutoAimComponent -> auto_aim_component + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + +wheel_leg: + ros__parameters: + serial_filter_bottom_board: "AF-CBB3-7053-32C5-F3B5-B460-A786-DA9A-1FF1" + serial_filter_top_board: "AF-46C7-7B12-9364-64F6-D95A-E4F4-9299-1B75" + yaw_motor_zero_point: 58379 + pitch_motor_zero_point: 12637 + + # 腿驱动硬门。false:四个 DM 关节永不使能,两个轮子转矩恒为 0, + # 云台单独按遥控器在线 + 非双下使能(不再依赖 /wheel_leg/enable_request)。 + leg_drive_enabled: false + + # 腿不使能,但 configure_motors() 仍校验这几个参数,保留原值以通过启动检查。 + dm_master_ids: [1, 2, 1, 2] + dm_mit_position_max: [3.141592653589793, 3.141592653589793, 3.141592653589793, 3.141592653589793] + dm_mit_velocity_max: [45.0, 45.0, 45.0, 45.0] + dm_mit_torque_max: [54.0, 54.0, 54.0, 54.0] + dm_control_torque_max: [40.0, 40.0, 40.0, 40.0] + + feedback_timeout_s: 0.05 + +gimbal_controller: + ros__parameters: + upper_limit: -0.6109 # up 35 deg + lower_limit: 0.5236 # down 30 deg + +# 这里不带 reset_interface:台架没有 /chassis/reset_count 生产者,积分为 0 也用不到。 +yaw_angle_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 5.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + output_min: -4.5 + output_max: 4.5 + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 1.5 + ki: 0.0 + kd: 0.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 6.0 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + output_min: -4.5 + output_max: 4.5 + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + feedforward: /gimbal/pitch/torque_ff + kp: 1.5 + ki: 0.0 + kd: 0.0 + +pitch_gravity_ff: + ros__parameters: + pitch_gravity_ff_gain: 0.9 + pitch_gravity_ff_phase: 0.75 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 660.0 + - 660.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 10000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 20.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.20 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 1.583 + ki: 0.0 + kd: 0.0 + +auto_aim_component: + ros__parameters: + # 裁判系统 ID 缺席时不强行绑定阵营,避免误判敌我。 + dangerous_fallback: "" + # 需要左键 / 左拨杆 DOWN 的开火意图,台架更安全;确认无误后可改 false 全自动开火。 + manual_shoot: true + enable_rune: true + track_ids: [FULL] + # 相机在 PitchLink 下的平移外参(米),需按实车相机安装填写。 + camera_translation: [0.0, 0.0, 0.0] + + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.04 + offset_yaw: 0.0 + offset_pitch: 0.0 + attack_window: 80.0 + degraded_angle_speed: 12.0 + window_redundancy: 0.8 + window_hysteresis: 0.2 + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 + rune_idle_duration: 0.4 + rune_shoot_duration: 0.2 + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 2000.0 + gain: 16.9807 + framerate: 249.1 + invert_image: false + rls_tau_sec: 10.0 + # 见文件头:WheelLeg 硬件不提供曝光触发与 IMU 快照,退回自由采集 + 单位姿态。 + use_hardware_sync: false + imu_snapshot_topic: "" + delay_ms: 6.5 + +# 发弹链路诊断:摩擦轮就绪 → 热量允许 → 拨弹轮指令/反馈/力矩,外加底盘母线电压。 +value_broadcaster: + ros__parameters: + update_rate: 50.0 + forward_list: + - /gimbal/friction_ready + - /gimbal/friction_jammed + - /gimbal/bullet_fired + - /gimbal/bullet_feeder/control_velocity + - /gimbal/bullet_feeder/velocity + - /chassis/voltage + - /chassis/supercap/enabled + - /gimbal/yaw/angle + - /gimbal/pitch/angle diff --git a/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-prone-v4.yaml b/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-prone-v4.yaml new file mode 100644 index 000000000..4dd029e3c --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-prone-v4.yaml @@ -0,0 +1,412 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::WheelLeg -> wheel_leg + + - rmcs_core::controller::chassis::WheelLegChassisController -> wheel_leg_chassis_controller + - rmcs_core::controller::chassis::WheelLegStateEstimator -> wheel_leg_state_estimator + - rmcs_core::controller::chassis::WheelLegRlController -> wheel_leg_rl_controller + + - rmcs_core::controller::chassis::WheelLegObservation -> wheel_leg_observation + - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::controller::gimbal::PitchGravityFeedforward -> pitch_gravity_ff + - 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::referee::Status -> referee_status + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + - rmcs_rl::RlBridge -> rl_bridge + + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_rl::WheelLegMirrorComponent -> wheel_leg_mirror + - rmcs_core::debug::ValueCollector -> value_collector + +wheel_leg: + ros__parameters: + serial_filter_bottom_board: "AF-CBB3-7053-32C5-F3B5-B460-A786-DA9A-1FF1" + serial_filter_top_board: "AF-46C7-7B12-9364-64F6-D95A-E4F4-9299-1B75" + yaw_motor_zero_point: 58379 + pitch_motor_zero_point: 12637 + + + dm_master_ids: [1, 2, 1, 2] + dm_mit_position_max: [3.141592653589793, 3.141592653589793, 3.141592653589793, 3.141592653589793] + dm_mit_velocity_max: [45.0, 45.0, 45.0, 45.0] + dm_mit_torque_max: [54.0, 54.0, 54.0, 54.0] + dm_control_torque_max: [40.0, 40.0, 40.0, 40.0] + + feedback_timeout_s: 0.04 + + +gimbal_controller: + ros__parameters: + upper_limit: -0.6109 # up 35 deg + lower_limit: 0.5236 # down 30 deg + +yaw_angle_pid_controller: + ros__parameters: + reset_interface: /chassis/reset_count + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 5.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + output_min: -4.5 + output_max: 4.5 + reset_interface: /chassis/reset_count + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 1.5 + ki: 0.0 + kd: 0.0 + +pitch_angle_pid_controller: + ros__parameters: + reset_interface: /chassis/reset_count + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 6.0 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + output_min: -4.5 + output_max: 4.5 + reset_interface: /chassis/reset_count + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + feedforward: /gimbal/pitch/torque_ff + kp: 1.5 + ki: 0.0 + kd: 0.0 + +pitch_gravity_ff: + ros__parameters: + pitch_gravity_ff_gain: 0.9 + pitch_gravity_ff_phase: 0.75 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 660.0 + - 660.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 10000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 20.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.20 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 1.583 + ki: 0.0 + kd: 0.0 + +wheel_leg_chassis_controller: + ros__parameters: + vx_max: 2.0 + vx_bias: 0.0 + vy_max: 0.0 + yaw_rate_max: 4.0 # Follow yaw cap; was 10.47, i.e. effectively unlimited. + spin_yaw_rate: 10.471975511965978 + yaw_stick_rate: 3.141592653589793 + following_velocity_kp: 15.0 + following_velocity_ki: 0.0 + following_velocity_kd: 0.0 + + deadzone: 0.05 + angular_z_invert: false + + command_height_min: 0.23 + command_height_max: 0.42 + default_command_height: 0.305 + height_rate: 0.1 + + arm_dwell_seconds: 0.0 + +wheel_leg_state_estimator: + ros__parameters: + policy: prone_motion + dt: 0.001 + velocity_sign: 1.0 + +wheel_leg_rl_controller: + ros__parameters: + policy: prone_motion + action_history_source: mapped + inference_timeout: 0.06 + heading_hold_enabled: false + height_reference_rate_max: 0.6666666666666666 + + self_righting: + enabled: false + simulation_only: true + hardware_mapping_verified: false + +wheel_leg_observation: + ros__parameters: + policy: prone_motion + +rl_bridge: + ros__parameters: + rl_base: /wheel_leg/rl + rl_model_path: models/wheel_leg/prone_motion_v4_100hz.onnx + model_type: mlp + + policy_rate: 100.0 + history_length: 1 + rl_obs_size: 35 + rl_feature_size: 35 + rl_action_size: 6 + obs_clip: 100.0 + + inference_timeout: 0.06 + max_action_age: 0.06 + + enable_interface: /wheel_leg/rl/input/enable + reset_interface: /chassis/reset_count + enable_default: false + invalid_value: "nan" + + observation_frame_sequence_interface: /wheel_leg/rl/input/frame_sequence + + normalization_from_metadata: false + expected_obs_normalization: embedded + expected_observation_version: wheel-leg-prone-motion-v4-odom-v2 + expected_observation_contract_sha256: 6613ab1b2a538fc31558b668117dbcb77bed4f84e93133d9c087c5752ec6c967 + expected_model_spec_sha256: 8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812 + expected_metadata: + leg_winding_version: paired_branch107_v1 + leg_target_model_spec_sha256: 8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812 + + observations: + order: [command, height, gyro, gravity, position, velocity, previous_action, auxiliary] + command: {source: /wheel_leg/rl/features/command, type: array, size: 3, precision: float32, clip: [-100.0, 100.0]} + height: {source: /wheel_leg/rl/features/height, precision: float32, scale: 5.0, clip: [-100.0, 100.0]} + gyro: {source: /wheel_leg/rl/features/gyro, type: array, size: 3, precision: float32, scale: 0.5, clip: [-100.0, 100.0]} + gravity: {source: /wheel_leg/rl/features/gravity, type: array, size: 3, precision: float32, clip: [-100.0, 100.0]} + position: {source: /wheel_leg/rl/features/position, type: array, size: 6} + velocity: {source: /wheel_leg/rl/features/velocity, type: array, size: 6, precision: float32, scale: 0.1, clip: [-100.0, 100.0]} + previous_action: {source: /wheel_leg/rl/features/previous_action, type: array, size: 6, precision: float32, clip: [-100.0, 100.0]} + auxiliary: {source: /wheel_leg/rl/features/auxiliary, type: array, size: 7} + actions: + order: [left_hip, left_knee, right_hip, right_knee, left_wheel, right_wheel] + left_hip: {output: /wheel_leg/rl/raw/left_hip_joint, scale: 0.25, clip: [-4.0, 4.0]} + left_knee: {output: /wheel_leg/rl/raw/left_knee_joint, scale: 0.25, clip: [-4.0, 4.0]} + right_hip: {output: /wheel_leg/rl/raw/right_hip_joint, scale: 0.25, clip: [-4.0, 4.0]} + right_knee: {output: /wheel_leg/rl/raw/right_knee_joint, scale: 0.25, clip: [-4.0, 4.0]} + left_wheel: {output: /wheel_leg/rl/raw/left_wheel, scale: 10.0, clip: [-90.0, 90.0]} + right_wheel: {output: /wheel_leg/rl/raw/right_wheel, scale: 10.0, clip: [-90.0, 90.0]} + +wheel_leg_mirror: + ros__parameters: + enabled: true + update_rate: 100.0 + +value_broadcaster: + ros__parameters: + update_rate: 50.0 + forward_list: + - /wheel_leg/rl/state + - /wheel_leg/rl/fault_latched + - /wheel_leg/feedback_fresh + - /wheel_leg/dm_control_ready + # frame_sequence 是 std::size_t,ValueBroadcaster 只转发 double/bool/int/uint8 + - /wheel_leg/recovery/phase + - /wheel_leg/recovery/fault + - /wheel_leg/left_hip_joint/status_code + - /wheel_leg/left_hip_joint/fault_code + - /wheel_leg/left_knee_joint/status_code + - /wheel_leg/left_knee_joint/fault_code + - /wheel_leg/right_hip_joint/status_code + - /wheel_leg/right_hip_joint/fault_code + - /wheel_leg/right_knee_joint/status_code + - /wheel_leg/right_knee_joint/fault_code + # 云台/发射机构遥测:只转发角度与转速,不转发力矩(torque)。 + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/left_friction/velocity + - /gimbal/right_friction/velocity + - /gimbal/bullet_feeder/velocity + +value_collector: + ros__parameters: + # 轮腿全过程记录(1000 Hz executor,每 5 拍落一行 → 200 Hz) + # 共 95 个信号 / 161 列(含 index),约 19 MB/分钟,/tmp 若是 tmpfs 请注意容量。 + # 列名规则:标量=信号名;Eigen::Vector3d=name/x,y,z;Quaterniond=name/w,x,y,z; + # 6 维向量=name[0..5],顺序固定为 left_hip, left_knee, right_hip, right_knee, + # left_wheel, right_wheel;2 维向量=name[0], name[1](左、右腿)。 + # 一次性记录请把下面这条从 components 列表注释掉,或用完删除 CSV。 + csv_path: "/tmp/wheel_leg_.csv" + write_interval: 5 + flush_interval: 200 + signals: + # --- 时间基:单调时钟,单位秒 --- + - /predefined/timestamp + + # --- 电源:四个 DM 关节共用底盘母线(DM 反馈帧只有 POS/VEL/T/T_MOS/T_Rotor,无电压字段) --- + - /chassis/voltage + - /chassis/supercap/voltage + - /chassis/power + + # --- 四个驱动腿关节(DM8009):反馈 / 下发力矩 / 驱动状态 / 策略原始动作 --- + - /wheel_leg/left_hip_joint/angle + - /wheel_leg/left_hip_joint/velocity + - /wheel_leg/left_hip_joint/torque + - /wheel_leg/left_hip_joint/control_torque + - /wheel_leg/left_hip_joint/max_torque + - /wheel_leg/left_hip_joint/temperature_mos + - /wheel_leg/left_hip_joint/temperature_rotor + - /wheel_leg/left_hip_joint/status_code + - /wheel_leg/left_hip_joint/fault_code + - /wheel_leg/rl/raw/left_hip_joint + + - /wheel_leg/left_knee_joint/angle + - /wheel_leg/left_knee_joint/velocity + - /wheel_leg/left_knee_joint/torque + - /wheel_leg/left_knee_joint/control_torque + - /wheel_leg/left_knee_joint/max_torque + - /wheel_leg/left_knee_joint/temperature_mos + - /wheel_leg/left_knee_joint/temperature_rotor + - /wheel_leg/left_knee_joint/status_code + - /wheel_leg/left_knee_joint/fault_code + - /wheel_leg/rl/raw/left_knee_joint + + - /wheel_leg/right_hip_joint/angle + - /wheel_leg/right_hip_joint/velocity + - /wheel_leg/right_hip_joint/torque + - /wheel_leg/right_hip_joint/control_torque + - /wheel_leg/right_hip_joint/max_torque + - /wheel_leg/right_hip_joint/temperature_mos + - /wheel_leg/right_hip_joint/temperature_rotor + - /wheel_leg/right_hip_joint/status_code + - /wheel_leg/right_hip_joint/fault_code + - /wheel_leg/rl/raw/right_hip_joint + + - /wheel_leg/right_knee_joint/angle + - /wheel_leg/right_knee_joint/velocity + - /wheel_leg/right_knee_joint/torque + - /wheel_leg/right_knee_joint/control_torque + - /wheel_leg/right_knee_joint/max_torque + - /wheel_leg/right_knee_joint/temperature_mos + - /wheel_leg/right_knee_joint/temperature_rotor + - /wheel_leg/right_knee_joint/status_code + - /wheel_leg/right_knee_joint/fault_code + - /wheel_leg/rl/raw/right_knee_joint + + # --- 两个驱动轮(DJI):反馈 / 下发力矩 / 温度 / 策略原始动作 --- + - /wheel_leg/left_wheel/angle + - /wheel_leg/left_wheel/velocity + - /wheel_leg/left_wheel/torque + - /wheel_leg/left_wheel/control_torque + - /wheel_leg/left_wheel/max_torque + - /wheel_leg/left_wheel/temperature_c + - /wheel_leg/rl/raw/left_wheel + + - /wheel_leg/right_wheel/angle + - /wheel_leg/right_wheel/velocity + - /wheel_leg/right_wheel/torque + - /wheel_leg/right_wheel/control_torque + - /wheel_leg/right_wheel/max_torque + - /wheel_leg/right_wheel/temperature_c + - /wheel_leg/rl/raw/right_wheel + + # --- 状态估计(机体) --- + - /wheel_leg/state/position + - /wheel_leg/state/velocity + - /wheel_leg/state/gravity + - /wheel_leg/state/attitude + - /wheel_leg/state/passive_knee + - /wheel_leg/state/passive_knee_rate + - /wheel_leg/state/shank + - /wheel_leg/state/shank_rate + - /wheel_leg/state/leg_length + - /wheel_leg/state/forward_velocity + - /wheel_leg/state/distance + - /wheel_leg/state/com_offset + - /wheel_leg/state/base_height + - /wheel_leg/state/valid + + # --- IMU --- + - /wheel_leg/imu/quaternion + - /wheel_leg/imu/angular_velocity + + # --- RL 闭环:输入 / 施加动作 / 观测帧 / 推理链路 --- + - /wheel_leg/rl/applied_action + - /wheel_leg/rl/input/command + - /wheel_leg/rl/input/height + - /wheel_leg/rl/input/frame + - /wheel_leg/rl/state + - /wheel_leg/rl/valid + - /wheel_leg/rl/healthy + - /wheel_leg/rl/action_seq + - /wheel_leg/rl/action_steady_ns + - /wheel_leg/rl/action_applied + - /wheel_leg/rl/fault_latched + + # --- 自起恢复 --- + - /wheel_leg/recovery/phase + - /wheel_leg/recovery/fault + - /wheel_leg/recovery/capture_ready + - /wheel_leg/recovery/capture_blockers + + # --- 硬件诊断 / 底盘指令 --- + - /wheel_leg/feedback_fresh + - /wheel_leg/feedback_available + - /wheel_leg/dm_control_ready + - /wheel_leg/dr16_fresh + - /chassis/control_velocity + - /chassis/control_height + - /chassis/control_enable + - /chassis/reset_count diff --git a/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-wheelbipe-jump-v4.yaml b/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-wheelbipe-jump-v4.yaml new file mode 100644 index 000000000..62d70dff7 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-wheelbipe-jump-v4.yaml @@ -0,0 +1,479 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::WheelLeg -> wheel_leg + + - rmcs_core::controller::chassis::WheelLegChassisController -> wheel_leg_chassis_controller + - rmcs_core::controller::chassis::WheelLegStateEstimator -> wheel_leg_state_estimator + - rmcs_core::controller::chassis::WheelLegRlController -> wheel_leg_rl_controller + + - rmcs_core::controller::chassis::WheelLegObservation -> wheel_leg_observation + + - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::controller::gimbal::PitchGravityFeedforward -> pitch_gravity_ff + - 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::referee::Status -> referee_status + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + + - rmcs_rl::RlBridge -> rl_bridge + + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_rl::WheelLegMirrorComponent -> wheel_leg_mirror + - rmcs_core::debug::ValueCollector -> value_collector + +wheel_leg: + ros__parameters: + serial_filter_bottom_board: "AF-CBB3-7053-32C5-F3B5-B460-A786-DA9A-1FF1" + serial_filter_top_board: "AF-46C7-7B12-9364-64F6-D95A-E4F4-9299-1B75" + yaw_motor_zero_point: 58379 + pitch_motor_zero_point: 12637 + + + dm_master_ids: [1, 2, 1, 2] + dm_mit_position_max: [3.141592653589793, 3.141592653589793, 3.141592653589793, 3.141592653589793] + dm_mit_velocity_max: [45.0, 45.0, 45.0, 45.0] + dm_mit_torque_max: [54.0, 54.0, 54.0, 54.0] + dm_control_torque_max: [40.0, 40.0, 40.0, 40.0] + + feedback_timeout_s: 0.05 + + +wheel_leg_chassis_controller: + ros__parameters: + vx_max: 3.0 + # Keep the policy command at zero while the stand drift hold is active. + vx_bias: 0.0 + vy_max: 0.0 + yaw_rate_max: 4.0 # Follow-mode yaw cap only; at policy training envelope. + spin_yaw_rate: 10.471975511965978 + yaw_stick_rate: 3.141592653589793 + following_velocity_kp: 15.0 + following_velocity_ki: 0.0 + following_velocity_kd: 0.0 + + # Low-bandwidth stand hold. It uses wheel-odometry distance/velocity and only + # engages for zero-stick AUTO stance; motion commands bypass it. + deadzone: 0.05 + angular_z_invert: false + + command_height_min: 0.23 + command_height_max: 0.42 + default_command_height: 0.305 + height_rate: 0.1 + + arm_dwell_seconds: 0.0 + +wheel_leg_state_estimator: + ros__parameters: + policy: wheelbipe-jump-v4 + dt: 0.001 + velocity_sign: 1.0 + +wheel_leg_rl_controller: + ros__parameters: + policy: wheelbipe-jump-v4 + action_history_source: raw + inference_timeout: 0.06 + drive_loss_debounce: 0.1 + heading_hold_enabled: false + height_reference_rate_max: 0.6666666666666666 + + jump: + enabled: true + target_apex: 0.46 + + self_righting: + enabled: false + simulation_only: true + hardware_mapping_verified: false + # Recovery reference rates capped at 2 rad/s; dynamic RL capture, no standing dwell. + fold_plant_speed_rad_s: 2.0 + insert_speed_rad_s: 2.0 + orbit_speed_rad_s: 2.0 + push_speed_rad_s: 2.0 + capture_speed_rad_s: 2.0 + side_speed_rad_s: 2.0 + +wheel_leg_observation: + ros__parameters: + policy: wheelbipe-jump-v4 + +gimbal_controller: + ros__parameters: + upper_limit: -0.6109 # up 35 deg + lower_limit: 0.5236 # down 30 deg + +yaw_angle_pid_controller: + ros__parameters: + reset_interface: /chassis/reset_count + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 5.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + output_min: -4.5 + output_max: 4.5 + reset_interface: /chassis/reset_count + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 1.5 + ki: 0.0 + kd: 0.0 + +pitch_angle_pid_controller: + ros__parameters: + reset_interface: /chassis/reset_count + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 6.0 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + output_min: -4.5 + output_max: 4.5 + reset_interface: /chassis/reset_count + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + feedforward: /gimbal/pitch/torque_ff + kp: 1.5 + ki: 0.0 + kd: 0.0 + +pitch_gravity_ff: + ros__parameters: + pitch_gravity_ff_gain: 0.9 + pitch_gravity_ff_phase: 0.75 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 660.0 + - 660.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 10000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 20.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.20 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 1.583 + ki: 0.0 + kd: 0.0 + +rl_bridge: + ros__parameters: + rl_base: /wheel_leg/rl + rl_model_path: models/wheel_leg/wheelbipe_jump_v4.onnx + model_type: mlp + + policy_rate: 50.0 + history_length: 1 + rl_obs_size: 35 + rl_feature_size: 35 + rl_action_size: 6 + obs_clip: 100.0 + + inference_timeout: 0.06 + max_action_age: 0.06 + + enable_interface: /wheel_leg/rl/input/enable + reset_interface: /chassis/reset_count + enable_default: false + invalid_value: "nan" + + observation_frame_sequence_interface: /wheel_leg/rl/input/frame_sequence + + normalization_from_metadata: false + expected_obs_normalization: none + expected_observation_version: wheelbipe-v202-foundation-jump-v4 + expected_observation_contract_sha256: f907a791b3d47f0f5e6a17a095dc51841f8085cadf1bbfda9ba273ade70efedd + expected_model_spec_sha256: 67f824bc29b05647e381f8584528d686955c2cfd99fd783ba40f382279bf2284 + expected_metadata: + leg_winding_version: paired_full_vehicle110_jump_v4 + leg_target_model_spec_sha256: 67f824bc29b05647e381f8584528d686955c2cfd99fd783ba40f382279bf2284 + wheelbipe_deployment_contract_sha256: "8ce02510cc775eb5c8ab1118aa2830d6eb8a5c2cd4c4d30a967b5c329c092102" + training_source_commit: "0f5cbd3805c2dab0c0595fe43ae61ad34d6f5a29" + gas_model_sha256: "13db9ada6fa0e111a51406fc950ca0b970d1fd3de0d9ceeac07b22053a952aa6" + feedback_pd_hz: "200" + gas_spring_compensation_enabled: "false" + tuck_spring_compensation: "model" + leg_kp: "80" + leg_kd: "1.5" + leg_target_overtravel_rad: "0.2" + leg_target_overtravel_scope: "jump-request" + pilot: "false" + + observations: + order: [command, height, gyro, gravity, position, velocity, previous_action, auxiliary] + command: {source: /wheel_leg/rl/features/command, type: array, size: 3, precision: float32, clip: [-100.0, 100.0]} + height: {source: /wheel_leg/rl/features/height, precision: float32, source_clip: [0.0, 1.0], scale: 5.0} + gyro: {source: /wheel_leg/rl/features/gyro, type: array, size: 3, precision: float32, source_clip: [-100.0, 100.0], scale: 0.5} + gravity: {source: /wheel_leg/rl/features/gravity, type: array, size: 3, precision: float32, clip: [-100.0, 100.0]} + position: {source: /wheel_leg/rl/features/position, type: array, size: 6, select: [0, 2, 1, 3, 4, 5], precision: float32, clip: [-100.0, 100.0]} + velocity: {source: /wheel_leg/rl/features/velocity, type: array, size: 6, select: [0, 2, 1, 3, 4, 5], precision: float32, source_clip: [-200.0, 200.0], scale: 0.1} + previous_action: {source: /wheel_leg/rl/features/previous_action, type: array, size: 6, select: [0, 2, 1, 3, 4, 5], precision: float32, clip: [-100.0, 100.0]} + auxiliary: {source: /wheel_leg/rl/features/auxiliary, type: array, size: 7} + actions: + order: [left_hip, right_hip, left_knee, right_knee, left_wheel, right_wheel] + left_hip: {output: /wheel_leg/rl/raw/left_hip_joint, scale: 0.5} + right_hip: {output: /wheel_leg/rl/raw/right_hip_joint, scale: 0.5} + left_knee: {output: /wheel_leg/rl/raw/left_knee_joint, scale: 0.5} + right_knee: {output: /wheel_leg/rl/raw/right_knee_joint, scale: 0.5} + left_wheel: {output: /wheel_leg/rl/raw/left_wheel, scale: 10.0, clip: [-90.0, 90.0]} + right_wheel: {output: /wheel_leg/rl/raw/right_wheel, scale: 10.0, clip: [-90.0, 90.0]} + +wheel_leg_mirror: + ros__parameters: + enabled: true + update_rate: 100.0 + +value_broadcaster: + ros__parameters: + update_rate: 50.0 + forward_list: + - /chassis/jump/active + - /chassis/jump/state + - /chassis/jump/elapsed + - /chassis/jump/target_apex + - /chassis/jump/reference_height + - /chassis/jump/accepted_count + - /chassis/jump/tuck_gate_left + - /chassis/jump/tuck_gate_right + - /wheel_leg/rl/state + - /wheel_leg/rl/fault_latched + - /wheel_leg/feedback_fresh + - /wheel_leg/feedback_available + - /wheel_leg/dm_control_ready + - /wheel_leg/recovery/phase + - /wheel_leg/recovery/fault + - /wheel_leg/recovery/capture_ready + - /wheel_leg/recovery/capture_blockers + - /wheel_leg/left_hip_joint/status_code + - /wheel_leg/left_hip_joint/fault_code + - /wheel_leg/left_knee_joint/status_code + - /wheel_leg/left_knee_joint/fault_code + - /wheel_leg/right_hip_joint/status_code + - /wheel_leg/right_hip_joint/fault_code + - /wheel_leg/right_knee_joint/status_code + - /wheel_leg/right_knee_joint/fault_code + # 云台/发射机构遥测:只转发角度与转速,不转发力矩(torque)。 + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/left_friction/velocity + - /gimbal/right_friction/velocity + - /gimbal/bullet_feeder/velocity + +value_collector: + ros__parameters: + # 轮腿全过程记录(1000 Hz executor,每 5 拍落一行 → 200 Hz) + # 包含逐轴收帧时间/序号与机身比力,便于区分反馈停更和机械卡滞。 + # 列名规则:标量=信号名;Eigen::Vector3d=name/x,y,z;Quaterniond=name/w,x,y,z; + # 6 维向量=name[0..5],顺序固定为 left_hip, left_knee, right_hip, right_knee, + # left_wheel, right_wheel;2 维向量=name[0], name[1](左、右腿)。 + # 一次性记录请把下面这条从 components 列表注释掉,或用完删除 CSV。 + csv_path: "/tmp/wheel_leg_.csv" + write_interval: 5 + flush_interval: 200 + signals: + - /chassis/jump/active + - /chassis/jump/state + - /chassis/jump/elapsed + - /chassis/jump/target_apex + - /chassis/jump/reference_height + - /chassis/jump/accepted_count + - /chassis/jump/tuck_gate_left + - /chassis/jump/tuck_gate_right + - /wheel_leg/rl/input/context + - /wheel_leg/rl/pd_target + - /wheel_leg/rl/pd_requested_effort + - /wheel_leg/rl/tuck_compensation + # --- 时间基:单调时钟,单位秒 --- + - /predefined/timestamp + + # --- 电源:四个 DM 关节共用底盘母线(DM 反馈帧只有 POS/VEL/T/T_MOS/T_Rotor,无电压字段) --- + - /chassis/voltage + - /chassis/supercap/voltage + - /chassis/power + + # --- 四个驱动腿关节(DM8009):反馈 / 下发力矩 / 驱动状态 / 策略原始动作 --- + - /wheel_leg/left_hip_joint/angle + - /wheel_leg/left_hip_joint/velocity + - /wheel_leg/left_hip_joint/feedback_steady_ns + - /wheel_leg/left_hip_joint/feedback_sequence + - /wheel_leg/left_hip_joint/torque + - /wheel_leg/left_hip_joint/control_torque + - /wheel_leg/left_hip_joint/max_torque + - /wheel_leg/left_hip_joint/temperature_mos + - /wheel_leg/left_hip_joint/temperature_rotor + - /wheel_leg/left_hip_joint/status_code + - /wheel_leg/left_hip_joint/fault_code + - /wheel_leg/rl/raw/left_hip_joint + + - /wheel_leg/left_knee_joint/angle + - /wheel_leg/left_knee_joint/velocity + - /wheel_leg/left_knee_joint/feedback_steady_ns + - /wheel_leg/left_knee_joint/feedback_sequence + - /wheel_leg/left_knee_joint/torque + - /wheel_leg/left_knee_joint/control_torque + - /wheel_leg/left_knee_joint/max_torque + - /wheel_leg/left_knee_joint/temperature_mos + - /wheel_leg/left_knee_joint/temperature_rotor + - /wheel_leg/left_knee_joint/status_code + - /wheel_leg/left_knee_joint/fault_code + - /wheel_leg/rl/raw/left_knee_joint + + - /wheel_leg/right_hip_joint/angle + - /wheel_leg/right_hip_joint/velocity + - /wheel_leg/right_hip_joint/feedback_steady_ns + - /wheel_leg/right_hip_joint/feedback_sequence + - /wheel_leg/right_hip_joint/torque + - /wheel_leg/right_hip_joint/control_torque + - /wheel_leg/right_hip_joint/max_torque + - /wheel_leg/right_hip_joint/temperature_mos + - /wheel_leg/right_hip_joint/temperature_rotor + - /wheel_leg/right_hip_joint/status_code + - /wheel_leg/right_hip_joint/fault_code + - /wheel_leg/rl/raw/right_hip_joint + + - /wheel_leg/right_knee_joint/angle + - /wheel_leg/right_knee_joint/velocity + - /wheel_leg/right_knee_joint/feedback_steady_ns + - /wheel_leg/right_knee_joint/feedback_sequence + - /wheel_leg/right_knee_joint/torque + - /wheel_leg/right_knee_joint/control_torque + - /wheel_leg/right_knee_joint/max_torque + - /wheel_leg/right_knee_joint/temperature_mos + - /wheel_leg/right_knee_joint/temperature_rotor + - /wheel_leg/right_knee_joint/status_code + - /wheel_leg/right_knee_joint/fault_code + - /wheel_leg/rl/raw/right_knee_joint + + # --- 两个驱动轮(DJI):反馈 / 下发力矩 / 温度 / 策略原始动作 --- + - /wheel_leg/left_wheel/angle + - /wheel_leg/left_wheel/velocity + - /wheel_leg/left_wheel/feedback_steady_ns + - /wheel_leg/left_wheel/feedback_sequence + - /wheel_leg/left_wheel/torque + - /wheel_leg/left_wheel/control_torque + - /wheel_leg/left_wheel/max_torque + - /wheel_leg/left_wheel/temperature_c + - /wheel_leg/rl/raw/left_wheel + + - /wheel_leg/right_wheel/angle + - /wheel_leg/right_wheel/velocity + - /wheel_leg/right_wheel/feedback_steady_ns + - /wheel_leg/right_wheel/feedback_sequence + - /wheel_leg/right_wheel/torque + - /wheel_leg/right_wheel/control_torque + - /wheel_leg/right_wheel/max_torque + - /wheel_leg/right_wheel/temperature_c + - /wheel_leg/rl/raw/right_wheel + + # --- 状态估计(机体) --- + - /wheel_leg/state/position + - /wheel_leg/state/velocity + - /wheel_leg/state/gravity + - /wheel_leg/state/attitude + - /wheel_leg/state/passive_knee + - /wheel_leg/state/passive_knee_rate + - /wheel_leg/state/shank + - /wheel_leg/state/shank_rate + - /wheel_leg/state/leg_length + - /wheel_leg/state/forward_velocity + - /wheel_leg/state/distance + - /wheel_leg/state/com_offset + - /wheel_leg/state/base_height + - /wheel_leg/state/valid + + # --- IMU --- + - /wheel_leg/imu/quaternion + - /wheel_leg/imu/angular_velocity + - /wheel_leg/imu/last_steady_ns + - /wheel_leg/imu/sequence + - /wheel_leg/imu/acceleration + - /wheel_leg/imu/acceleration_steady_ns + - /wheel_leg/imu/acceleration_sequence + + # --- RL 闭环:输入 / 施加动作 / 观测帧 / 推理链路 --- + - /wheel_leg/rl/applied_action + - /wheel_leg/rl/input/command + - /wheel_leg/rl/input/height + - /wheel_leg/rl/input/frame + - /wheel_leg/rl/state + - /wheel_leg/rl/valid + - /wheel_leg/rl/healthy + - /wheel_leg/rl/action_seq + - /wheel_leg/rl/action_steady_ns + - /wheel_leg/rl/action_applied + - /wheel_leg/rl/fault_latched + + # --- 自起恢复 --- + - /wheel_leg/recovery/phase + - /wheel_leg/recovery/fault + - /wheel_leg/recovery/capture_ready + - /wheel_leg/recovery/capture_blockers + + # --- 硬件诊断 / 底盘指令 --- + - /wheel_leg/feedback_fresh + - /wheel_leg/feedback_available + - /wheel_leg/dm_control_ready + - /wheel_leg/dr16_fresh + - /chassis/control_velocity + - /chassis/control_height + - /chassis/control_enable + - /chassis/reset_count diff --git a/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-wheelbipe-recovery.yaml b/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-wheelbipe-recovery.yaml new file mode 100644 index 000000000..5d14a9cf5 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-wheelbipe-recovery.yaml @@ -0,0 +1,451 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::WheelLeg -> wheel_leg + + - rmcs_core::controller::chassis::WheelLegChassisController -> wheel_leg_chassis_controller + - rmcs_core::controller::chassis::WheelLegStateEstimator -> wheel_leg_state_estimator + - rmcs_core::controller::chassis::WheelLegRlController -> wheel_leg_rl_controller + + - rmcs_core::controller::chassis::WheelLegObservation -> wheel_leg_observation + + - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::controller::gimbal::PitchGravityFeedforward -> pitch_gravity_ff + - 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::referee::Status -> referee_status + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + + - rmcs_rl::RlBridge -> rl_bridge + + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_rl::WheelLegMirrorComponent -> wheel_leg_mirror + - rmcs_core::debug::ValueCollector -> value_collector + +wheel_leg: + ros__parameters: + serial_filter_bottom_board: "AF-CBB3-7053-32C5-F3B5-B460-A786-DA9A-1FF1" + serial_filter_top_board: "AF-46C7-7B12-9364-64F6-D95A-E4F4-9299-1B75" + yaw_motor_zero_point: 58379 + pitch_motor_zero_point: 12637 + + + + dm_master_ids: [1, 2, 1, 2] + dm_mit_position_max: [3.141592653589793, 3.141592653589793, 3.141592653589793, 3.141592653589793] + dm_mit_velocity_max: [45.0, 45.0, 45.0, 45.0] + dm_mit_torque_max: [54.0, 54.0, 54.0, 54.0] + dm_control_torque_max: [40.0, 40.0, 40.0, 40.0] + + # Warning threshold only; after the first complete sample, stale CAN data + # is retained for diagnosis and does not force a control shutdown. + feedback_timeout_s: 0.05 + + +wheel_leg_chassis_controller: + ros__parameters: + vx_max: 3.0 + vx_bias: -0.32 + vy_max: 0.0 + yaw_rate_max: 4.0 # Follow-mode yaw cap only; at policy training envelope. + spin_yaw_rate: 10.471975511965978 + yaw_stick_rate: 3.141592653589793 + following_velocity_kp: 15.0 + following_velocity_ki: 0.0 + following_velocity_kd: 0.0 + + deadzone: 0.05 + angular_z_invert: false + + command_height_min: 0.23 + command_height_max: 0.42 + default_command_height: 0.305 + height_rate: 0.1 + + arm_dwell_seconds: 0.0 + +wheel_leg_state_estimator: + ros__parameters: + policy: wheelbipe + dt: 0.001 + velocity_sign: 1.0 + +wheel_leg_rl_controller: + ros__parameters: + policy: wheelbipe + action_history_source: raw + inference_timeout: 0.06 + heading_hold_enabled: false + height_reference_rate_max: 0.6666666666666666 + + # 2026-11-05:实车放行自起。simulation_only 与 hardware_mapping_verified + # 必须同时为 false/true,否则控制器会要求无人发布的 /wheel_leg/simulation。 + # 首次运行请吊起或架空;映射假设(髋膝正向、力矩取反、气簧符号、零位) + # 的静态核对见 .codex/report/zh-cn/wheel_leg_selfright_hw_mapping_20261105。 + self_righting: + enabled: true + simulation_only: false + hardware_mapping_verified: true + # Recovery reference rates capped at 2 rad/s; dynamic RL capture, no standing dwell. + fold_plant_speed_rad_s: 2.0 + insert_speed_rad_s: 2.0 + orbit_speed_rad_s: 2.0 + push_speed_rad_s: 2.0 + capture_speed_rad_s: 2.0 + side_speed_rad_s: 2.0 + drive_loss_debounce: 0.1 + +wheel_leg_observation: + ros__parameters: + policy: wheelbipe + +gimbal_controller: + ros__parameters: + upper_limit: -0.6109 # up 35 deg + lower_limit: 0.5236 # down 30 deg + +yaw_angle_pid_controller: + ros__parameters: + reset_interface: /chassis/reset_count + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 5.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + output_min: -4.5 + output_max: 4.5 + reset_interface: /chassis/reset_count + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 1.5 + ki: 0.0 + kd: 0.0 + +pitch_angle_pid_controller: + ros__parameters: + reset_interface: /chassis/reset_count + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 6.0 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + output_min: -4.5 + output_max: 4.5 + reset_interface: /chassis/reset_count + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + feedforward: /gimbal/pitch/torque_ff + kp: 1.5 + ki: 0.0 + kd: 0.0 + +pitch_gravity_ff: + ros__parameters: + pitch_gravity_ff_gain: 0.9 + pitch_gravity_ff_phase: 0.75 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 660.0 + - 660.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 10000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 20.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.20 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 1.583 + ki: 0.0 + kd: 0.0 + +rl_bridge: + ros__parameters: + rl_base: /wheel_leg/rl + rl_model_path: models/wheel_leg/wheelbipe_v202_16001_50hz.onnx + model_type: mlp + + policy_rate: 50.0 + history_length: 1 + rl_obs_size: 35 + rl_feature_size: 35 + rl_action_size: 6 + obs_clip: 100.0 + + inference_timeout: 0.06 + max_action_age: 0.06 + + enable_interface: /wheel_leg/rl/input/enable + reset_interface: /chassis/reset_count + enable_default: false + invalid_value: "nan" + + observation_frame_sequence_interface: /wheel_leg/rl/input/frame_sequence + + normalization_from_metadata: false + expected_obs_normalization: none + expected_observation_version: wheelbipe-v202-foundation-v2 + expected_observation_contract_sha256: bd8397a8cb057c5d7859ba79ffbc1310c7c75d2a9ddcfdb7ea844d02db001e69 + expected_model_spec_sha256: 8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812 + expected_metadata: + leg_winding_version: paired_full_vehicle110_v1 + leg_target_model_spec_sha256: 8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812 + + observations: + order: [command, height, gyro, gravity, position, velocity, previous_action, auxiliary] + command: {source: /wheel_leg/rl/features/command, type: array, size: 3, precision: float32, clip: [-100.0, 100.0]} + height: {source: /wheel_leg/rl/features/height, precision: float32, source_clip: [0.0, 1.0], scale: 5.0} + gyro: {source: /wheel_leg/rl/features/gyro, type: array, size: 3, precision: float32, source_clip: [-100.0, 100.0], scale: 0.5} + gravity: {source: /wheel_leg/rl/features/gravity, type: array, size: 3, precision: float32, clip: [-100.0, 100.0]} + position: {source: /wheel_leg/rl/features/position, type: array, size: 6, select: [0, 2, 1, 3, 4, 5], precision: float32, clip: [-100.0, 100.0]} + velocity: {source: /wheel_leg/rl/features/velocity, type: array, size: 6, select: [0, 2, 1, 3, 4, 5], precision: float32, source_clip: [-200.0, 200.0], scale: 0.1} + previous_action: {source: /wheel_leg/rl/features/previous_action, type: array, size: 6, select: [0, 2, 1, 3, 4, 5], precision: float32, clip: [-100.0, 100.0]} + auxiliary: {source: /wheel_leg/rl/features/auxiliary, type: array, size: 7} + actions: + order: [left_hip, right_hip, left_knee, right_knee, left_wheel, right_wheel] + left_hip: {output: /wheel_leg/rl/raw/left_hip_joint, scale: 0.5} + right_hip: {output: /wheel_leg/rl/raw/right_hip_joint, scale: 0.5} + left_knee: {output: /wheel_leg/rl/raw/left_knee_joint, scale: 0.5} + right_knee: {output: /wheel_leg/rl/raw/right_knee_joint, scale: 0.5} + left_wheel: {output: /wheel_leg/rl/raw/left_wheel, scale: 10.0, clip: [-90.0, 90.0]} + right_wheel: {output: /wheel_leg/rl/raw/right_wheel, scale: 10.0, clip: [-90.0, 90.0]} + +wheel_leg_mirror: + ros__parameters: + # Telemetry switch only: publishes even when the chassis/DM drives are disabled. + enabled: true + update_rate: 100.0 + +value_broadcaster: + ros__parameters: + update_rate: 50.0 + forward_list: + - /wheel_leg/rl/state + - /wheel_leg/rl/fault_latched + - /wheel_leg/feedback_fresh + - /wheel_leg/feedback_available + - /wheel_leg/dm_control_ready + - /wheel_leg/recovery/phase + - /wheel_leg/recovery/fault + - /wheel_leg/recovery/capture_ready + - /wheel_leg/recovery/capture_blockers + - /wheel_leg/left_hip_joint/status_code + - /wheel_leg/left_hip_joint/fault_code + - /wheel_leg/left_knee_joint/status_code + - /wheel_leg/left_knee_joint/fault_code + - /wheel_leg/right_hip_joint/status_code + - /wheel_leg/right_hip_joint/fault_code + - /wheel_leg/right_knee_joint/status_code + - /wheel_leg/right_knee_joint/fault_code + # 云台/发射机构遥测:只转发角度与转速,不转发力矩(torque)。 + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/left_friction/velocity + - /gimbal/right_friction/velocity + - /gimbal/bullet_feeder/velocity + - /wheel_leg/right_wheel/velocity + - /wheel_leg/left_wheel/velocity + +value_collector: + ros__parameters: + # 轮腿全过程记录(1000 Hz executor,每 5 拍落一行 → 200 Hz) + # 包含逐轴收帧时间/序号与机身比力,便于区分反馈停更和机械卡滞。 + # 列名规则:标量=信号名;Eigen::Vector3d=name/x,y,z;Quaterniond=name/w,x,y,z; + # 6 维向量=name[0..5],顺序固定为 left_hip, left_knee, right_hip, right_knee, + # left_wheel, right_wheel;2 维向量=name[0], name[1](左、右腿)。 + # 一次性记录请把下面这条从 components 列表注释掉,或用完删除 CSV。 + csv_path: "/tmp/wheel_leg_.csv" + write_interval: 5 + flush_interval: 200 + signals: + # --- 时间基:单调时钟,单位秒 --- + - /predefined/timestamp + + # --- 电源:四个 DM 关节共用底盘母线(DM 反馈帧只有 POS/VEL/T/T_MOS/T_Rotor,无电压字段) --- + - /chassis/voltage + - /chassis/supercap/voltage + - /chassis/power + + # --- 四个驱动腿关节(DM8009):反馈 / 下发力矩 / 驱动状态 / 策略原始动作 --- + - /wheel_leg/left_hip_joint/angle + - /wheel_leg/left_hip_joint/velocity + - /wheel_leg/left_hip_joint/feedback_steady_ns + - /wheel_leg/left_hip_joint/feedback_sequence + - /wheel_leg/left_hip_joint/torque + - /wheel_leg/left_hip_joint/control_torque + - /wheel_leg/left_hip_joint/max_torque + - /wheel_leg/left_hip_joint/temperature_mos + - /wheel_leg/left_hip_joint/temperature_rotor + - /wheel_leg/left_hip_joint/status_code + - /wheel_leg/left_hip_joint/fault_code + - /wheel_leg/rl/raw/left_hip_joint + + - /wheel_leg/left_knee_joint/angle + - /wheel_leg/left_knee_joint/velocity + - /wheel_leg/left_knee_joint/feedback_steady_ns + - /wheel_leg/left_knee_joint/feedback_sequence + - /wheel_leg/left_knee_joint/torque + - /wheel_leg/left_knee_joint/control_torque + - /wheel_leg/left_knee_joint/max_torque + - /wheel_leg/left_knee_joint/temperature_mos + - /wheel_leg/left_knee_joint/temperature_rotor + - /wheel_leg/left_knee_joint/status_code + - /wheel_leg/left_knee_joint/fault_code + - /wheel_leg/rl/raw/left_knee_joint + + - /wheel_leg/right_hip_joint/angle + - /wheel_leg/right_hip_joint/velocity + - /wheel_leg/right_hip_joint/feedback_steady_ns + - /wheel_leg/right_hip_joint/feedback_sequence + - /wheel_leg/right_hip_joint/torque + - /wheel_leg/right_hip_joint/control_torque + - /wheel_leg/right_hip_joint/max_torque + - /wheel_leg/right_hip_joint/temperature_mos + - /wheel_leg/right_hip_joint/temperature_rotor + - /wheel_leg/right_hip_joint/status_code + - /wheel_leg/right_hip_joint/fault_code + - /wheel_leg/rl/raw/right_hip_joint + + - /wheel_leg/right_knee_joint/angle + - /wheel_leg/right_knee_joint/velocity + - /wheel_leg/right_knee_joint/feedback_steady_ns + - /wheel_leg/right_knee_joint/feedback_sequence + - /wheel_leg/right_knee_joint/torque + - /wheel_leg/right_knee_joint/control_torque + - /wheel_leg/right_knee_joint/max_torque + - /wheel_leg/right_knee_joint/temperature_mos + - /wheel_leg/right_knee_joint/temperature_rotor + - /wheel_leg/right_knee_joint/status_code + - /wheel_leg/right_knee_joint/fault_code + - /wheel_leg/rl/raw/right_knee_joint + + # --- 两个驱动轮(DJI):反馈 / 下发力矩 / 温度 / 策略原始动作 --- + - /wheel_leg/left_wheel/angle + - /wheel_leg/left_wheel/velocity + - /wheel_leg/left_wheel/feedback_steady_ns + - /wheel_leg/left_wheel/feedback_sequence + - /wheel_leg/left_wheel/torque + - /wheel_leg/left_wheel/control_torque + - /wheel_leg/left_wheel/max_torque + - /wheel_leg/left_wheel/temperature_c + - /wheel_leg/rl/raw/left_wheel + + - /wheel_leg/right_wheel/angle + - /wheel_leg/right_wheel/velocity + - /wheel_leg/right_wheel/feedback_steady_ns + - /wheel_leg/right_wheel/feedback_sequence + - /wheel_leg/right_wheel/torque + - /wheel_leg/right_wheel/control_torque + - /wheel_leg/right_wheel/max_torque + - /wheel_leg/right_wheel/temperature_c + - /wheel_leg/rl/raw/right_wheel + + # --- 状态估计(机体) --- + - /wheel_leg/state/position + - /wheel_leg/state/velocity + - /wheel_leg/state/gravity + - /wheel_leg/state/attitude + - /wheel_leg/state/passive_knee + - /wheel_leg/state/passive_knee_rate + - /wheel_leg/state/shank + - /wheel_leg/state/shank_rate + - /wheel_leg/state/leg_length + - /wheel_leg/state/forward_velocity + - /wheel_leg/state/distance + - /wheel_leg/state/com_offset + - /wheel_leg/state/base_height + - /wheel_leg/state/valid + + # --- IMU --- + - /wheel_leg/imu/quaternion + - /wheel_leg/imu/angular_velocity + - /wheel_leg/imu/last_steady_ns + - /wheel_leg/imu/sequence + - /wheel_leg/imu/acceleration + - /wheel_leg/imu/acceleration_steady_ns + - /wheel_leg/imu/acceleration_sequence + + # --- RL 闭环:输入 / 施加动作 / 观测帧 / 推理链路 --- + - /wheel_leg/rl/applied_action + - /wheel_leg/rl/input/command + - /wheel_leg/rl/input/height + - /wheel_leg/rl/input/frame + - /wheel_leg/rl/state + - /wheel_leg/rl/valid + - /wheel_leg/rl/healthy + - /wheel_leg/rl/action_seq + - /wheel_leg/rl/action_steady_ns + - /wheel_leg/rl/action_applied + - /wheel_leg/rl/fault_latched + + # --- 自起恢复 --- + - /wheel_leg/recovery/phase + - /wheel_leg/recovery/fault + - /wheel_leg/recovery/capture_ready + - /wheel_leg/recovery/capture_blockers + + # --- 硬件诊断 / 底盘指令 --- + - /wheel_leg/feedback_fresh + - /wheel_leg/feedback_available + - /wheel_leg/dm_control_ready + - /wheel_leg/dr16_fresh + - /chassis/control_velocity + - /chassis/control_height + - /chassis/control_enable + - /chassis/reset_count diff --git a/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-wheelbipe.yaml b/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-wheelbipe.yaml new file mode 100644 index 000000000..048b43d3d --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/wheel-leg-infantry-wheelbipe.yaml @@ -0,0 +1,451 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::WheelLeg -> wheel_leg + + - rmcs_core::controller::chassis::WheelLegChassisController -> wheel_leg_chassis_controller + - rmcs_core::controller::chassis::WheelLegStateEstimator -> wheel_leg_state_estimator + - rmcs_core::controller::chassis::WheelLegRlController -> wheel_leg_rl_controller + + - rmcs_core::controller::chassis::WheelLegObservation -> wheel_leg_observation + + - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::controller::gimbal::PitchGravityFeedforward -> pitch_gravity_ff + - 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::referee::Status -> referee_status + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + + - rmcs_rl::RlBridge -> rl_bridge + + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_rl::WheelLegMirrorComponent -> wheel_leg_mirror + - rmcs_core::debug::ValueCollector -> value_collector + +wheel_leg: + ros__parameters: + serial_filter_bottom_board: "AF-CBB3-7053-32C5-F3B5-B460-A786-DA9A-1FF1" + serial_filter_top_board: "AF-46C7-7B12-9364-64F6-D95A-E4F4-9299-1B75" + yaw_motor_zero_point: 58379 + pitch_motor_zero_point: 12637 + + + dm_master_ids: [1, 2, 1, 2] + dm_mit_position_max: [3.141592653589793, 3.141592653589793, 3.141592653589793, 3.141592653589793] + dm_mit_velocity_max: [45.0, 45.0, 45.0, 45.0] + dm_mit_torque_max: [54.0, 54.0, 54.0, 54.0] + dm_control_torque_max: [40.0, 40.0, 40.0, 40.0] + + feedback_timeout_s: 0.05 + + +wheel_leg_chassis_controller: + ros__parameters: + vx_max: 3.0 + # Keep the policy command at zero while the stand drift hold is active. + vx_bias: 0.0 + vy_max: 0.0 + yaw_rate_max: 4.0 # Follow-mode yaw cap only; at policy training envelope. + spin_yaw_rate: 10.471975511965978 + yaw_stick_rate: 3.141592653589793 + following_velocity_kp: 15.0 + following_velocity_ki: 0.0 + following_velocity_kd: 0.0 + + # Low-bandwidth stand hold. It uses wheel-odometry distance/velocity and only + # engages for zero-stick AUTO stance; motion commands bypass it. + deadzone: 0.05 + angular_z_invert: false + + command_height_min: 0.23 + command_height_max: 0.42 + default_command_height: 0.305 + height_rate: 0.1 + + arm_dwell_seconds: 0.0 + +wheel_leg_state_estimator: + ros__parameters: + policy: wheelbipe + dt: 0.001 + velocity_sign: 1.0 + +wheel_leg_rl_controller: + ros__parameters: + policy: wheelbipe + action_history_source: raw + inference_timeout: 0.06 + drive_loss_debounce: 0.1 + heading_hold_enabled: false + height_reference_rate_max: 0.6666666666666666 + + self_righting: + enabled: false + simulation_only: true + hardware_mapping_verified: false + # Recovery reference rates capped at 2 rad/s; dynamic RL capture, no standing dwell. + fold_plant_speed_rad_s: 2.0 + insert_speed_rad_s: 2.0 + orbit_speed_rad_s: 2.0 + push_speed_rad_s: 2.0 + capture_speed_rad_s: 2.0 + side_speed_rad_s: 2.0 + +wheel_leg_observation: + ros__parameters: + policy: wheelbipe + +gimbal_controller: + ros__parameters: + upper_limit: -0.6109 # up 35 deg + lower_limit: 0.5236 # down 30 deg + +yaw_angle_pid_controller: + ros__parameters: + reset_interface: /chassis/reset_count + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 5.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + output_min: -4.5 + output_max: 4.5 + reset_interface: /chassis/reset_count + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 1.5 + ki: 0.0 + kd: 0.0 + +pitch_angle_pid_controller: + ros__parameters: + reset_interface: /chassis/reset_count + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 6.0 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + output_min: -4.5 + output_max: 4.5 + reset_interface: /chassis/reset_count + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + feedforward: /gimbal/pitch/torque_ff + kp: 1.5 + ki: 0.0 + kd: 0.0 + +pitch_gravity_ff: + ros__parameters: + pitch_gravity_ff_gain: 0.9 + pitch_gravity_ff_phase: 0.75 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 660.0 + - 660.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 10000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 20.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.20 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 1.583 + ki: 0.0 + kd: 0.0 + +rl_bridge: + ros__parameters: + rl_base: /wheel_leg/rl + rl_model_path: models/wheel_leg/wheelbipe_v202_16001_50hz.onnx + model_type: mlp + + policy_rate: 50.0 + history_length: 1 + rl_obs_size: 35 + rl_feature_size: 35 + rl_action_size: 6 + obs_clip: 100.0 + + inference_timeout: 0.06 + max_action_age: 0.06 + + enable_interface: /wheel_leg/rl/input/enable + reset_interface: /chassis/reset_count + enable_default: false + invalid_value: "nan" + + observation_frame_sequence_interface: /wheel_leg/rl/input/frame_sequence + + normalization_from_metadata: false + expected_obs_normalization: none + expected_observation_version: wheelbipe-v202-foundation-v2 + expected_observation_contract_sha256: bd8397a8cb057c5d7859ba79ffbc1310c7c75d2a9ddcfdb7ea844d02db001e69 + expected_model_spec_sha256: 8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812 + expected_metadata: + leg_winding_version: paired_full_vehicle110_v1 + leg_target_model_spec_sha256: 8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812 + + observations: + order: [command, height, gyro, gravity, position, velocity, previous_action, auxiliary] + command: {source: /wheel_leg/rl/features/command, type: array, size: 3, precision: float32, clip: [-100.0, 100.0]} + height: {source: /wheel_leg/rl/features/height, precision: float32, source_clip: [0.0, 1.0], scale: 5.0} + gyro: {source: /wheel_leg/rl/features/gyro, type: array, size: 3, precision: float32, source_clip: [-100.0, 100.0], scale: 0.5} + gravity: {source: /wheel_leg/rl/features/gravity, type: array, size: 3, precision: float32, clip: [-100.0, 100.0]} + position: {source: /wheel_leg/rl/features/position, type: array, size: 6, select: [0, 2, 1, 3, 4, 5], precision: float32, clip: [-100.0, 100.0]} + velocity: {source: /wheel_leg/rl/features/velocity, type: array, size: 6, select: [0, 2, 1, 3, 4, 5], precision: float32, source_clip: [-200.0, 200.0], scale: 0.1} + previous_action: {source: /wheel_leg/rl/features/previous_action, type: array, size: 6, select: [0, 2, 1, 3, 4, 5], precision: float32, clip: [-100.0, 100.0]} + auxiliary: {source: /wheel_leg/rl/features/auxiliary, type: array, size: 7} + actions: + order: [left_hip, right_hip, left_knee, right_knee, left_wheel, right_wheel] + left_hip: {output: /wheel_leg/rl/raw/left_hip_joint, scale: 0.5} + right_hip: {output: /wheel_leg/rl/raw/right_hip_joint, scale: 0.5} + left_knee: {output: /wheel_leg/rl/raw/left_knee_joint, scale: 0.5} + right_knee: {output: /wheel_leg/rl/raw/right_knee_joint, scale: 0.5} + left_wheel: {output: /wheel_leg/rl/raw/left_wheel, scale: 10.0, clip: [-90.0, 90.0]} + right_wheel: {output: /wheel_leg/rl/raw/right_wheel, scale: 10.0, clip: [-90.0, 90.0]} + +wheel_leg_mirror: + ros__parameters: + enabled: true + update_rate: 100.0 + +value_broadcaster: + ros__parameters: + update_rate: 50.0 + forward_list: + - /wheel_leg/rl/state + - /wheel_leg/rl/fault_latched + - /wheel_leg/feedback_fresh + - /wheel_leg/feedback_available + - /wheel_leg/dm_control_ready + - /wheel_leg/recovery/phase + - /wheel_leg/recovery/fault + - /wheel_leg/recovery/capture_ready + - /wheel_leg/recovery/capture_blockers + - /wheel_leg/left_hip_joint/status_code + - /wheel_leg/left_hip_joint/fault_code + - /wheel_leg/left_knee_joint/status_code + - /wheel_leg/left_knee_joint/fault_code + - /wheel_leg/right_hip_joint/status_code + - /wheel_leg/right_hip_joint/fault_code + - /wheel_leg/right_knee_joint/status_code + - /wheel_leg/right_knee_joint/fault_code + # 云台/发射机构遥测:只转发角度与转速,不转发力矩(torque)。 + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/left_friction/velocity + - /gimbal/right_friction/velocity + - /gimbal/bullet_feeder/velocity + # 发弹链路诊断:摩擦轮就绪 → 拨弹轮指令/反馈,外加底盘母线电压。 + - /gimbal/friction_ready + - /gimbal/friction_jammed + - /gimbal/bullet_fired + - /gimbal/bullet_feeder/control_velocity + - /chassis/voltage + - /chassis/supercap/enabled + +value_collector: + ros__parameters: + # 轮腿全过程记录(1000 Hz executor,每 5 拍落一行 → 200 Hz) + # 包含逐轴收帧时间/序号与机身比力,便于区分反馈停更和机械卡滞。 + # 列名规则:标量=信号名;Eigen::Vector3d=name/x,y,z;Quaterniond=name/w,x,y,z; + # 6 维向量=name[0..5],顺序固定为 left_hip, left_knee, right_hip, right_knee, + # left_wheel, right_wheel;2 维向量=name[0], name[1](左、右腿)。 + # 一次性记录请把下面这条从 components 列表注释掉,或用完删除 CSV。 + csv_path: "/tmp/wheel_leg_.csv" + write_interval: 5 + flush_interval: 200 + signals: + # --- 时间基:单调时钟,单位秒 --- + - /predefined/timestamp + + # --- 电源:四个 DM 关节共用底盘母线(DM 反馈帧只有 POS/VEL/T/T_MOS/T_Rotor,无电压字段) --- + - /chassis/voltage + - /chassis/supercap/voltage + - /chassis/power + + # --- 四个驱动腿关节(DM8009):反馈 / 下发力矩 / 驱动状态 / 策略原始动作 --- + - /wheel_leg/left_hip_joint/angle + - /wheel_leg/left_hip_joint/velocity + - /wheel_leg/left_hip_joint/feedback_steady_ns + - /wheel_leg/left_hip_joint/feedback_sequence + - /wheel_leg/left_hip_joint/torque + - /wheel_leg/left_hip_joint/control_torque + - /wheel_leg/left_hip_joint/max_torque + - /wheel_leg/left_hip_joint/temperature_mos + - /wheel_leg/left_hip_joint/temperature_rotor + - /wheel_leg/left_hip_joint/status_code + - /wheel_leg/left_hip_joint/fault_code + - /wheel_leg/rl/raw/left_hip_joint + + - /wheel_leg/left_knee_joint/angle + - /wheel_leg/left_knee_joint/velocity + - /wheel_leg/left_knee_joint/feedback_steady_ns + - /wheel_leg/left_knee_joint/feedback_sequence + - /wheel_leg/left_knee_joint/torque + - /wheel_leg/left_knee_joint/control_torque + - /wheel_leg/left_knee_joint/max_torque + - /wheel_leg/left_knee_joint/temperature_mos + - /wheel_leg/left_knee_joint/temperature_rotor + - /wheel_leg/left_knee_joint/status_code + - /wheel_leg/left_knee_joint/fault_code + - /wheel_leg/rl/raw/left_knee_joint + + - /wheel_leg/right_hip_joint/angle + - /wheel_leg/right_hip_joint/velocity + - /wheel_leg/right_hip_joint/feedback_steady_ns + - /wheel_leg/right_hip_joint/feedback_sequence + - /wheel_leg/right_hip_joint/torque + - /wheel_leg/right_hip_joint/control_torque + - /wheel_leg/right_hip_joint/max_torque + - /wheel_leg/right_hip_joint/temperature_mos + - /wheel_leg/right_hip_joint/temperature_rotor + - /wheel_leg/right_hip_joint/status_code + - /wheel_leg/right_hip_joint/fault_code + - /wheel_leg/rl/raw/right_hip_joint + + - /wheel_leg/right_knee_joint/angle + - /wheel_leg/right_knee_joint/velocity + - /wheel_leg/right_knee_joint/feedback_steady_ns + - /wheel_leg/right_knee_joint/feedback_sequence + - /wheel_leg/right_knee_joint/torque + - /wheel_leg/right_knee_joint/control_torque + - /wheel_leg/right_knee_joint/max_torque + - /wheel_leg/right_knee_joint/temperature_mos + - /wheel_leg/right_knee_joint/temperature_rotor + - /wheel_leg/right_knee_joint/status_code + - /wheel_leg/right_knee_joint/fault_code + - /wheel_leg/rl/raw/right_knee_joint + + # --- 两个驱动轮(DJI):反馈 / 下发力矩 / 温度 / 策略原始动作 --- + - /wheel_leg/left_wheel/angle + - /wheel_leg/left_wheel/velocity + - /wheel_leg/left_wheel/feedback_steady_ns + - /wheel_leg/left_wheel/feedback_sequence + - /wheel_leg/left_wheel/torque + - /wheel_leg/left_wheel/control_torque + - /wheel_leg/left_wheel/max_torque + - /wheel_leg/left_wheel/temperature_c + - /wheel_leg/rl/raw/left_wheel + + - /wheel_leg/right_wheel/angle + - /wheel_leg/right_wheel/velocity + - /wheel_leg/right_wheel/feedback_steady_ns + - /wheel_leg/right_wheel/feedback_sequence + - /wheel_leg/right_wheel/torque + - /wheel_leg/right_wheel/control_torque + - /wheel_leg/right_wheel/max_torque + - /wheel_leg/right_wheel/temperature_c + - /wheel_leg/rl/raw/right_wheel + + # --- 状态估计(机体) --- + - /wheel_leg/state/position + - /wheel_leg/state/velocity + - /wheel_leg/state/gravity + - /wheel_leg/state/attitude + - /wheel_leg/state/passive_knee + - /wheel_leg/state/passive_knee_rate + - /wheel_leg/state/shank + - /wheel_leg/state/shank_rate + - /wheel_leg/state/leg_length + - /wheel_leg/state/forward_velocity + - /wheel_leg/state/distance + - /wheel_leg/state/com_offset + - /wheel_leg/state/base_height + - /wheel_leg/state/valid + + # --- IMU --- + - /wheel_leg/imu/quaternion + - /wheel_leg/imu/angular_velocity + - /wheel_leg/imu/last_steady_ns + - /wheel_leg/imu/sequence + - /wheel_leg/imu/acceleration + - /wheel_leg/imu/acceleration_steady_ns + - /wheel_leg/imu/acceleration_sequence + + # --- RL 闭环:输入 / 施加动作 / 观测帧 / 推理链路 --- + - /wheel_leg/rl/applied_action + - /wheel_leg/rl/input/command + - /wheel_leg/rl/input/height + - /wheel_leg/rl/input/frame + - /wheel_leg/rl/state + - /wheel_leg/rl/valid + - /wheel_leg/rl/healthy + - /wheel_leg/rl/action_seq + - /wheel_leg/rl/action_steady_ns + - /wheel_leg/rl/action_applied + - /wheel_leg/rl/fault_latched + + # --- 自起恢复 --- + - /wheel_leg/recovery/phase + - /wheel_leg/recovery/fault + - /wheel_leg/recovery/capture_ready + - /wheel_leg/recovery/capture_blockers + + # --- 硬件诊断 / 底盘指令 --- + - /wheel_leg/feedback_fresh + - /wheel_leg/feedback_available + - /wheel_leg/dm_control_ready + - /wheel_leg/dr16_fresh + - /chassis/control_velocity + - /chassis/control_height + - /chassis/control_enable + - /chassis/reset_count diff --git a/rmcs_ws/src/rmcs_bringup/package.xml b/rmcs_ws/src/rmcs_bringup/package.xml index c64c661fd..6d4d432b9 100644 --- a/rmcs_ws/src/rmcs_bringup/package.xml +++ b/rmcs_ws/src/rmcs_bringup/package.xml @@ -12,6 +12,7 @@ joint_state_broadcaster robot_state_publisher + rmcs_rl rviz2 xacro @@ -21,4 +22,4 @@ ament_cmake - \ No newline at end of file + diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 0ae6350ad..b013bf468 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -40,4 +40,19 @@ target_link_libraries(${PROJECT_NAME} librmcs-sdk) pluginlib_export_plugin_description_file(rmcs_executor plugins.xml) +if(BUILD_TESTING) + enable_testing() + add_executable(wheelbipe_jump_test test/wheelbipe_jump_test.cpp) + target_include_directories(wheelbipe_jump_test PRIVATE ${PROJECT_SOURCE_DIR}/src) + add_test(NAME wheelbipe_jump_control COMMAND wheelbipe_jump_test) + add_executable(wheel_leg_encoder_unwrap test/wheel_leg_encoder_unwrap.cpp) + target_include_directories(wheel_leg_encoder_unwrap PRIVATE ${PROJECT_SOURCE_DIR}/src) + add_test(NAME wheel_leg_encoder_unwrap COMMAND wheel_leg_encoder_unwrap) + ament_auto_add_executable(wheel_leg_state_estimator test/wheel_leg_state_estimator.cpp) + add_test(NAME wheel_leg_state_estimator COMMAND wheel_leg_state_estimator) + ament_auto_add_executable(wheelbipe_jump_components test/wheelbipe_jump_components.cpp) + add_test(NAME wheelbipe_jump_components COMMAND wheelbipe_jump_components --ros-args + --params-file ${PROJECT_SOURCE_DIR}/../rmcs_bringup/config/wheel-leg-infantry-wheelbipe-jump-v4.yaml) +endif() + ament_auto_package() diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index b3d6d14af..9d1ed7a9d 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -6,11 +6,14 @@ TODO: Package description developer TODO: License declaration + GPL-3.0-only ament_cmake rclcpp std_msgs + sensor_msgs + geometry_msgs std_srvs pluginlib tf2 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index a066759ae..dbeec4efe 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,10 +1,14 @@ + + + + @@ -22,8 +26,10 @@ + + diff --git a/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp b/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp index ec612065f..db0aab36e 100644 --- a/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp +++ b/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp @@ -1,8 +1,18 @@ +#include + +#include +#include +#include +#include + +#include #include #include #include +#include #include -#include +#include +#include namespace rmcs_core::broadcaster { @@ -12,6 +22,20 @@ class ValueBroadcaster public: ValueBroadcaster() : Node{get_component_name()} { + rcl_interfaces::msg::ParameterDescriptor rate_descriptor; + rate_descriptor.read_only = true; + const double update_rate = declare_parameter("update_rate", 0.0, rate_descriptor); + if (!std::isfinite(update_rate) || update_rate < 0.0) + throw std::invalid_argument( + "Value broadcaster update_rate must be finite and nonnegative"); + if (update_rate > 0.0) { + const auto period = std::chrono::duration{1.0 / update_rate}; + if (!std::isfinite(period.count()) || period < Clock::duration{1} + || period >= Clock::duration::max()) + throw std::invalid_argument("Value broadcaster period exceeds the clock range"); + publish_period_ = std::chrono::duration_cast(period); + register_input("/predefined/timestamp", timestamp_); + } declare_parameter>("forward_list", std::vector{}); parameter_subscriber_ = std::make_unique(this); parameter_callback_ = parameter_subscriber_->add_parameter_callback( @@ -27,6 +51,16 @@ class ValueBroadcaster forward_units_.emplace( name, std::make_unique>(this, name)); + } else if (output.type.get() == typeid(bool)) { + forward_units_.emplace( + name, std::make_unique>(this, name)); + } else if (output.type.get() == typeid(int)) { + forward_units_.emplace( + name, std::make_unique>(this, name)); + } else if (output.type.get() == typeid(std::uint8_t)) { + forward_units_.emplace( + name, + std::make_unique>(this, name)); } } std::vector forward_list; @@ -35,6 +69,18 @@ class ValueBroadcaster } void update() override { + if (publish_period_ != Clock::duration::zero()) { + const auto now = *timestamp_; + if (last_publish_timestamp_ && now >= *last_publish_timestamp_) { + const auto elapsed = now - *last_publish_timestamp_; + if (elapsed < publish_period_) + return; + // Keep the publication phase through missed ticks without replaying stale samples. + last_publish_timestamp_ = now - elapsed % publish_period_; + } else { + last_publish_timestamp_ = now; + } + } for (auto& [name, unit] : forward_units_) unit->update(); } @@ -109,6 +155,11 @@ class ValueBroadcaster rclcpp::Publisher::SharedPtr publisher_; }; + using Clock = std::chrono::steady_clock; + InputInterface timestamp_; + Clock::duration publish_period_{}; + std::optional last_publish_timestamp_; + std::map> forward_units_; std::unique_ptr parameter_subscriber_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/capture.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/capture.hpp new file mode 100644 index 000000000..70ddacdb2 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/capture.hpp @@ -0,0 +1,76 @@ +#pragma once + +#include "joint_coordinates.hpp" +#include "model/body_geometry.hpp" +#include "model/recovery_profile.hpp" + +#include +#include + +namespace rmcs_core::controller::chassis::wheel_leg { + +// dev/wheel_leg_rl @ 0fcc9b45, controller_recovery.cpp and recovery YAML. +// Moving capture: neither script completion nor stationary balance is required. +struct RecoveryCaptureLimits { + double tilt = std::numbers::pi / 6.; + double leg_error = .45; + double leg_velocity = 4.; + double wheel_velocity = 20.; + double gyro = 1.; + double upright_gyro = 2.; + double upright_tilt = .2; + std::array hinge_min{kMechanicalBounds[0][0], kMechanicalBounds[1][0]}; + std::array hinge_max = kRecoveryCaptureHingeMax; +}; + +enum RecoveryCaptureBlocker { + kCaptureSensing = 1 << 0, + kCaptureTilt = 1 << 1, + kCaptureGyro = 1 << 2, + kCaptureWheelVelocity = 1 << 3, + kCaptureLegVelocity = 1 << 4, + kCaptureReference = 1 << 5, + kCaptureHinge = 1 << 6, + kCaptureSupportingAcceleration = 1 << 7, + kCaptureOutwardTilt = 1 << 9, +}; + +inline int recovery_capture_blockers( + const JointReferenceRecoveryFeedback& feedback, const Eigen::Vector3d& acceleration, + const Eigen::Vector4d& reference, const RecoveryCaptureLimits& limits = {}) { + if (!feedback.q.allFinite() || !feedback.dq.allFinite() || !feedback.gravity.allFinite() + || !feedback.gyro.allFinite() || !acceleration.allFinite() + || !std::isfinite(feedback.estimated_height) + || std::abs(feedback.gravity.squaredNorm() - 1.) > 1e-3) + return kCaptureSensing; + int blockers = 0; + const auto& g = feedback.gravity; + if (-g.z() < std::cos(limits.tilt)) + blockers |= kCaptureTilt; + const bool upright = -g.z() >= std::cos(limits.upright_tilt); + if (feedback.gyro.norm() > (upright ? limits.upright_gyro : limits.gyro)) + blockers |= kCaptureGyro; + if (feedback.dq.head<4>().cwiseAbs().maxCoeff() > limits.leg_velocity) + blockers |= kCaptureLegVelocity; + if (feedback.dq.tail<2>().cwiseAbs().maxCoeff() > limits.wheel_velocity) + blockers |= kCaptureWheelVelocity; + for (int i = 0; i < 4; ++i) + if (std::abs(std::remainder(reference[i] - feedback.q[i], 2. * std::numbers::pi)) + > limits.leg_error) + blockers |= kCaptureReference; + for (int side = 0; side < 2; ++side) { + const int hip = 2 * side; + const double relative = (side == 0 ? 1. : -1.) * (feedback.q[hip + 1] - feedback.q[hip]); + if (relative < kMechanicalBounds[side][0] || relative > kMechanicalBounds[side][1] + || relative < limits.hinge_min[side] || relative > limits.hinge_max[side]) + blockers |= kCaptureHinge; + } + if (acceleration.dot(-g) < 6.) + blockers |= kCaptureSupportingAcceleration; + if (-g.z() < std::cos(std::numbers::pi / 6.) + && g.x() * feedback.gyro.y() - g.y() * feedback.gyro.x() > .05 * std::hypot(g.x(), g.y())) + blockers |= kCaptureOutwardTilt; + return blockers; +} + +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/encoder_unwrap.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/encoder_unwrap.hpp new file mode 100644 index 000000000..fa5a96270 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/encoder_unwrap.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include +#include + +namespace rmcs_core::controller::chassis { + +// DM MIT feedback wraps at +/- P_MAX, so one wrap moves the raw angle by +// 2 * P_MAX. The span is configuration-dependent and must not be hard-coded to +// 2 pi; callers provide the configured span for each drive. +class WheelLegEncoderUnwrap { +public: + // Requires a finite raw angle and span > pi. Returns true when this frame + // re-anchored the continuous angle to the raw value: the first frame, or a + // step no joint can make between two frames (a drive restart or a + // corrupted frame). + bool update(double raw, double span) { + const double step = std::remainder(raw - raw_, span); + const bool anchor = !valid_ || !(std::abs(step) <= kMaxStep); + value_ = anchor ? raw : value_ + step; + raw_ = raw; + valid_ = true; + return anchor; + } + + bool valid() const { return valid_; } + double value() const { return value_; } + double raw() const { return raw_; } + + static constexpr double kMaxStep = std::numbers::pi / 2; + +private: + double value_ = 0, raw_ = 0; + bool valid_ = false; +}; + +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/gas_spring.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/gas_spring.hpp new file mode 100644 index 000000000..d6eeb94b7 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/gas_spring.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "model/calibration.hpp" + +namespace rmcs_core::controller::chassis { + +struct GasSpringCompensation { + bool valid = true; + std::array torque{}; + std::array beta_deg{}, compression_m{}, force_n{}, weight{}; + std::array extrapolated{}; +}; + +// Inputs and efforts are in the four active model coordinates. The knee-drive +// angle is not the passive knee hinge angle. Calibration/reversal belongs to +// the caller: tau_api = J_motor_to_model^T * tau_model. +inline GasSpringCompensation gas_spring_compensation(const std::array& q, double gain) { + GasSpringCompensation result; + if (!std::ranges::all_of(q, [](double value) { return std::isfinite(value); }) + || !std::isfinite(gain) || gain < 0.0 || gain > 1.0) { + result.valid = false; + return result; + } + for (std::size_t side = 0; side < 2; ++side) { + const std::span geometry{ + side == 0 ? full_vehicle_calibration110::kLeft : full_vehicle_calibration110::kRight}; + const double delta = side == 0 ? q[1] - q[0] : q[2] - q[3]; + if (delta < geometry.front().delta_rad || delta > geometry.back().delta_rad) { + result.beta_deg[side] = std::numeric_limits::quiet_NaN(); + continue; // Geometry covers the complete configured soft-limit interval. + } + auto upper = std::lower_bound( + geometry.begin() + 1, geometry.end(), delta, + [](const auto& knot, double value) { return knot.delta_rad < value; }); + upper = std::min(upper, geometry.end() - 1); + const auto& high = *upper; + const auto& low = *(upper - 1); + const double u = (delta - low.delta_rad) / (high.delta_rad - low.delta_rad); + const double beta = std::lerp(low.beta_deg, high.beta_deg, u); + const double compression = std::lerp(low.compression_m, high.compression_m, u); + const auto& curve = full_vehicle_v202::kForce; + const double s = + std::clamp(compression, curve.front().compression_m, curve.back().compression_m); + auto force_upper = + std::lower_bound(curve.begin() + 1, curve.end(), s, [](const auto& knot, double value) { + return knot.compression_m < value; + }); + force_upper = std::min(force_upper, curve.end() - 1); + const auto& f_high = *force_upper; + const auto& f_low = *(force_upper - 1); + double force = std::lerp( + f_low.force_n, f_high.force_n, + (s - f_low.compression_m) / (f_high.compression_m - f_low.compression_m)); + const bool low_extension = compression < curve.front().compression_m; + const bool high_extension = compression > curve.back().compression_m; + if (low_extension) + force = curve.front().force_n + * std::exp( + full_vehicle_v202::kLowCompressionRate + * (compression - curve.front().compression_m)); + if (high_extension) + force = curve.back().force_n + + full_vehicle_v202::kHighCompressionSlope + * (compression - curve.back().compression_m); + const double weight = gain; + result.beta_deg[side] = beta; + result.compression_m[side] = compression; + result.force_n[side] = force; + result.weight[side] = weight; + result.extrapolated[side] = compression < curve.front().compression_m - 1e-8 + || compression > curve.back().compression_m + 1e-8; + // Virtual work: the commanded holding effort opposes the axial + // extension force, tau_hold = -(dl/dq)^T F. Friction is not included. + result.torque[2 * side] = + -weight * force * std::lerp(low.hip_length_jacobian, high.hip_length_jacobian, u); + result.torque[2 * side + 1] = + -weight * force * std::lerp(low.knee_length_jacobian, high.knee_length_jacobian, u); + } + return result; +} + +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/joint_coordinates.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/joint_coordinates.hpp new file mode 100644 index 000000000..db26dc42f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/joint_coordinates.hpp @@ -0,0 +1,76 @@ +#pragma once + +#include +#include +#include +#include + +#include "model/calibration.hpp" + +namespace rmcs_core::controller::chassis { +using JointVector = std::array; +using LegVector = std::array; +inline constexpr std::array kMotorNames{"left_hip_joint", "left_knee_joint", + "right_hip_joint", "right_knee_joint", + "left_wheel", "right_wheel"}; +inline constexpr JointVector kNominalPosition{ + -.42, .13722330866417465, .42, -.13721605817245947, 0., 0.}; +inline constexpr LegVector kMotorOffsets{1.6, 3.0282761689183353, -1.6, -3.0283102010168}; +inline constexpr std::array, 2> kMechanicalBounds{ + {{-.14089326779459427, 1.4282761689183354}, {-.14084732125908661, 1.4283102010167998}}}; +using LegBounds = std::array, 2>; + +inline JointVector model_position(const JointVector& api) { + auto q = api; + for (int i = 0; i < 4; ++i) + q[i] = -api[i] + kMotorOffsets[i]; + return q; +} +inline JointVector model_velocity(const JointVector& api) { + auto dq = api; + for (int i = 0; i < 4; ++i) + dq[i] = -api[i]; + return dq; +} +inline JointVector motor_effort(const JointVector& model) { return model_velocity(model); } +inline double forward_wheel_velocity(const JointVector& velocity) { + return .5 * (velocity[4] - velocity[5]); +} + +// The drive span unwrap upstream already returns a continuous hip angle; this +// only picks the relative (closed-chain) branch, so no 2 pi multiple is ever +// added to the absolute angle here. +inline LegVector continuous_legs(const LegVector& measured, const LegBounds& bounds) { + constexpr double tau = 2 * std::numbers::pi; + auto result = measured; + for (int side = 0; side < 2; ++side) { + const int hip = 2 * side, aux = hip + 1; + const double sign = side == 0 ? 1. : -1.; + const double midpoint = (bounds[side][0] + bounds[side][1]) / 2; + const double delta = sign * (measured[aux] - measured[hip]); + const double relative = delta + tau * std::nearbyint((midpoint - delta) / tau); + result[aux] = result[hip] + sign * relative; + } + return result; +} +inline LegVector + paired_targets(const LegVector& desired, const LegVector& current, const LegBounds& bounds) { + constexpr double tau = 2 * std::numbers::pi; + LegVector targets; + for (int side = 0; side < 2; ++side) { + const int hip = 2 * side, aux = hip + 1; + const double sign = side == 0 ? 1. : -1.; + const double mean = (desired[hip] + desired[aux]) / 2; + const double delta = + std::clamp(sign * (desired[aux] - desired[hip]), bounds[side][0], bounds[side][1]); + const double turns = + std::floor(((current[hip] * .5 + current[aux] * .5 - mean) / tau) + .5); + targets[hip] = mean - sign * delta / 2 + turns * tau; + targets[aux] = mean + sign * delta / 2 + turns * tau; + const double actual = sign * (targets[aux] - targets[hip]); + if (actual < bounds[side][0] || actual > bounds[side][1]) + targets[aux] = std::nextafter(targets[hip] + sign * delta, targets[hip]); + } + return targets; +} +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/joint_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/joint_solver.hpp new file mode 100644 index 000000000..ccb0a5c82 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/joint_solver.hpp @@ -0,0 +1,48 @@ +#pragma once +#include "gas_spring.hpp" +#include "joint_coordinates.hpp" +#include +#include +#include +namespace rmcs_core::controller::chassis { +class JointSolver { +public: + static JointVector + pd(const JointVector& target, const JointVector& q, const JointVector& dq, double kp, + double kd, double gas_gain) { + const auto gas = gas_spring_compensation({q[0], q[1], q[2], q[3]}, gas_gain); + JointVector effort; + for (int i = 0; i < 4; ++i) + effort[i] = kp * (target[i] - q[i]) - kd * dq[i] + gas.torque[i]; + for (int i = 4; i < 6; ++i) + effort[i] = std::clamp(.5 * (target[i] - dq[i]), -4.5, 4.5); + return effort; + } + static JointVector constrain( + JointVector effort, const JointVector& q, const JointVector& caps, double margin = .03, + const LegBounds& mechanical_bounds = kMechanicalBounds) { + for (int i = 0; i < 4; ++i) + effort[i] = std::clamp(effort[i], -40., 40.); + for (int side = 0; side < 2; ++side) { + const int hip = 2 * side, crank = hip + 1; + const double sign = side == 0 ? 1. : -1.; + const double relative = sign * (q[crank] - q[hip]); + const double outward = sign * (effort[crank] - effort[hip]); + if ((relative >= mechanical_bounds[side][1] - margin && outward > 0.) + || (relative <= mechanical_bounds[side][0] + margin && outward < 0.)) { + effort[hip] += sign * outward / 2.; + effort[crank] -= sign * outward / 2.; + } + } + double scale = 1.; + for (int i = 0; i < 4; ++i) + if (std::abs(effort[i]) > std::min(40., caps[i])) + scale = std::min(scale, std::min(40., caps[i]) / std::abs(effort[i])); + for (int i = 0; i < 4; ++i) + effort[i] *= scale; + for (int i = 4; i < 6; ++i) + effort[i] = std::clamp(effort[i], -caps[i], caps[i]); + return motor_effort(effort); + } +}; +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/jump_command.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/jump_command.hpp new file mode 100644 index 000000000..fb7c5ba52 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/jump_command.hpp @@ -0,0 +1,96 @@ +#pragma once + +#include +#include +#include +#include + +#include "model/jump_v4.hpp" + +namespace rmcs_core::controller::chassis::wheel_leg { + +class JumpCommand { +public: + enum State { IDLE, CROUCH, PUSH, FLIGHT_REFERENCE, RECOVER_REFERENCE }; + + explicit JumpCommand(double apex = .46) + : apex_(apex) { + if (!std::isfinite(apex) || apex < wheelbipe_jump_v4::kMinimumApex + || apex > wheelbipe_jump_v4::kMaximumApex) + throw std::invalid_argument("Jump apex must be within the v4 training range"); + } + + void reset() { active_ = false; } + + bool request(double now) { + if (active_ || !std::isfinite(now)) + return false; + started_ = now; + active_ = true; + return true; + } + + void update(double now) { + if (active_ + && (!std::isfinite(now) || now < started_ + || now - started_ >= wheelbipe_jump_v4::kRequestDuration)) + reset(); + } + + bool active() const { return active_; } + double elapsed(double now) const { return active_ ? std::clamp(now - started_, 0., 6.) : 0.; } + double apex() const { return apex_; } + + State state(double now) const { + if (!active_) + return IDLE; + const double t = elapsed(now); + if (t < wheelbipe_jump_v4::kCrouchSeconds) + return CROUCH; + if (t - wheelbipe_jump_v4::kCrouchSeconds <= push_duration()) + return PUSH; + if (t - wheelbipe_jump_v4::kCrouchSeconds + < push_duration() + 2. * release_velocity() / 9.81) + return FLIGHT_REFERENCE; + return RECOVER_REFERENCE; + } + + double height(double now) const { + const double t = elapsed(now); + switch (state(now)) { + case CROUCH: { + const double p = t / wheelbipe_jump_v4::kCrouchSeconds; + return wheelbipe_jump_v4::kStandingHeight + + (wheelbipe_jump_v4::kCrouchHeight - wheelbipe_jump_v4::kStandingHeight) + * (3. * p * p - 2. * p * p * p); + } + case PUSH: { + const double p = t - wheelbipe_jump_v4::kCrouchSeconds; + return wheelbipe_jump_v4::kCrouchHeight + + .5 * release_velocity() / push_duration() * p * p; + } + case FLIGHT_REFERENCE: return wheelbipe_jump_v4::kReleaseHeight; + default: return wheelbipe_jump_v4::kStandingHeight; + } + } + + std::array context(double now) const { + if (!active_) + return {1., 0., 0., 0., 0., 0., 0.}; + return {0., 0., 0., 0., 1., apex_, elapsed(now)}; + } + +private: + double release_velocity() const { + return std::sqrt(2. * 9.81 * (apex_ - wheelbipe_jump_v4::kReleaseHeight)); + } + double push_duration() const { + return 2. * (wheelbipe_jump_v4::kReleaseHeight - wheelbipe_jump_v4::kCrouchHeight) + / release_velocity(); + } + + double apex_, started_ = 0.; + bool active_ = false; +}; + +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/jump_control.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/jump_control.hpp new file mode 100644 index 000000000..8081e0419 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/jump_control.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include +#include + +#include "joint_coordinates.hpp" +#include "model/jump_v4.hpp" + +namespace rmcs_core::controller::chassis::wheel_leg { + +inline LegBounds jump_target_bounds(bool requested) { + auto bounds = wheelbipe_jump_v4::kBounds; + if (requested) + for (auto& side : bounds) { + side[0] -= wheelbipe_jump_v4::kOvertravel; + side[1] += wheelbipe_jump_v4::kOvertravel; + } + return bounds; +} + +inline LegVector jump_spring_holding(const LegVector& q) { + LegVector holding{}; + for (std::size_t side = 0; side < 2; ++side) { + const std::span geometry{ + side == 0 ? wheelbipe_jump_v4::kLeft : wheelbipe_jump_v4::kRight}; + const double delta = side == 0 ? q[1] - q[0] : q[2] - q[3]; + if (delta < geometry.front().delta_rad || delta > geometry.back().delta_rad) + continue; + auto upper = std::lower_bound( + geometry.begin() + 1, geometry.end(), delta, + [](const auto& knot, double value) { return knot.delta_rad < value; }); + upper = std::min(upper, geometry.end() - 1); + const auto& hi = *upper; + const auto& lo = *(upper - 1); + const double blend = (delta - lo.delta_rad) / (hi.delta_rad - lo.delta_rad); + const double compression = std::lerp(lo.compression_m, hi.compression_m, blend); + const auto& curve = wheelbipe_jump_v4::kForce; + auto force_upper = std::lower_bound( + curve.begin() + 1, curve.end(), compression, + [](const auto& knot, double value) { return knot.compression_m < value; }); + force_upper = std::min(force_upper, curve.end() - 1); + const auto& fh = *force_upper; + const auto& fl = *(force_upper - 1); + double force = std::lerp( + fl.force_n, fh.force_n, + (compression - fl.compression_m) / (fh.compression_m - fl.compression_m)); + if (compression < curve.front().compression_m) + force = curve.front().force_n + * std::exp( + wheelbipe_jump_v4::kLowCompressionRate + * (compression - curve.front().compression_m)); + else if (compression > curve.back().compression_m) + force = curve.back().force_n + + wheelbipe_jump_v4::kHighCompressionSlope + * (compression - curve.back().compression_m); + holding[2 * side] = + -force * std::lerp(lo.hip_length_jacobian, hi.hip_length_jacobian, blend); + holding[2 * side + 1] = + -force * std::lerp(lo.knee_length_jacobian, hi.knee_length_jacobian, blend); + } + return holding; +} + +struct JumpTuckEffort { + LegVector torque{}; + std::array gate{}; +}; + +inline double smooth_ramp(double value) { + const double x = std::clamp(value, 0., 1.); + return x * x * (3. - 2. * x); +} + +inline JumpTuckEffort jump_tuck_effort( + const LegVector& q, const LegVector& target, const LegVector& pd, bool flight_reference) { + JumpTuckEffort result; + if (!flight_reference) + return result; + const auto holding = jump_spring_holding(q); + for (int side = 0; side < 2; ++side) { + const int hip = 2 * side, aux = hip + 1; + const double sign = side == 0 ? 1. : -1.; + const double error = sign * ((q[aux] - q[hip]) - (target[aux] - target[hip])); + const double inward_pd = -.5 * sign * (pd[aux] - pd[hip]); + const double gate = smooth_ramp( + (error - wheelbipe_jump_v4::kErrorOn) + / (wheelbipe_jump_v4::kErrorFull - wheelbipe_jump_v4::kErrorOn)) + * smooth_ramp(inward_pd / wheelbipe_jump_v4::kInwardPdFull); + const double inward = std::max(0., -.5 * sign * (holding[aux] - holding[hip])); + const double magnitude = + std::min(inward * wheelbipe_jump_v4::kTuckGain, wheelbipe_jump_v4::kTuckCap) * gate; + result.torque[hip] = sign * magnitude; + result.torque[aux] = -sign * magnitude; + result.gate[side] = gate; + } + return result; +} + +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/jump_trigger.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/jump_trigger.hpp new file mode 100644 index 000000000..3e89715fd --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/jump_trigger.hpp @@ -0,0 +1,10 @@ +#pragma once + +namespace rmcs_core::controller::chassis::wheel_leg { + +inline bool jump_requested( + bool was_armed, bool ctrl, bool last_ctrl, bool was_middle, bool left_middle, bool right_up) { + return was_armed && ((!last_ctrl && ctrl) || (was_middle && left_middle && right_up)); +} + +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/leg_kinematics.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/leg_kinematics.hpp new file mode 100644 index 000000000..7d1ce0673 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/leg_kinematics.hpp @@ -0,0 +1,147 @@ +#pragma once + +#include +#include +#include + +#include "model/leg_transmission.hpp" + +namespace rmcs_core::controller::chassis::wheel_leg { + +// Reduced sagittal six-bar model. Inputs are continuous hardware output-shaft +// angles; beta is the passive inner knee angle, not the active knee-drive axis. +// Encoder signs/zeros and the crank table are shared with target reconstruction. + +[[nodiscard]] inline double wrap_to_pi(double angle) { + return std::remainder(angle, 2.0 * std::numbers::pi); +} + +// Per-side calibration: the ONLY hardware-dependent part. Real robot and sim +// each own a set of these; the maths below is identical for both. +struct LegCalibration { + double hip_sign = 1.0; + double hip_zero = 0.0; + double crank_sign = 1.0; +}; + +// The current encoder reference: thigh perpendicular to the chassis and +// passive knee at 110 degrees. Left/right installation signs mirror each other. +struct CanonicalCalibration { + static constexpr double kHipSign[2] = {-1.0, 1.0}; + static constexpr double kHipZero[2] = {-1.5707963267948966, -1.5707963267948966}; + static constexpr double kCrankSign[2] = {-1.0, 1.0}; + [[nodiscard]] static LegCalibration side(int i) { + return LegCalibration{kHipSign[i], kHipZero[i], kCrankSign[i]}; + } +}; + +// Forward result of the solver. +struct LegShape { + double alpha = 0.0; // reduced hip + double crank_rel = 0.0; // relative crank (encoder frame) + double beta = 0.0; // interior knee, clamped to the mechanical limits + double beta_raw = 0.0; // interior knee, unclamped (for diagnostics) + double shank = 0.0; // phi + double axle_angle = 0.0; // hip->axle line angle to horizontal + double leg_length = 0.0; // L0 + double knee_x = 0.0; // knee position relative to the hip (fwd x, up z) + double knee_z = 0.0; + double axle_x = 0.0; // axle (wheel) position relative to the hip + double axle_z = 0.0; +}; + +class WheelLegLegModel { +public: + WheelLegLegModel( + double thigh_length, double shank_length, double knee_limit_min, double knee_limit_max, + const LegLinkage& linkage) + : thigh_length_(thigh_length) + , shank_length_(shank_length) + , knee_limit_min_(knee_limit_min) + , knee_limit_max_(knee_limit_max) + , linkage_(&linkage) {} + + // Forward F: raw joint angles -> reduced leg shape. + [[nodiscard]] LegShape + forward(double q_hip_raw, double q_crank_raw, const LegCalibration& calib) const { + LegShape out; + out.crank_rel = wrap_to_pi(calib.crank_sign * (q_crank_raw - q_hip_raw)); + out.beta_raw = linkage_->knee_from_encoder_crank(out.crank_rel); + out.beta = std::clamp(out.beta_raw, knee_limit_min_, knee_limit_max_); + out.alpha = wrap_to_pi(q_hip_raw / calib.hip_sign + calib.hip_zero); + out.shank = std::numbers::pi - out.beta - out.alpha; + + fk(out.beta, out.shank, out.knee_x, out.knee_z, out.axle_x, out.axle_z); + out.axle_angle = std::atan2(out.axle_z, out.axle_x); + out.leg_length = std::hypot(out.axle_x, out.axle_z); + return out; + } + + void + fk(double beta, double shank, double& knee_x, double& knee_z, double& axle_x, + double& axle_z) const { + const double psi1 = beta + shank; + knee_x = -thigh_length_ * std::cos(psi1); + knee_z = -thigh_length_ * std::sin(psi1); + axle_x = knee_x + shank_length_ * std::cos(shank); + axle_z = knee_z + shank_length_ * std::sin(shank); + } + + // Inverse F^-1: reduced (alpha, beta) -> raw joint targets (single turn; the + // caller aligns to the multi-turn encoder). + bool joint_targets( + double alpha, double beta, const LegCalibration& calib, double& q_hip_target, + double& q_crank_target) const { + double crank_rel = 0.0; + if (!linkage_->encoder_crank_from_knee( + std::clamp(beta, knee_limit_min_, knee_limit_max_), crank_rel)) + return false; + q_hip_target = calib.hip_sign * (alpha - calib.hip_zero); + q_crank_target = q_hip_target + calib.crank_sign * crank_rel; + return true; + } + + // Hip->axle line angle for a given (beta, alpha) without the full FK; the + // plan/teleop layers use this to go from a knee target to a leg-angle target. + [[nodiscard]] double axle_line(double beta, double alpha) const { + return std::atan2( + shank_length_ * std::sin(beta), thigh_length_ - shank_length_ * std::cos(beta)) + - alpha; + } + + [[nodiscard]] double knee_from_encoder_crank(double c_encoder) const { + return linkage_->knee_from_encoder_crank(c_encoder); + } + + // Interior knee <-> leg length (cosine law): the reduced-model relation the + // trajectory / planning layer works in. + [[nodiscard]] double leg_length_from_knee(double beta) const { + return std::sqrt( + thigh_length_ * thigh_length_ + shank_length_ * shank_length_ + - 2.0 * thigh_length_ * shank_length_ * std::cos(beta)); + } + [[nodiscard]] double knee_from_leg_length(double leg_length) const { + const double c = (thigh_length_ * thigh_length_ + shank_length_ * shank_length_ + - leg_length * leg_length) + / (2.0 * thigh_length_ * shank_length_); + return std::acos(std::clamp(c, -1.0, 1.0)); + } + + [[nodiscard]] bool encoder_crank_from_knee(double beta, double& c_encoder) const { + return linkage_->encoder_crank_from_knee(beta, c_encoder); + } + + [[nodiscard]] double thigh_length() const { return thigh_length_; } + [[nodiscard]] double shank_length() const { return shank_length_; } + [[nodiscard]] double knee_limit_min() const { return knee_limit_min_; } + [[nodiscard]] double knee_limit_max() const { return knee_limit_max_; } + +private: + double thigh_length_; + double shank_length_; + double knee_limit_min_; + double knee_limit_max_; + const LegLinkage* linkage_; +}; + +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/body_geometry.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/body_geometry.hpp new file mode 100644 index 000000000..795b0cd02 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/body_geometry.hpp @@ -0,0 +1,79 @@ +#pragma once +#include +#include +#include +#include + +// Generated by scripts/export_recovery_profile.py. +// Current v2.0.2 model SHA-256: 8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812 +namespace rmcs_core::controller::chassis::wheel_leg::body_geometry { +inline constexpr std::array, 121> kVertices{{ + {-0.2915f, -0.13455862f, -0.13069504f}, {-0.2915f, -0.13455862f, -0.06761809f}, + {-0.2915f, -0.08504454f, -0.01810401f}, {-0.2915f, -0.01382023f, -0.01810401f}, + {-0.2915f, 0.06141245f, -0.13069504f}, {-0.2915f, 0.07859602f, -0.11351148f}, + {-0.2915f, 0.07859602f, -0.11052026f}, {-0.29147853f, -0.13458009f, -0.13071652f}, + {-0.29147333f, -0.05154509f, -0.01348551f}, {-0.29147333f, -0.00337209f, -0.13192275f}, + {-0.29147333f, 0.07646599f, -0.01348551f}, {-0.29147333f, 0.13437474f, -0.13192275f}, + {-0.29147333f, 0.13444318f, -0.13185431f}, {-0.29147333f, 0.13444318f, -0.0714627f}, + {-0.27264753f, -0.07037089f, 0.0053403f}, {-0.27209409f, -0.00337209f, -0.15130199f}, + {-0.24235514f, 0.02636686f, -0.18104094f}, {-0.24235514f, 0.13437474f, -0.18104094f}, + {-0.24194627f, -0.13458009f, -0.18024878f}, {-0.22179806f, -0.07037089f, 0.05618976f}, + {-0.22179806f, 0.07646599f, 0.05618976f}, {-0.21652318f, -0.08504454f, 0.05687281f}, + {-0.20260966f, -0.22344896f, -0.13071652f}, {-0.20260966f, -0.1739167f, -0.18024878f}, + {-0.195407f, 0.18132287f, -0.18104094f}, {-0.195407f, 0.2305095f, -0.13185431f}, + {-0.17749999f, 0.17869371f, 0.01720456f}, {-0.17749999f, 0.18404455f, 0.01185372f}, + {-0.13951123f, -0.28654739f, -0.06761809f}, {-0.13501539f, 0.29090112f, -0.0714627f}, + {-0.12933881f, 0.29373941f, -0.06862441f}, {-0.12810409f, -0.29225096f, -0.06191452f}, + {-0.12248901f, -0.00908841f, 0.07221555f}, {-0.12248901f, 0.17869371f, 0.07221555f}, + {-0.10483428f, -0.25523497f, 0.01728151f}, {-0.08768517f, 0.02636686f, -0.18104094f}, + {-0.06862697f, -0.25523497f, 0.05348882f}, {-0.06614378f, 0.29540077f, -0.07123088f}, + {-0.06614378f, 0.29540077f, 0.01185372f}, {-0.06609949f, -0.29396975f, -0.06454799f}, + {-0.06609949f, -0.29396975f, 0.01728151f}, {-0.06390499f, -0.29396975f, 0.01947602f}, + {-0.06390499f, -0.25995695f, 0.05348882f}, {-0.06385247f, 0.23733025f, 0.07221555f}, + {-0.06385247f, 0.29540077f, 0.01414503f}, {-0.06243513f, 0.18940909f, -0.18093121f}, + {-0.06243513f, 0.29540077f, -0.07493953f}, {-0.06243323f, -0.29396975f, -0.06821425f}, + {-0.06243323f, -0.18997623f, -0.17220777f}, {-0.0494845f, -0.25208322f, 0.05687281f}, + {-0.04612551f, 0.18132287f, -0.18104094f}, {-0.03601828f, -0.01423728f, 0.08066164f}, + {-0.03601828f, 0.14277389f, 0.08066164f}, {-0.02380754f, 0.09024449f, -0.18104094f}, + {-0.02380754f, 0.1590049f, -0.18104094f}, {-0.01493952f, -0.03531604f, 0.08066164f}, + {0.03986876f, 0.07229927f, -0.18100792f}, {0.03986876f, 0.17509914f, -0.18100792f}, + {0.04553087f, 0.22432304f, 0.08066164f}, {0.04886866f, -0.1739167f, -0.18024878f}, + {0.04946913f, 0.22432304f, 0.08066164f}, {0.0494845f, -0.24153895f, 0.06741707f}, + {0.05016672f, -0.0146926f, 0.08166049f}, {0.05016672f, 0.06526834f, 0.08166049f}, + {0.05468506f, -0.23113785f, 0.07261763f}, {0.06043295f, 0.07553457f, 0.08166049f}, + {0.06243323f, -0.29396975f, -0.06821425f}, {0.06243323f, -0.18997623f, -0.17220777f}, + {0.06243513f, 0.18940909f, -0.18093121f}, {0.06243513f, 0.29540077f, -0.07493953f}, + {0.06385247f, 0.23733025f, 0.07221555f}, {0.06385247f, 0.29540077f, 0.01414503f}, + {0.06390499f, -0.29396975f, 0.01947602f}, {0.06390499f, -0.25995695f, 0.05348882f}, + {0.06478477f, -0.15800059f, -0.18024878f}, {0.06609949f, -0.29396975f, -0.06454799f}, + {0.06609949f, -0.29396975f, 0.01728151f}, {0.06614378f, 0.29540077f, -0.07123088f}, + {0.06614378f, 0.29540077f, 0.01185372f}, {0.06862697f, -0.25523497f, 0.05348882f}, + {0.08895354f, 0.04459745f, -0.18093121f}, {0.09263798f, 0.20854473f, 0.07221555f}, + {0.09524155f, -0.19058136f, 0.07261763f}, {0.10315185f, -0.06767773f, 0.08166049f}, + {0.10483428f, -0.25523497f, 0.01728151f}, {0.1069456f, 0.06258952f, -0.18093121f}, + {0.1069456f, 0.14489862f, -0.18093121f}, {0.12201826f, 0.15177391f, 0.08066164f}, + {0.15299981f, 0.20854473f, 0.01185372f}, {0.19308718f, 0.07553457f, 0.08166049f}, + {0.1965951f, -0.06767773f, 0.08166049f}, {0.1965951f, 0.07202664f, 0.08166049f}, + {0.19863972f, -0.06972234f, 0.07961588f}, {0.20105008f, -0.07625772f, 0.07261369f}, + {0.26199993f, -0.11689678f, -0.16012558f}, {0.26243384f, 0.00658312f, -0.16088976f}, + {0.26243384f, 0.11706133f, -0.16088976f}, {0.27199607f, -0.00297911f, -0.15132753f}, + {0.2723161f, 0.15359102f, -0.12436007f}, {0.2723161f, 0.15359102f, -0.07562488f}, + {0.27301174f, -0.15303232f, -0.12399004f}, {0.27301174f, -0.15303232f, -0.07612258f}, + {0.27319639f, -0.06972234f, 0.00505921f}, {0.279678f, -0.14636606f, -0.1306563f}, + {0.279678f, -0.13457485f, -0.14244752f}, {0.27975142f, 0.13437891f, -0.14357218f}, + {0.27975142f, 0.1461557f, -0.13179539f}, {0.29146922f, -0.13457485f, -0.1306563f}, + {0.2914733f, -0.05144543f, -0.0132177f}, {0.2914733f, -0.00297911f, -0.1318503f}, + {0.2914733f, 0.07202664f, -0.0132177f}, {0.2914733f, 0.13437891f, -0.1318503f}, + {0.2914733f, 0.13443382f, -0.13179539f}, {0.2914733f, 0.13443382f, -0.07562488f}, + {0.29148147f, -0.1345626f, -0.13064405f}, {0.29148147f, -0.1345626f, -0.07612258f}, + {0.29148147f, -0.07625772f, -0.0178177f}, {0.29148147f, -0.01408803f, -0.0178177f}, + {0.29148147f, 0.06148198f, -0.13064405f}, {0.29148147f, 0.07861453f, -0.11351151f}, + {0.29148147f, 0.07861453f, -0.11052026f}, +}}; +inline float support_depth(const Eigen::Vector3f& gravity) { + float depth = 0.0f; + for (const auto& p : kVertices) + depth = std::max(depth, gravity.dot(Eigen::Vector3f{p[0], p[1], p[2]})); + return depth; +} +} // namespace rmcs_core::controller::chassis::wheel_leg::body_geometry diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/calibration.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/calibration.hpp new file mode 100644 index 000000000..22da2b656 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/calibration.hpp @@ -0,0 +1,73 @@ +#pragma once + +#include "full_vehicle.hpp" + +// Generated by tools/sync_wheel_leg_calibration.py; deployment calibration only. +// Calibration JSON SHA256: fa4f1142a0c4d4b0ee1e142399869623c0a62f42875ad78ba10a14c1ea9f8292 +namespace rmcs_core::controller::chassis::full_vehicle_calibration110 { +inline constexpr std::string_view kModelSpecSha256 = + "7f62e0b47546a047219c078bdaa44fc2d82553af04b36b72ec1a94be29a22e18"; +inline constexpr std::string_view kCalibrationSha256 = + "fa4f1142a0c4d4b0ee1e142399869623c0a62f42875ad78ba10a14c1ea9f8292"; +static_assert(kModelSpecSha256 == full_vehicle_v202::kModelSpecSha256); +inline constexpr double kEncoderZeroInnerDeg = 110.0; +inline constexpr std::array kLegModelOffsets{ + 1.6000000000000001, 3.0282761689183353, -1.6000000000000001, -3.0283102010167999}; +inline constexpr std::array kHingeMin{-0.14089326779459427, -0.14084732125908661}; +inline constexpr std::array kHingeMax{1.4282761689183354, 1.4283102010167998}; +using GeometryKnot = gas_spring_model::GeometryKnot; +inline constexpr auto kLeft = [] { + std::array result{}; + for (std::size_t i = 0; i < full_vehicle_v202::kLeft.size(); ++i) + result[i] = full_vehicle_v202::kLeft[i]; + result[277] = { + 1.4036218628388806, 108.75, 0.016531185774092738, -0.041249833676382686, + 0.041250044608950043}; + result[278] = { + 1.408556209911497, 109, 0.016327785112466409, -0.041192405656176989, 0.041192616590464026}; + result[279] = { + 1.4134888007530308, 109.25, 0.016124741920915765, -0.041134244935186859, + 0.041134455872180675}; + result[280] = { + 1.418419648726353, 109.5, 0.015922059054273491, -0.041075354783896567, 0.04107556573884602}; + result[281] = { + 1.4233487670611564, 109.75, 0.015719739352581828, -0.041015738125413703, + 0.041015949062397528}; + result[282] = { + 1.4282761689183354, 110, 0.015517785643440356, -0.04095539822173111, 0.040955609167264595}; + result[283] = { + 1.4332018673155118, 110.25, 0.015316200743265444, -0.040894338080110559, + 0.04089454898704209}; + result[284] = { + 1.4381258751718267, 110.5, 0.015114987456153313, -0.040832560827206543, + 0.040832771748420926}; + return result; +}(); +inline constexpr auto kRight = [] { + std::array result{}; + for (std::size_t i = 0; i < full_vehicle_v202::kRight.size(); ++i) + result[i] = full_vehicle_v202::kRight[i]; + result[277] = { + 1.403655904215249, 108.75, 0.016531088950099575, 0.041250505881512543, + -0.041250298616911062}; + result[278] = { + 1.4085902491532705, 109, 0.016327687126931271, 0.041193076246540637, -0.041192868987654473}; + result[279] = { + 1.4135228380054787, 109.25, 0.016124642776482977, 0.041134913973605365, + -0.041134706734537348}; + result[280] = { + 1.4184536841179425, 109.5, 0.015921958753102407, 0.04107602215912079, + -0.041075814914581038}; + result[281] = { + 1.4233828007362028, 109.75, 0.015719637897352048, 0.041016403925356992, + -0.041016196699227736}; + result[282] = { + 1.4283102010167998, 110, 0.015517683036690835, 0.040956062366274866, -0.040955855153934913}; + result[283] = { + 1.4332358979798345, 110.25, 0.015316096987625677, 0.040895000685956778, + -0.040894793466338314}; + result[284] = { + 1.4381599045371354, 110.5, 0.0151148825540693, 0.040833221735045971, -0.040833014520947314}; + return result; +}(); +} // namespace rmcs_core::controller::chassis::full_vehicle_calibration110 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/full_vehicle.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/full_vehicle.hpp new file mode 100644 index 000000000..ca6978115 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/full_vehicle.hpp @@ -0,0 +1,632 @@ +#pragma once + +#include + +#include "gas_spring_types.hpp" + +// Frozen full v2.0.2 geometry and identified gas-spring data; calibration.hpp updates the +// 110-degree endpoint. +namespace rmcs_core::controller::chassis::full_vehicle_v202 { +inline constexpr std::string_view kModelSpecSha256 = + "7f62e0b47546a047219c078bdaa44fc2d82553af04b36b72ec1a94be29a22e18"; +inline constexpr std::string_view kObservationContractSha256 = + "6062baf37967d853446a95317aae4a0b315c4267da7387ab7ae020f37a021e35"; +inline constexpr std::string_view kGasModelSha256 = + "69d9afbbf32f8835214c5a191d32a6576872b3578ed9a4c2b4cdd099f022c9f1"; +inline constexpr std::array kNominalPosition{ + -0.42, 0.13722330866417465, 0.42, -0.13721605817245947, 0.0, 0.0}; +using GeometryKnot = gas_spring_model::GeometryKnot; +using ForceKnot = gas_spring_model::ForceKnot; +inline constexpr std::array kForce{{ + {0.022997316706837613, 86.965688801928167}, + {0.023851879860738903, 131.18023500886301}, + {0.024710385829300824, 185.68333781640229}, + {0.025572620660114301, 309.43303430630766}, + {0.026438366973481182, 346.73845194767057}, + {0.027307403932652949, 346.73845194774657}, + {0.029054448982586897, 350.90237841326041}, + {0.032577919665348412, 366.38968062207243}, + {0.037017290950069205, 391.09833613120429}, + {0.041465512658364498, 415.32100551306803}, + {0.04588815350788536, 446.86098014388892}, + {0.050248466389995548, 476.85464906482719}, + {0.054507465043132433, 508.41922419868212}, + {0.058624096921555034, 548.70255419074226}, + {0.060221345430025809, 567.00029919244162}, + {0.061786172348818356, 589.48924401103636}, +}}; +inline constexpr double kLowCompressionRate = 594.93986075435475; +inline constexpr double kHighCompressionSlope = 14371.522210231198; +inline constexpr std::array kLeft{{ + {-0.15582807669237769, 39.5, 0.073087248891484741, -0.019319089777503323, 0.019319239361219376}, + {-0.14834309508965904, 39.75, 0.072941844014837359, -0.019533166083065545, + 0.019533316611368257}, + {-0.14089326779459427, 40, 0.072795528285080402, -0.019746949018395299, 0.019747100426180303}, + {-0.133477989896715, 40.25, 0.072648306993020773, -0.019960416029759204, 0.019960568363070974}, + {-0.12609666995839353, 40.5, 0.072500185445195611, -0.020173545030941292, 0.020173698258970363}, + {-0.11874872965038941, 40.75, 0.072351168963446233, -0.020386314287545337, + 0.020386468353867981}, + {-0.11143360339886305, 41, 0.072201262884494841, -0.020598702345588749, 0.020598857324468173}, + {-0.10415073804344871, 41.25, 0.072050472559523654, -0.020810688187387932, + 0.020810844019633734}, + {-0.096899592505969079, 41.5, 0.071898803353756369, -0.021022251104780537, + 0.021022407791236625}, + {-0.089679637469452234, 41.75, 0.071746260646042642, -0.021233370717218036, + 0.021233528261294238}, + {-0.082490355067072862, 42, 0.071592849828444882, -0.021444027080959827, 0.021444185450390618}, + {-0.075331238580673476, 42.25, 0.071438576305827978, -0.021654200507561006, + 0.02165435971580576}, + {-0.068201792148537332, 42.5, 0.071283445495451583, -0.02186387178152438, 0.021864031789537525}, + {-0.061101530482089605, 42.75, 0.071127462826565388, -0.02207302179208101, + 0.022073182620483828}, + {-0.054029978591208894, 43, 0.070970633740007172, -0.022281632009917868, 0.022281793636974329}, + {-0.046986671517872391, 43.25, 0.070812963687803598, -0.022489684171352969, + 0.022489846597265217}, + {-0.039971154077841908, 43.5, 0.070654458132774253, -0.022697160283842305, + 0.022697323448949508}, + {-0.032982980610104573, 43.75, 0.070495122548138056, -0.022904042727665552, + 0.022904206683834333}, + {-0.026021714733816814, 44, 0.07033496241712342, -0.023110314249904479, 0.02311047894939619}, + {-0.019086929112498818, 44.25, 0.070173983232580936, -0.023315957863079106, + 0.023316123335438088}, + {-0.012178205225224392, 44.5, 0.070012190496598986, -0.023520956972309826, + 0.023521123164616686}, + {-0.0052951331445782457, 44.75, 0.06984958972012309, -0.023725295188943729, + 0.023725462144920717}, + {0.0015626886788440153, 45, 0.069686186422577748, -0.023928956654327427, 0.023929124289550824}, + {0.0083956536256196146, 45.25, 0.069521986131491589, -0.024131925502052343, + 0.024132093880541716}, + {0.015204147110576827, 45.5, 0.069356994382126141, -0.024334186484023718, 0.024334355562479565}, + {0.021988546779951879, 45.75, 0.069191216717106915, -0.024535724516947049, + 0.024535894299191036}, + {0.028749222702838417, 46, 0.06902465868605881, -0.024736524827663614, 0.024736695303081008}, + {0.035486537557128051, 46.25, 0.0688573258452439, -0.024936573035379772, 0.024936744196080107}, + {0.042200846810079296, 46.5, 0.068689223757202697, -0.025135854892633952, 0.025136026720066679}, + {0.048892498893745584, 46.75, 0.068520357990399225, -0.025334356553250142, + 0.025334529069621692}, + {0.055561835375373181, 47, 0.068350734118868689, -0.025532064533080961, 0.025532237696424653}, + {0.062209191122991558, 47.25, 0.068180357721868923, -0.025728965477534921, + 0.025729139255224048}, + {0.06883489446629848, 47.5, 0.068009234383535178, -0.025925046378662442, 0.025925220810579463}, + {0.07543926735301909, 47.75, 0.067837369692537952, -0.026120294620553541, 0.026120469697974269}, + {0.082022625500886293, 48, 0.067664769241744901, -0.026314697756090799, 0.026314873435847985}, + {0.088585278545361412, 48.25, 0.067491438627885064, -0.02650824350406128, 0.026508419827290769}, + {0.095127530183260756, 48.5, 0.067317383451217516, -0.026700920181196935, 0.026701097111306693}, + {0.10164967831238031, 48.75, 0.067142609315203244, -0.026892716084689655, 0.026892893606049018}, + {0.10815201516728218, 49, 0.066967121826179851, -0.027083619842784346, 0.027083797970132051}, + {0.11463482745134485, 49.25, 0.066790926593040312, -0.027273620421484623, 0.027273799153774647}, + {0.12109839646518533, 49.5, 0.066614029226915131, -0.027462707008410608, 0.027462886296818201}, + {0.1275429982315916, 49.75, 0.066436435340857525, -0.027650869037831316, 0.027651048874754447}, + {0.13396890361705055, 50, 0.066258150549532768, -0.02783809614658439, 0.027838276561555297}, + {0.1403763784499959, 50.25, 0.06607918046891037, -0.028024378321916593, 0.028024559289211742}, + {0.14676568363585446, 50.5, 0.0658995307159599, -0.0282097057855204, 0.028209887300927883}, + {0.1531370752690126, 50.75, 0.06571920690835073, -0.028394068880527118, 0.028394250939065113}, + {0.15949080474179461, 51, 0.065538214664154451, -0.028577458352823437, 0.028577640926276932}, + {0.16582711885051643, 51.25, 0.065356559601551512, -0.028759865063595014, 0.028760048172797259}, + {0.17214625989874405, 51.5, 0.065174247338540922, -0.02894128017786507, 0.028941463786422095}, + {0.17844846579781387, 51.75, 0.064991283492653637, -0.029121694995154435, 0.029121879147893409}, + {0.184733970164705, 52, 0.064807673680669156, -0.029301101249137679, 0.029301285885734663}, + {0.19100300241734686, 52.25, 0.064623423518336151, -0.029479490614994663, 0.029479675795515026}, + {0.19725578786743497, 52.5, 0.064438538620095701, -0.029656855268894694, 0.029657040890809949}, + {0.20349254781083265, 52.75, 0.064253024598808897, -0.029833187355385982, 0.029833373463692608}, + {0.20971349961562208, 53, 0.064066887065487216, -0.030008479366998797, 0.030008665945459256}, + {0.21591885680788997, 53.25, 0.063880131629026865, -0.030182724089660828, 0.030182911123232514}, + {0.22210882915530133, 53.5, 0.063692763895946122, -0.030355914272701429, 0.030356101790213141}, + {0.22828362274852781, 53.75, 0.06350478947012661, -0.030528043112985376, 0.030528231070784118}, + {0.23444344008060375, 54, 0.063316213952557446, -0.030699103826506008, 0.030699292259975697}, + {0.24058848012425466, 54.25, 0.063127042941083428, -0.030869090006282851, 0.030869278901204611}, + {0.24671893840727516, 54.5, 0.062937282030155819, -0.031037995259918655, 0.031038184562201337}, + {0.25283500708599899, 54.75, 0.062746936810587503, -0.031205813553315476, 0.031206003285196265}, + {0.2589368750169167, 55, 0.062556012869310965, -0.031372538926821403, 0.031372729083376683}, + {0.26502472782651393, 55.25, 0.062364515789139374, -0.03153816561798084, 0.03153835617474704}, + {0.27109874797934258, 55.5, 0.062172451148531906, -0.031702688069152107, 0.031702879056157041}, + {0.27715911484442551, 55.75, 0.061979824521361762, -0.031866100964157817, 0.031866292368022686}, + {0.2832060047599923, 56, 0.061786641476687332, -0.032028399118859074, 0.032028590895361753}, + {0.28923959109663894, 56.25, 0.06159290757852736, -0.032189577447698625, 0.032189769646871536}, + {0.29526004431891345, 56.5, 0.061398628385639142, -0.032349631174556914, 0.032349823732040039}, + {0.30126753204540729, 56.75, 0.061203809451299912, -0.032508555595765154, 0.032508748530297171}, + {0.30726221910737223, 57, 0.061008456323091265, -0.03266634616603592, 0.032666539480251056}, + {0.31324426760591634, 57.25, 0.060812574542687632, -0.032822998654203683, 0.032823192355634689}, + {0.31921383696781497, 57.5, 0.060616169645646922, -0.032978508713731369, 0.032978702771882695}, + {0.32517108399995959, 57.75, 0.060419247161205746, -0.03313287240577456, 0.033133066837725877}, + {0.33111616294252461, 58, 0.060221812612076167, -0.033286085995680559, 0.033286280800635781}, + {0.33704922552083244, 58.25, 0.060023871514247415, -0.03343814567713449, 0.033438340807535105}, + {0.34297042099599556, 58.5, 0.059825429376789294, -0.033589047818225748, 0.033589243286016812}, + {0.34887989621435106, 58.75, 0.059626491701659602, -0.033738789096417199, 0.033738984889641332}, + {0.35477779565571915, 59, 0.059427063983514453, -0.033887366228742, 0.033887562373211833}, + {0.36066426148052427, 59.25, 0.059227151709521614, -0.03403477611909117, 0.034034972602029978}, + {0.36653943357581226, 59.5, 0.059026760359176984, -0.034181015720431362, 0.034181212514644954}, + {0.37240344960018018, 59.75, 0.058825895404124293, -0.034326082284512838, 0.034326279370412549}, + {0.37825644502765726, 60, 0.05862456230797778, -0.034469973140905608, 0.034470170555121415}, + {0.38409855319057334, 60.25, 0.058422766526147418, -0.034612685617933348, 0.03461288333353546}, + {0.38992990532142108, 60.5, 0.058220513505668127, -0.034754217287148781, 0.034754415305822239}, + {0.39575063059375409, 60.75, 0.05801780868503105, -0.034894565947204581, 0.034894764271239109}, + {0.40156085616214671, 61, 0.05781465749401829, -0.035033729307709593, 0.03503392793708765}, + {0.40736070720123935, 61.25, 0.057611065353540325, -0.03517170545629944, 0.035171904345384641}, + {0.41315030694387217, 61.5, 0.057407037675476433, -0.035308492375931144, 0.035308691550764937}, + {0.418929776718376, 61.75, 0.057202579862518456, -0.035444088280050745, 0.035444287776388528}, + {0.42469923598499376, 62, 0.056997697308016092, -0.035578491523419908, 0.035578691249642191}, + {0.43045880237149109, 62.25, 0.056792395395827006, -0.035711700459646223, 0.035711900452944534}, + {0.43620859170796206, 62.5, 0.056586679500167769, -0.035843713646333819, 0.035843913918060688}, + {0.44194871806084074, 62.75, 0.056380554985469192, -0.035974529852641435, 0.035974730378966091}, + {0.44767929376617127, 63, 0.056174027206233129, -0.036104147747942905, 0.036104348548446297}, + {0.45340042946211517, 63.25, 0.05596710150689322, -0.036232566252562437, 0.03623276730281888}, + {0.45911223412075192, 63.5, 0.055759783221677184, -0.036359784287265218, 0.036359985569035913}, + {0.4648148150791615, 63.75, 0.055552077674473052, -0.03648580103947896, 0.036486002609740775}, + {0.4705082780698277, 64, 0.055343990178696754, -0.036610615656669898, 0.036610817418956054}, + {0.47619272725037393, 64.25, 0.055135526037163138, -0.036734227346030213, 0.036734429369477573}, + {0.48186826523263598, 64.5, 0.054926690541960213, -0.036856635670547477, 0.036856837907446038}, + {0.48753499311111342, 64.75, 0.054717488974324019, -0.0369778400078562, 0.036978042458471749}, + {0.49319301049079051, 65, 0.05450792660451817, -0.037097839981591108, 0.037098042640872742}, + {0.49884241551435321, 65.25, 0.054298008691714764, -0.037216635241024032, 0.037216838135046226}, + {0.50448330488882931, 65.5, 0.054087740483878044, -0.03733422560869476, 0.03733442871617787}, + {0.51011577391163065, 65.75, 0.053877127217650589, -0.037450610841496423, 0.037450814169477098}, + {0.51573991649606188, 66, 0.053666174118242246, -0.037565790982832414, 0.03756599450716952}, + {0.52135582519626267, 66.25, 0.053454886399320903, -0.037679766069334431, 0.037679969796413415}, + {0.52696359123163061, 66.5, 0.053243269262905971, -0.037792536152970624, 0.037792740100510369}, + {0.53256330451071188, 66.75, 0.053031327899264465, -0.037904101524302847, 0.037904305658197746}, + {0.53815505365459559, 67, 0.052819067486809618, -0.038014462353110401, 0.038014666670655284}, + {0.54373892601981066, 67.25, 0.052606493192000336, -0.038123619104062367, 0.038123823601184881}, + {0.54931500772072961, 67.5, 0.052393610169245541, -0.038231572169700689, 0.038231776841960108}, + {0.55488338365152901, 67.75, 0.052180423560808042, -0.038338322074701281, 0.038338526969497078}, + {0.56044413750765409, 68, 0.051966938496712668, -0.038443869489669158, 0.038444074515495197}, + {0.56599735180686173, 68.25, 0.051753160094656199, -0.03854821496638644, 0.038548420156456487}, + {0.57154310790982343, 68.5, 0.051539093459918119, -0.038651359300460708, 0.038651564708177322}, + {0.57708148604027976, 68.75, 0.05132474368527587, -0.03875330330328508, 0.038753508848347995}, + {0.58261256530481087, 69, 0.051110115850920068, -0.03885404782059898, 0.038854253525337168}, + {0.58813642371217223, 69.25, 0.050895215024373719, -0.038953593906118744, 0.038953799769980479}, + {0.59365313819224852, 69.5, 0.050680046260411671, -0.03905194236058751, 0.039052148410224974}, + {0.59916278461461414, 69.75, 0.050464614600983879, -0.039149094524372097, 0.039149300700742462}, + {0.604665437806714, 70, 0.050248925075139519, -0.039245051383483609, 0.03924525775157095}, + {0.61016117157168415, 70.25, 0.050032982698953635, -0.03933981418977528, 0.039340020693678424}, + {0.61565005870579359, 70.5, 0.049816792475455493, -0.039433384197042842, 0.039433590824105802}, + {0.62113217101555174, 70.75, 0.049600359394559645, -0.039525762714552715, 0.039525969475368017}, + {0.62660757933446254, 71, 0.049383688432997744, -0.039616951167995462, 0.039617158096877825}, + {0.63207635353944114, 71.25, 0.049166784554253273, -0.039706950988937284, 0.039707158058076619}, + {0.63753856256690566, 71.5, 0.048949652708497771, -0.039795763633445946, 0.039795970840342865}, + {0.64299427442854684, 71.75, 0.048732297832528859, -0.039883390723162215, 0.039883598004053833}, + {0.6484435562267814, 72, 0.048514724849710415, -0.0399698338281268, 0.039970041263932277}, + {0.65388647416990842, 72.25, 0.048296938669914302, -0.040055094610481234, 0.040055302190965425}, + {0.65932309358695163, 72.5, 0.048078944189463929, -0.040139174808004219, 0.040139382500575138}, + {0.66475347894221892, 72.75, 0.047860746291079573, -0.040222076116140371, 0.04022228391565652}, + {0.67017769384957893, 73, 0.047642349843825649, -0.040303800428897585, 0.040304008354424603}, + {0.67559580108644468, 73.25, 0.047423759703059419, -0.040384349519807849, 0.040384557574335345}, + {0.68100786260750512, 73.5, 0.047204980710381608, -0.040463725300792008, 0.040463933487017656}, + {0.68641393955816588, 73.75, 0.046986017693588636, -0.040541929881379668, 0.040542138145284345}, + {0.69181409228774138, 74, 0.046766875466626515, -0.040618965063519696, 0.040619173452215607}, + {0.69720838036240318, 74.25, 0.046547558829546011, -0.040694832940609216, 0.040695041416245081}, + {0.70259686257785425, 74.5, 0.046328072568460194, -0.040769535635955201, 0.0407697442261159}, + {0.70797959697177637, 74.75, 0.046108421455502827, -0.040843075200703516, 0.040843283893527427}, + {0.71335664083603745, 75, 0.04588861024878868, -0.040915453918292266, 0.040915662703183531}, + {0.71872805072865731, 75.25, 0.045668643692375367, -0.040986673906719007, 0.040986882784539047}, + {0.72409388248555784, 75.5, 0.045448526516226148, -0.0410567373820514, 0.041056946346088474}, + {0.72945419123208044, 75.75, 0.045228263436175684, -0.041125646737891708, 0.041125855800980937}, + {0.73480903139429254, 76, 0.045007859153894936, -0.041193404089766239, 0.041193613219420167}, + {0.74015845671006564, 76.25, 0.044787318356859993, -0.041260012077430408, 0.041260221306813671}, + {0.74550252023996988, 76.5, 0.044566645718320572, -0.041325472820015474, 0.041325682143545317}, + {0.75084127437794579, 76.75, 0.044345845897270322, -0.041389788878941458, 0.041389998281034734}, + {0.75617477086177676, 77, 0.044124923538419389, -0.041452962704323804, 0.041453172190606291}, + {0.76150306078337437, 77.25, 0.043903883272166822, -0.041514996763910739, 0.041515206317941922}, + {0.7668261945988748, 77.5, 0.043682729714575583, -0.041575893565865107, 0.041576103187425939}, + {0.77214422213852774, 77.75, 0.043461467467348393, -0.041635655643666891, 0.041635865349470003}, + {0.77745719261643176, 78, 0.043240101117804519, -0.041694285667101483, 0.041694495433383938}, + {0.78276515464006491, 78.25, 0.043018635238858682, -0.041751786169660521, 0.041751996032417471}, + {0.78806815621966009, 78.5, 0.042797074389000309, -0.041808159783756127, 0.041808369700023607}, + {0.79336624477739126, 78.75, 0.042575423112274496, -0.041863409203831858, 0.041863619177369471}, + {0.79865946715640268, 79, 0.042353685938264329, -0.041917537144363748, 0.041917747175040265}, + {0.80394786962967446, 79.25, 0.042131867382073855, -0.041970546285193644, 0.041970756392983144}, + {0.80923149790871984, 79.5, 0.041909971944312674, -0.04202243939131789, 0.042022649559662117}, + {0.81451039715212858, 79.75, 0.041688004111081631, -0.042073219223792369, 0.042073429486135461}, + {0.81978461197396224, 80, 0.041465968353959201, -0.042122888671658976, 0.042123098938704029}, + {0.82505418645198381, 80.25, 0.041243869129989619, -0.04217145037812281, 0.042171660724134308}, + {0.83031916413575613, 80.5, 0.041021710881671669, -0.042218907239397807, 0.042219117637308179}, + {0.83557958805458377, 80.75, 0.040799498036948739, -0.042265262169979834, 0.042265472642613433}, + {0.84083550072531787, 81, 0.040577235009199839, -0.042310518150625365, 0.042310728627361072}, + {0.84608694416002028, 81.25, 0.040354926197231356, -0.042354677818440933, 0.04235488835066703}, + {0.85133395987349703, 81.5, 0.040132575985270647, -0.042397744335847642, 0.042397954979201846}, + {0.85657658889068733, 81.75, 0.039910188742959714, -0.042439720580733697, 0.042439931212266857}, + {0.86181487175393423, 82, 0.039687768825350404, -0.042480609460833207, 0.042480820158389665}, + {0.86704884853011976, 82.25, 0.039465320572900323, -0.04252041403815926, 0.042520624752003189}, + {0.87227855881768246, 82.5, 0.039242848311469944, -0.042559137280844594, 0.042559348047837287}, + {0.8775040417534985, 82.75, 0.039020356352320304, -0.042596782129128208, 0.042596992957985191}, + {0.8827253360196563, 83, 0.038797848992111847, -0.04263335176338906, 0.042633562638391242}, + {0.88794247985011077, 83.25, 0.038575330512904141, -0.042668849156767275, 0.042669060056931662}, + {0.8931555110372138, 83.5, 0.038352805182156106, -0.042703277436044296, 0.042703488361102548}, + {0.89836446693814387, 83.75, 0.038130277252727898, -0.042736639551429922, 0.042736850521257586}, + {0.90356938448121138, 84, 0.037907750962882902, -0.042768938705061155, 0.042769149692507946}, + {0.90877030017207172, 84.25, 0.037685230536290387, -0.042800178017286694, 0.042800389028959773}, + {0.9139672500998155, 84.5, 0.03746272018203, -0.042830360564991288, 0.042830571611697454}, + {0.91916026994296263, 84.75, 0.037240224094596278, -0.042859489474161405, 0.042859700555695879}, + {0.9243493949753554, 85, 0.037017746453903412, -0.04288756789077014, 0.042887779025514439}, + {0.92953466007195651, 85.25, 0.036795291425292434, -0.042914598999623521, 0.042914810157375027}, + {0.93471609971451808, 85.5, 0.036572863159537373, -0.042940586030412731, 0.042940797200163083}, + {0.93989374799721048, 85.75, 0.036350465792853157, -0.042965532154878527, 0.042965743340687423}, + {0.94506763863209864, 86, 0.036128103446903974, -0.042989440465282469, 0.042989651679975704}, + {0.95023780495456101, 86.25, 0.035905780228812384, -0.043012314277878438, 0.043012525490450537}, + {0.95540427992860888, 86.5, 0.035683500231168966, -0.04303415680302225, 0.043034368053948613}, + {0.96056709615211244, 86.75, 0.035461267532042672, -0.04305497114601986, 0.043055182403818726}, + {0.96572628586195042, 87, 0.035239086194992116, -0.043074760633336706, 0.043074971948352414}, + {0.97088188093905337, 87.25, 0.035016960269076899, -0.043093528596974795, 0.043093739907709594}, + {0.97603391291338637, 87.5, 0.034794893788870462, -0.043111278146092002, 0.043111489475361808}, + {0.98118241296883935, 87.75, 0.034572890774472433, -0.043128012533165005, 0.043128223901517271}, + {0.98632741194803564, 88, 0.034350955231522776, -0.043143735189318133, 0.043143946507595343}, + {0.99146894035705646, 88.25, 0.034129091151215284, -0.043158449236970209, 0.043158660615922773}, + {0.99660702837010184, 88.5, 0.033907302510312889, -0.04317215801788421, 0.043172369392494747}, + {1.0017417058340627, 88.75, 0.033685593271162467, -0.043184864793511746, 0.043185076189633687}, + {1.0068730022730241, 89, 0.033463967381710799, -0.04319657284417544, 0.04319678421291745}, + {1.0120009468926932, 89.25, 0.033242428775521048, -0.043207285531058459, 0.043207496942133605}, + {1.0171255685847573, 89.5, 0.03302098137178943, -0.04321700615836982, 0.043217217590580004}, + {1.0222468959311626, 89.75, 0.032799629075362532, -0.043225737987903745, 0.043225949439981648}, + {1.0273649572083425, 90, 0.032578375776755431, -0.043233484406397793, 0.043233695842548381}, + {1.0324797803913588, 90.25, 0.03235722535216988, -0.043240248552374794, 0.043240459999585701}, + {1.0375913931579837, 90.5, 0.032136181663513189, -0.043246033979963461, 0.043246245429418018}, + {1.0426998228927191, 90.75, 0.031915248558417333, -0.043250843933814453, 0.043251055389646631}, + {1.0478050966907433, 91, 0.031694429870259007, -0.043254681714979926, 0.043254893174855424}, + {1.0529072413618106, 91.25, 0.031473729418179552, -0.043257550641082443, 0.043257762113326048}, + {1.058006283434066, 91.5, 0.031253151007105621, -0.04325945414953003, 0.043259665601236896}, + {1.0631022491578237, 91.75, 0.031032698427770261, -0.043260395498083526, 0.04326060696209183}, + {1.0681951645092616, 92, 0.030812375456734271, -0.043260378006447331, 0.043260589496325108}, + {1.0732850551940813, 92.25, 0.030592185856408063, -0.043259405113076188, 0.043259616589969574}, + {1.0783719466510946, 92.5, 0.030372133375073766, -0.043257480086755951, 0.043257691552814836}, + {1.0834558640557539, 92.75, 0.030152221746908028, -0.043254606334269807, 0.043254817803981771}, + {1.088536832323638, 93, 0.029932454692004687, -0.04325078719399994, 0.043250998670322394}, + {1.0936148761138722, 93.25, 0.029712835916398325, -0.043246025979983116, 0.043246237439235224}, + {1.0986900198325018, 93.5, 0.02949336911208756, -0.043240326092102321, 0.043240537613179864}, + {1.1037622876358095, 93.75, 0.029274057957059514, -0.04323369087907214, 0.043233902347751874}, + {1.1088317034335899, 94, 0.029054906115313743, -0.043226123678010542, 0.043226335135226557}, + {1.1138982908923536, 94.25, 0.028835917236886853, -0.043217627802488612, 0.043217839292545746}, + {1.1189620734385051, 94.5, 0.028617094957877891, -0.043208206703763186, 0.043208418208238952}, + {1.1240230742614574, 94.75, 0.028398442900472692, -0.043197863710643547, 0.043198075166468453}, + {1.1290813163167046, 95, 0.028179964672970223, -0.043186602047013689, 0.043186813521526701}, + {1.1341368223288419, 95.25, 0.027961663869807959, -0.04317442530995319, 0.043174636748181172}, + {1.1391896147945482, 95.5, 0.027743544071588272, -0.043161336585302579, 0.043161548037487951}, + {1.1442397159855122, 95.75, 0.027525608845103966, -0.043147339433511789, 0.043147550860232531}, + {1.1492871479513274, 96, 0.027307861743365899, -0.043132437155649184, 0.043132648591028167}, + {1.1543319325223271, 96.25, 0.027090306305629183, -0.043116633047995123, 0.043116844479613059}, + {1.159374091312396, 96.5, 0.026872946057420126, -0.043099930445533841, 0.0431001418452992}, + {1.1644136457217127, 96.75, 0.026655784510563982, -0.043082332803602075, 0.043082544201629769}, + {1.1694506169394878, 97, 0.026438825163212269, -0.043063843322484752, 0.043064054750403868}, + {1.1744850259466215, 97.25, 0.02622207149987011, -0.043044465478557004, 0.043044676875007959}, + {1.1795168935183502, 97.5, 0.026005526991424784, -0.043024202540036027, 0.043024413962084507}, + {1.1845462402268361, 97.75, 0.025789195095173502, -0.04300305792008452, 0.043003269325082749}, + {1.1895730864437342, 98, 0.025573079254851611, -0.042981034909690463, 0.042981246323678501}, + {1.1945974523427023, 98.25, 0.025357182900661523, -0.042958136762227039, 0.042958348149104986}, + {1.1996193579018939, 98.5, 0.025141509449301033, -0.042934366940113933, 0.042934578283119751}, + {1.2046388229063949, 98.75, 0.024926062303991962, -0.042909728692829607, 0.04290994006419202}, + {1.2096558669506376, 99, 0.024710844854509813, -0.042884225324523406, 0.042884436672286697}, + {1.2146705094407746, 99.25, 0.02449586047721241, -0.04285786030866956, 0.04285807163972144}, + {1.2196827695970176, 99.5, 0.024281112535069344, -0.042830636730586957, 0.042830848063461491}, + {1.2246926664559457, 99.75, 0.024066604377691626, -0.042802558071840802, 0.04280276935033811}, + {1.2297002188727715, 100, 0.023852339341360849, -0.04277362764206416, 0.0427738389205361}, + {1.2347054455235817, 100.25, 0.023638320749059305, -0.042743848670255047, 0.04274405995526237}, + {1.239708364907546, 100.5, 0.023424551910499788, -0.042713224469200062, 0.042713435777138098}, + {1.2447089953490853, 100.75, 0.0232110361221552, -0.042681758461687147, 0.042681969732918601}, + {1.2497073550000177, 101, 0.022997776667289027, -0.042649453847408758, 0.042649665113220381}, + {1.2547034618416666, 101.25, 0.022784776815985452, -0.042616313877407719, 0.042616525143701789}, + {1.2596973336869453, 101.5, 0.022572039825179427, -0.042582341894911169, 0.042582553152165248}, + {1.2646889881824064, 101.75, 0.022359568938687166, -0.042547541104491216, 0.042547752360185265}, + {1.2696784428102625, 102, 0.022147367387236473, -0.042511914972366449, 0.042512126196697864}, + {1.2746657148903757, 102.25, 0.021935438388497819, -0.042475466616562874, 0.042475677813860968}, + {1.2796508215822298, 102.5, 0.021723785147114233, -0.042438199384057962, 0.042438410564273332}, + {1.2846337798868583, 102.75, 0.021512410854732524, -0.042400116492887108, 0.042400327663708437}, + {1.289614606648759, 103, 0.021301318690033606, -0.042361221195425514, 0.042361432354068473}, + {1.2945933185577674, 103.25, 0.02109051181876382, -0.042321516777278578, 0.042321727940412779}, + {1.2995699321509224, 103.5, 0.020879993393765506, -0.042281006489900907, 0.042281217651954695}, + {1.3045444638142845, 103.75, 0.020669766555008066, -0.042239693563422775, 0.042239904719395482}, + {1.3095169297847478, 104, 0.020459834429618587, -0.042197581237192916, 0.042197792379246923}, + {1.314487346151807, 104.25, 0.020250200131913559, -0.042154672771210766, 0.042154883851030611}, + {1.3194557288593198, 104.5, 0.020040866763429373, -0.042110971394297148, 0.042111182507461375}, + {1.3244220937072304, 104.75, 0.019831837412953586, -0.042066480260972705, 0.042066691366398179}, + {1.329386456353272, 105, 0.019623115156556056, -0.042021202689394466, 0.042021413769789795}, + {1.33434883231465, 105.25, 0.01941470305762031, -0.041975141830742413, 0.041975352924184473}, + {1.3393092369696988, 105.5, 0.019206604166874204, -0.041928300983211375, 0.041928512046879807}, + {1.3442676855595108, 105.75, 0.018998821522421744, -0.041880683301428911, 0.041880894312107009}, + {1.3492241931895546, 106, 0.01879135814977375, -0.041832291799125731, 0.041832502881037514}, + {1.3541787748312533, 106.25, 0.018584217061879543, -0.041783130037951977, 0.041783341047132716}, + {1.3591314453235648, 106.5, 0.01837740125915778, -0.041733200772297641, 0.041733411813136834}, + {1.3640822193745203, 106.75, 0.018170913729527838, -0.041682507571202232, 0.041682718584172385}, + {1.3690311115627418, 107, 0.017964757448441115, -0.0416310534435681, 0.041631264437316462}, + {1.3739781363389594, 107.25, 0.017758935378912079, -0.041578841475442108, 0.041579052491137469}, + {1.378923308027481, 107.5, 0.017553450471549673, -0.041525874940217533, 0.041526085917512223}, + {1.3838666408276614, 107.75, 0.017348305664588386, -0.041472157005383592, 0.041472367980015856}, + {1.3888081488153481, 108, 0.017143503883919275, -0.041417690730223555, 0.041417901696952641}, + {1.3937478459442911, 108.25, 0.016939048043121581, -0.041362479353159509, 0.041362690322962692}, + {1.3986857460475635, 108.5, 0.016734941043493295, -0.04130652588453898, 0.041306736869607896}, +}}; +inline constexpr std::array kRight{{ + {-0.15578179287215238, 39.5, 0.073087502903561655, 0.019319956432047335, -0.019319809558622305}, + {-0.14829698130038488, 39.75, 0.07294209705800124, 0.019534035731794189, -0.01953388790890263}, + {-0.14084732125908661, 40, 0.072795780352963288, 0.019747821514849568, -0.019747672828034919}, + {-0.13343220789652188, 40.25, 0.072648558079311309, 0.019961291357206437, + -0.019961141771985439}, + {-0.12605104983226534, 40.5, 0.072500435543639344, 0.020174423096429785, -0.020174272640682497}, + {-0.11870326879280541, 40.75, 0.072351418067845383, 0.020387195027956755, + -0.020387043700454785}, + {-0.11138829925860044, 41, 0.072201510988708542, 0.020599585695857899, -0.02059943352719474}, + {-0.10410558812217902, 41.25, 0.072050719657467774, 0.020811574089551341, + -0.020811421046450632}, + {-0.096854594356919455, 41.5, 0.071899049439403548, 0.021023139483936172, -0.02102298558504204}, + {-0.089634788696093204, 41.75, 0.071746505713422393, 0.021234261446217463, + -0.02123410675246494}, + {-0.08244565332184417, 42, 0.071593093871643393, 0.021444920203399599, -0.021444764659806336}, + {-0.075286681563754898, 42.25, 0.071438819318988225, 0.021655095910286644, + -0.021654939567962068}, + {-0.068157377606650693, 42.5, 0.071283687472773333, 0.021864769295031145, + -0.021864612164956155}, + {-0.061057256207329658, 42.75, 0.07112770376230515, 0.022073921500479793, + -0.022073763564708679}, + {-0.053985842419918995, 43, 0.070970873628478079, 0.022282533815183778, -0.022282375102552002}, + {-0.046942671329554153, 43.25, 0.070813202523375418, 0.022490587990661743, + -0.022490428482857445}, + {-0.039927287794080502, 43.5, 0.070654695909873264, 0.022698066067934005, + -0.022697905798968472}, + {-0.032939246193542993, 43.75, 0.070495359261247267, 0.022904950355339578, + -0.022904789345071959}, + {-0.025978110187147185, 44, 0.070335198060782186, 0.023111223726060611, -0.023111061968172551}, + {-0.019043452477490785, 44.25, 0.070174217801384928, 0.023316869131916496, + -0.023316706605751695}, + {-0.012134854581773602, 44.5, 0.070012423985200342, 0.023521869948726678, + -0.023521706719102947}, + {-0.0052519066097882483, 44.75, 0.069849822123230168, 0.023726209888670455, + -0.023726045945929641}, + {0.001605792951558549, 45, 0.069686417734955, 0.023929872881814602, -0.023929708219577312}, + {0.0084386374474048065, 45.25, 0.069522216347959673, 0.024132843301204424, + -0.024132677935989392}, + {0.015247012257973813, 45.5, 0.06935722349756164, 0.024335105804699438, -0.024334939745507968}, + {0.022031294995729589, 45.75, 0.069191444726442383, 0.024536645300263362, -0.0245364785294526}, + {0.028791855696793156, 46, 0.069024885584282436, 0.024737446987164371, -0.024737279569410953}, + {0.035529057006853437, 46.25, 0.068857551627399663, 0.024937496494181137, -0.02493732839555236}, + {0.042243254361724103, 46.5, 0.068689448418390128, 0.025136779647699923, -0.025136610882205375}, + {0.048934796162746819, 46.75, 0.068520581525773339, 0.025335282573121735, + -0.025335113177885109}, + {0.055604023947175452, 47, 0.068350956523639789, 0.025532991722683862, -0.025532821654443504}, + {0.062251272553738968, 47.25, 0.068180578991302559, 0.02572989378557029, -0.025729723097092875}, + {0.068876870283509473, 47.5, 0.068009454512951942, 0.025925975822527082, -0.025925804501457459}, + {0.075481139056247093, 47.75, 0.067837588677313501, 0.026121225121881673, + -0.026121053183831591}, + {0.082064394562355758, 48, 0.06766498707730946, 0.026315629173326218, -0.026315456621798291}, + {0.088626946410597818, 48.25, 0.067491655309723619, 0.026509175938872739, + -0.026509002786686487}, + {0.095169098271689678, 48.5, 0.067317598974869733, 0.026701853499313141, -0.026701679771455839}, + {0.10169114801791784, 48.75, 0.067142823676262897, 0.026893650270529625, -0.026893475920953775}, + {0.10819338785891947, 49, 0.066967335020295018, 0.0270845548930293, -0.027084379955761274}, + {0.1146761044736902, 49.25, 0.066791138615913043, 0.027274556206039309, -0.027274380714132761}, + {0.12113957913902468, 49.5, 0.066614240074301445, 0.027463643550654327, -0.027463467467988594}, + {0.12758408785440883, 49.75, 0.066436645008567108, 0.027651806257939494, -0.027651629618967447}, + {0.13400990146355202, 50, 0.066258359033428915, 0.027839034001407748, -0.027838856797176136}, + {0.14041728577260332, 50.25, 0.066079387764909653, 0.028025316797586172, -0.028025139090052654}, + {0.14680650166520209, 50.5, 0.065899736820032209, 0.028210644795562713, -0.028210466542239895}, + {0.15317780521442553, 50.75, 0.065719411816518677, 0.028395008427948884, -0.028394829664523547}, + {0.15953144779174805, 51, 0.065538418372493815, 0.028578398398130166, -0.028578219072887264}, + {0.16586767617309134, 51.25, 0.065356762106190402, 0.028760805557932378, -0.028760625710703265}, + {0.17218673264207168, 51.5, 0.065174448635660145, 0.028942221098654006, -0.028942040749343693}, + {0.17848885509050011, 51.75, 0.064991483578486053, 0.029122636349857137, -0.029122455476759092}, + {0.18477427711625521, 52, 0.064807872551499862, 0.029302042891568951, -0.029301861531179123}, + {0.19104322811857216, 52.25, 0.064623621170501952, 0.029480432574344875, -0.029480250749581646}, + {0.19729593339084911, 52.5, 0.064438735049984971, 0.029657797460673974, -0.02965761512984565}, + {0.20353261421104224, 52.75, 0.064253219802861722, 0.02983412985959898, -0.029833947046926823}, + {0.20975348792970755, 53, 0.06406708104019472, 0.030009422018729599, -0.030009238791277031}, + {0.21595876805577269, 53.25, 0.063880324370931191, 0.0301836669001369, -0.030183483163762813}, + {0.22214866434010411, 53.5, 0.063692955401640383, 0.030356857254265675, -0.030356673070238857}, + {0.22832338285692394, 53.75, 0.063504979736254408, 0.030528986163991755, -0.030528801510811766}, + {0.23448312608316396, 54, 0.063316402975812841, 0.030700046993751163, -0.030699861935003552}, + {0.2406280929757792, 54.25, 0.063127230718210442, 0.030870033187278961, -0.030869847700412754}, + {0.24675847904712217, 54.5, 0.062937468557948614, 0.031038938467303462, -0.03103875253987419}, + {0.25287447643839539, 54.75, 0.06274712208588995, 0.031206756764814082, -0.031206570408675981}, + {0.25897627399127959, 55, 0.062556196889016299, 0.031373482088190165, -0.031373295327739283}, + {0.26506405731774491, 55.25, 0.062364698550190285, 0.031539108706101958, -0.031538921509823556}, + {0.2711380088681295, 55.5, 0.062172632647919966, 0.0317036310253003, -0.031703443470772516}, + {0.27719830799752504, 55.75, 0.061980004756127291, 0.031867043812694555, -0.031866855843504412}, + {0.28324513103052046, 56, 0.061786820443919338, 0.032029341778764507, -0.032029153402397065}, + {0.28927865132433533, 56.25, 0.061593085275363141, 0.032190519925934982, -0.032190331146797695}, + {0.29529903933041973, 56.5, 0.061398804809264261, 0.032350573442448918, -0.032350384304854166}, + {0.30130646265452193, 56.75, 0.061203984598947485, 0.032509497581852682, -0.032509308065452158}, + {0.30730108611531254, 57, 0.061008630192042244, 0.03266728792884005, -0.032667098047322506}, + {0.31328307180156484, 57.25, 0.060812747130270191, 0.032823940068270649, -0.032823749825353452}, + {0.3192525791279614, 57.5, 0.060616340949236583, 0.032979449845210458, -0.032979259251341785}, + {0.32520976488955128, 57.75, 0.060419417178224646, 0.033133813311230845, -0.033133622349621313}, + {0.33115478331488285, 58, 0.06022198133999343, 0.033287026483000477, -0.03328683516893817}, + {0.33708778611789386, 58.25, 0.060024038950578271, 0.033439085735734356, -0.033438894082470569}, + {0.34300892254852505, 58.5, 0.059825595519095318, 0.033589987508218344, -0.033589795498299657}, + {0.3489183394421646, 58.75, 0.059626656547548056, 0.033739728303814172, -0.03373953599297172}, + {0.35481618126789521, 59, 0.059427227530638539, 0.033888304983629297, -0.033888112318112909}, + {0.36070259017561251, 59.25, 0.059227313955579591, 0.034035714318115043, -0.034035521376745509}, + {0.36657770604203405, 59.5, 0.059026921301912665, 0.034181953505944829, -0.034181760213093798}, + {0.37244166651562699, 59.75, 0.058826055041326118, 0.034327019560063426, -0.034326825953930391}, + {0.37829460706048784, 60, 0.058624720637478908, 0.034470909808912244, -0.034470715914122541}, + {0.38413666099919858, 60.25, 0.058422923545825511, 0.034613621747038768, -0.034613427510965307}, + {0.38996795955469105, 60.5, 0.058220669213444889, 0.034755152846398379, -0.034754958347601887}, + {0.39578863189114183, 60.75, 0.058017963078872012, 0.034895500901009936, -0.034895306090334824}, + {0.40159880515392438, 61, 0.057814810571932732, 0.035034663647085483, -0.035034468572044494}, + {0.40739860450864768, 61.25, 0.057611217113580818, 0.035172639233639114, -0.035172443829973074}, + {0.41318815317929586, 61.5, 0.057407188115738826, 0.035309425430385247, -0.035309229780608131}, + {0.41896757248550126, 61.75, 0.057202728981141267, 0.035445020623578898, -0.035444824748032365}, + {0.4247369818789784, 62, 0.056997845103180657, 0.035579423197070532, -0.035579227017417808}, + {0.43049649897912362, 62.25, 0.056792541865756822, 0.035712631390615278, -0.035712434945610702}, + {0.43624623960780934, 62.5, 0.056586824643128482, 0.035844643931277294, -0.035844447194716064}, + {0.44198631782340608, 62.75, 0.05638069879976837, 0.035975459310597813, -0.035975262355839102}, + {0.44771684595403888, 63, 0.056174169690219836, 0.036105076414537272, -0.036104879206232607}, + {0.45343793463010046, 63.25, 0.055967242658957896, 0.036233494150182163, -0.036233296657725636}, + {0.45914969281603818, 63.5, 0.055759923040251229, 0.036360711439355521, -0.036360513752811963}, + {0.46485222784144214, 63.75, 0.055552216158028772, 0.036486727321425674, -0.036486529373692393}, + {0.47054564543144556, 64, 0.05534412732574702, 0.036611541114268356, -0.036611342923055519}, + {0.47623004973644562, 64.25, 0.055135661846261216, 0.036735152030323112, -0.036734953623459493}, + {0.4819055433611949, 64.5, 0.054926825011699147, 0.036857559418026077, -0.03685736079057339}, + {0.48757222739322686, 64.75, 0.054717622103336749, 0.036978762903603558, -0.036978564029839803}, + {0.4932302014306908, 65, 0.054508058391477238, 0.037098761997758536, -0.037098562927692542}, + {0.49887956360956059, 65.25, 0.054298139135331824, 0.03721755632800932, -0.037217357043840125}, + {0.50452041063026387, 65.5, 0.054087869582903847, 0.037335145724792122, -0.037334946237447771}, + {0.51015283778374254, 65.75, 0.0538772549708747, 0.037451530066151088, -0.037451330385163642}, + {0.51577693897693799, 66, 0.053666300524492561, 0.037566709284192409, -0.037566509383009587}, + {0.52139280675774291, 66.25, 0.053455011457463619, 0.037680683391776978, -0.037680483249019764}, + {0.52700053233941468, 66.5, 0.053243392971845327, 0.037793452507090493, -0.037793252195467968}, + {0.53260020562447208, 66.75, 0.053031450257942557, 0.037905016906987068, -0.03790481638031179}, + {0.53819191522808008, 67, 0.052819188494205541, 0.038015376749684152, -0.038015176054841955}, + {0.54377574850094734, 67.25, 0.052606612847131001, 0.038124532488593529, -0.03812433161720339}, + {0.54935179155173253, 67.5, 0.052393728471164303, 0.03823248455221042, -0.038232283515744932}, + {0.55492012926899092, 67.75, 0.05218054050860528, 0.038339233441271339, -0.038339032186039949}, + {0.56048084534264864, 68, 0.051967054089515338, 0.038444779782846397, -0.038444578381247652}, + {0.56603402228504085, 68.25, 0.051753274331626922, 0.038549124213789226, -0.038548922604500782}, + {0.57157974145150492, 68.5, 0.051539206340255987, 0.03865226745198052, -0.038652065692835705}, + {0.57711808306054735, 68.75, 0.051324855208215475, 0.038754210357238056, -0.038754008445347543}, + {0.58264912621359977, 69, 0.05111022601573155, 0.038854953804463598, -0.038854751712235618}, + {0.58817294891435812, 69.25, 0.050895323830362257, 0.03895449876343219, -0.038954296518602244}, + {0.59368962808773595, 69.5, 0.050680153706917641, 0.039052846177973013, -0.039052643788013805}, + {0.59919923959841914, 69.75, 0.050464720687382073, 0.039149997140799524, -0.039149794589072022}, + {0.60470185826905176, 70, 0.050249029800839451, 0.039245952882233877, -0.039245750190035171}, + {0.61019755789804631, 70.25, 0.050033086063398839, 0.039340714548829349, -0.039340511728908768}, + {0.61568641127703039, 70.5, 0.04981689447812368, 0.039434283414173023, -0.039434080402719722}, + {0.62116849020795373, 70.75, 0.049600460034962025, 0.039526660775367178, -0.039526457670924123}, + {0.62664386551983298, 71, 0.049383787710679167, 0.039617848009277479, -0.039617644752685532}, + {0.63211260708517214, 71.25, 0.049166882468791723, 0.039707846661401446, -0.03970764327687603}, + {0.63757478383605881, 71.5, 0.048949749259504308, 0.039796658180465044, -0.039796454652837099}, + {0.64303046377991668, 71.75, 0.048732393019647352, 0.039884284041013449, -0.039884080383352727}, + {0.64847971401497739, 72, 0.048514818672617144, 0.03997072594881735, -0.039970522147417378}, + {0.65392260074541619, 72.25, 0.048297031128317763, 0.04005598552594572, -0.040055781580992662}, + {0.65935918929620674, 72.5, 0.048079035283104789, 0.040140064474668592, -0.040139860431005134}, + {0.6647895441276741, 72.75, 0.047860836019730221, 0.040222964575847703, -0.040222760421579309}, + {0.67021372884976849, 73, 0.047642438207290028, 0.040304687599806967, -0.040304483351764397}, + {0.67563180623605401, 73.25, 0.047423846701172861, 0.040385235461366256, -0.040385031110497439}, + {0.68104383823742953, 73.5, 0.047205066343010596, 0.040464610015507906, -0.040464405540350204}, + {0.68644988599557588, 73.75, 0.046986101960630459, 0.040542813296049085, -0.040542608696409001}, + {0.6918500098561462, 74, 0.04676695836800894, 0.040619847188616123, -0.040619642505573608}, + {0.69724426938170825, 74.25, 0.046547640365227433, 0.040695713852066939, -0.040695509066962635}, + {0.70263272336442206, 74.5, 0.046328152738429038, 0.040770415250978957, -0.040770210330594969}, + {0.70801542983848398, 74.75, 0.046108500259777474, 0.04084395358707156, -0.04084374858441675}, + {0.71339244609233576, 75, 0.04588868768741735, 0.040916330925281887, -0.040916125810149168}, + {0.71876382868062794, 75.25, 0.045668719765435475, 0.040987549579848237, -0.040987344394946966}, + {0.72412963343596481, 75.5, 0.045448601223824804, 0.041057611809206376, -0.041057406497491206}, + {0.72948991548042996, 75.75, 0.045228336778448577, 0.041126519766741858, -0.041126314412083295}, + {0.73484472923687738, 76, 0.045007931131006901, 0.041194275915015038, -0.041194070453693402}, + {0.74019412844002996, 76.25, 0.044787388969004339, 0.041260882508529627, -0.041260676947913771}, + {0.74553816614735302, 76.5, 0.044566714965718832, 0.041326341927476348, -0.041326136292765547}, + {0.75087689474973107, 76.75, 0.044345913780172538, 0.041390656646562572, -0.041390450921398765}, + {0.75621036598194702, 77, 0.044124990057103165, 0.041453829089677235, -0.041453623311958343}, + {0.76153863093296237, 77.25, 0.043903948426937678, 0.041515861812918102, -0.041515655960456449}, + {0.76686174005600072, 77.5, 0.04368279350576651, 0.041576757269653333, -0.041576551328694415}, + {0.77217974317845894, 77.75, 0.043461529895319581, 0.041636517963119235, -0.041636311959098116}, + {0.77749268951162009, 78, 0.043240162182943409, 0.041695146623391785, -0.041694940542711063}, + {0.7828006276602022, 78.25, 0.043018694941579318, 0.041752645719284186, -0.041752439583165324}, + {0.78810360563171034, 78.5, 0.042797132729743684, 0.041809017984963656, -0.041808811795108251}, + {0.7934016708456455, 78.75, 0.042575480091507772, 0.041864266007288563, -0.041864059727138254}, + {0.7986948701425185, 79, 0.042353741556481132, 0.041918392547820392, -0.041918186179727701}, + {0.80398324979271774, 79.25, 0.042131921639793651, 0.041971400281696283, -0.041971193877563227}, + {0.80926685550521049, 79.5, 0.041910024842080798, 0.042023292090680973, -0.04202308560850232}, + {0.81454573243607831, 79.75, 0.041688055649469022, 0.042074070510655134, -0.042073863970478874}, + {0.8198199251969156, 80, 0.041466018533562264, 0.04212373848559553, -0.042123531890621083}, + {0.82508947786306219, 80.25, 0.041243917951429954, 0.042172298748682099, -0.042172092103948559}, + {0.83035443398169129, 80.5, 0.041021758345595898, 0.04221975417840091, -0.042219547499082404}, + {0.83561483657976154, 80.75, 0.040799544144028234, 0.042266107782507105, -0.042265901054688672}, + {0.84087072817181241, 81, 0.040577279760130487, 0.042311362255762749, -0.042311155462972982}, + {0.84612215076763819, 81.25, 0.040354969592733658, 0.042355520560808026, -0.042355313696560509}, + {0.85136914587980472, 81.5, 0.040132618026089251, 0.042398585709261069, -0.042398378814671844}, + {0.85661175453105565, 81.75, 0.03991022942986315, 0.042440560414206401, -0.042440353486627884}, + {0.86185001726156973, 82, 0.039687808159131185, 0.042481447879501907, -0.042481240889364918}, + {0.86708397413610494, 82.25, 0.039465358554374566, 0.042521250983218031, -0.042521043956679029}, + {0.87231366475100169, 82.5, 0.039242884941477249, 0.042559972718411172, -0.042559765671031098}, + {0.87753912824108116, 82.75, 0.039020391631723467, 0.042597616246695752, -0.042597409148358169}, + {0.88276040328640715, 83, 0.03879788292179677, 0.042634184398078367, -0.04263397725614964}, + {0.88797752811894082, 83.25, 0.038575363093779451, 0.042669680318276816, -0.042669473152578873}, + {0.89319054052907898, 83.5, 0.038352836415153399, 0.04270410709506426, -0.04270389984871082}, + {0.8983994778720692, 83.75, 0.038130307138801077, 0.042737467743641538, -0.042737260507376064}, + {0.9036043770743325, 84, 0.037907779503008159, 0.042769765415739826, -0.042769558158242821}, + {0.90880527463965488, 84.25, 0.037685257731466318, 0.042801003284401506, -0.042800795963726344}, + {0.9140022066552973, 84.5, 0.037462746033276997, 0.042831184276559582, -0.042830976944312961}, + {0.91919520879797623, 84.75, 0.037240248602956411, 0.042860311696402323, -0.042860104361281272}, + {0.92438431633976448, 85, 0.037017769620440615, 0.042888388707355007, -0.042888181332607286}, + {0.92956956415387504, 85.25, 0.036795313251091873, 0.042915418400571559, -0.042915210963320176}, + {0.93475098672035561, 85.5, 0.036572883645705531, 0.04294140388108298, -0.042941196429323238}, + {0.93992861813168394, 85.75, 0.036350484940517694, 0.042966348480488269, -0.042966141009868349}, + {0.94510249209827202, 86, 0.036128121257213326, 0.042990255335161265, -0.042990047870466973}, + {0.95027264195386829, 86.25, 0.03590579670293579, 0.043013127682016948, -0.043012920142306524}, + {0.95543910066088267, 86.5, 0.035683515370296202, 0.043034968638624209, -0.043034761094967407}, + {0.96060190081561125, 86.75, 0.035461281337383993, 0.04305578149405636, -0.043055573945121675}, + {0.96576107465338445, 87, 0.035239098667777843, 0.043075569512414431, -0.043075361929228929}, + {0.97091665405361272, 87.25, 0.035016971410557651, 0.043094335943560061, -0.043094128361560249}, + {0.9760686705447672, 87.5, 0.034794903600316576, 0.043112083964074413, -0.043111876355065547}, + {0.98121715530926901, 87.75, 0.034572899257174147, 0.043128816872400735, -0.04312860927212913}, + {0.9863621391882933, 88, 0.034350962386789696, 0.043144537983724925, -0.043144330335372835}, + {0.99150365268650731, 88.25, 0.03412909698037668, 0.043159250518753058, -0.043159042858801608}, + {0.99664172597671685, 88.5, 0.033907307014717135, 0.043172957780828213, -0.043172750111559133}, + {1.0017763889044409, 88.75, 0.03368559645217703, 0.043185662984979716, -0.043185455340888038}, + {1.0069076709924221, 89, 0.033463969240722091, 0.043197369606618533, -0.043197161930268187}, + {1.0120356014450413, 89.25, 0.033242429313934257, 0.043208080809083026, -0.043207873102789555}, + {1.0171602091526903, 89.5, 0.03302098059102844, 0.04321779983318258, -0.043217592148428283}, + {1.0222815226960451, 89.75, 0.032799626976869625, 0.043226530153021789, -0.043226322420956052}, + {1.0273995703502821, 90, 0.032578372361991327, 0.043234274955252235, -0.043234067234389301}, + {1.0325143800892358, 90.25, 0.0323572206226133, 0.043241037692774598, -0.043240829960423055}, + {1.0376259795894711, 90.5, 0.032136175620661012, 0.043246821598747306, -0.043246613855153208}, + {1.0427343962343052, 90.75, 0.031915241203784313, 0.043251629997902032, -0.043251422244437371}, + {1.0478396571177577, 91, 0.031694421205377445, 0.043255466171757156, -0.043255258430949843}, + {1.0529417890484365, 91.25, 0.031473719444599493, 0.043258333607035404, -0.043258125893999744}, + {1.0580408185533716, 91.5, 0.031253139726394576, 0.043260235556664785, -0.043260027804683458}, + {1.0631367718817759, 91.75, 0.031032685841512815, 0.043261175358637154, -0.043260967585654349}, + {1.0682296750087557, 92, 0.030812361566532408, 0.043261156367794783, -0.043260948630491479}, + {1.0733195536389517, 92.25, 0.030592170663880619, 0.043260181925522133, -0.043259974149221267}, + {1.0784064332101393, 92.5, 0.030372116881856426, 0.043258255353734794, -0.043258047603211572}, + {1.0834903388967578, 92.75, 0.030152203954653303, 0.043255380034739131, -0.043255172286525159}, + {1.0885712956133911, 93, 0.029932435602381562, 0.043251559376389401, -0.043251351597336542}, + {1.0936493280181867, 93.25, 0.02971281553109216, 0.043246796610098941, -0.043246588836639586}, + {1.0987244605162367, 93.5, 0.029493347432800278, 0.043241095119121623, -0.043240887374232158}, + {1.103796717262888, 93.75, 0.029274034985509081, 0.043234458356491978, -0.043234250599974475}, + {1.1088661221670142, 94, 0.029054881853234162, 0.043226889573985676, -0.043226681814116791}, + {1.1139326988942329, 94.25, 0.02883589168602807, 0.043218392271199051, -0.043218184485790387}, + {1.1189964708700681, 94.5, 0.028617068120005494, 0.043208969542783041, -0.043208761789576618}, + {1.1240574612830734, 94.75, 0.028398414777368104, 0.043198624972484952, -0.043198417184401212}, + {1.1291156930879016, 95, 0.028179935266430305, 0.043187361735197924, -0.043187154027948947}, + {1.134171189008327, 95.25, 0.027961633181644956, 0.043175183407157737, -0.043174975647910584}, + {1.1392239715402199, 95.5, 0.027743512103629561, 0.04316209315228714, -0.04316188541090725}, + {1.1442740629544883, 95.75, 0.027525575599192324, 0.043148094415013744, -0.043147886709758519}, + {1.1493214852999543, 96, 0.027307827221358889, 0.043133190561832922, -0.043132982841560925}, + {1.1543662604062022, 96.25, 0.027090270509399307, 0.043117384885138416, -0.043117177188340655}, + {1.1594084098863828, 96.5, 0.026872908988854881, 0.043100680720434353, -0.043100473031530161}, + {1.1644479551399638, 96.75, 0.026655746171565201, 0.043083081461965052, -0.043082873789188618}, + {1.1694849173554558, 97, 0.026438785555696315, 0.043064590525627833, -0.043064382832268766}, + {1.174519317513079, 97.25, 0.026222030625767993, 0.043045211094204956, -0.043045003397045756}, + {1.1795511763874069, 97.5, 0.026005484852681657, 0.043024946582063128, -0.043024738891265124}, + {1.1845805145499573, 97.75, 0.025789151693748597, 0.043003800376181726, -0.043003592706292892}, + {1.1896073523717574, 98, 0.025573034592718383, 0.042981775742322294, -0.042981568084069277}, + {1.1946317100258552, 98.25, 0.025357136979807175, 0.042958876080399722, -0.04295866841192289}, + {1.1996536074898041, 98.5, 0.0251414622717267, 0.042935104628538501, -0.042934896999404636}, + {1.2046730645481167, 98.75, 0.024926013871712477, 0.042910464831477826, -0.04291025720100549}, + {1.2096901007946612, 99, 0.024710795169553615, 0.042884959895016457, -0.042884752266811842}, + {1.2147047356350487, 99.25, 0.024495809541621373, 0.042858593209751099, -0.042858385583831739}, + {1.2197169882889605, 99.5, 0.024281060350898727, 0.042831368116919802, -0.042831160514864922}, + {1.2247268777924656, 99.75, 0.024066550947009928, 0.042803287867827895, -0.042803080265441273}, + {1.2297344230002787, 100, 0.023852284666249716, 0.04277435586765891, -0.042774148227157124}, + {1.2347396425880091, 100.25, 0.023638264831613541, 0.042744575299069207, -0.042744367717727307}, + {1.2397425550543624, 100.5, 0.023424494752826965, 0.042713949530627439, -0.042713741952709175}, + {1.2447431787233125, 100.75, 0.023210977726375932, 0.042682481962278911, -0.042682274382234979}, + {1.2497415317462468, 101, 0.022997717035536443, 0.042650175653599104, -0.042649968155669536}, + {1.2547376321040726, 101.25, 0.02278471595040546, 0.042617034128739094, -0.04261682659204323}, + {1.2597314976093061, 101.5, 0.022571977727930299, 0.042583060557048746, -0.042582853038032942}, + {1.2647231459081179, 101.75, 0.02235950561193966, 0.042548258265315012, -0.042548050766089814}, + {1.2697125944823482, 102, 0.022147302833173774, 0.042512630485861874, -0.042512422966036877}, + {1.2746998606515123, 102.25, 0.021935372609315155, 0.042476180585328274, -0.042475973116976595}, + {1.2796849615747572, 102.5, 0.021723718145019048, 0.042438911687778823, -0.042438704205566564}, + {1.2846679142527946, 102.75, 0.021512342631944258, 0.042400827234113245, -0.042400619752246425}, + {1.2896487355298183, 103, 0.021301249248783634, 0.042361930329469652, -0.042361722870714276}, + {1.2946274420953774, 103.25, 0.021090441161295312, 0.042322224327054889, -0.042322016866041895}, + {1.2996040504862343, 103.5, 0.020879921522333506, 0.042281712449368514, -0.042281504978466604}, + {1.3045785770881966, 103.75, 0.020669693471878837, 0.042240397863721205, -0.042240190435905399}, + {1.3095510381379116, 104, 0.020459760137070487, 0.042198283990879673, -0.042198076589396373}, + {1.3145214497246518, 104.25, 0.020250124632236008, 0.042155373925822012, -0.042155166517917148}, + {1.319489827792063, 104.5, 0.020040790058923258, 0.04211167094454004, -0.042111463542860134}, + {1.3244561881398964, 104.75, 0.019831759505931086, 0.042067178239546942, -0.042066970871874672}, + {1.329420546425703, 105, 0.019623036049340563, 0.042021899068217863, -0.042021691709415734}, + {1.334382918166527, 105.25, 0.019414622752546087, 0.041975836632935075, -0.041975629229204649}, + {1.3393433187405532, 105.5, 0.019206522666286779, 0.041928994122452184, -0.041928786782088737}, + {1.3443017633887435, 105.75, 0.018998738828677364, 0.041881374776247052, -0.041881167448371275}, + {1.3492582672164459, 106, 0.018791274265239619, 0.041832981852670412, -0.041832774488239056}, + {1.3542128451949889, 106.25, 0.018584131988933376, 0.041783818288201172, -0.041783610960105862}, + {1.3591655121632433, 106.5, 0.018377315000188056, 0.04173388757558244, -0.041733680270283506}, + {1.3641162828291666, 106.75, 0.018170826286933783, 0.041683192708169794, -0.041682985406529711}, + {1.369065171771334, 107, 0.017964668824632102, 0.041631736958166111, -0.041631529633442722}, + {1.3740121934404328, 107.25, 0.017758845576308101, 0.041579523413498597, -0.041579316108251628}, + {1.3789573621607509, 107.5, 0.01755335949258096, 0.041526555273418975, -0.041526347983715781}, + {1.3839006921316401, 107.75, 0.017348213511695375, 0.041472835690187317, -0.041472628425265855}, + {1.3888421974289529, 108, 0.017143410559552638, 0.041418367847609126, -0.041418160586803053}, + {1.3937818920064722, 108.25, 0.016938953549741953, 0.041363154786816236, -0.041362947561894355}, + {1.3987197896973089, 108.5, 0.016734845383571388, 0.041307199796141225, -0.041306992541980253}, +}}; +} // namespace rmcs_core::controller::chassis::full_vehicle_v202 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/gas_spring_types.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/gas_spring_types.hpp new file mode 100644 index 000000000..9602f24ae --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/gas_spring_types.hpp @@ -0,0 +1,9 @@ +#pragma once +namespace rmcs_core::controller::chassis::gas_spring_model { +struct GeometryKnot { + double delta_rad, beta_deg, compression_m, hip_length_jacobian, knee_length_jacobian; +}; +struct ForceKnot { + double compression_m, force_n; +}; +} // namespace rmcs_core::controller::chassis::gas_spring_model diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/jump_v4.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/jump_v4.hpp new file mode 100644 index 000000000..6d37914e6 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/jump_v4.hpp @@ -0,0 +1,620 @@ +#pragma once + +#include +#include +#include "gas_spring_types.hpp" + +// Generated by tools/sync_wheelbipe_jump_contract.py from the pinned training commit. +namespace rmcs_core::controller::chassis::wheelbipe_jump_v4 { +inline constexpr std::string_view kTrainingCommit = "0f5cbd3805c2dab0c0595fe43ae61ad34d6f5a29"; +inline constexpr std::string_view kModelSpecSha256 = "67f824bc29b05647e381f8584528d686955c2cfd99fd783ba40f382279bf2284"; +inline constexpr std::string_view kGasModelSha256 = "13db9ada6fa0e111a51406fc950ca0b970d1fd3de0d9ceeac07b22053a952aa6"; +inline constexpr std::string_view kObservationSha256 = "f907a791b3d47f0f5e6a17a095dc51841f8085cadf1bbfda9ba273ade70efedd"; +inline constexpr std::string_view kDeploymentSha256 = "8ce02510cc775eb5c8ab1118aa2830d6eb8a5c2cd4c4d30a967b5c329c092102"; +inline constexpr double kLegKp = 80; +inline constexpr double kLegKd = 1.5; +inline constexpr double kOvertravel = 0.20000000000000001; +inline constexpr double kStandingHeight = 0.30499999999999999; +inline constexpr double kCrouchHeight = 0.25; +inline constexpr double kReleaseHeight = 0.39000000000000001; +inline constexpr double kCrouchSeconds = 0.34999999999999998; +inline constexpr double kRequestDuration = 3.7000000000000002; +inline constexpr double kMinimumApex = 0.41999999999999998; +inline constexpr double kMaximumApex = 0.54000000000000004; +inline constexpr double kMovingVxMax = 0.5; +inline constexpr double kMechanicalMin = -0.14084732125908495; +inline constexpr double kMechanicalMax = 1.4282761689185115; +inline constexpr double kMechanicalMargin = 0.029999999999999999; +inline constexpr double kLowCompressionRate = 594.93986075435475; +inline constexpr double kHighCompressionSlope = 14371.522210231198; +inline constexpr double kTuckGain = 0.75; +inline constexpr double kTuckCap = 16; +inline constexpr double kErrorOn = 0.02; +inline constexpr double kErrorFull = 0.10000000000000001; +inline constexpr double kInwardPdFull = 2; +inline constexpr std::array, 2> kBounds{{{-0.14089326779459421,1.4282761689185115},{-0.14084732125908495,1.4283102010168576}}}; +inline constexpr std::array kForce{{ + {0.022997316706837613,86.965688801928167}, + {0.023851879860738903,131.18023500886301}, + {0.024710385829300824,185.68333781640229}, + {0.025572620660114301,309.43303430630766}, + {0.026438366973481182,346.73845194767057}, + {0.027307403932652949,346.73845194774657}, + {0.029054448982586897,350.90237841326041}, + {0.032577919665348412,366.38968062207243}, + {0.037017290950069205,391.09833613120429}, + {0.041465512658364498,415.32100551306803}, + {0.04588815350788536,446.86098014388892}, + {0.050248466389995548,476.85464906482719}, + {0.054507465043132433,508.41922419868212}, + {0.058624096921555034,548.70255419074226}, + {0.060221345430025809,567.00029919244162}, + {0.061786172348818356,589.48924401103636}, +}}; +inline constexpr std::array kLeft{{ + {-0.14089326779459421,40,0.072795528285080402,-0.01974694901368379,0.019747100415708929}, + {-0.13347798989671683,40.25,0.072648306993020745,-0.019960416024254579,0.019960568367004605}, + {-0.12609666995839292,40.5,0.072500185445195597,-0.020173545023556144,0.020173698241898186}, + {-0.11874872965038757,40.75,0.072351168963446191,-0.020386314289125324,0.02038646835743077}, + {-0.11143360339886205,41,0.072201262884494855,-0.020598702338358005,0.020598857326022235}, + {-0.10415073804344993,41.25,0.072050472559523654,-0.020810688186742532,0.020810844013580909}, + {-0.096899592505971133,41.5,0.071898803353756369,-0.021022251084427068,0.02102240778160111}, + {-0.089679637469451678,41.75,0.071746260646042656,-0.021233370716555011,0.021233528258665785}, + {-0.082490355067071863,42,0.071592849828444896,-0.021444027078137473,0.021444185457811876}, + {-0.075331238580673865,42.25,0.071438576305827992,-0.021654200516255884,0.021654359731569539}, + {-0.068201792148537332,42.5,0.071283445495451556,-0.021863871773341481,0.021864031780079812}, + {-0.061101530482088939,42.75,0.071127462826565402,-0.022073021786864488,0.022073182622457943}, + {-0.054029978591209227,43,0.070970633740007144,-0.022281632028772536,0.022281793645372777}, + {-0.046986671517871503,43.25,0.070812963687803626,-0.022489684182666836,0.022489846586270096}, + {-0.039971154077842797,43.5,0.070654458132774225,-0.022697160278930706,0.022697323434194311}, + {-0.032982980610105073,43.75,0.07049512254813807,-0.022904042733160795,0.022904206688953072}, + {-0.026021714733816592,44,0.070334962417123462,-0.023110314233151769,0.02311047895883217}, + {-0.019086929112498929,44.25,0.070173983232580964,-0.023315957844147472,0.023316123313915527}, + {-0.012178205225225336,44.5,0.070012190496599014,-0.023520956966075063,0.023521123181450193}, + {-0.0052951331445780792,44.75,0.06984958972012309,-0.023725295203312402,0.023725462156892918}, + {0.0015626886788435157,45,0.069686186422577734,-0.023928956654486744,0.023929124282281999}, + {0.0083956536256200587,45.25,0.069521986131491589,-0.024131925499820572,0.024132093850413178}, + {0.015204147110577104,45.5,0.069356994382126128,-0.024334186488631004,0.024334355567609794}, + {0.021988546779951545,45.75,0.069191216717106929,-0.024535724512859403,0.024535894300523442}, + {0.02874922270283875,46,0.069024658686058851,-0.024736524826208139,0.024736695296286859}, + {0.035486537557127773,46.25,0.068857325845243914,-0.024936573037941667,0.0249367442055341}, + {0.042200846810079962,46.5,0.068689223757202683,-0.025135854870679125,0.025136026700773584}, + {0.048892498893744474,46.75,0.068520357990399211,-0.025334356549856885,0.025334529059231198}, + {0.055561835375373458,47,0.068350734118868689,-0.025532064510706082,0.025532237684129877}, + {0.062209191122992002,47.25,0.068180357721868937,-0.025728965454425268,0.025729139233194892}, + {0.068834894466298147,47.5,0.06800923438353515,-0.0259250463853197,0.025925220849277869}, + {0.075439267353018868,47.75,0.06783736969253798,-0.026120294598453192,0.026120469682418879}, + {0.082022625500885293,48,0.067664769241744874,-0.026314697735926984,0.026314873420019452}, + {0.088585278545361745,48.25,0.06749143862788505,-0.02650824351759215,0.026508419849011644}, + {0.095127530183259756,48.5,0.06731738345121753,-0.026700920203875683,0.026701097139810337}, + {0.10164967831237998,48.75,0.067142609315203272,-0.026892716081749091,0.026892893609942847}, + {0.10815201516728151,49,0.066967121826179851,-0.02708361985050875,0.027083797975392898}, + {0.11463482745134373,49.25,0.066790926593040312,-0.027273620430141338,0.027273799154122091}, + {0.12109839646518455,49.5,0.066614029226915131,-0.027462707015945748,0.027462886284493365}, + {0.1275429982315906,49.75,0.066436435340857497,-0.0276508690310385,0.027651048875331513}, + {0.13396890361705122,50,0.066258150549532768,-0.02783809614780125,0.027838276546084922}, + {0.14037637844999656,50.25,0.066079180468910328,-0.028024378337957845,0.028024559299152096}, + {0.14676568363585313,50.5,0.065899530715959914,-0.028209705754999453,0.028209887263552558}, + {0.15313707526901271,50.75,0.065719206908350744,-0.028394068860858795,0.028394250913993252}, + {0.15949080474179439,51,0.065538214664154451,-0.028577458369012848,0.028577640955811889}, + {0.16582711885051621,51.25,0.06535655960155147,-0.028759865038811311,0.028760048148751743}, + {0.17214625989874427,51.5,0.065174247338540922,-0.028941280156959431,0.02894146377499332}, + {0.17844846579781376,51.75,0.064991283492653637,-0.029121694986855989,0.029121879130425882}, + {0.184733970164705,52,0.064807673680669156,-0.029301101228217108,0.029301285868888943}, + {0.19100300241734652,52.25,0.064623423518336179,-0.029479490604044339,0.029479675764146923}, + {0.19725578786743575,52.5,0.064438538620095701,-0.029656855268104632,0.029657040893394243}, + {0.20349254781083298,52.75,0.064253024598808897,-0.029833187350067597,0.02983337345611764}, + {0.2097134996156223,53,0.064066887065487244,-0.030008479366861934,0.030008665960432807}, + {0.21591885680789019,53.25,0.063880131629026865,-0.030182724089199808,0.03018291116194699}, + {0.22210882915530167,53.5,0.063692763895946122,-0.030355914272643447,0.030356101771245175}, + {0.22828362274852881,53.75,0.06350478947012661,-0.030528043093083573,0.030528231057155714}, + {0.23444344008060308,54,0.063316213952557446,-0.030699103820108153,0.030699292265500139}, + {0.24058848012425499,54.25,0.063127042941083428,-0.030869090011297784,0.030869278885241574}, + {0.24671893840727593,54.5,0.062937282030155819,-0.031037995273397068,0.031038184569737504}, + {0.25283500708599943,54.75,0.062746936810587503,-0.031205813544466887,0.031206003271135985}, + {0.25893687501691781,55,0.062556012869310951,-0.031372538932458782,0.031372729096874052}, + {0.26502472782651371,55.25,0.062364515789139333,-0.031538165604964086,0.031538356175586924}, + {0.27109874797934258,55.5,0.062172451148531954,-0.031702688067135609,0.031702879073571943}, + {0.27715911484442451,55.75,0.061979824521361755,-0.031866100963510946,0.031866292349452985}, + {0.2832060047599923,56,0.061786641476687311,-0.03202839910818539,0.032028590885637198}, + {0.28923959109663894,56.25,0.061592907578527346,-0.032189577425645766,0.032189769624818509}, + {0.29526004431891312,56.5,0.061398628385639156,-0.032349631199462991,0.032349823782062082}, + {0.30126753204540618,56.75,0.061203809451299933,-0.03250855557972665,0.032508748532003195}, + {0.30726221910737223,57,0.061008456323091251,-0.032666346183436112,0.03266653951231907}, + {0.31324426760591689,57.25,0.060812574542687625,-0.032822998655858526,0.032823192355489195}, + {0.31921383696781375,57.5,0.060616169645646902,-0.032978508718181476,0.032978702790327608}, + {0.32517108399995914,57.75,0.060419247161205739,-0.033132872442640016,0.033133066881888884}, + {0.33111616294252494,58,0.060221812612076181,-0.033286086018322114,0.033286280810966518}, + {0.33704922552083277,58.25,0.06002387151424745,-0.033438145699248356,0.03343834083148528}, + {0.34297042099599667,58.5,0.059825429376789294,-0.033589047846949438,0.033589243312681427}, + {0.34887989621435161,58.75,0.059626491701659595,-0.033738789097860267,0.033738984889593926}, + {0.35477779565571904,59,0.059427063983514432,-0.033887366241432959,0.033887562382614755}, + {0.36066426148052483,59.25,0.0592271517095216,-0.03403477611728023,0.034034972575566202}, + {0.36653943357581237,59.5,0.059026760359176964,-0.034181015747292209,0.034181212542136352}, + {0.37240344960018007,59.75,0.058825895404124327,-0.034326082298148763,0.034326279386808767}, + {0.37825644502765804,60,0.058624562307977773,-0.034469973142643717,0.034470170550444823}, + {0.38409855319057434,60.25,0.058422766526147363,-0.03461268560136338,0.034612883327047483}, + {0.38992990532142097,60.5,0.058220513505668127,-0.034754217290792977,0.034754415321793852}, + {0.39575063059375398,60.75,0.05801780868503105,-0.034894565940225275,0.034894764239482401}, + {0.40156085616214704,61,0.057814657494018276,-0.03503372931001919,0.035033927940266274}, + {0.40736070720124001,61.25,0.057611065353540297,-0.035171705448888313,0.035171904338383853}, + {0.4131503069438725,61.5,0.057407037675476467,-0.035308492379018008,0.035308691557174809}, + {0.41892977671837622,61.75,0.057202579862518456,-0.035444088269723228,0.035444287757987747}, + {0.42469923598499343,62,0.056997697308016092,-0.035578491508343524,0.035578691227958814}, + {0.43045880237149153,62.25,0.056792395395827006,-0.035711700465431095,0.035711900476218861}, + {0.43620859170796128,62.5,0.056586679500167769,-0.035843713674373889,0.035843913939326233}, + {0.44194871806084007,62.75,0.056380554985469206,-0.035974529877171646,0.035974730395279764}, + {0.44767929376617083,63,0.056174027206233115,-0.03610414774041687,0.03610434852883454}, + {0.4534004294621154,63.25,0.055967101506893213,-0.03623256627180349,0.036232767283625456}, + {0.45911223412075247,63.5,0.055759783221677191,-0.036359784294300257,0.036359985575213083}, + {0.46481481507916128,63.75,0.055552077674473045,-0.036485801029562559,0.036486002581040788}, + {0.4705082780698277,64,0.05534399017869672,-0.036610615645340183,0.036610817362391579}, + {0.47619272725037393,64.25,0.055135526037163179,-0.03673422733447701,0.036734429349538522}, + {0.48186826523263576,64.5,0.054926690541960241,-0.036856635668703508,0.036856837894684913}, + {0.4875349931111142,64.75,0.054717488974324033,-0.036977839996291118,0.03697804245405395}, + {0.49319301049079095,65,0.054507926604518198,-0.03709783998197852,0.03709804266030825}, + {0.49884241551435443,65.25,0.054298008691714819,-0.037216635244778196,0.037216838133808938}, + {0.50448330488882931,65.5,0.054087740483878079,-0.037334225590723524,0.037334428672331832}, + {0.51011577391163043,65.75,0.053877127217650617,-0.03745061083963197,0.037450814166237578}, + {0.51573991649606121,66,0.05366617411824226,-0.037565790987116376,0.037565994500125821}, + {0.52135582519626289,66.25,0.053454886399320903,-0.037679766043299701,0.037679969759291554}, + {0.52696359123163061,66.5,0.053243269262905971,-0.037792536160930146,0.037792740086595333}, + {0.53256330451071188,66.75,0.053031327899264465,-0.03790410152377427,0.03790430566444819}, + {0.53815505365459648,67,0.052819067486809618,-0.038014462353729239,0.038014666645756479}, + {0.54373892601981022,67.25,0.052606493192000336,-0.038123619089797056,0.038123823596776407}, + {0.54931500772072983,67.5,0.052393610169245541,-0.038231572188673235,0.038231776857400757}, + {0.55488338365152834,67.75,0.052180423560808042,-0.038338322083169674,0.038338526967978848}, + {0.56044413750765365,68,0.051966938496712668,-0.038443869460336122,0.038444074490149249}, + {0.56599735180686217,68.25,0.051753160094656178,-0.038548214960697047,0.038548420174945142}, + {0.57154310790982343,68.5,0.051539093459918098,-0.038651359300783728,0.03865156469212977}, + {0.57708148604027976,68.75,0.051324743685275835,-0.038753303313278475,0.038753508873542009}, + {0.58261256530481087,69,0.051110115850920082,-0.038854047827516947,0.03885425352703703}, + {0.58813642371217223,69.25,0.050895215024373719,-0.038953593863930047,0.038953799742317441}, + {0.5936531381922483,69.5,0.050680046260411657,-0.039051942369723924,0.039052148393501462}, + {0.59916278461461325,69.75,0.050464614600983865,-0.039149094525704253,0.039149300709067691}, + {0.604665437806714,70,0.050248925075139519,-0.039245051348704707,0.039245257704883296}, + {0.6101611715716837,70.25,0.050032982698953607,-0.0393398141884157,0.039340020678765242}, + {0.61565005870579248,70.5,0.049816792475455521,-0.039433384175209085,0.039433590782709749}, + {0.62113217101555085,70.75,0.049600359394559604,-0.03952576270311331,0.039525969464876687}, + {0.62660757933446276,71,0.049383688432997709,-0.039616951154548996,0.039617158083062654}, + {0.63207635353944003,71.25,0.049166784554253246,-0.039706950965687993,0.039707158028885303}, + {0.63753856256690566,71.5,0.048949652708497771,-0.039795763632842041,0.039795970824713367}, + {0.64299427442854662,71.75,0.048732297832528811,-0.039883390715416744,0.039883598022195044}, + {0.6484435562267814,72,0.048514724849710394,-0.039969833795173493,0.039970041252137822}, + {0.65388647416990864,72.25,0.048296938669914323,-0.040055094626822219,0.040055302188632791}, + {0.65932309358695185,72.5,0.048078944189463915,-0.040139174802027611,0.040139382466283624}, + {0.66475347894221959,72.75,0.047860746291079587,-0.040222076108791416,0.040222283926755475}, + {0.67017769384957893,73,0.047642349843825635,-0.040303800408535762,0.040304008347085751}, + {0.67559580108644557,73.25,0.047423759703059412,-0.040384349527946228,0.040384557567147539}, + {0.68100786260750534,73.5,0.04720498071038158,-0.040463725309720977,0.040463933515441863}, + {0.68641393955816521,73.75,0.046986017693588657,-0.040541929862924375,0.040542138140072737}, + {0.6918140922877416,74,0.046766875466626529,-0.040618965050931266,0.04061917344131355}, + {0.69720838036240385,74.25,0.046547558829546018,-0.040694832939778602,0.040695041420072353}, + {0.70259686257785448,74.5,0.046328072568460166,-0.040769535599506079,0.040769744177042211}, + {0.70797959697177659,74.75,0.046108421455502847,-0.040843075211779323,0.040843283874514524}, + {0.713356640836037,75,0.045888610248788729,-0.040915453933365376,0.040915662714278545}, + {0.71872805072865731,75.25,0.045668643692375367,-0.04098667390549865,0.040986882766290811}, + {0.7240938824855585,75.5,0.045448526516226148,-0.041056737376074348,0.041056946351043899}, + {0.72945419123208111,75.75,0.045228263436175684,-0.041125646760486578,0.041125855829079794}, + {0.73480903139429299,76,0.045007859153894936,-0.041193404115758336,0.041193613265126106}, + {0.74015845671006519,76.25,0.044787318356860006,-0.041260012084225584,0.04126022132843471}, + {0.74550252023997077,76.5,0.044566645718320586,-0.041325472842150157,0.041325682145079978}, + {0.75084127437794579,76.75,0.04434584589727035,-0.041389788876953271,0.041389998276851803}, + {0.7561747708617772,77,0.044124923538419417,-0.041452962664242532,0.0414531721519108}, + {0.7615030607833746,77.25,0.043903883272166835,-0.041514996762563039,0.041515206324923282}, + {0.76682619459887458,77.5,0.043682729714575583,-0.041575893563667088,0.041576103198573522}, + {0.77214422213852774,77.75,0.043461467467348393,-0.041635655659335025,0.041635865361911828}, + {0.77745719261643154,78,0.043240101117804519,-0.04169428567302691,0.041694495441968293}, + {0.78276515464006513,78.25,0.043018635238858682,-0.041751786153220727,0.041751996020681692}, + {0.78806815621966075,78.5,0.042797074389000309,-0.041808159811494605,0.041808369743455309}, + {0.79336624477739082,78.75,0.04257542311227451,-0.041863409187297251,0.041863619160778465}, + {0.79865946715640224,79,0.042353685938264357,-0.041917537171844876,0.041917747216542678}, + {0.80394786962967468,79.25,0.042131867382073862,-0.041970546274862131,0.041970756391756348}, + {0.80923149790872007,79.5,0.041909971944312674,-0.042022439379057586,0.042022649520310651}, + {0.81451039715212836,79.75,0.041688004111081631,-0.042073219243027815,0.042073429491929548}, + {0.8197846119739618,80,0.041465968353959201,-0.042122888627521338,0.042123098884367161}, + {0.82505418645198403,80.25,0.041243869129989619,-0.042171450387905318,0.042171660729135085}, + {0.8303191641357559,80.5,0.041021710881671669,-0.042218907241819092,0.042219117667153527}, + {0.83557958805458443,80.75,0.040799498036948739,-0.042265262163675821,0.042265472653354397}, + {0.84083550072531743,81,0.040577235009199839,-0.042310518129338393,0.042310728605943593}, + {0.84608694416002073,81.25,0.040354926197231356,-0.042354677830226783,0.042354888363629495}, + {0.8513339598734968,81.5,0.040132575985270647,-0.042397744332585086,0.042397954982744179}, + {0.85657658889068711,81.75,0.0399101887429597,-0.042439720569605377,0.042439931220499605}, + {0.86181487175393379,82,0.03968776882535039,-0.042480609476143572,0.042480820166178601}, + {0.86704884853012043,82.25,0.039465320572900323,-0.042520414040601306,0.042520624753858594}, + {0.87227855881768335,82.5,0.039242848311469944,-0.042559137280574311,0.042559348064557523}, + {0.87750404175349761,82.75,0.039020356352320325,-0.042596782189850413,0.042596993006779771}, + {0.88272533601965697,83,0.038797848992111854,-0.042633351771030059,0.042633562637742539}, + {0.88794247985011099,83.25,0.038575330512904141,-0.042668849155491129,0.042669060067023146}, + {0.89315551103721424,83.5,0.038352805182156106,-0.042703277408084606,0.042703488340093076}, + {0.89836446693814387,83.75,0.038130277252727898,-0.042736639574177115,0.042736850534225379}, + {0.90356938448121182,84,0.037907750962882902,-0.042768938701613968,0.042769149688378083}, + {0.90877030017207261,84.25,0.037685230536290359,-0.04280017800384095,0.042800389028249952}, + {0.91396725009981528,84.5,0.037462720182030021,-0.042830360547373214,0.042830571604946355}, + {0.91916026994296196,84.75,0.037240224094596271,-0.042859489459649514,0.042859700528750655}, + {0.92434939497535606,85,0.037017746453903405,-0.042887567876241595,0.042887778965944146}, + {0.92953466007195495,85.25,0.036795291425292434,-0.042914599026733835,0.042914810178968921}, + {0.93471609971451874,85.5,0.036572863159537373,-0.04294058603031764,0.042940797222241867}, + {0.93989374799721093,85.75,0.036350465792853157,-0.042965532173728394,0.042965743352460339}, + {0.94506763863209886,86,0.036128103446903953,-0.04298944046417108,0.042989651680960472}, + {0.95023780495456012,86.25,0.035905780228812391,-0.04301231428308927,0.043012525490154441}, + {0.95540427992860932,86.5,0.035683500231168966,-0.043034156782982447,0.043034368036973469}, + {0.96056709615211333,86.75,0.035461267532042672,-0.043054971135463527,0.043055182394935387}, + {0.96572628586195064,87,0.035239086194992116,-0.043074760654249811,0.043074971956068964}, + {0.9708818809390527,87.25,0.035016960269076899,-0.04309352858196408,0.043093739886712779}, + {0.9760339129133857,87.5,0.034794893788870462,-0.043111278129532971,0.043111489467083375}, + {0.98118241296883912,87.75,0.034572890774472433,-0.043128012540550376,0.043128223893534379}, + {0.98632741194803453,88,0.034350955231522776,-0.043143735236785774,0.043143946560314395}, + {0.99146894035705668,88.25,0.034129091151215284,-0.043158449214773242,0.043158660597409471}, + {0.99660702837010184,88.5,0.033907302510312889,-0.043172158020882923,0.043172369373912056}, + {1.0017417058340636,88.75,0.033685593271162467,-0.043184864791971644,0.04318507618987677}, + {1.0068730022730248,89,0.033463967381710799,-0.043196572864627192,0.043196784263133892}, + {1.0120009468926929,89.25,0.033242428775521048,-0.043207285524183958,0.043207496939823453}, + {1.0171255685847567,89.5,0.03302098137178943,-0.043217006150190695,0.043217217590484469}, + {1.0222468959311624,89.75,0.032799629075362532,-0.043225737989627588,0.043225949442992351}, + {1.0273649572083423,90,0.032578375776755431,-0.043233484376423492,0.043233695826878082}, + {1.0324797803913588,90.25,0.032357225352169901,-0.043240248594235031,0.0432404600598289}, + {1.0375913931579839,90.5,0.032136181663513147,-0.0432460340123349,0.043246245447581155}, + {1.0426998228927191,90.75,0.031915248558417375,-0.043250843921073145,0.043251055376068992}, + {1.0478050966907428,91,0.031694429870259007,-0.043254681718313093,0.043254893197704702}, + {1.0529072413618104,91.25,0.031473729418179552,-0.043257550639332287,0.043257762116223897}, + {1.0580062834340653,91.5,0.031253151007105621,-0.043259454168762479,0.043259665621869947}, + {1.0631022491578233,91.75,0.031032698427770261,-0.043260395474957858,0.043260606920779543}, + {1.0681951645092613,92,0.030812375456734271,-0.043260378017931034,0.043260589493207935}, + {1.0732850551940822,92.25,0.030592185856408063,-0.043259405101162052,0.043259616571594606}, + {1.0783719466510946,92.5,0.030372133375073766,-0.043257480071037191,0.043257691537095577}, + {1.0834558640557546,92.75,0.030152221746908028,-0.043254606360482784,0.043254817836192339}, + {1.0885368323236375,93,0.029932454692004687,-0.043250787157327053,0.043250998610924463}, + {1.0936148761138722,93.25,0.029712835916398325,-0.043246025980295477,0.043246237452462477}, + {1.0986900198325009,93.5,0.02949336911208756,-0.043240326104370064,0.043240537627406261}, + {1.1037622876358104,93.75,0.029274057957059514,-0.043233690874361019,0.043233902329489593}, + {1.1088317034335904,94,0.029054906115313743,-0.04322612367251899,0.043226335125687743}, + {1.1138982908923538,94.25,0.028835917236886853,-0.043217627825117622,0.043217839302215955}, + {1.1189620734385055,94.5,0.028617094957877891,-0.043208206685437234,0.043208418168515839}, + {1.1240230742614576,94.75,0.028398442900472692,-0.043197863730689179,0.04319807517514257}, + {1.1290813163167048,95,0.028179964672970216,-0.043186602045295008,0.043186813519466405}, + {1.1341368223288433,95.25,0.027961663869807987,-0.043174425278975581,0.043174636729259808}, + {1.1391896147945486,95.5,0.027743544071588272,-0.043161336556975183,0.043161548024339413}, + {1.1442397159855124,95.75,0.027525608845103966,-0.043147339436513721,0.043147550853110728}, + {1.1492871479513269,96,0.027307861743365913,-0.043132437138590995,0.043132648579094324}, + {1.1543319325223269,96.25,0.027090306305629183,-0.043116633040206132,0.043116844483539418}, + {1.1593740913123962,96.5,0.026872946057420154,-0.04309993041315846,0.043100141854418517}, + {1.1644136457217125,96.75,0.026655784510564016,-0.043082332754987462,0.043082544164718295}, + {1.1694506169394883,97,0.026438825163212297,-0.043063843359209542,0.043064054777552763}, + {1.1744850259466215,97.25,0.02622207149987011,-0.043044465490907957,0.043044676879385735}, + {1.1795168935183484,97.5,0.026005526991424784,-0.043024202553380742,0.043024413955640051}, + {1.1845462402268365,97.75,0.025789195095173502,-0.043003057907761211,0.043003269319421}, + {1.1895730864437331,98,0.025573079254851611,-0.042981034909690685,0.042981246316104338}, + {1.1945974523427025,98.25,0.025357182900661523,-0.042958136772919264,0.042958348173459282}, + {1.1996193579018946,98.5,0.025141509449301033,-0.042934366936995871,0.042934578267755319}, + {1.2046388229063949,98.75,0.024926062303991962,-0.042909728685198045,0.042909940053576345}, + {1.2096558669506376,99,0.024710844854509827,-0.042884225327591952,0.042884436688649774}, + {1.2146705094407741,99.25,0.024495860477212389,-0.042857860295507533,0.042858071627052297}, + {1.2196827695970187,99.5,0.024281112535069337,-0.042830636748793061,0.042830848071882921}, + {1.2246926664559461,99.75,0.024066604377691619,-0.042802558065733354,0.042802769354879089}, + {1.2297002188727715,100,0.023852339341360815,-0.042773627650194324,0.042773838950781906}, + {1.2347054455235817,100.25,0.023638320749059319,-0.042743848660052486,0.042744059937582957}, + {1.2397083649075464,100.5,0.023424551910499768,-0.042713224484523304,0.04271343578989778}, + {1.244708995349086,100.75,0.023211036122155172,-0.042681758425717864,0.04268196971564131}, + {1.2497073550000182,101,0.022997776667289062,-0.042649453834326223,0.042649665084285748}, + {1.2547034618416664,101.25,0.022784776815985459,-0.042616313838960362,0.042616525090949597}, + {1.2596973336869455,101.5,0.022572039825179427,-0.0425823418711333,0.042582553107880894}, + {1.2646889881824066,101.75,0.022359568938687166,-0.042547541107566644,0.042547752358060242}, + {1.2696784428102619,102,0.022147367387236473,-0.042511914989148303,0.04251212621340611}, + {1.2746657148903757,102.25,0.021935438388497819,-0.042475466629107339,0.042475677821802948}, + {1.2796508215822302,102.5,0.021723785147114233,-0.042438199404242594,0.042438410566841722}, + {1.2846337798868588,102.75,0.021512410854732524,-0.042400116479959449,0.042400327637873048}, + {1.2896146066487582,103,0.021301318690033606,-0.042361221142273531,0.042361432294732937}, + {1.2945933185577663,103.25,0.02109051181876382,-0.042321516757353239,0.042321727928555708}, + {1.299569932150922,103.5,0.020879993393765506,-0.042281006492249529,0.042281217645791902}, + {1.3045444638142849,103.75,0.020669766555008066,-0.042239693560307878,0.042239904709162279}, + {1.3095169297847469,104,0.020459834429618587,-0.04219758126607781,0.042197792398305067}, + {1.3144873461518063,104.25,0.020250200131913566,-0.042154672736922416,0.042154883849982505}, + {1.3194557288593196,104.5,0.020040866763429373,-0.042110971382184281,0.042111182479337428}, + {1.3244220937072295,104.75,0.019831837412953586,-0.042066480248634908,0.042066691355668095}, + {1.3293864563532725,105,0.019623115156556049,-0.042021202685308401,0.042021413773353611}, + {1.33434883231465,105.25,0.01941470305762031,-0.041975141856123999,0.041975352950746614}, + {1.3393092369696991,105.5,0.019206604166874204,-0.041928300967060628,0.041928512036170817}, + {1.344267685559511,105.75,0.018998821522421744,-0.041880683326974033,0.041880894338668262}, + {1.3492241931895534,106,0.01879135814977375,-0.041832291800490251,0.041832502870719712}, + {1.354178774831253,106.25,0.018584217061879543,-0.041783130044627859,0.041783341053103995}, + {1.3591314453235652,106.5,0.01837740125915778,-0.041733200798910353,0.041733411824529665}, + {1.3640822193745206,106.75,0.018170913729527838,-0.041682507574776373,0.041682718608742897}, + {1.3690311115627414,107,0.017964757448441115,-0.041631053418500819,0.041631264402514356}, + {1.3739781363389587,107.25,0.017758935378912079,-0.041578841474567418,0.04157905249416427}, + {1.3789233080274805,107.5,0.017553450471549666,-0.041525874955566144,0.041526085960111203}, + {1.3838666408276621,107.75,0.017348305664588372,-0.041472156998661081,0.0414723679750999}, + {1.3888081488153481,108,0.017143503883919289,-0.041417690746155089,0.041417901710570915}, + {1.3937478459442914,108.25,0.016939048043121581,-0.041362479329356272,0.041362690271261826}, + {1.3986857460475637,108.5,0.016734941043493295,-0.041306525894044155,0.041306736881463191}, + {1.4036218628389299,108.75,0.016531185774082829,-0.041249833626507637,0.04125004457681819}, + {1.4085562099142224,109,0.016327785111719562,-0.041192405662172027,0.041192616603312859}, + {1.4134888007526869,109.25,0.016124741921045238,-0.041134244942813314,0.041134455875962372}, + {1.4184196487183076,109.5,0.015922059054544982,-0.041075354759761096,0.041075565678418746}, + {1.4233487670611205,109.75,0.015719739352577929,-0.04101573810433351,0.041015949037463084}, + {1.4282761689185115,110,0.015517785643408694,-0.040955398215443417,0.040955609144615934}, +}}; +inline constexpr std::array kRight{{ + {-0.14084732125908495,40,0.072795780352963274,0.019747821526528039,-0.019747672825233435}, + {-0.1334322078965231,40.25,0.072648558079311323,0.019961291341424978,-0.019961141764082358}, + {-0.12605104983226534,40.5,0.072500435543639316,0.020174423101001,-0.020174272647664755}, + {-0.11870326879280674,40.75,0.072351418067845411,0.020387195029482878,-0.020387043712307099}, + {-0.11138829925859983,41,0.072201510988708556,0.02059958570048805,-0.020599433520667971}, + {-0.10410558812217852,41.25,0.072050719657467788,0.020811574093828909,-0.020811421053081463}, + {-0.096854594356919732,41.5,0.071899049439403548,0.021023139464641561,-0.021022985581177905}, + {-0.089634788696091761,41.75,0.071746505713422393,0.021234261439258658,-0.021234106750884656}, + {-0.082445653321844836,42,0.071593093871643379,0.021444920189389729,-0.021444764653169048}, + {-0.075286681563754732,42.25,0.071438819318988198,0.02165509589532965,-0.021654939553085215}, + {-0.068157377606650027,42.5,0.071283687472773333,0.02186476928898267,-0.021864612165416842}, + {-0.061057256207330657,42.75,0.071127703762305136,0.02207392148642982,-0.022073763562246079}, + {-0.053985842419919328,43,0.070970873628478037,0.022282533811498924,-0.022282375097956355}, + {-0.046942671329554764,43.25,0.070813202523375404,0.022490587985040961,-0.022490428474080466}, + {-0.03992728779408089,43.5,0.070654695909873319,0.022698066046157928,-0.022697905782933684}, + {-0.032939246193542659,43.75,0.070495359261247267,0.022904950354729937,-0.022904789347529445}, + {-0.025978110187147407,44,0.070335198060782145,0.023111223719082575,-0.023111061966510887}, + {-0.019043452477490119,44.25,0.070174217801384928,0.023316869132509997,-0.02331670664416173}, + {-0.012134854581772492,44.5,0.07001242398520037,0.023521869944591874,-0.02352170673525162}, + {-0.0052519066097874711,44.75,0.069849822123230168,0.023726209896160484,-0.023726045944262898}, + {0.0016057929515613245,45,0.069686417734955028,0.023929872876919289,-0.023929708215282872}, + {0.0084386374474052506,45.25,0.069522216347959701,0.024132843292230904,-0.024132677922804498}, + {0.015247012257973647,45.5,0.069357223497561585,0.024335105815760386,-0.024334939760170368}, + {0.022031294995728534,45.75,0.069191444726442397,0.024536645298976881,-0.024536478536051894}, + {0.028791855696793378,46,0.069024885584282436,0.024737447004385571,-0.024737279591539856}, + {0.035529057006852161,46.25,0.068857551627399691,0.024937496503809237,-0.02493732840231493}, + {0.042243254361723603,46.5,0.068689448418390128,0.025136779639218485,-0.025136610862264479}, + {0.048934796162745375,46.75,0.068520581525773339,0.02533528257283749,-0.025335113193571013}, + {0.055604023947176229,47,0.068350956523639789,0.025532991715382806,-0.025532821647623983}, + {0.062251272553739356,47.25,0.068180578991302559,0.025729893772699252,-0.025729723072019858}, + {0.068876870283509639,47.5,0.068009454512951956,0.025925975816888609,-0.025925804503800703}, + {0.075481139056245872,47.75,0.067837588677313543,0.026121225118534645,-0.0261210531913415}, + {0.082064394562356202,48,0.067664987077309446,0.026315629178499795,-0.026315456626048221}, + {0.088626946410598262,48.25,0.067491655309723647,0.026509175945516484,-0.026509002790281705}, + {0.095169098271689456,48.5,0.067317598974869747,0.026701853519170142,-0.026701679786675814}, + {0.10169114801791729,48.75,0.067142823676262897,0.026893650254420021,-0.026893475901647351}, + {0.10819338785891913,49,0.066967335020295018,0.027084554871353365,-0.027084379919717943}, + {0.1146761044736892,49.25,0.066791138615913057,0.027274556210883785,-0.027274380699864213}, + {0.12113957913902512,49.5,0.066614240074301431,0.027463643555285879,-0.027463467481546381}, + {0.12758408785440972,49.75,0.066436645008567108,0.027651806245590552,-0.027651629600661497}, + {0.13400990146355091,50,0.066258359033428915,0.027839034005732913,-0.027838856803525595}, + {0.14041728577260354,50.25,0.066079387764909625,0.028025316795958193,-0.028025139079591921}, + {0.14680650166520243,50.5,0.065899736820032195,0.028210644816768632,-0.028210466566918044}, + {0.15317780521442564,50.75,0.065719411816518664,0.028395008434043894,-0.028394829668341517}, + {0.15953144779174827,51,0.065538418372493815,0.028578398419636511,-0.028578219100784889}, + {0.16586767617309089,51.25,0.06535676210619043,0.028760805581125225,-0.028760625760593964}, + {0.17218673264207146,51.5,0.065174448635660145,0.028942221073663347,-0.028942040719559262}, + {0.17848885509050111,51.75,0.064991483578486053,0.029122636358224877,-0.029122455503968362}, + {0.18477427711625533,52,0.064807872551499862,0.029302042902552113,-0.029301861521223792}, + {0.19104322811857094,52.25,0.064623621170501952,0.029480432579333436,-0.029480250768846628}, + {0.19729593339084844,52.5,0.064438735049984971,0.029657797443868351,-0.02965761512704005}, + {0.20353261421104191,52.75,0.064253219802861722,0.029834129859810959,-0.02983394704151731}, + {0.20975348792970755,53,0.06406708104019472,0.030009422043668642,-0.030009238804835494}, + {0.21595876805577202,53.25,0.063880324370931219,0.030183666900604516,-0.030183483177077888}, + {0.222148664340104,53.5,0.063692955401640383,0.030356857227101845,-0.030356673082267176}, + {0.22832338285692483,53.75,0.063504979736254394,0.030528986176004649,-0.03052880154498298}, + {0.23448312608316474,54,0.063316402975812841,0.030700046989141465,-0.030699861926795746}, + {0.24062809297578053,54.25,0.063127230718210414,0.030870033193912121,-0.03086984767774046}, + {0.24675847904712161,54.5,0.062937468557948628,0.031038938493286244,-0.031038752564294371}, + {0.25287447643839472,54.75,0.062747122085889923,0.031206756764450203,-0.031206570400122237}, + {0.25897627399128015,55,0.062556196889016313,0.031373482070357644,-0.031373295296715897}, + {0.26506405731774491,55.25,0.062364698550190265,0.03153910871891203,-0.031538921521951903}, + {0.27113800886812917,55.5,0.062172632647919966,0.031703631043897527,-0.031703443474098682}, + {0.27719830799752626,55.75,0.061980004756127263,0.031867043817739055,-0.031866855857976273}, + {0.28324513103052046,56,0.061786820443919338,0.032029341772312564,-0.032029153408337445}, + {0.28927865132433578,56.25,0.061593085275363162,0.032190519948776336,-0.03219033116040388}, + {0.29529903933041984,56.5,0.06139880480926424,0.032350573452546494,-0.032350384328299564}, + {0.30130646265452249,56.75,0.061203984598947478,0.03250949758797847,-0.032509308094185355}, + {0.30730108611531221,57,0.061008630192042231,0.032667287941386895,-0.032667098065183524}, + {0.31328307180156484,57.25,0.060812747130270163,0.032823940057333759,-0.032823749829356118}, + {0.31925257912796329,57.5,0.060616340949236583,0.032979449844820707,-0.032979259254387057}, + {0.3252097648895505,57.75,0.060419417178224674,0.03313381327728105,-0.033133622332675035}, + {0.33115478331488329,58,0.060221981339993416,0.033287026486672422,-0.033286835171672073}, + {0.33708778611789364,58.25,0.060024038950578278,0.033439085734535065,-0.033438894076953177}, + {0.3430089225485255,58.5,0.05982559551909529,0.033589987497809642,-0.033589795488020345}, + {0.34891833944216416,58.75,0.059626656547548104,0.033739728292080218,-0.033739535969493833}, + {0.3548161812678961,59,0.059427227530638546,0.033888305001523761,-0.033888112349466704}, + {0.36070259017561263,59.25,0.059227313955579633,0.034035714346854498,-0.034035521392040829}, + {0.36657770604203416,59.5,0.059026921301912672,0.034181953498094275,-0.034181760200202561}, + {0.37244166651562621,59.75,0.058826055041326125,0.034327019553398751,-0.034326825948608079}, + {0.37829460706048706,60,0.05862472063747888,0.034470909806485726,-0.034470715901596366}, + {0.38413666099919869,60.25,0.058422923545825539,0.034613621737503944,-0.034613427526393993}, + {0.38996795955469093,60.5,0.058220669213444862,0.034755152839746443,-0.034754958341392875}, + {0.39578863189114205,60.75,0.058017963078872012,0.034895500897814159,-0.034895306075582527}, + {0.40159880515392438,61,0.057814810571932732,0.035034663655623015,-0.03503446857652158}, + {0.40739860450864823,61.25,0.057611217113580818,0.035172639224709736,-0.035172443811940887}, + {0.41318815317929442,61.5,0.057407188115738826,0.035309425427482381,-0.035309229770945749}, + {0.41896757248550115,61.75,0.057202728981141267,0.035445020636373711,-0.035444824749847635}, + {0.42473698187897929,62,0.056997845103180671,0.03557942315170156,-0.035579226969738954}, + {0.43049649897912473,62.25,0.056792541865756822,0.035712631406771202,-0.035712434938277235}, + {0.43624623960780967,62.5,0.056586824643128482,0.035844643937837629,-0.035844447201115966}, + {0.44198631782340564,62.75,0.056380698799768376,0.035975459322598685,-0.035975262374216499}, + {0.44771684595403843,63,0.05617416969021985,0.036105076424152491,-0.036104879216324839}, + {0.45343793463009979,63.25,0.055967242658957903,0.036233494170023528,-0.036233296717728639}, + {0.45914969281603862,63.5,0.05575992304025125,0.036360711429367545,-0.036360513751650371}, + {0.46485222784144192,63.75,0.0555522161580288,0.036486727319014811,-0.036486529377775495}, + {0.47054564543144523,64,0.055344127325747033,0.036611541106937609,-0.036611342925128555}, + {0.47623004973644539,64.25,0.05513566184626123,0.036735152029884865,-0.036734953609571921}, + {0.48190554336119512,64.5,0.054926825011699161,0.036857559408983623,-0.036857360773226301}, + {0.48757222739322709,64.75,0.054717622103336784,0.036978762890802909,-0.036978564025823335}, + {0.4932302014306908,65,0.054508058391477224,0.037098761957401555,-0.037098562871980822}, + {0.49887956360956048,65.25,0.054298139135331831,0.037217556332661744,-0.037217357038951002}, + {0.50452041063026365,65.5,0.054087869582903875,0.037335145720915397,-0.037334946248389379}, + {0.51015283778374276,65.75,0.053877254970874713,0.037451530065659461,-0.037451330349838802}, + {0.51577693897693866,66,0.053666300524492568,0.037566709298959548,-0.037566509404362507}, + {0.52139280675774269,66.25,0.053455011457463592,0.037680683384273446,-0.037680483254981897}, + {0.52700053233941491,66.5,0.053243392971845327,0.037793452511768494,-0.037793252211250954}, + {0.53260020562447186,66.75,0.053031450257942529,0.037905016903446088,-0.037904816376417114}, + {0.5381919152280803,67,0.052819188494205534,0.038015376719868453,-0.038015176018747376}, + {0.54377574850094734,67.25,0.052606612847131015,0.038124532485241835,-0.038124331615435235}, + {0.54935179155173253,67.5,0.052393728471164316,0.038232484537317021,-0.038232283502435953}, + {0.5549201292689907,67.75,0.052180540508605266,0.038339233455588526,-0.038339032182150304}, + {0.56048084534264819,68,0.051967054089515317,0.038444779750234324,-0.038444578350446922}, + {0.56603402228504063,68.25,0.051753274331626881,0.038549124218678399,-0.038548922609050108}, + {0.57157974145150447,68.5,0.05153920634025598,0.038652267458857359,-0.03865206569859507}, + {0.57711808306054668,68.75,0.051324855208215454,0.038754210364377879,-0.038754008485559495}, + {0.58264912621359999,69,0.05111022601573155,0.038854953792575891,-0.038854751683293935}, + {0.58817294891435856,69.25,0.050895323830362257,0.038954498788173725,-0.038954296532288518}, + {0.59368962808773551,69.5,0.050680153706917641,0.039052846150078709,-0.039052643768189101}, + {0.59919923959841981,69.75,0.050464720687382073,0.03914999714901244,-0.039149794589634995}, + {0.60470185826905198,70,0.050249029800839451,0.039245952874433394,-0.039245750185869149}, + {0.61019755789804697,70.25,0.050033086063398839,0.039340714566176195,-0.039340511750687542}, + {0.61568641127703061,70.5,0.04981689447812368,0.039434283408787553,-0.039434080421840198}, + {0.62116849020795306,70.75,0.049600460034962025,0.039526660773063084,-0.039526457683217955}, + {0.62664386551983298,71,0.049383787710679167,0.039617848027896203,-0.039617644799468034}, + {0.63211260708517281,71.25,0.049166882468791723,0.039707846665401003,-0.039707643273639313}, + {0.63757478383605792,71.5,0.048949749259504308,0.039796658187555713,-0.039796454650006641}, + {0.64303046377991757,71.75,0.048732393019647352,0.039884284024341014,-0.039884080359064392}, + {0.64847971401497739,72,0.048514818672617123,0.03997072595180326,-0.039970522149389225}, + {0.65392260074541531,72.25,0.048297031128317819,0.040055985512143795,-0.040055781583982687}, + {0.65935918929620674,72.5,0.048079035283104761,0.040140064476037143,-0.040139860458741961}, + {0.66478954412767433,72.75,0.047860836019730242,0.040222964572543561,-0.040222760424430549}, + {0.67021372884976871,73,0.047642438207290028,0.040304687557542754,-0.040304483304886514}, + {0.67563180623605446,73.25,0.047423846701172868,0.040385235481440136,-0.040385031115535992}, + {0.68104383823743042,73.5,0.047205066343010568,0.040464610044892997,-0.040464405577391629}, + {0.68644988599557588,73.75,0.046986101960630453,0.040542813298737775,-0.040542608701723916}, + {0.69185000985614642,74,0.04676695836800894,0.040619847200546545,-0.040619642523450357}, + {0.69724426938170869,74.25,0.046547640365227447,0.040695713850829575,-0.040695509060131217}, + {0.70263272336442206,74.5,0.046328152738429017,0.040770415245073903,-0.040770210337388937}, + {0.7080154298384842,74.75,0.046108500259777516,0.040843953577098947,-0.040843748584525011}, + {0.71339244609233687,75,0.045888687687417357,0.04091633093459416,-0.040916125839259354}, + {0.71876382868062816,75.25,0.045668719765435468,0.040987549579011608,-0.040987344422615993}, + {0.72412963343596504,75.5,0.045448601223824811,0.041057611830544537,-0.041057406525668617}, + {0.72948991548043018,75.75,0.045228336778448577,0.041126519762042804,-0.041126314407408909}, + {0.73484472923687782,76,0.045007931131006908,0.041194275904435251,-0.041194070440522583}, + {0.74019412844003063,76.25,0.044787388969004319,0.041260882505884208,-0.04126067696829832}, + {0.7455381661473528,76.5,0.044566714965718804,0.041326341961329303,-0.041326136351384053}, + {0.75087689474973107,76.75,0.044345913780172511,0.041390656693243884,-0.041390450963403207}, + {0.75621036598194813,77,0.044124990057103207,0.041453829104364237,-0.041453623330707401}, + {0.76153863093296215,77.25,0.043903948426937678,0.041515861787150429,-0.041515655922623886}, + {0.7668617400560005,77.5,0.043682793505766454,0.041576757289474284,-0.041576551351907166}, + {0.7721797431784585,77.75,0.043461529895319567,0.041636517986826035,-0.041636311985591506}, + {0.77749268951162032,78,0.043240162182943409,0.041695146606656262,-0.04169494056353569}, + {0.78280062766020153,78.25,0.04301869494157938,0.041752645735372497,-0.041752439576426589}, + {0.78810360563171034,78.5,0.042797132729743656,0.041809018004141919,-0.041808811800025041}, + {0.79340167084564528,78.75,0.042575480091507814,0.041864265998183915,-0.041864059737722101}, + {0.79869487014251828,79,0.042353741556481167,0.04191839252939375,-0.041918186161705284}, + {0.80398324979271907,79.25,0.042131921639793665,0.041971400318436866,-0.041971193921633537}, + {0.80926685550521005,79.5,0.041910024842080826,0.042023292047682244,-0.042023085552235773}, + {0.81454573243607875,79.75,0.041688055649469008,0.0420740704915663,-0.042073863962885281}, + {0.81981992519691627,80,0.041466018533562306,0.042123738455011696,-0.042123531861942079}, + {0.8250894778630613,80.25,0.041243917951429954,0.042172298752891454,-0.042172092093260977}, + {0.83035443398169062,80.5,0.041021758345595911,0.042219754189112696,-0.04221954750399904}, + {0.83561483657976132,80.75,0.040799544144028248,0.042266107756225982,-0.042265901024732273}, + {0.8408707281718133,81,0.040577279760130494,0.042311362246308194,-0.042311155430870204}, + {0.84612215076763864,81.25,0.040354969592733644,0.042355520576474938,-0.04235531371488116}, + {0.85136914587980472,81.5,0.040132618026089209,0.042398585646475605,-0.042398378751532545}, + {0.8566117545310552,81.75,0.039910229429863123,0.042440560391601941,-0.042440353462908642}, + {0.8618500172615704,82,0.039687808159131199,0.042481447878935125,-0.042481240917742739}, + {0.8670839741361045,82.25,0.039465358554374566,0.042521250986633355,-0.042521043940561068}, + {0.87231366475100125,82.5,0.039242884941477249,0.042559972721437488,-0.042559765661534701}, + {0.87753912824108116,82.75,0.039020391631723467,0.042597616248618776,-0.042597409162052256}, + {0.88276040328640648,83,0.03879788292179677,0.042634184421197048,-0.042633977264918015}, + {0.88797752811894171,83.25,0.038575363093779451,0.042669680333848249,-0.042669473186591915}, + {0.8931905405290792,83.5,0.038352836415153399,0.042704107100341823,-0.042703899885172557}, + {0.89839947787206986,83.75,0.038130307138801077,0.042737467740011699,-0.042737260506331184}, + {0.9036043770743325,84,0.037907779503008159,0.042769765387191648,-0.042769558134640798}, + {0.90880527463965555,84.25,0.037685257731466318,0.042801003285843048,-0.042800795980571425}, + {0.91400220665529686,84.5,0.037462746033276997,0.042831184260345524,-0.04283097693491307}, + {0.91919520879797623,84.75,0.037240248602956411,0.04286031172843454,-0.042860104375103243}, + {0.92438431633976448,85,0.037017769620440615,0.042888388717124928,-0.042888181326625835}, + {0.92956956415387504,85.25,0.036795313251091873,0.042915418378989587,-0.04291521094890207}, + {0.93475098672035606,85.5,0.036572883645705531,0.042941403881554055,-0.042941196422330269}, + {0.93992861813168394,85.75,0.036350484940517694,0.042966348505248553,-0.0429661410350895}, + {0.94510249209827157,86,0.036128121257213326,0.042990255333791409,-0.042990047854811871}, + {0.95027264195386807,86.25,0.035905796702935783,0.04301312765840281,-0.043012920152179106}, + {0.95543910066088267,86.5,0.035683515370296182,0.04303496864000398,-0.043034761102853418}, + {0.96060190081561214,86.75,0.035461281337383993,0.043055781493425954,-0.04305557395036258}, + {0.96576107465338468,87,0.035239098667777843,0.043075569511401235,-0.043075361922521309}, + {0.9709166540536136,87.25,0.035016971410557651,0.043094335958006727,-0.043094128368453062}, + {0.97606867054476742,87.5,0.034794903600316576,0.043112083983895044,-0.04311187638835054}, + {0.98121715530926901,87.75,0.034572899257174147,0.043128816886945379,-0.043128609278714564}, + {0.98636213918829307,88,0.034350962386789696,0.0431445379607047,-0.043144330306882112}, + {0.9915036526865082,88.25,0.03412909698037668,0.043159250500346705,-0.043159042839566647}, + {0.99664172597671619,88.5,0.033907307014717135,0.043172957779916803,-0.043172750108711466}, + {1.0017763889044407,88.75,0.03368559645217703,0.043185663006088088,-0.043185455360639467}, + {1.006907670992421,89,0.033463969240722091,0.043197369611135177,-0.043197161932109408}, + {1.012035601445042,89.25,0.033242429313934257,0.043208080769897024,-0.043207873049854219}, + {1.0171602091526915,89.5,0.033020980591028461,0.043217799836418443,-0.043217592153643576}, + {1.0222815226960458,89.75,0.032799626976869646,0.043226530136964446,-0.043226322400851586}, + {1.0273995703502821,90,0.032578372361991265,0.043234274936508159,-0.043234067235691821}, + {1.0325143800892358,90.25,0.032357220622613328,0.043241037693833334,-0.04324082997526333}, + {1.0376259795894716,90.5,0.032136175620661012,0.043246821606827732,-0.043246613850303442}, + {1.0427343962343065,90.75,0.031915241203784313,0.043251630002203958,-0.043251422259874078}, + {1.0478396571177568,91,0.031694421205377445,0.043255466186286388,-0.043255258444601638}, + {1.0529417890484356,91.25,0.031473719444599493,0.043258333623696306,-0.04325812589077755}, + {1.0580408185533721,91.5,0.031253139726394576,0.043260235535370839,-0.043260027781557145}, + {1.0631367718817766,91.75,0.031032685841512815,0.043261175363101943,-0.04326096758646248}, + {1.0682296750087557,92,0.030812361566532408,0.043261156361899908,-0.043260948616188843}, + {1.0733195536389515,92.25,0.030592170663880619,0.043260181900347625,-0.04325997412204681}, + {1.0784064332101395,92.5,0.030372116881856426,0.0432582553600337,-0.043258047596792755}, + {1.0834903388967583,92.75,0.030152203954653303,0.043255380021070079,-0.043255172274416859}, + {1.0885712956133899,93,0.029932435602381562,0.043251559378017182,-0.043251351597019851}, + {1.093649328018186,93.25,0.02971281553109216,0.043246796604458099,-0.043246588841254791}, + {1.0987244605162356,93.5,0.029493347432800278,0.043241095125597616,-0.043240887353292901}, + {1.1037967172628877,93.75,0.029274034985509081,0.043234458343326315,-0.043234250589908318}, + {1.1088661221670149,94,0.029054881853234162,0.043226889601101735,-0.043226681822995251}, + {1.1139326988942333,94.25,0.028835891686028063,0.043218392231715676,-0.043218184487950596}, + {1.1189964708700677,94.5,0.028617068120005494,0.04320896955353952,-0.043208761805145192}, + {1.1240574612830738,94.75,0.028398414777368104,0.043198624956385955,-0.043198417177084086}, + {1.1291156930879027,95,0.028179935266430312,0.043187361732268732,-0.043187154008145476}, + {1.1341711890083268,95.25,0.027961633181644928,0.043175183372119189,-0.043174975622506127}, + {1.1392239715402204,95.5,0.027743512103629547,0.043162093169083836,-0.043161885407901272}, + {1.1442740629544894,95.75,0.027525575599192317,0.043148094446783393,-0.043147886713794818}, + {1.1493214852999545,96,0.027307827221358889,0.043133190552430548,-0.043132982856211449}, + {1.1543662604062024,96.25,0.027090270509399307,0.043117384878419263,-0.043117177169325026}, + {1.1594084098863824,96.5,0.026872908988854881,0.043100680732956489,-0.043100473049955693}, + {1.1644479551399642,96.75,0.026655746171565201,0.04308308146691768,-0.043082873791168402}, + {1.1694849173554558,97,0.026438785555696342,0.043064590528145569,-0.043064382842919663}, + {1.1745193175130781,97.25,0.026222030625768014,0.04304521109647088,-0.043045003407060231}, + {1.1795511763874065,97.5,0.026005484852681664,0.043024946600620104,-0.043024738916013099}, + {1.1845805145499568,97.75,0.025789151693748603,0.043003800394104681,-0.043003592733121189}, + {1.1896073523717579,98,0.025573034592718376,0.042981775770859883,-0.04298156811176531}, + {1.1946317100258548,98.25,0.02535713697980721,0.042958876053506956,-0.042958668400458373}, + {1.1996536074898041,98.5,0.025141462271726728,0.042935104652162326,-0.042934897037744897}, + {1.2046730645481161,98.75,0.024926013871712435,0.042910464833166732,-0.042910257178773337}, + {1.2096901007946617,99,0.024710795169553615,0.042884959885238175,-0.042884752270869575}, + {1.2147047356350498,99.25,0.024495809541621373,0.042858593188433332,-0.042858385561929301}, + {1.2197169882889609,99.5,0.024281060350898727,0.042831368116557085,-0.042831160498602819}, + {1.2247268777924647,99.75,0.024066550947009928,0.042803287900663747,-0.042803080293506858}, + {1.229734423000278,100,0.023852284666249716,0.042774355857187786,-0.042774148242260751}, + {1.2347396425880097,100.25,0.023638264831613541,0.042744575304555235,-0.042744367719638861}, + {1.2397425550543628,100.5,0.023424494752826965,0.042713949561734466,-0.042713742004282816}, + {1.2447431787233125,100.75,0.023210977726375932,0.042682481964089143,-0.042682274385049797}, + {1.2497415317462461,101,0.022997717035536443,0.042650175638018546,-0.042649968130271984}, + {1.2547376321040733,101.25,0.02278471595040546,0.042617034144751216,-0.042616826604590617}, + {1.2597314976093075,101.5,0.022571977727930299,0.042583060554373706,-0.042582853044157334}, + {1.2647231459081181,101.75,0.02235950561193966,0.042548258249941733,-0.04254805075483728}, + {1.2697125944823493,102,0.022147302833173774,0.042512630500566646,-0.04251242298619444}, + {1.2746998606515125,102.25,0.021935372609315155,0.042476180572627094,-0.042475973088406969}, + {1.2796849615747565,102.5,0.021723718145019048,0.042438911702771039,-0.042438704210287823}, + {1.2846679142527948,102.75,0.021512342631944258,0.042400827233668649,-0.042400619754982424}, + {1.2896487355298172,103,0.021301249248783634,0.04236193031586085,-0.042361722853633446}, + {1.2946274420953783,103.25,0.021090441161295312,0.042322224315711011,-0.042322016863519482}, + {1.2996040504862338,103.5,0.020879921522333506,0.042281712448488544,-0.042281504962646418}, + {1.3045785770881968,103.75,0.020669693471878837,0.042240397880240095,-0.042240190470327739}, + {1.3095510381379118,104,0.020459760137070487,0.042198283999100347,-0.042198076585318531}, + {1.3145214497246518,104.25,0.020250124632236008,0.042155373924072349,-0.042155166520834092}, + {1.3194898277920644,104.5,0.020040790058923258,0.042111670943351991,-0.042111463514344805}, + {1.3244561881398962,104.75,0.019831759505931086,0.042067178262623121,-0.042066970898255548}, + {1.3294205464257034,105,0.019623036049340563,0.042021899050582921,-0.04202169169556598}, + {1.334382918166527,105.25,0.019414622752546087,0.041975836657044463,-0.041975629283935327}, + {1.3393433187405519,105.5,0.019206522666286779,0.041928994113242871,-0.041928786768834776}, + {1.3443017633887435,105.75,0.018998738828677364,0.041881374776246892,-0.041881167448371094}, + {1.3492582672164459,106,0.018791274265239619,0.041832981852885281,-0.041832774491148833}, + {1.3542128451949889,106.25,0.018584131988933376,0.041783818298299656,-0.041783610990856881}, + {1.3591655121632435,106.5,0.018377315000188056,0.041733887579768571,-0.041733680259803362}, + {1.3641162828291664,106.75,0.018170826286933783,0.041683192716561734,-0.041682985399044727}, + {1.3690651717713327,107,0.017964668824632102,0.041631736949248967,-0.041631529629667645}, + {1.3740121934404319,107.25,0.017758845576308101,0.041579523426561182,-0.04157931610995462}, + {1.3789573621607507,107.5,0.01755335949258096,0.041526555289850359,-0.041526347996612437}, + {1.3839006921316415,107.75,0.017348213511695375,0.041472835695786435,-0.041472628431247341}, + {1.3888421974289535,108,0.017143410559552638,0.041418367829953735,-0.041418160548106049}, + {1.3937818920064724,108.25,0.016938953549741953,0.04136315480243663,-0.041362947551687887}, + {1.3987197896973085,108.5,0.016734845383571388,0.041307199795198833,-0.041306992539488441}, + {1.4036559042152865,108.75,0.016531088950099186,0.041250505829181078,-0.04125029858041606}, + {1.4085902491563165,109,0.016327687126164545,0.041193076214110898,-0.041192868977241663}, + {1.4135228379997289,109.25,0.016124642776418965,0.041134913962167695,-0.041134706738650217}, + {1.4184536841096236,109.5,0.0159219587533573,0.041076022162711397,-0.041075814887105273}, + {1.4233828007361584,109.75,0.015719637897348308,0.041016403918549528,-0.041016196665515842}, + {1.4283102010168576,110,0.015517683036666084,0.040956062396396944,-0.040955855196883606}, +}}; +} // namespace rmcs_core::controller::chassis::wheelbipe_jump_v4 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/leg_transmission.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/leg_transmission.hpp new file mode 100644 index 000000000..f2e074b0c --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/leg_transmission.hpp @@ -0,0 +1,153 @@ +#pragma once + +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis::wheel_leg { + +// Current six-bar wheel-leg transmission. +// +// The leg is a Stephenson six-bar: two coaxial hip motors drive the thigh +// (``L_joint1``/``R_joint1``) and the crank (``L_crank``/``R_crank``); the crank +// drives the shank through ``LL_joint2 -> LL_joint3 -> LL_link4``. Unlike the +// V4 four-bar this has no simple closed form, so the map between the reduced +// knee angle and the crank is tabulated from the MuJoCo closed-chain model +// (``models/wheelleg/calibrate_kinematics.py``) and interpolated linearly. +// +// Conventions: +// * reduced knee beta : interior thigh-shank angle, straight = pi. +// * relative crank c : crank angle measured in the thigh frame, +// ``c = q_crank - q_hip`` (same for both legs). +// * the tabulated crank is the native joint angle at ``q_hip = 0``. +// +// Mechanical limits and motor zero: +// * the knee is mechanically limited to 40..110 deg (interior angle); it +// cannot straighten (max 110 deg), by design. +// * both coaxial hip motors (thigh and crank) are zeroed at the same pose: +// knee interior angle = 110 deg (the real-robot encoder zero, +// `encoder_zero_inner_knee_deg`) AND thigh perpendicular to the chassis. +// The model shows the thigh is perpendicular at ``q_hip = 0``, so the hip +// motor zero is ``q_hip = 0`` and the crank motor zero is ``crank(110deg)``. +class LegLinkage { +public: + [[nodiscard]] double knee_min() const { return kBetaGrid.front(); } + [[nodiscard]] double knee_max() const { return kBetaGrid.back(); } + [[nodiscard]] double assembly_knee() const { return kAssemblyKnee; } + [[nodiscard]] double assembly_crank() const { return kAssemblyCrank; } + + // Physical knee limits (interior angle), in radians. + static constexpr double kKneeLimitMinDeg = 40.0; + static constexpr double kKneeLimitMaxDeg = 110.0; + static constexpr double kKneeMotorZeroDeg = 110.0; + + [[nodiscard]] static constexpr double knee_limit_min() { + return kKneeLimitMinDeg * std::numbers::pi / 180.0; + } + [[nodiscard]] static constexpr double knee_limit_max() { + return kKneeLimitMaxDeg * std::numbers::pi / 180.0; + } + [[nodiscard]] static constexpr double knee_motor_zero() { + return kKneeMotorZeroDeg * std::numbers::pi / 180.0; + } + + // Relative crank at the motor zero (encoder reads 0 here). + [[nodiscard]] double crank_motor_zero() const { + double crank = 0.0; + (void)crank_from_knee(knee_motor_zero(), crank); + return crank; + } + + // Reduced knee -> relative crank. Returns false outside [knee_min, knee_max]. + [[nodiscard]] bool crank_from_knee(double knee, double& crank) const { + if (knee < knee_min() || knee > knee_max()) + return false; + crank = interpolate(kBetaGrid, kCrankGrid, knee); + return true; + } + + // Relative crank -> reduced knee (clamped to the tabulated range). + [[nodiscard]] double knee_from_crank(double crank) const { + const double clamped = std::clamp(crank, kCrankGrid.front(), kCrankGrid.back()); + return interpolate(kCrankGrid, kBetaGrid, clamped); + } + + // The table above is in the MODEL frame (crank = 0 at the CAD assembly, + // knee = 45 deg). The real DM encoders are zeroed at the MOTOR zero (knee + // 110 deg), so the encoder relative crank c = q_crank - q_hip differs from + // the tabulated crank by crank_motor_zero(). The real-hardware controllers + // MUST go through these encoder-frame helpers, not the raw table. + [[nodiscard]] double knee_from_encoder_crank(double c_encoder) const { + return knee_from_crank(c_encoder + crank_motor_zero()); + } + [[nodiscard]] double knee_velocity_from_encoder_crank(double c_encoder, double velocity) const { + const double crank = c_encoder + crank_motor_zero(); + if (crank <= kCrankGrid.front() || crank >= kCrankGrid.back()) + return 0.; + const auto upper = static_cast( + std::lower_bound(kCrankGrid.begin(), kCrankGrid.end(), crank) - kCrankGrid.begin()); + return velocity * (kBetaGrid[upper] - kBetaGrid[upper - 1]) + / (kCrankGrid[upper] - kCrankGrid[upper - 1]); + } + [[nodiscard]] bool encoder_crank_from_knee(double knee, double& c_encoder) const { + double crank = 0.0; + if (!crank_from_knee(knee, crank)) + return false; + c_encoder = crank - crank_motor_zero(); + return true; + } + +private: + template + static double + interpolate(const std::array& xs, const std::array& ys, double x) { + const std::size_t upper = + static_cast(std::lower_bound(xs.begin(), xs.end(), x) - xs.begin()); + if (upper == 0) + return ys.front(); + if (upper >= N) + return ys.back(); + const double x0 = xs[upper - 1]; + const double x1 = xs[upper]; + const double t = (x - x0) / (x1 - x0); + return ys[upper - 1] + t * (ys[upper] - ys[upper - 1]); + } + + static constexpr std::size_t kSize = 101; + static constexpr double kAssemblyKnee = 0.787329; + static constexpr double kAssemblyCrank = 0.0; + + // beta uniform in [0.326383, 2.586387]. + static constexpr std::array kBetaGrid = { + 0.326383, 0.348983, 0.371583, 0.394183, 0.416783, 0.439383, 0.461983, 0.484583, 0.507183, + 0.529783, 0.552383, 0.574983, 0.597583, 0.620183, 0.642783, 0.665383, 0.687983, 0.710583, + 0.733183, 0.755783, 0.778383, 0.800983, 0.823584, 0.846184, 0.868784, 0.891384, 0.913984, + 0.936584, 0.959184, 0.981784, 1.004384, 1.026984, 1.049584, 1.072184, 1.094784, 1.117384, + 1.139984, 1.162584, 1.185184, 1.207784, 1.230384, 1.252984, 1.275584, 1.298184, 1.320784, + 1.343384, 1.365985, 1.388585, 1.411185, 1.433785, 1.456385, 1.478985, 1.501585, 1.524185, + 1.546785, 1.569385, 1.591985, 1.614585, 1.637185, 1.659785, 1.682385, 1.704985, 1.727585, + 1.750185, 1.772785, 1.795385, 1.817985, 1.840585, 1.863185, 1.885785, 1.908386, 1.930986, + 1.953586, 1.976186, 1.998786, 2.021386, 2.043986, 2.066586, 2.089186, 2.111786, 2.134386, + 2.156986, 2.179586, 2.202186, 2.224786, 2.247386, 2.269986, 2.292586, 2.315186, 2.337786, + 2.360386, 2.382986, 2.405586, 2.428186, 2.450787, 2.473387, 2.495987, 2.518587, 2.541187, + 2.563787, 2.586387}; + + // relative crank (native joint at q_hip = 0) for each kBetaGrid entry. + static constexpr std::array kCrankGrid = { + -1.850000, -1.486884, -1.277137, -1.120978, -0.994545, -0.887366, -0.793800, -0.710363, + -0.634808, -0.565499, -0.501285, -0.441311, -0.384908, -0.331530, -0.280773, -0.232300, + -0.185822, -0.141099, -0.097952, -0.056207, -0.015719, 0.023633, 0.061954, 0.099336, + 0.135849, 0.171583, 0.206584, 0.240922, 0.274630, 0.307767, 0.340369, 0.372461, + 0.404086, 0.435272, 0.466043, 0.496423, 0.526433, 0.556094, 0.585422, 0.614435, + 0.643147, 0.671573, 0.699723, 0.727606, 0.755236, 0.782624, 0.809777, 0.836696, + 0.863397, 0.889886, 0.916158, 0.942230, 0.968100, 0.993772, 1.019255, 1.044541, + 1.069644, 1.094554, 1.119283, 1.143822, 1.168180, 1.192350, 1.216334, 1.240134, + 1.263737, 1.287153, 1.310376, 1.333393, 1.356210, 1.378823, 1.401219, 1.423392, + 1.445340, 1.467054, 1.488524, 1.509740, 1.530686, 1.551353, 1.571727, 1.591793, + 1.611535, 1.630933, 1.649966, 1.668598, 1.686813, 1.704583, 1.721883, 1.738662, + 1.754860, 1.770493, 1.785406, 1.799669, 1.813056, 1.825600, 1.837208, 1.847746, + 1.857053, 1.864945, 1.871347, 1.875673, 1.880000}; +}; + +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/mechanics.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/mechanics.hpp new file mode 100644 index 000000000..94cb2c4c5 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/mechanics.hpp @@ -0,0 +1,8 @@ +#pragma once +namespace rmcs_core::controller::chassis::wheel_leg::mechanics { +inline constexpr double kThighLength = .21, kShankLength = .25045, kWheelRadius = .06; +inline constexpr double kBodyMass = 11.62, kThighMass = .426, kShankMass = .507, kWheelMass = .40; +inline constexpr double kThighComOffset = .06623, kShankComOffset = .15344; +inline constexpr double kThighComAngle = -.096, kShankComAngle = .002; +inline constexpr double kBodyComForward = .00918, kBodyComUp = -.052; +} // namespace rmcs_core::controller::chassis::wheel_leg::mechanics diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/observation_geometry.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/observation_geometry.hpp new file mode 100644 index 000000000..be27d060f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/observation_geometry.hpp @@ -0,0 +1,100 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis::standing_reference { + +using Vector3 = std::array; + +struct GeometryKnot { + float delta_rad; + Vector3 center_m; + float passive_inner_deg; +}; + +struct SideGeometry { + Vector3 hip_origin_m; + Vector3 hip_axis; + float hip_reference_rad; + float wheel_radius_m; + std::vector knots; +}; + +using GeometryTable = std::array; + +inline float norm(const Vector3& value) { + return std::sqrt(value[0] * value[0] + value[1] * value[1] + value[2] * value[2]); +} + +inline bool finite(const Vector3& value) { + return std::all_of(value.begin(), value.end(), [](float v) { return std::isfinite(v); }); +} + +struct CoordinateResult { + std::array coordinate_m{}; + bool valid = false; +}; + +inline CoordinateResult wheel_angle_coordinate( + const GeometryTable& table, const std::array& position, const Vector3& gravity) { + const float magnitude = norm(gravity); + if (!finite(gravity) || !(magnitude > .5F && magnitude < 1.5F) + || !std::all_of(position.begin(), position.end(), [](float v) { return std::isfinite(v); }) + || !(gravity[2] / magnitude < -.94F)) + return {}; + const float pitch = std::atan2(gravity[0] / magnitude, -gravity[2] / magnitude); + CoordinateResult result{{}, true}; + for (std::size_t side = 0; side < 2; ++side) { + const auto& geometry = table[side]; + const std::size_t i = 2 * side; + const float difference = position[i + 1] - position[i]; + const float delta = std::atan2(std::sin(difference), std::cos(difference)); + if (!(delta >= geometry.knots.front().delta_rad + && delta <= geometry.knots.back().delta_rad)) + return {}; + const auto upper_iterator = std::upper_bound( + geometry.knots.begin(), geometry.knots.end(), delta, + [](float value, const GeometryKnot& knot) { return value < knot.delta_rad; }); + const auto upper_index = std::clamp( + upper_iterator - geometry.knots.begin(), 1, geometry.knots.size() - 1); + const auto& lower = geometry.knots[upper_index - 1]; + const auto& upper = geometry.knots[upper_index]; + const float blend = + std::clamp((delta - lower.delta_rad) / (upper.delta_rad - lower.delta_rad), 0.F, 1.F); + Vector3 value{}; + for (std::size_t j = 0; j < 3; ++j) + value[j] = lower.center_m[j] + blend * (upper.center_m[j] - lower.center_m[j]) + - geometry.hip_origin_m[j]; + const auto& axis = geometry.hip_axis; + const Vector3 cross{ + axis[1] * value[2] - axis[2] * value[1], axis[2] * value[0] - axis[0] * value[2], + axis[0] * value[1] - axis[1] * value[0]}; + const float dot = axis[0] * value[0] + axis[1] * value[1] + axis[2] * value[2]; + const float cosine = std::cos(position[i] - geometry.hip_reference_rad); + const float sine = std::sin(position[i] - geometry.hip_reference_rad); + Vector3 center{}; + for (std::size_t j = 0; j < 3; ++j) + center[j] = cosine * value[j] + sine * cross[j] + (1.F - cosine) * axis[j] * dot + + geometry.hip_origin_m[j]; + const float center_x = std::cos(pitch) * center[0] + std::sin(pitch) * center[2]; + constexpr float degrees_to_radians = .017453292519943295F; + const float lower_beta = lower.passive_inner_deg * degrees_to_radians; + const float upper_beta = upper.passive_inner_deg * degrees_to_radians; + const float beta = lower_beta + blend * (upper_beta - lower_beta); + const float spin = position[i] + (side == 0 ? beta : -beta) + position[4 + side]; + const float sign = axis[1] > 0.F ? 1.F : axis[1] < 0.F ? -1.F : 0.F; + result.coordinate_m[side] = geometry.wheel_radius_m * (sign * spin + pitch) - center_x; + if (!std::isfinite(result.coordinate_m[side])) + return {}; + } + return result; +} + +} // namespace rmcs_core::controller::chassis::standing_reference diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/prone_odometry.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/prone_odometry.hpp new file mode 100644 index 000000000..4a479af3c --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/prone_odometry.hpp @@ -0,0 +1,590 @@ +#pragma once + +#include "observation_geometry.hpp" + +// Frozen 2026-11-03 friction_v1/training/encoder_geometry.json. +// Source SHA256: c30136fdae9b504b0033629c4b95e187cbcb841b7307f05be3d0d6f9ce6850b9 +namespace rmcs_core::controller::chassis::standing_reference { +inline const GeometryTable& friction_geometry110() { + static const GeometryTable table{{ + SideGeometry{ + {0.0F, 0.183799997F, -0.000836969994F}, + {-3.67316579e-06F, 0.99998939F, -0.00460365694F}, + 0.419999987F, + 0.0599999987F, + { + GeometryKnot{-0.140893266F, {-0.0824079663F, 0.218508527F, -0.140189767F}, 40.0F}, + GeometryKnot{-0.133477986F, {-0.0833899751F, 0.218506336F, -0.140664697F}, 40.25F}, + GeometryKnot{-0.126096666F, {-0.0843740404F, 0.218504176F, -0.14113535F}, 40.5F}, + GeometryKnot{-0.118748732F, {-0.0853601545F, 0.21850203F, -0.141601712F}, 40.75F}, + GeometryKnot{-0.111433603F, {-0.0863482952F, 0.218499899F, -0.142063767F}, 41.0F}, + GeometryKnot{-0.104150735F, {-0.0873384401F, 0.218497783F, -0.142521486F}, 41.25F}, + GeometryKnot{-0.0968995914F, {-0.0883305743F, 0.218495697F, -0.142974898F}, 41.5F}, + GeometryKnot{-0.0896796361F, {-0.0893246755F, 0.218493626F, -0.143423975F}, 41.75F}, + GeometryKnot{-0.0824903548F, {-0.0903207213F, 0.218491569F, -0.1438687F}, 42.0F}, + GeometryKnot{-0.0753312409F, {-0.0913187042F, 0.218489543F, -0.144309089F}, 42.25F}, + GeometryKnot{-0.0682017952F, {-0.0923186019F, 0.218487531F, -0.144745111F}, 42.5F}, + GeometryKnot{-0.0611015297F, {-0.0933203921F, 0.218485534F, -0.145176768F}, 42.75F}, + GeometryKnot{-0.0540299788F, {-0.0943240523F, 0.218483567F, -0.145604044F}, 43.0F}, + GeometryKnot{-0.046986673F, {-0.0953295678F, 0.218481615F, -0.146026954F}, 43.25F}, + GeometryKnot{-0.0399711542F, {-0.0963369161F, 0.218479678F, -0.146445453F}, 43.5F}, + GeometryKnot{-0.032982979F, {-0.0973460898F, 0.218477771F, -0.146859556F}, 43.75F}, + GeometryKnot{-0.0260217153F, {-0.0983570516F, 0.218475878F, -0.147269264F}, 44.0F}, + GeometryKnot{-0.019086929F, {-0.0993697941F, 0.218474016F, -0.147674546F}, 44.25F}, + GeometryKnot{-0.012178205F, {-0.100384302F, 0.218472168F, -0.148075417F}, 44.5F}, + GeometryKnot{-0.00529513322F, {-0.101400539F, 0.218470335F, -0.148471847F}, 44.75F}, + GeometryKnot{0.00156268873F, {-0.102418505F, 0.218468532F, -0.148863837F}, 45.0F}, + GeometryKnot{0.00839565322F, {-0.103438169F, 0.218466744F, -0.149251387F}, 45.25F}, + GeometryKnot{0.0152041474F, {-0.104459509F, 0.218464971F, -0.14963448F}, 45.5F}, + GeometryKnot{0.0219885465F, {-0.105482519F, 0.218463227F, -0.150013119F}, 45.75F}, + GeometryKnot{0.0287492219F, {-0.106507167F, 0.218461499F, -0.150387287F}, 46.0F}, + GeometryKnot{0.035486538F, {-0.10753344F, 0.2184598F, -0.150756985F}, 46.25F}, + GeometryKnot{0.0422008485F, {-0.108561315F, 0.218458101F, -0.151122198F}, 46.5F}, + GeometryKnot{0.048892498F, {-0.109590769F, 0.218456447F, -0.151482925F}, 46.75F}, + GeometryKnot{0.0555618368F, {-0.110621795F, 0.218454793F, -0.151839152F}, 47.0F}, + GeometryKnot{0.0622091927F, {-0.111654364F, 0.218453184F, -0.152190879F}, 47.25F}, + GeometryKnot{0.0688348934F, {-0.112688452F, 0.218451574F, -0.152538091F}, 47.5F}, + GeometryKnot{0.0754392669F, {-0.113724053F, 0.218449995F, -0.152880803F}, 47.75F}, + GeometryKnot{0.0820226222F, {-0.114761136F, 0.21844843F, -0.153218985F}, 48.0F}, + GeometryKnot{0.0885852799F, {-0.11579968F, 0.218446895F, -0.153552637F}, 48.25F}, + GeometryKnot{0.0951275304F, {-0.116839677F, 0.218445376F, -0.153881758F}, 48.5F}, + GeometryKnot{0.101649679F, {-0.117881097F, 0.218443871F, -0.154206336F}, 48.75F}, + GeometryKnot{0.108152017F, {-0.118923925F, 0.218442395F, -0.154526368F}, 49.0F}, + GeometryKnot{0.114634827F, {-0.119968139F, 0.218440935F, -0.15484184F}, 49.25F}, + GeometryKnot{0.121098399F, {-0.121013716F, 0.218439505F, -0.155152753F}, 49.5F}, + GeometryKnot{0.127543002F, {-0.122060642F, 0.218438089F, -0.155459106F}, 49.75F}, + GeometryKnot{0.133968905F, {-0.123108901F, 0.218436703F, -0.155760899F}, 50.0F}, + GeometryKnot{0.140376374F, {-0.124158457F, 0.218435332F, -0.156058103F}, 50.25F}, + GeometryKnot{0.146765679F, {-0.125209302F, 0.218433976F, -0.156350717F}, 50.5F}, + GeometryKnot{0.153137073F, {-0.126261413F, 0.21843265F, -0.156638756F}, 50.75F}, + GeometryKnot{0.159490809F, {-0.127314776F, 0.218431339F, -0.156922206F}, 51.0F}, + GeometryKnot{0.165827125F, {-0.128369361F, 0.218430057F, -0.157201037F}, 51.25F}, + GeometryKnot{0.172146261F, {-0.129425168F, 0.218428791F, -0.157475278F}, 51.5F}, + GeometryKnot{0.178448468F, {-0.130482137F, 0.218427539F, -0.157744914F}, 51.75F}, + GeometryKnot{0.184733972F, {-0.131540284F, 0.218426317F, -0.158009931F}, 52.0F}, + GeometryKnot{0.19100301F, {-0.132599577F, 0.21842511F, -0.158270329F}, 52.25F}, + GeometryKnot{0.19725579F, {-0.133660004F, 0.218423933F, -0.158526093F}, 52.5F}, + GeometryKnot{0.203492552F, {-0.134721532F, 0.218422771F, -0.158777237F}, 52.75F}, + GeometryKnot{0.209713504F, {-0.135784134F, 0.218421638F, -0.159023747F}, 53.0F}, + GeometryKnot{0.215918854F, {-0.136847809F, 0.21842052F, -0.159265608F}, 53.25F}, + GeometryKnot{0.222108826F, {-0.137912542F, 0.218419418F, -0.159502834F}, 53.5F}, + GeometryKnot{0.228283629F, {-0.138978288F, 0.218418345F, -0.159735411F}, 53.75F}, + GeometryKnot{0.234443441F, {-0.140045032F, 0.218417287F, -0.15996334F}, 54.0F}, + GeometryKnot{0.240588486F, {-0.141112775F, 0.218416259F, -0.160186604F}, 54.25F}, + GeometryKnot{0.246718943F, {-0.142181471F, 0.218415245F, -0.160405219F}, 54.5F}, + GeometryKnot{0.252835006F, {-0.143251121F, 0.218414262F, -0.160619155F}, 54.75F}, + GeometryKnot{0.258936882F, {-0.14432168F, 0.218413293F, -0.160828441F}, 55.0F}, + GeometryKnot{0.265024722F, {-0.145393148F, 0.21841234F, -0.161033034F}, 55.25F}, + GeometryKnot{0.271098763F, {-0.14646551F, 0.218411416F, -0.161232963F}, 55.5F}, + GeometryKnot{0.277159125F, {-0.147538722F, 0.218410522F, -0.161428198F}, 55.75F}, + GeometryKnot{0.283206016F, {-0.148612767F, 0.218409643F, -0.161618754F}, 56.0F}, + GeometryKnot{0.289239585F, {-0.149687648F, 0.218408778F, -0.161804631F}, 56.25F}, + GeometryKnot{0.295260042F, {-0.150763333F, 0.218407944F, -0.161985815F}, 56.5F}, + GeometryKnot{0.301267534F, {-0.151839793F, 0.218407124F, -0.162162289F}, 56.75F}, + GeometryKnot{0.307262212F, {-0.152917013F, 0.218406335F, -0.16233407F}, 57.0F}, + GeometryKnot{0.313244253F, {-0.153994963F, 0.21840556F, -0.162501156F}, 57.25F}, + GeometryKnot{0.319213837F, {-0.155073643F, 0.2184048F, -0.162663534F}, 57.5F}, + GeometryKnot{0.325171083F, {-0.156153023F, 0.21840407F, -0.162821203F}, 57.75F}, + GeometryKnot{0.33111617F, {-0.157233074F, 0.218403369F, -0.162974164F}, 58.0F}, + GeometryKnot{0.337049216F, {-0.158313781F, 0.218402684F, -0.163122401F}, 58.25F}, + GeometryKnot{0.342970431F, {-0.159395128F, 0.218402013F, -0.163265929F}, 58.5F}, + GeometryKnot{0.348879904F, {-0.160477087F, 0.218401372F, -0.163404748F}, 58.75F}, + GeometryKnot{0.354777783F, {-0.161559641F, 0.218400747F, -0.163538828F}, 59.0F}, + GeometryKnot{0.360664248F, {-0.162642777F, 0.218400151F, -0.163668185F}, 59.25F}, + GeometryKnot{0.366539419F, {-0.163726464F, 0.218399569F, -0.163792819F}, 59.5F}, + GeometryKnot{0.372403443F, {-0.164810687F, 0.218399018F, -0.163912728F}, 59.75F}, + GeometryKnot{0.37825644F, {-0.165895417F, 0.218398482F, -0.1640279F}, 60.0F}, + GeometryKnot{0.38409856F, {-0.166980639F, 0.218397975F, -0.164138332F}, 60.25F}, + GeometryKnot{0.389929891F, {-0.168066338F, 0.218397483F, -0.164244041F}, 60.5F}, + GeometryKnot{0.395750642F, {-0.169152483F, 0.218397006F, -0.164344996F}, 60.75F}, + GeometryKnot{0.401560843F, {-0.170239061F, 0.218396559F, -0.164441228F}, 61.0F}, + GeometryKnot{0.407360703F, {-0.171326041F, 0.218396142F, -0.164532706F}, 61.25F}, + GeometryKnot{0.413150311F, {-0.172413424F, 0.21839574F, -0.164619446F}, 61.5F}, + GeometryKnot{0.418929785F, {-0.173501164F, 0.218395352F, -0.164701432F}, 61.75F}, + GeometryKnot{0.424699247F, {-0.174589261F, 0.218394995F, -0.16477868F}, 62.0F}, + GeometryKnot{0.430458814F, {-0.175677672F, 0.218394652F, -0.164851174F}, 62.25F}, + GeometryKnot{0.436208606F, {-0.176766396F, 0.218394339F, -0.164918929F}, 62.5F}, + GeometryKnot{0.441948712F, {-0.177855417F, 0.218394041F, -0.164981931F}, 62.75F}, + GeometryKnot{0.447679281F, {-0.178944677F, 0.218393773F, -0.165040165F}, 63.0F}, + GeometryKnot{0.453400433F, {-0.180034205F, 0.21839352F, -0.16509366F}, 63.25F}, + GeometryKnot{0.459112227F, {-0.181123942F, 0.218393296F, -0.165142402F}, 63.5F}, + GeometryKnot{0.464814812F, {-0.182213888F, 0.218393087F, -0.165186375F}, 63.75F}, + GeometryKnot{0.470508277F, {-0.183304012F, 0.218392909F, -0.16522561F}, 64.0F}, + GeometryKnot{0.476192713F, {-0.1843943F, 0.218392745F, -0.165260077F}, 64.25F}, + GeometryKnot{0.481868267F, {-0.185484722F, 0.218392596F, -0.165289789F}, 64.5F}, + GeometryKnot{0.487535F, {-0.186575264F, 0.218392476F, -0.165314734F}, 64.75F}, + GeometryKnot{0.493193001F, {-0.18766591F, 0.218392387F, -0.165334925F}, 65.0F}, + GeometryKnot{0.498842418F, {-0.18875663F, 0.218392313F, -0.165350363F}, 65.25F}, + GeometryKnot{0.504483283F, {-0.18984741F, 0.218392253F, -0.165361047F}, 65.5F}, + GeometryKnot{0.510115802F, {-0.190938219F, 0.218392223F, -0.165366963F}, 65.75F}, + GeometryKnot{0.515739918F, {-0.192029044F, 0.218392223F, -0.16536811F}, 66.0F}, + GeometryKnot{0.521355808F, {-0.193119869F, 0.218392223F, -0.165364519F}, 66.25F}, + GeometryKnot{0.526963592F, {-0.194210678F, 0.218392268F, -0.165356144F}, 66.5F}, + GeometryKnot{0.532563329F, {-0.195301428F, 0.218392327F, -0.165343031F}, 66.75F}, + GeometryKnot{0.538155079F, {-0.196392104F, 0.218392402F, -0.16532515F}, 67.0F}, + GeometryKnot{0.543738902F, {-0.197482705F, 0.218392506F, -0.165302515F}, 67.25F}, + GeometryKnot{0.549315035F, {-0.198573187F, 0.218392625F, -0.165275112F}, 67.5F}, + GeometryKnot{0.554883361F, {-0.199663535F, 0.21839276F, -0.165242955F}, 67.75F}, + GeometryKnot{0.560444117F, {-0.200753748F, 0.218392938F, -0.165206045F}, 68.0F}, + GeometryKnot{0.565997362F, {-0.201843783F, 0.218393117F, -0.165164381F}, 68.25F}, + GeometryKnot{0.571543097F, {-0.202933624F, 0.218393326F, -0.165117949F}, 68.5F}, + GeometryKnot{0.577081501F, {-0.204023257F, 0.218393564F, -0.165066764F}, 68.75F}, + GeometryKnot{0.582612574F, {-0.205112651F, 0.218393818F, -0.16501084F}, 69.0F}, + GeometryKnot{0.588136435F, {-0.206201792F, 0.218394086F, -0.164950147F}, 69.25F}, + GeometryKnot{0.593653142F, {-0.207290649F, 0.218394384F, -0.164884716F}, 69.5F}, + GeometryKnot{0.599162757F, {-0.208379224F, 0.218394712F, -0.164814517F}, 69.75F}, + GeometryKnot{0.604665458F, {-0.209467471F, 0.218395039F, -0.164739579F}, 70.0F}, + GeometryKnot{0.610161185F, {-0.21055539F, 0.218395412F, -0.164659902F}, 70.25F}, + GeometryKnot{0.615650058F, {-0.211642951F, 0.218395799F, -0.164575472F}, 70.5F}, + GeometryKnot{0.621132195F, {-0.212730125F, 0.218396202F, -0.164486289F}, 70.75F}, + GeometryKnot{0.626607597F, {-0.213816911F, 0.218396634F, -0.164392382F}, 71.0F}, + GeometryKnot{0.632076383F, {-0.214903265F, 0.218397081F, -0.164293721F}, 71.25F}, + GeometryKnot{0.637538552F, {-0.215989187F, 0.218397558F, -0.164190322F}, 71.5F}, + GeometryKnot{0.642994285F, {-0.217074633F, 0.218398049F, -0.164082184F}, 71.75F}, + GeometryKnot{0.64844358F, {-0.218159616F, 0.218398556F, -0.163969308F}, 72.0F}, + GeometryKnot{0.653886497F, {-0.219244093F, 0.218399093F, -0.163851708F}, 72.25F}, + GeometryKnot{0.659323096F, {-0.220328033F, 0.218399659F, -0.16372937F}, 72.5F}, + GeometryKnot{0.664753497F, {-0.221411437F, 0.21840024F, -0.163602307F}, 72.75F}, + GeometryKnot{0.670177698F, {-0.222494274F, 0.218400836F, -0.163470522F}, 73.0F}, + GeometryKnot{0.67559582F, {-0.223576531F, 0.218401462F, -0.163334012F}, 73.25F}, + GeometryKnot{0.681007862F, {-0.224658176F, 0.218402117F, -0.163192779F}, 73.5F}, + GeometryKnot{0.686413944F, {-0.225739196F, 0.218402773F, -0.163046822F}, 73.75F}, + GeometryKnot{0.691814065F, {-0.226819575F, 0.218403473F, -0.162896156F}, 74.0F}, + GeometryKnot{0.697208405F, {-0.227899283F, 0.218404174F, -0.162740782F}, 74.25F}, + GeometryKnot{0.702596843F, {-0.228978306F, 0.218404919F, -0.162580684F}, 74.5F}, + GeometryKnot{0.70797962F, {-0.230056614F, 0.218405664F, -0.162415892F}, 74.75F}, + GeometryKnot{0.713356614F, {-0.231134191F, 0.218406439F, -0.162246391F}, 75.0F}, + GeometryKnot{0.718728065F, {-0.232211024F, 0.218407243F, -0.162072197F}, 75.25F}, + GeometryKnot{0.724093854F, {-0.233287081F, 0.218408063F, -0.161893308F}, 75.5F}, + GeometryKnot{0.729454219F, {-0.234362349F, 0.218408898F, -0.161709711F}, 75.75F}, + GeometryKnot{0.734809041F, {-0.235436812F, 0.218409762F, -0.161521435F}, 76.0F}, + GeometryKnot{0.740158439F, {-0.236510441F, 0.218410656F, -0.161328465F}, 76.25F}, + GeometryKnot{0.745502532F, {-0.237583205F, 0.21841155F, -0.161130816F}, 76.5F}, + GeometryKnot{0.75084126F, {-0.238655105F, 0.218412489F, -0.160928488F}, 76.75F}, + GeometryKnot{0.756174743F, {-0.239726126F, 0.218413427F, -0.160721496F}, 77.0F}, + GeometryKnot{0.761503041F, {-0.240796208F, 0.218414411F, -0.16050981F}, 77.25F}, + GeometryKnot{0.766826212F, {-0.241865382F, 0.218415394F, -0.160293475F}, 77.5F}, + GeometryKnot{0.772144198F, {-0.242933586F, 0.218416408F, -0.160072476F}, 77.75F}, + GeometryKnot{0.777457178F, {-0.244000822F, 0.218417451F, -0.159846812F}, 78.0F}, + GeometryKnot{0.78276515F, {-0.24506706F, 0.218418494F, -0.1596165F}, 78.25F}, + GeometryKnot{0.788068175F, {-0.24613227F, 0.218419582F, -0.159381524F}, 78.5F}, + GeometryKnot{0.793366253F, {-0.247196466F, 0.218420684F, -0.159141913F}, 78.75F}, + GeometryKnot{0.798659444F, {-0.248259589F, 0.218421802F, -0.158897668F}, 79.0F}, + GeometryKnot{0.803947866F, {-0.249321654F, 0.218422934F, -0.158648774F}, 79.25F}, + GeometryKnot{0.80923152F, {-0.250382602F, 0.218424097F, -0.158395246F}, 79.5F}, + GeometryKnot{0.814510405F, {-0.251442462F, 0.218425289F, -0.158137098F}, 79.75F}, + GeometryKnot{0.819784641F, {-0.25250116F, 0.218426496F, -0.157874331F}, 80.0F}, + GeometryKnot{0.825054169F, {-0.253558725F, 0.218427718F, -0.157606944F}, 80.25F}, + GeometryKnot{0.830319166F, {-0.254615098F, 0.218428969F, -0.157334954F}, 80.5F}, + GeometryKnot{0.835579574F, {-0.255670249F, 0.218430236F, -0.157058343F}, 80.75F}, + GeometryKnot{0.840835512F, {-0.256724209F, 0.218431532F, -0.156777129F}, 81.0F}, + GeometryKnot{0.846086919F, {-0.257776946F, 0.218432844F, -0.156491339F}, 81.25F}, + GeometryKnot{0.851333976F, {-0.258828402F, 0.21843417F, -0.156200945F}, 81.5F}, + GeometryKnot{0.856576562F, {-0.259878606F, 0.218435526F, -0.155905962F}, 81.75F}, + GeometryKnot{0.861814857F, {-0.260927498F, 0.218436897F, -0.155606404F}, 82.0F}, + GeometryKnot{0.86704886F, {-0.26197505F, 0.218438298F, -0.155302271F}, 82.25F}, + GeometryKnot{0.872278571F, {-0.26302129F, 0.218439713F, -0.154993579F}, 82.5F}, + GeometryKnot{0.877504051F, {-0.26406619F, 0.218441159F, -0.154680312F}, 82.75F}, + GeometryKnot{0.882725358F, {-0.265109688F, 0.218442619F, -0.154362485F}, 83.0F}, + GeometryKnot{0.887942493F, {-0.266151786F, 0.218444094F, -0.154040128F}, 83.25F}, + GeometryKnot{0.893155515F, {-0.267192483F, 0.218445599F, -0.153713211F}, 83.5F}, + GeometryKnot{0.898364484F, {-0.26823175F, 0.218447119F, -0.153381765F}, 83.75F}, + GeometryKnot{0.9035694F, {-0.269269526F, 0.218448669F, -0.153045788F}, 84.0F}, + GeometryKnot{0.908770323F, {-0.270305842F, 0.218450218F, -0.152705282F}, 84.25F}, + GeometryKnot{0.913967252F, {-0.271340668F, 0.218451813F, -0.152360246F}, 84.5F}, + GeometryKnot{0.919160247F, {-0.272374004F, 0.218453422F, -0.152010709F}, 84.75F}, + GeometryKnot{0.924349368F, {-0.273405761F, 0.218455046F, -0.151656672F}, 85.0F}, + GeometryKnot{0.929534674F, {-0.274435997F, 0.218456686F, -0.151298136F}, 85.25F}, + GeometryKnot{0.934716105F, {-0.275464624F, 0.218458354F, -0.150935099F}, 85.5F}, + GeometryKnot{0.939893723F, {-0.276491702F, 0.218460038F, -0.150567591F}, 85.75F}, + GeometryKnot{0.945067644F, {-0.27751714F, 0.218461752F, -0.150195599F}, 86.0F}, + GeometryKnot{0.950237811F, {-0.278540939F, 0.21846348F, -0.149819121F}, 86.25F}, + GeometryKnot{0.955404282F, {-0.279563099F, 0.218465239F, -0.149438202F}, 86.5F}, + GeometryKnot{0.960567117F, {-0.28058359F, 0.218466997F, -0.149052814F}, 86.75F}, + GeometryKnot{0.965726256F, {-0.281602353F, 0.2184688F, -0.148662984F}, 87.0F}, + GeometryKnot{0.970881879F, {-0.282619447F, 0.218470603F, -0.1482687F}, 87.25F}, + GeometryKnot{0.976033926F, {-0.283634812F, 0.218472436F, -0.147869989F}, 87.5F}, + GeometryKnot{0.981182396F, {-0.284648389F, 0.218474284F, -0.147466853F}, 87.75F}, + GeometryKnot{0.98632741F, {-0.285660237F, 0.218476161F, -0.147059307F}, 88.0F}, + GeometryKnot{0.991468966F, {-0.286670268F, 0.218478054F, -0.146647334F}, 88.25F}, + GeometryKnot{0.996607006F, {-0.28767851F, 0.218479961F, -0.146230966F}, 88.5F}, + GeometryKnot{1.00174165F, {-0.288684934F, 0.218481898F, -0.145810202F}, 88.75F}, + GeometryKnot{1.00687301F, {-0.289689481F, 0.21848385F, -0.145385057F}, 89.0F}, + GeometryKnot{1.01200092F, {-0.29069218F, 0.218485832F, -0.144955516F}, 89.25F}, + GeometryKnot{1.01712561F, {-0.291693002F, 0.218487829F, -0.144521624F}, 89.5F}, + GeometryKnot{1.02224684F, {-0.292691916F, 0.218489841F, -0.144083351F}, 89.75F}, + GeometryKnot{1.02736497F, {-0.293688923F, 0.218491867F, -0.143640742F}, 90.0F}, + GeometryKnot{1.03247976F, {-0.294683963F, 0.218493924F, -0.143193766F}, 90.25F}, + GeometryKnot{1.03759134F, {-0.295677066F, 0.218495995F, -0.14274247F}, 90.5F}, + GeometryKnot{1.04269981F, {-0.296668172F, 0.218498096F, -0.142286837F}, 90.75F}, + GeometryKnot{1.04780507F, {-0.297657281F, 0.218500212F, -0.141826883F}, 91.0F}, + GeometryKnot{1.05290723F, {-0.298644394F, 0.218502343F, -0.141362622F}, 91.25F}, + GeometryKnot{1.05800629F, {-0.29962945F, 0.218504488F, -0.140894055F}, 91.5F}, + GeometryKnot{1.06310225F, {-0.300612479F, 0.218506664F, -0.140421197F}, 91.75F}, + GeometryKnot{1.06819522F, {-0.301593393F, 0.218508855F, -0.139944062F}, 92.0F}, + GeometryKnot{1.0732851F, {-0.30257225F, 0.218511075F, -0.139462635F}, 92.25F}, + GeometryKnot{1.078372F, {-0.303548992F, 0.21851331F, -0.138976961F}, 92.5F}, + GeometryKnot{1.08345592F, {-0.304523587F, 0.21851556F, -0.138487011F}, 92.75F}, + GeometryKnot{1.08853686F, {-0.305496067F, 0.218517825F, -0.137992829F}, 93.0F}, + GeometryKnot{1.09361482F, {-0.306466341F, 0.21852012F, -0.1374944F}, 93.25F}, + GeometryKnot{1.09869003F, {-0.307434469F, 0.218522429F, -0.136991739F}, 93.5F}, + GeometryKnot{1.10376227F, {-0.308400363F, 0.218524754F, -0.136484861F}, 93.75F}, + GeometryKnot{1.10883164F, {-0.309364051F, 0.218527108F, -0.135973781F}, 94.0F}, + GeometryKnot{1.11389828F, {-0.310325503F, 0.218529478F, -0.135458484F}, 94.25F}, + GeometryKnot{1.11896205F, {-0.311284691F, 0.218531862F, -0.134939015F}, 94.5F}, + GeometryKnot{1.12402308F, {-0.312241614F, 0.218534276F, -0.134415358F}, 94.75F}, + GeometryKnot{1.12908137F, {-0.313196242F, 0.218536705F, -0.133887529F}, 95.0F}, + GeometryKnot{1.1341368F, {-0.314148545F, 0.218539149F, -0.133355543F}, 95.25F}, + GeometryKnot{1.1391896F, {-0.315098524F, 0.218541607F, -0.132819399F}, 95.5F}, + GeometryKnot{1.14423966F, {-0.316046149F, 0.218544096F, -0.132279128F}, 95.75F}, + GeometryKnot{1.1492871F, {-0.316991419F, 0.218546599F, -0.131734714F}, 96.0F}, + GeometryKnot{1.15433192F, {-0.317934304F, 0.218549117F, -0.131186202F}, 96.25F}, + GeometryKnot{1.15937412F, {-0.318874776F, 0.218551666F, -0.130633563F}, 96.5F}, + GeometryKnot{1.16441369F, {-0.319812834F, 0.218554214F, -0.130076826F}, 96.75F}, + GeometryKnot{1.16945064F, {-0.320748448F, 0.218556792F, -0.129516006F}, 97.0F}, + GeometryKnot{1.17448497F, {-0.321681619F, 0.218559399F, -0.128951102F}, 97.25F}, + GeometryKnot{1.17951691F, {-0.322612315F, 0.218562007F, -0.128382146F}, 97.5F}, + GeometryKnot{1.18454623F, {-0.323540509F, 0.218564644F, -0.127809122F}, 97.75F}, + GeometryKnot{1.18957305F, {-0.324466199F, 0.218567297F, -0.12723206F}, 98.0F}, + GeometryKnot{1.19459748F, {-0.325389355F, 0.218569964F, -0.126650959F}, 98.25F}, + GeometryKnot{1.19961941F, {-0.326309979F, 0.218572661F, -0.126065835F}, 98.5F}, + GeometryKnot{1.20463884F, {-0.32722804F, 0.218575373F, -0.125476718F}, 98.75F}, + GeometryKnot{1.20965588F, {-0.328143507F, 0.2185781F, -0.124883577F}, 99.0F}, + GeometryKnot{1.21467054F, {-0.329056382F, 0.218580842F, -0.124286458F}, 99.25F}, + GeometryKnot{1.21968281F, {-0.329966664F, 0.218583599F, -0.12368536F}, 99.5F}, + GeometryKnot{1.2246927F, {-0.330874294F, 0.218586385F, -0.123080298F}, 99.75F}, + GeometryKnot{1.22970021F, {-0.331779271F, 0.218589187F, -0.12247128F}, 100.0F}, + GeometryKnot{1.23470545F, {-0.332681596F, 0.218592003F, -0.121858321F}, 100.25F}, + GeometryKnot{1.23970842F, {-0.333581239F, 0.218594849F, -0.121241428F}, 100.5F}, + GeometryKnot{1.24470901F, {-0.33447817F, 0.218597695F, -0.120620616F}, 100.75F}, + GeometryKnot{1.24970734F, {-0.335372388F, 0.218600571F, -0.119995892F}, 101.0F}, + GeometryKnot{1.2547034F, {-0.336263865F, 0.218603462F, -0.119367279F}, 101.25F}, + GeometryKnot{1.25969732F, {-0.3371526F, 0.218606368F, -0.118734777F}, 101.5F}, + GeometryKnot{1.26468897F, {-0.338038564F, 0.218609303F, -0.118098408F}, 101.75F}, + GeometryKnot{1.26967847F, {-0.338921756F, 0.218612239F, -0.11745818F}, 102.0F}, + GeometryKnot{1.27466571F, {-0.339802116F, 0.218615204F, -0.116814099F}, 102.25F}, + GeometryKnot{1.27965081F, {-0.340679675F, 0.218618184F, -0.116166189F}, 102.5F}, + GeometryKnot{1.28463376F, {-0.341554403F, 0.218621179F, -0.11551445F}, 102.75F}, + GeometryKnot{1.28961456F, {-0.34242627F, 0.218624189F, -0.114858903F}, 103.0F}, + GeometryKnot{1.29459333F, {-0.343295276F, 0.218627229F, -0.114199556F}, 103.25F}, + GeometryKnot{1.29956996F, {-0.344161391F, 0.218630269F, -0.113536432F}, 103.5F}, + GeometryKnot{1.30454445F, {-0.345024616F, 0.218633339F, -0.112869523F}, 103.75F}, + GeometryKnot{1.30951691F, {-0.345884919F, 0.218636423F, -0.112198867F}, 104.0F}, + GeometryKnot{1.31448734F, {-0.346742272F, 0.218639523F, -0.111524455F}, 104.25F}, + GeometryKnot{1.31945574F, {-0.347596675F, 0.218642652F, -0.110846311F}, 104.5F}, + GeometryKnot{1.32442212F, {-0.348448128F, 0.218645781F, -0.110164441F}, 104.75F}, + GeometryKnot{1.32938647F, {-0.3492966F, 0.21864894F, -0.109478869F}, 105.0F}, + GeometryKnot{1.3343488F, {-0.350142062F, 0.218652114F, -0.1087896F}, 105.25F}, + GeometryKnot{1.33930922F, {-0.350984514F, 0.218655288F, -0.108096644F}, 105.5F}, + GeometryKnot{1.34426773F, {-0.351823926F, 0.218658492F, -0.107400022F}, 105.75F}, + GeometryKnot{1.34922421F, {-0.352660298F, 0.218661726F, -0.10669975F}, 106.0F}, + GeometryKnot{1.35417879F, {-0.353493601F, 0.218664959F, -0.105995826F}, 106.25F}, + GeometryKnot{1.35913146F, {-0.354323804F, 0.218668208F, -0.105288275F}, 106.5F}, + GeometryKnot{1.36408222F, {-0.355150938F, 0.218671486F, -0.104577109F}, 106.75F}, + GeometryKnot{1.36903107F, {-0.355974972F, 0.218674764F, -0.103862345F}, 107.0F}, + GeometryKnot{1.37397814F, {-0.356795847F, 0.218678072F, -0.10314399F}, 107.25F}, + GeometryKnot{1.3789233F, {-0.357613593F, 0.218681395F, -0.102422059F}, 107.5F}, + GeometryKnot{1.38386667F, {-0.35842821F, 0.218684733F, -0.101696566F}, 107.75F}, + GeometryKnot{1.38880813F, {-0.359239608F, 0.218688086F, -0.100967526F}, 108.0F}, + GeometryKnot{1.39374781F, {-0.360047847F, 0.218691453F, -0.100234948F}, 108.25F}, + GeometryKnot{1.39868569F, {-0.360852867F, 0.218694836F, -0.0994988605F}, 108.5F}, + GeometryKnot{1.40362191F, {-0.361654669F, 0.218698248F, -0.0987592563F}, 108.75F}, + GeometryKnot{1.40855622F, {-0.362453252F, 0.218701661F, -0.0980161726F}, 109.0F}, + GeometryKnot{1.41348875F, {-0.363248557F, 0.218705088F, -0.0972696021F}, 109.25F}, + GeometryKnot{1.4184196F, {-0.364040613F, 0.218708545F, -0.0965195745F}, 109.5F}, + GeometryKnot{1.42334878F, {-0.364829391F, 0.218712017F, -0.0957660973F}, 109.75F}, + GeometryKnot{1.42827618F, {-0.365614891F, 0.218715489F, -0.0950091779F}, 110.0F}, + }}, + SideGeometry{ + {0.0F, -0.182799995F, 0.000832410005F}, + {3.67316579e-06F, -0.99998939F, 0.00460365694F}, + -0.419999987F, + 0.0599999987F, + { + GeometryKnot{-1.42831016F, {-0.365614623F, -0.218583763F, -0.0930143222F}, 110.0F}, + GeometryKnot{-1.42338276F, {-0.364829153F, -0.21858725F, -0.0937712342F}, 109.75F}, + GeometryKnot{-1.41845369F, {-0.364040375F, -0.218590707F, -0.0945247114F}, 109.5F}, + GeometryKnot{-1.41352284F, {-0.363248318F, -0.218594164F, -0.0952747464F}, 109.25F}, + GeometryKnot{-1.4085902F, {-0.362452984F, -0.218597591F, -0.0960213095F}, 109.0F}, + GeometryKnot{-1.40365589F, {-0.361654431F, -0.218601018F, -0.0967644006F}, 108.75F}, + GeometryKnot{-1.39871979F, {-0.360852599F, -0.218604416F, -0.0975039974F}, 108.5F}, + GeometryKnot{-1.3937819F, {-0.360047579F, -0.218607798F, -0.0982400924F}, 108.25F}, + GeometryKnot{-1.38884223F, {-0.35923937F, -0.218611166F, -0.0989726633F}, 108.0F}, + GeometryKnot{-1.38390064F, {-0.358427942F, -0.218614519F, -0.0997017026F}, 107.75F}, + GeometryKnot{-1.37895739F, {-0.357613355F, -0.218617857F, -0.100427195F}, 107.5F}, + GeometryKnot{-1.37401223F, {-0.356795609F, -0.218621179F, -0.101149127F}, 107.25F}, + GeometryKnot{-1.36906517F, {-0.355974704F, -0.218624488F, -0.101867482F}, 107.0F}, + GeometryKnot{-1.36411631F, {-0.3551507F, -0.218627766F, -0.102582254F}, 106.75F}, + GeometryKnot{-1.35916555F, {-0.354323566F, -0.218631044F, -0.103293419F}, 106.5F}, + GeometryKnot{-1.35421288F, {-0.353493333F, -0.218634292F, -0.104000971F}, 106.25F}, + GeometryKnot{-1.3492583F, {-0.35266003F, -0.218637526F, -0.104704887F}, 106.0F}, + GeometryKnot{-1.34430182F, {-0.351823658F, -0.21864076F, -0.105405167F}, 105.75F}, + GeometryKnot{-1.33934331F, {-0.350984246F, -0.218643963F, -0.106101789F}, 105.5F}, + GeometryKnot{-1.33438289F, {-0.350141793F, -0.218647152F, -0.106794737F}, 105.25F}, + GeometryKnot{-1.32942057F, {-0.349296331F, -0.218650311F, -0.107484013F}, 105.0F}, + GeometryKnot{-1.32445621F, {-0.348447859F, -0.21865347F, -0.108169585F}, 104.75F}, + GeometryKnot{-1.31948984F, {-0.347596437F, -0.2186566F, -0.108851448F}, 104.5F}, + GeometryKnot{-1.31452143F, {-0.346742004F, -0.218659729F, -0.1095296F}, 104.25F}, + GeometryKnot{-1.309551F, {-0.345884651F, -0.218662828F, -0.110204004F}, 104.0F}, + GeometryKnot{-1.30457854F, {-0.345024347F, -0.218665913F, -0.110874668F}, 103.75F}, + GeometryKnot{-1.29960406F, {-0.344161153F, -0.218668982F, -0.111541569F}, 103.5F}, + GeometryKnot{-1.29462743F, {-0.343295038F, -0.218672022F, -0.112204701F}, 103.25F}, + GeometryKnot{-1.28964877F, {-0.342426032F, -0.218675062F, -0.112864047F}, 103.0F}, + GeometryKnot{-1.28466797F, {-0.341554135F, -0.218678072F, -0.113519594F}, 102.75F}, + GeometryKnot{-1.27968502F, {-0.340679437F, -0.218681067F, -0.114171326F}, 102.5F}, + GeometryKnot{-1.27469981F, {-0.339801878F, -0.218684047F, -0.114819244F}, 102.25F}, + GeometryKnot{-1.26971257F, {-0.338921487F, -0.218687013F, -0.115463316F}, 102.0F}, + GeometryKnot{-1.26472318F, {-0.338038296F, -0.218689963F, -0.116103552F}, 101.75F}, + GeometryKnot{-1.25973153F, {-0.337152332F, -0.218692884F, -0.116739921F}, 101.5F}, + GeometryKnot{-1.25473762F, {-0.336263627F, -0.21869579F, -0.117372423F}, 101.25F}, + GeometryKnot{-1.24974155F, {-0.33537212F, -0.21869868F, -0.118001036F}, 101.0F}, + GeometryKnot{-1.24474323F, {-0.334477901F, -0.218701556F, -0.118625753F}, 100.75F}, + GeometryKnot{-1.23974252F, {-0.333580971F, -0.218704402F, -0.119246565F}, 100.5F}, + GeometryKnot{-1.23473966F, {-0.332681328F, -0.218707249F, -0.119863458F}, 100.25F}, + GeometryKnot{-1.22973442F, {-0.331779003F, -0.218710065F, -0.120476417F}, 100.0F}, + GeometryKnot{-1.22472692F, {-0.330874026F, -0.218712866F, -0.121085435F}, 99.75F}, + GeometryKnot{-1.21971703F, {-0.329966396F, -0.218715653F, -0.121690504F}, 99.5F}, + GeometryKnot{-1.21470475F, {-0.329056144F, -0.21871841F, -0.122291602F}, 99.25F}, + GeometryKnot{-1.20969009F, {-0.328143239F, -0.218721151F, -0.122888722F}, 99.0F}, + GeometryKnot{-1.20467305F, {-0.327227771F, -0.218723878F, -0.123481855F}, 98.75F}, + GeometryKnot{-1.19965363F, {-0.326309711F, -0.21872659F, -0.124070987F}, 98.5F}, + GeometryKnot{-1.1946317F, {-0.325389087F, -0.218729287F, -0.124656104F}, 98.25F}, + GeometryKnot{-1.18960738F, {-0.32446593F, -0.218731955F, -0.125237197F}, 98.0F}, + GeometryKnot{-1.18458056F, {-0.323540241F, -0.218734607F, -0.125814259F}, 97.75F}, + GeometryKnot{-1.17955112F, {-0.322612047F, -0.218737245F, -0.126387283F}, 97.5F}, + GeometryKnot{-1.1745193F, {-0.32168135F, -0.218739852F, -0.126956254F}, 97.25F}, + GeometryKnot{-1.16948497F, {-0.32074821F, -0.21874246F, -0.127521142F}, 97.0F}, + GeometryKnot{-1.1644479F, {-0.319812566F, -0.218745038F, -0.128081962F}, 96.75F}, + GeometryKnot{-1.15940845F, {-0.318874538F, -0.218747601F, -0.1286387F}, 96.5F}, + GeometryKnot{-1.15436625F, {-0.317934036F, -0.218750134F, -0.129191339F}, 96.25F}, + GeometryKnot{-1.14932144F, {-0.31699118F, -0.218752652F, -0.129739866F}, 96.0F}, + GeometryKnot{-1.14427412F, {-0.31604591F, -0.218755156F, -0.130284265F}, 95.75F}, + GeometryKnot{-1.13922393F, {-0.315098256F, -0.218757644F, -0.130824551F}, 95.5F}, + GeometryKnot{-1.13417125F, {-0.314148277F, -0.218760103F, -0.13136068F}, 95.25F}, + GeometryKnot{-1.1291157F, {-0.313195974F, -0.218762547F, -0.131892666F}, 95.0F}, + GeometryKnot{-1.12405741F, {-0.312241346F, -0.218764976F, -0.132420495F}, 94.75F}, + GeometryKnot{-1.1189965F, {-0.311284453F, -0.21876739F, -0.132944152F}, 94.5F}, + GeometryKnot{-1.11393273F, {-0.310325265F, -0.218769774F, -0.133463636F}, 94.25F}, + GeometryKnot{-1.1088661F, {-0.309363812F, -0.218772143F, -0.133978918F}, 94.0F}, + GeometryKnot{-1.10379672F, {-0.308400124F, -0.218774498F, -0.134489998F}, 93.75F}, + GeometryKnot{-1.09872448F, {-0.307434201F, -0.218776822F, -0.134996876F}, 93.5F}, + GeometryKnot{-1.09364939F, {-0.306466103F, -0.218779132F, -0.135499537F}, 93.25F}, + GeometryKnot{-1.08857131F, {-0.305495799F, -0.218781427F, -0.135997966F}, 93.0F}, + GeometryKnot{-1.08349037F, {-0.304523349F, -0.218783692F, -0.136492163F}, 92.75F}, + GeometryKnot{-1.07840645F, {-0.303548723F, -0.218785942F, -0.136982098F}, 92.5F}, + GeometryKnot{-1.07331955F, {-0.302571982F, -0.218788177F, -0.137467787F}, 92.25F}, + GeometryKnot{-1.06822968F, {-0.301593155F, -0.218790397F, -0.137949198F}, 92.0F}, + GeometryKnot{-1.06313682F, {-0.300612211F, -0.218792588F, -0.138426349F}, 91.75F}, + GeometryKnot{-1.05804086F, {-0.299629211F, -0.218794763F, -0.138899207F}, 91.5F}, + GeometryKnot{-1.0529418F, {-0.298644125F, -0.218796909F, -0.139367774F}, 91.25F}, + GeometryKnot{-1.04783964F, {-0.297657043F, -0.21879904F, -0.139832035F}, 91.0F}, + GeometryKnot{-1.04273438F, {-0.296667904F, -0.218801156F, -0.140291989F}, 90.75F}, + GeometryKnot{-1.03762603F, {-0.295676798F, -0.218803257F, -0.140747607F}, 90.5F}, + GeometryKnot{-1.03251433F, {-0.294683725F, -0.218805328F, -0.141198918F}, 90.25F}, + GeometryKnot{-1.02739954F, {-0.293688655F, -0.218807384F, -0.141645879F}, 90.0F}, + GeometryKnot{-1.02228153F, {-0.292691678F, -0.218809411F, -0.142088503F}, 89.75F}, + GeometryKnot{-1.01716018F, {-0.291692764F, -0.218811437F, -0.142526761F}, 89.5F}, + GeometryKnot{-1.01203561F, {-0.290691942F, -0.218813419F, -0.142960668F}, 89.25F}, + GeometryKnot{-1.0069077F, {-0.289689243F, -0.218815401F, -0.143390194F}, 89.0F}, + GeometryKnot{-1.00177634F, {-0.288684666F, -0.218817353F, -0.143815339F}, 88.75F}, + GeometryKnot{-0.996641755F, {-0.287678242F, -0.21881929F, -0.144236103F}, 88.5F}, + GeometryKnot{-0.991503656F, {-0.286670029F, -0.218821198F, -0.144652486F}, 88.25F}, + GeometryKnot{-0.986362159F, {-0.285659969F, -0.21882309F, -0.145064443F}, 88.0F}, + GeometryKnot{-0.981217146F, {-0.28464815F, -0.218824968F, -0.145472005F}, 87.75F}, + GeometryKnot{-0.976068676F, {-0.283634543F, -0.218826815F, -0.145875141F}, 87.5F}, + GeometryKnot{-0.970916629F, {-0.282619178F, -0.218828648F, -0.146273851F}, 87.25F}, + GeometryKnot{-0.965761065F, {-0.281602114F, -0.218830466F, -0.146668121F}, 87.0F}, + GeometryKnot{-0.960601926F, {-0.280583322F, -0.218832254F, -0.14705795F}, 86.75F}, + GeometryKnot{-0.955439091F, {-0.279562831F, -0.218834028F, -0.147443339F}, 86.5F}, + GeometryKnot{-0.95027262F, {-0.278540671F, -0.218835771F, -0.147824273F}, 86.25F}, + GeometryKnot{-0.945102513F, {-0.277516872F, -0.2188375F, -0.148200735F}, 86.0F}, + GeometryKnot{-0.939928591F, {-0.276491433F, -0.218839213F, -0.148572728F}, 85.75F}, + GeometryKnot{-0.934750974F, {-0.275464386F, -0.218840897F, -0.14894025F}, 85.5F}, + GeometryKnot{-0.929569542F, {-0.274435729F, -0.218842566F, -0.149303272F}, 85.25F}, + GeometryKnot{-0.924384296F, {-0.273405522F, -0.218844205F, -0.149661809F}, 85.0F}, + GeometryKnot{-0.919195235F, {-0.272373736F, -0.218845844F, -0.150015861F}, 84.75F}, + GeometryKnot{-0.91400218F, {-0.27134043F, -0.218847439F, -0.150365397F}, 84.5F}, + GeometryKnot{-0.908805251F, {-0.270305604F, -0.218849033F, -0.150710419F}, 84.25F}, + GeometryKnot{-0.903604388F, {-0.269269288F, -0.218850598F, -0.151050925F}, 84.0F}, + GeometryKnot{-0.898399472F, {-0.268231481F, -0.218852133F, -0.151386902F}, 83.75F}, + GeometryKnot{-0.893190563F, {-0.267192215F, -0.218853652F, -0.151718348F}, 83.5F}, + GeometryKnot{-0.88797754F, {-0.266151518F, -0.218855157F, -0.152045265F}, 83.25F}, + GeometryKnot{-0.882760406F, {-0.26510942F, -0.218856633F, -0.152367637F}, 83.0F}, + GeometryKnot{-0.877539158F, {-0.264065921F, -0.218858093F, -0.152685449F}, 82.75F}, + GeometryKnot{-0.872313678F, {-0.263021052F, -0.218859538F, -0.152998716F}, 82.5F}, + GeometryKnot{-0.867083967F, {-0.261974812F, -0.218860954F, -0.153307408F}, 82.25F}, + GeometryKnot{-0.861850023F, {-0.26092723F, -0.218862355F, -0.153611541F}, 82.0F}, + GeometryKnot{-0.856611729F, {-0.259878337F, -0.218863726F, -0.153911099F}, 81.75F}, + GeometryKnot{-0.851369143F, {-0.258828163F, -0.218865082F, -0.154206082F}, 81.5F}, + GeometryKnot{-0.846122146F, {-0.257776678F, -0.218866408F, -0.154496476F}, 81.25F}, + GeometryKnot{-0.840870738F, {-0.25672397F, -0.218867719F, -0.15478228F}, 81.0F}, + GeometryKnot{-0.83561486F, {-0.255670011F, -0.218869016F, -0.15506348F}, 80.75F}, + GeometryKnot{-0.830354452F, {-0.25461483F, -0.218870282F, -0.15534009F}, 80.5F}, + GeometryKnot{-0.825089455F, {-0.253558457F, -0.218871534F, -0.155612081F}, 80.25F}, + GeometryKnot{-0.819819927F, {-0.252500892F, -0.218872756F, -0.155879468F}, 80.0F}, + GeometryKnot{-0.814545751F, {-0.251442194F, -0.218873963F, -0.15614225F}, 79.75F}, + GeometryKnot{-0.809266865F, {-0.250382364F, -0.218875155F, -0.156400397F}, 79.5F}, + GeometryKnot{-0.803983271F, {-0.249321386F, -0.218876317F, -0.156653911F}, 79.25F}, + GeometryKnot{-0.798694849F, {-0.248259336F, -0.21887745F, -0.156902805F}, 79.0F}, + GeometryKnot{-0.793401659F, {-0.247196198F, -0.218878567F, -0.157147065F}, 78.75F}, + GeometryKnot{-0.78810358F, {-0.246132016F, -0.21887967F, -0.157386675F}, 78.5F}, + GeometryKnot{-0.782800615F, {-0.245066792F, -0.218880758F, -0.157621637F}, 78.25F}, + GeometryKnot{-0.777492702F, {-0.244000554F, -0.218881801F, -0.157851949F}, 78.0F}, + GeometryKnot{-0.772179723F, {-0.242933318F, -0.218882844F, -0.158077613F}, 77.75F}, + GeometryKnot{-0.766861737F, {-0.241865113F, -0.218883857F, -0.158298612F}, 77.5F}, + GeometryKnot{-0.761538625F, {-0.240795955F, -0.218884856F, -0.158514962F}, 77.25F}, + GeometryKnot{-0.756210387F, {-0.239725858F, -0.218885824F, -0.158726633F}, 77.0F}, + GeometryKnot{-0.750876904F, {-0.238654852F, -0.218886763F, -0.15893364F}, 76.75F}, + GeometryKnot{-0.745538175F, {-0.237582952F, -0.218887702F, -0.159135967F}, 76.5F}, + GeometryKnot{-0.740194142F, {-0.236510172F, -0.218888596F, -0.159333602F}, 76.25F}, + GeometryKnot{-0.734844744F, {-0.235436544F, -0.21888949F, -0.159526572F}, 76.0F}, + GeometryKnot{-0.729489923F, {-0.234362096F, -0.218890354F, -0.159714848F}, 75.75F}, + GeometryKnot{-0.724129617F, {-0.233286828F, -0.218891189F, -0.159898445F}, 75.5F}, + GeometryKnot{-0.718763828F, {-0.232210755F, -0.218892008F, -0.160077333F}, 75.25F}, + GeometryKnot{-0.713392437F, {-0.231133938F, -0.218892813F, -0.160251543F}, 75.0F}, + GeometryKnot{-0.708015442F, {-0.230056345F, -0.218893588F, -0.160421029F}, 74.75F}, + GeometryKnot{-0.702632725F, {-0.228978038F, -0.218894333F, -0.160585836F}, 74.5F}, + GeometryKnot{-0.697244287F, {-0.22789903F, -0.218895078F, -0.160745919F}, 74.25F}, + GeometryKnot{-0.691850007F, {-0.226819322F, -0.218895778F, -0.160901293F}, 74.0F}, + GeometryKnot{-0.686449885F, {-0.225738943F, -0.218896478F, -0.161051959F}, 73.75F}, + GeometryKnot{-0.681043863F, {-0.224657923F, -0.218897134F, -0.161197916F}, 73.5F}, + GeometryKnot{-0.675631821F, {-0.223576277F, -0.21889779F, -0.161339149F}, 73.25F}, + GeometryKnot{-0.670213699F, {-0.222494021F, -0.218898416F, -0.161475658F}, 73.0F}, + GeometryKnot{-0.664789557F, {-0.221411183F, -0.218899012F, -0.161607444F}, 72.75F}, + GeometryKnot{-0.659359217F, {-0.22032778F, -0.218899593F, -0.161734521F}, 72.5F}, + GeometryKnot{-0.653922617F, {-0.219243824F, -0.218900159F, -0.161856845F}, 72.25F}, + GeometryKnot{-0.6484797F, {-0.218159348F, -0.218900695F, -0.16197446F}, 72.0F}, + GeometryKnot{-0.643030465F, {-0.217074379F, -0.218901202F, -0.162087321F}, 71.75F}, + GeometryKnot{-0.637574792F, {-0.215988919F, -0.218901694F, -0.162195459F}, 71.5F}, + GeometryKnot{-0.632112622F, {-0.214902997F, -0.218902171F, -0.162298858F}, 71.25F}, + GeometryKnot{-0.626643836F, {-0.213816643F, -0.218902618F, -0.162397519F}, 71.0F}, + GeometryKnot{-0.621168494F, {-0.212729871F, -0.21890305F, -0.162491441F}, 70.75F}, + GeometryKnot{-0.615686417F, {-0.211642683F, -0.218903452F, -0.162580609F}, 70.5F}, + GeometryKnot{-0.610197544F, {-0.210555136F, -0.21890384F, -0.162665039F}, 70.25F}, + GeometryKnot{-0.604701877F, {-0.209467217F, -0.218904212F, -0.162744731F}, 70.0F}, + GeometryKnot{-0.599199235F, {-0.208378956F, -0.21890454F, -0.162819669F}, 69.75F}, + GeometryKnot{-0.59368962F, {-0.207290396F, -0.218904868F, -0.162889853F}, 69.5F}, + GeometryKnot{-0.588172972F, {-0.206201524F, -0.218905166F, -0.162955284F}, 69.25F}, + GeometryKnot{-0.582649112F, {-0.205112383F, -0.218905434F, -0.163015977F}, 69.0F}, + GeometryKnot{-0.577118099F, {-0.204022989F, -0.218905687F, -0.163071916F}, 68.75F}, + GeometryKnot{-0.571579754F, {-0.202933356F, -0.218905926F, -0.163123086F}, 68.5F}, + GeometryKnot{-0.566034019F, {-0.201843515F, -0.218906134F, -0.163169518F}, 68.25F}, + GeometryKnot{-0.560480833F, {-0.20075348F, -0.218906328F, -0.163211182F}, 68.0F}, + GeometryKnot{-0.554920137F, {-0.199663281F, -0.218906492F, -0.163248092F}, 67.75F}, + GeometryKnot{-0.549351811F, {-0.198572934F, -0.218906626F, -0.163280249F}, 67.5F}, + GeometryKnot{-0.543775737F, {-0.197482437F, -0.21890676F, -0.163307652F}, 67.25F}, + GeometryKnot{-0.538191915F, {-0.196391851F, -0.21890685F, -0.163330287F}, 67.0F}, + GeometryKnot{-0.532600224F, {-0.19530116F, -0.218906939F, -0.163348168F}, 66.75F}, + GeometryKnot{-0.527000546F, {-0.19421041F, -0.218906984F, -0.163361296F}, 66.5F}, + GeometryKnot{-0.521392822F, {-0.193119615F, -0.218907028F, -0.163369656F}, 66.25F}, + GeometryKnot{-0.515776932F, {-0.192028791F, -0.218907028F, -0.163373262F}, 66.0F}, + GeometryKnot{-0.510152817F, {-0.190937966F, -0.218907028F, -0.163372099F}, 65.75F}, + GeometryKnot{-0.504520416F, {-0.189847142F, -0.218906999F, -0.163366184F}, 65.5F}, + GeometryKnot{-0.498879552F, {-0.188756362F, -0.218906939F, -0.163355514F}, 65.25F}, + GeometryKnot{-0.493230194F, {-0.187665641F, -0.218906865F, -0.163340077F}, 65.0F}, + GeometryKnot{-0.487572223F, {-0.18657501F, -0.218906775F, -0.163319871F}, 64.75F}, + GeometryKnot{-0.48190555F, {-0.185484454F, -0.218906656F, -0.163294926F}, 64.5F}, + GeometryKnot{-0.476230055F, {-0.184394032F, -0.218906507F, -0.163265213F}, 64.25F}, + GeometryKnot{-0.47054565F, {-0.183303744F, -0.218906343F, -0.163230747F}, 64.0F}, + GeometryKnot{-0.464852214F, {-0.182213619F, -0.218906164F, -0.163191527F}, 63.75F}, + GeometryKnot{-0.459149688F, {-0.181123689F, -0.218905956F, -0.163147539F}, 63.5F}, + GeometryKnot{-0.453437924F, {-0.180033937F, -0.218905732F, -0.163098797F}, 63.25F}, + GeometryKnot{-0.447716832F, {-0.178944424F, -0.218905479F, -0.163045317F}, 63.0F}, + GeometryKnot{-0.441986322F, {-0.177855149F, -0.21890521F, -0.162987068F}, 62.75F}, + GeometryKnot{-0.436246246F, {-0.176766142F, -0.218904912F, -0.162924066F}, 62.5F}, + GeometryKnot{-0.430496514F, {-0.175677419F, -0.2189046F, -0.162856326F}, 62.25F}, + GeometryKnot{-0.424736977F, {-0.174588993F, -0.218904257F, -0.162783831F}, 62.0F}, + GeometryKnot{-0.418967575F, {-0.17350091F, -0.218903899F, -0.162706584F}, 61.75F}, + GeometryKnot{-0.413188159F, {-0.17241317F, -0.218903512F, -0.162624583F}, 61.5F}, + GeometryKnot{-0.407398611F, {-0.171325788F, -0.218903109F, -0.162537843F}, 61.25F}, + GeometryKnot{-0.401598811F, {-0.170238808F, -0.218902692F, -0.162446365F}, 61.0F}, + GeometryKnot{-0.39578864F, {-0.16915223F, -0.218902245F, -0.162350148F}, 60.75F}, + GeometryKnot{-0.389967948F, {-0.168066069F, -0.218901768F, -0.162249178F}, 60.5F}, + GeometryKnot{-0.384136647F, {-0.166980386F, -0.218901277F, -0.162143484F}, 60.25F}, + GeometryKnot{-0.378294617F, {-0.165895149F, -0.21890077F, -0.162033036F}, 60.0F}, + GeometryKnot{-0.372441679F, {-0.164810419F, -0.218900234F, -0.161917865F}, 59.75F}, + GeometryKnot{-0.366577715F, {-0.163726196F, -0.218899682F, -0.161797956F}, 59.5F}, + GeometryKnot{-0.360702604F, {-0.162642524F, -0.218899101F, -0.161673322F}, 59.25F}, + GeometryKnot{-0.354816169F, {-0.161559388F, -0.218898505F, -0.161543965F}, 59.0F}, + GeometryKnot{-0.348918349F, {-0.160476834F, -0.218897879F, -0.161409885F}, 58.75F}, + GeometryKnot{-0.343008935F, {-0.15939486F, -0.218897238F, -0.16127108F}, 58.5F}, + GeometryKnot{-0.33708778F, {-0.158313513F, -0.218896568F, -0.161127552F}, 58.25F}, + GeometryKnot{-0.331154794F, {-0.157232806F, -0.218895882F, -0.160979301F}, 58.0F}, + GeometryKnot{-0.325209767F, {-0.156152755F, -0.218895182F, -0.16082634F}, 57.75F}, + GeometryKnot{-0.31925258F, {-0.155073375F, -0.218894452F, -0.160668671F}, 57.5F}, + GeometryKnot{-0.313283086F, {-0.153994709F, -0.218893692F, -0.160506293F}, 57.25F}, + GeometryKnot{-0.307301074F, {-0.152916744F, -0.218892917F, -0.160339221F}, 57.0F}, + GeometryKnot{-0.301306456F, {-0.151839525F, -0.218892127F, -0.160167426F}, 56.75F}, + GeometryKnot{-0.295299053F, {-0.150763065F, -0.218891308F, -0.159990951F}, 56.5F}, + GeometryKnot{-0.289278656F, {-0.149687394F, -0.218890473F, -0.159809768F}, 56.25F}, + GeometryKnot{-0.283245116F, {-0.148612514F, -0.218889609F, -0.159623906F}, 56.0F}, + GeometryKnot{-0.277198315F, {-0.147538453F, -0.21888873F, -0.15943335F}, 55.75F}, + GeometryKnot{-0.271138012F, {-0.146465242F, -0.218887836F, -0.1592381F}, 55.5F}, + GeometryKnot{-0.265064061F, {-0.145392895F, -0.218886912F, -0.159038171F}, 55.25F}, + GeometryKnot{-0.258976281F, {-0.144321427F, -0.218885958F, -0.158833578F}, 55.0F}, + GeometryKnot{-0.252874464F, {-0.143250853F, -0.21888499F, -0.158624306F}, 54.75F}, + GeometryKnot{-0.246758476F, {-0.142181218F, -0.218884006F, -0.158410355F}, 54.5F}, + GeometryKnot{-0.240628093F, {-0.141112506F, -0.218882993F, -0.158191755F}, 54.25F}, + GeometryKnot{-0.234483123F, {-0.140044779F, -0.218881965F, -0.157968476F}, 54.0F}, + GeometryKnot{-0.228323385F, {-0.138978019F, -0.218880907F, -0.157740563F}, 53.75F}, + GeometryKnot{-0.222148672F, {-0.137912273F, -0.218879834F, -0.157507986F}, 53.5F}, + GeometryKnot{-0.215958774F, {-0.136847556F, -0.218878731F, -0.157270759F}, 53.25F}, + GeometryKnot{-0.209753484F, {-0.135783881F, -0.218877628F, -0.157028884F}, 53.0F}, + GeometryKnot{-0.203532621F, {-0.134721264F, -0.218876481F, -0.156782374F}, 52.75F}, + GeometryKnot{-0.197295934F, {-0.133659735F, -0.218875319F, -0.15653123F}, 52.5F}, + GeometryKnot{-0.191043228F, {-0.132599324F, -0.218874142F, -0.156275466F}, 52.25F}, + GeometryKnot{-0.18477428F, {-0.13154003F, -0.218872935F, -0.156015068F}, 52.0F}, + GeometryKnot{-0.178488851F, {-0.130481884F, -0.218871713F, -0.155750051F}, 51.75F}, + GeometryKnot{-0.172186732F, {-0.1294249F, -0.218870476F, -0.15548043F}, 51.5F}, + GeometryKnot{-0.165867671F, {-0.128369108F, -0.218869209F, -0.155206189F}, 51.25F}, + GeometryKnot{-0.159531444F, {-0.127314523F, -0.218867913F, -0.154927343F}, 51.0F}, + GeometryKnot{-0.153177798F, {-0.12626116F, -0.218866602F, -0.154643893F}, 50.75F}, + GeometryKnot{-0.146806508F, {-0.125209048F, -0.218865275F, -0.154355869F}, 50.5F}, + GeometryKnot{-0.140417293F, {-0.124158196F, -0.218863919F, -0.15406324F}, 50.25F}, + GeometryKnot{-0.134009898F, {-0.12310864F, -0.218862548F, -0.153766036F}, 50.0F}, + GeometryKnot{-0.127584085F, {-0.122060381F, -0.218861163F, -0.153464258F}, 49.75F}, + GeometryKnot{-0.121139579F, {-0.121013455F, -0.218859747F, -0.153157905F}, 49.5F}, + GeometryKnot{-0.114676103F, {-0.119967878F, -0.218858317F, -0.152846977F}, 49.25F}, + GeometryKnot{-0.10819339F, {-0.118923664F, -0.218856856F, -0.152531505F}, 49.0F}, + GeometryKnot{-0.101691149F, {-0.117880836F, -0.218855381F, -0.152211472F}, 48.75F}, + GeometryKnot{-0.0951690972F, {-0.116839416F, -0.218853876F, -0.151886895F}, 48.5F}, + GeometryKnot{-0.0886269435F, {-0.11579942F, -0.218852356F, -0.151557773F}, 48.25F}, + GeometryKnot{-0.0820643976F, {-0.114760876F, -0.218850821F, -0.151224121F}, 48.0F}, + GeometryKnot{-0.0754811391F, {-0.113723792F, -0.218849257F, -0.15088594F}, 47.75F}, + GeometryKnot{-0.06887687F, {-0.112688191F, -0.218847677F, -0.150543243F}, 47.5F}, + GeometryKnot{-0.0622512735F, {-0.111654103F, -0.218846083F, -0.150196016F}, 47.25F}, + GeometryKnot{-0.0556040257F, {-0.110621534F, -0.218844458F, -0.149844289F}, 47.0F}, + GeometryKnot{-0.048934795F, {-0.109590508F, -0.218842804F, -0.149488062F}, 46.75F}, + GeometryKnot{-0.0422432534F, {-0.108561054F, -0.21884115F, -0.149127334F}, 46.5F}, + GeometryKnot{-0.0355290584F, {-0.107533179F, -0.218839467F, -0.148762122F}, 46.25F}, + GeometryKnot{-0.028791856F, {-0.106506906F, -0.218837753F, -0.148392424F}, 46.0F}, + GeometryKnot{-0.0220312942F, {-0.105482258F, -0.218836024F, -0.148018256F}, 45.75F}, + GeometryKnot{-0.0152470125F, {-0.104459256F, -0.218834281F, -0.147639617F}, 45.5F}, + GeometryKnot{ + -0.00843863748F, {-0.103437908F, -0.218832508F, -0.147256523F}, 45.25F}, + GeometryKnot{-0.0016057929F, {-0.102418244F, -0.21883072F, -0.146868974F}, 45.0F}, + GeometryKnot{0.00525190681F, {-0.101400286F, -0.218828917F, -0.146476984F}, 44.75F}, + GeometryKnot{0.0121348547F, {-0.100384042F, -0.218827084F, -0.146080554F}, 44.5F}, + GeometryKnot{0.019043453F, {-0.0993695408F, -0.218825236F, -0.145679697F}, 44.25F}, + GeometryKnot{0.0259781107F, {-0.0983567908F, -0.218823373F, -0.145274401F}, 44.0F}, + GeometryKnot{0.0329392478F, {-0.097345829F, -0.218821481F, -0.144864708F}, 43.75F}, + GeometryKnot{0.0399272889F, {-0.0963366628F, -0.218819574F, -0.14445059F}, 43.5F}, + GeometryKnot{0.0469426699F, {-0.095329307F, -0.218817636F, -0.144032091F}, 43.25F}, + GeometryKnot{0.0539858416F, {-0.0943237916F, -0.218815684F, -0.143609196F}, 43.0F}, + GeometryKnot{0.0610572547F, {-0.0933201313F, -0.218813717F, -0.143181905F}, 42.75F}, + GeometryKnot{0.0681573749F, {-0.0923183411F, -0.218811721F, -0.142750248F}, 42.5F}, + GeometryKnot{0.075286679F, {-0.0913184509F, -0.218809709F, -0.142314225F}, 42.25F}, + GeometryKnot{0.0824456513F, {-0.0903204679F, -0.218807682F, -0.141873851F}, 42.0F}, + GeometryKnot{0.089634791F, {-0.0893244147F, -0.218805641F, -0.141429111F}, 41.75F}, + GeometryKnot{0.0968545973F, {-0.0883303136F, -0.21880357F, -0.140980035F}, 41.5F}, + GeometryKnot{0.104105592F, {-0.0873381793F, -0.218801469F, -0.140526637F}, 41.25F}, + GeometryKnot{0.111388296F, {-0.0863480344F, -0.218799368F, -0.140068904F}, 41.0F}, + GeometryKnot{0.118703268F, {-0.0853598937F, -0.218797237F, -0.139606848F}, 40.75F}, + GeometryKnot{0.126051053F, {-0.084373787F, -0.218795076F, -0.139140502F}, 40.5F}, + GeometryKnot{0.133432209F, {-0.0833897144F, -0.218792915F, -0.138669848F}, 40.25F}, + GeometryKnot{0.140847325F, {-0.0824077129F, -0.218790725F, -0.138194904F}, 40.0F}, + }}, + }}; + return table; +} +} // namespace rmcs_core::controller::chassis::standing_reference diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/recovery_geometry.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/recovery_geometry.hpp new file mode 100644 index 000000000..b39cc2fc1 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/recovery_geometry.hpp @@ -0,0 +1,820 @@ +#pragma once + +// Generated by export_traditional_recovery_geometry.py; do not hand edit. +// Model SHA-256: 8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812 +// Spring SHA-256: 698f67ccc7d6364b73f1b426ed3e6386ffc9e11912ef95a89d716eb7ee0e06b9 + +namespace rmcs_core::controller::chassis::wheel_leg::recovery_v202 { +inline constexpr double stand_knee = 63.67236314439243; +inline constexpr double stand_orbit = -0.6558605551087706; +inline const std::vector geometry = { + {41, -0.12928019621032033, -0.24070847479676338, 0.12927802462707638, 0.24067155027239368}, + {43, -0.15797928971948932, -0.21200407980934699, 0.15797733443883416, 0.21196826965624821}, + {45, -0.18577325273483644, -0.18420550330555688, 0.1857714960630793, 0.18417067069221504}, + {47, -0.21277075401954959, -0.15720397712854989, 0.21276918029719408, 0.15717000704835057}, + {49, -0.23906402917274674, -0.13090718339997484, 0.23906262453153804, 0.13087397863936928}, + {51, -0.26473183216453117, -0.10523629968024142, 0.26473058431080271, 0.10520377763603225}, + {53, -0.28984179512237618, -0.080123662903241563, 0.28984069497974757, 0.080091754884691141}, + {55, -0.31445225192544762, -0.055510832062070414, 0.31445128757575108, 0.055479475370782336}, + {57, -0.33861384294415109, -0.031347159709086414, 0.33861300579926529, 0.031316302320513488}, + {59, -0.36237068263203559, -0.0075884968631057571, 0.36236996504995667, 0.0075580938412039217}, + {61, -0.38576138190123749, 0.015803796746567303, 0.38576077707890843, -0.01583378433112799}, + {63, -0.40881987465100406, 0.038863680063730283, 0.40881937653183997, -0.0388932860212826}, + {65, -0.43157610104799687, 0.0616211146795098, 0.4315757042421336, -0.061650368433541151}, + {67, -0.45405657543043043, 0.084102633302109445, 0.45405627514596258, -0.084131560495992616}, + {69, -0.47628486083223587, 0.10633181474391788, 0.4762846528159117, -0.10636043774572494}, + {71, -0.49828196731717955, 0.12832968263577069, 0.49828184780332274, -0.12835802095997531}, + {73, -0.52006668800248368, 0.15011504176373364, 0.5200666536689994, -0.15014311242452727}, + {75, -0.54165588380539353, 0.17170476307417762, 0.54165593173603666, -0.17173258088231363}, + {77, -0.56306472563933674, 0.19311402608353928, 0.56306485329132383, -0.19314160389614948}, + {79, -0.5843069013897142, 0.21435652603065622, 0.58430710656614815, -0.21438387496172301}, + {81, -0.6053947933929692, 0.23544465150139829, 0.60539507421948657, -0.23547178109905462}, + {83, -0.6263396312116466, 0.25638963732323738, 0.62633998611684727, -0.25641655571890509}, + {85, -0.64715162360520739, 0.27720169663335847, 0.64715205130450049, -0.27722841066676429}, + {87, -0.66784007292131597, 0.2978901353361848, 0.66784057239194961, -0.2979166506714947}, + {89, -0.68841347453443036, 0.31846345165547241, 0.68841404504612991, -0.31848977282653412}, + {91, -0.70887960365119285, 0.33892942289388717, 0.70888024470703803, -0.33895555342590078}, + {93, -0.72924559117369536, 0.35929518140820688, 0.72924630253396816, -0.35932112384466658}, + {95, -0.74951799031281752, 0.37956728120704325, 0.74951877198684191, -0.37959303715815201}, + {97, -0.76970283517709936, 0.39975175655130124, 0.7697036874234362, -0.39977732672447047}, + {99, -0.78980569245954357, 0.41985417363353417, 0.78980661578872602, -0.41987955785247849}, + {101, -0.80983170715681074, 0.43987967626283714, 0.80983270232379212, -0.43990487348858848}, + {103, -0.82978564307354274, 0.45983302638298984, 0.82978671112315738, -0.45985803467381786}, + {105, -0.84967191887944438, 0.47971864000234155, 0.84967306112136132, -0.47974345653616923}, + {107, -0.86949464017256095, 0.4995406192315448, 0.86949585820722786, -0.49956524026927063}, + {109, -0.88925762813321751, 0.51930278082896497, 0.88925892386973604, -0.51932720167742075}, +}; + +inline constexpr std::array, 281> left = {{ + {-0.14089326779459421, 0.072795528285080402, -0.01974694901368379, 0.019747100415708929}, + {-0.13347798989671683, 0.072648306993020745, -0.019960416024254579, 0.019960568367004605}, + {-0.12609666995839292, 0.072500185445195597, -0.020173545023556144, 0.020173698241898186}, + {-0.11874872965038757, 0.072351168963446191, -0.020386314289125324, 0.02038646835743077}, + {-0.11143360339886205, 0.072201262884494855, -0.020598702338358005, 0.020598857326022235}, + {-0.10415073804344993, 0.072050472559523654, -0.020810688186742532, 0.020810844013580909}, + {-0.096899592505971133, 0.071898803353756369, -0.021022251084427068, 0.02102240778160111}, + {-0.089679637469451678, 0.071746260646042656, -0.021233370716555011, 0.021233528258665785}, + {-0.082490355067071863, 0.071592849828444896, -0.021444027078137473, 0.021444185457811876}, + {-0.075331238580673865, 0.071438576305827992, -0.021654200516255884, 0.021654359731569539}, + {-0.068201792148537332, 0.071283445495451556, -0.021863871773341481, 0.021864031780079812}, + {-0.061101530482088939, 0.071127462826565402, -0.022073021786864488, 0.022073182622457943}, + {-0.054029978591209227, 0.070970633740007144, -0.022281632028772536, 0.022281793645372777}, + {-0.046986671517871503, 0.070812963687803626, -0.022489684182666836, 0.022489846586270096}, + {-0.039971154077842797, 0.070654458132774225, -0.022697160278930706, 0.022697323434194311}, + {-0.032982980610105073, 0.07049512254813807, -0.022904042733160795, 0.022904206688953072}, + {-0.026021714733816592, 0.070334962417123462, -0.023110314233151769, 0.02311047895883217}, + {-0.019086929112498929, 0.070173983232580964, -0.023315957844147472, 0.023316123313915527}, + {-0.012178205225225336, 0.070012190496599014, -0.023520956966075063, 0.023521123181450193}, + {-0.0052951331445780792, 0.06984958972012309, -0.023725295203312402, 0.023725462156892918}, + {0.0015626886788435157, 0.069686186422577734, -0.023928956654486744, 0.023929124282281999}, + {0.0083956536256200587, 0.069521986131491589, -0.024131925499820572, 0.024132093850413178}, + {0.015204147110577104, 0.069356994382126128, -0.024334186488631004, 0.024334355567609794}, + {0.021988546779951545, 0.069191216717106929, -0.024535724512859403, 0.024535894300523442}, + {0.02874922270283875, 0.069024658686058851, -0.024736524826208139, 0.024736695296286859}, + {0.035486537557127773, 0.068857325845243914, -0.024936573037941667, 0.0249367442055341}, + {0.042200846810079962, 0.068689223757202683, -0.025135854870679125, 0.025136026700773584}, + {0.048892498893744474, 0.068520357990399211, -0.025334356549856885, 0.025334529059231198}, + {0.055561835375373458, 0.068350734118868689, -0.025532064510706082, 0.025532237684129877}, + {0.062209191122992002, 0.068180357721868937, -0.025728965454425268, 0.025729139233194892}, + {0.068834894466298147, 0.06800923438353515, -0.0259250463853197, 0.025925220849277869}, + {0.075439267353018868, 0.06783736969253798, -0.026120294598453192, 0.026120469682418879}, + {0.082022625500885293, 0.067664769241744874, -0.026314697735926984, 0.026314873420019452}, + {0.088585278545361745, 0.06749143862788505, -0.02650824351759215, 0.026508419849011644}, + {0.095127530183259756, 0.06731738345121753, -0.026700920203875683, 0.026701097139810337}, + {0.10164967831237998, 0.067142609315203272, -0.026892716081749091, 0.026892893609942847}, + {0.10815201516728151, 0.066967121826179851, -0.02708361985050875, 0.027083797975392898}, + {0.11463482745134373, 0.066790926593040312, -0.027273620430141338, 0.027273799154122091}, + {0.12109839646518455, 0.066614029226915131, -0.027462707015945748, 0.027462886284493365}, + {0.1275429982315906, 0.066436435340857497, -0.0276508690310385, 0.027651048875331513}, + {0.13396890361705122, 0.066258150549532768, -0.02783809614780125, 0.027838276546084922}, + {0.14037637844999656, 0.066079180468910328, -0.028024378337957845, 0.028024559299152096}, + {0.14676568363585313, 0.065899530715959914, -0.028209705754999453, 0.028209887263552558}, + {0.15313707526901271, 0.065719206908350744, -0.028394068860858795, 0.028394250913993252}, + {0.15949080474179439, 0.065538214664154451, -0.028577458369012848, 0.028577640955811889}, + {0.16582711885051621, 0.06535655960155147, -0.028759865038811311, 0.028760048148751743}, + {0.17214625989874427, 0.065174247338540922, -0.028941280156959431, 0.02894146377499332}, + {0.17844846579781376, 0.064991283492653637, -0.029121694986855989, 0.029121879130425882}, + {0.184733970164705, 0.064807673680669156, -0.029301101228217108, 0.029301285868888943}, + {0.19100300241734652, 0.064623423518336179, -0.029479490604044339, 0.029479675764146923}, + {0.19725578786743575, 0.064438538620095701, -0.029656855268104632, 0.029657040893394243}, + {0.20349254781083298, 0.064253024598808897, -0.029833187350067597, 0.02983337345611764}, + {0.2097134996156223, 0.064066887065487244, -0.030008479366861934, 0.030008665960432807}, + {0.21591885680789019, 0.063880131629026865, -0.030182724089199808, 0.03018291116194699}, + {0.22210882915530167, 0.063692763895946122, -0.030355914272643447, 0.030356101771245175}, + {0.22828362274852881, 0.06350478947012661, -0.030528043093083573, 0.030528231057155714}, + {0.23444344008060308, 0.063316213952557446, -0.030699103820108153, 0.030699292265500139}, + {0.24058848012425499, 0.063127042941083428, -0.030869090011297784, 0.030869278885241574}, + {0.24671893840727593, 0.062937282030155819, -0.031037995273397068, 0.031038184569737504}, + {0.25283500708599943, 0.062746936810587503, -0.031205813544466887, 0.031206003271135985}, + {0.25893687501691781, 0.062556012869310951, -0.031372538932458782, 0.031372729096874052}, + {0.26502472782651371, 0.062364515789139333, -0.031538165604964086, 0.031538356175586924}, + {0.27109874797934258, 0.062172451148531954, -0.031702688067135609, 0.031702879073571943}, + {0.27715911484442451, 0.061979824521361755, -0.031866100963510946, 0.031866292349452985}, + {0.2832060047599923, 0.061786641476687311, -0.03202839910818539, 0.032028590885637198}, + {0.28923959109663894, 0.061592907578527346, -0.032189577425645766, 0.032189769624818509}, + {0.29526004431891312, 0.061398628385639156, -0.032349631199462991, 0.032349823782062082}, + {0.30126753204540618, 0.061203809451299933, -0.03250855557972665, 0.032508748532003195}, + {0.30726221910737223, 0.061008456323091251, -0.032666346183436112, 0.03266653951231907}, + {0.31324426760591689, 0.060812574542687625, -0.032822998655858526, 0.032823192355489195}, + {0.31921383696781375, 0.060616169645646902, -0.032978508718181476, 0.032978702790327608}, + {0.32517108399995914, 0.060419247161205739, -0.033132872442640016, 0.033133066881888884}, + {0.33111616294252494, 0.060221812612076181, -0.033286086018322114, 0.033286280810966518}, + {0.33704922552083277, 0.06002387151424745, -0.033438145699248356, 0.03343834083148528}, + {0.34297042099599667, 0.059825429376789294, -0.033589047846949438, 0.033589243312681427}, + {0.34887989621435161, 0.059626491701659595, -0.033738789097860267, 0.033738984889593926}, + {0.35477779565571904, 0.059427063983514432, -0.033887366241432959, 0.033887562382614755}, + {0.36066426148052483, 0.0592271517095216, -0.03403477611728023, 0.034034972575566202}, + {0.36653943357581237, 0.059026760359176964, -0.034181015747292209, 0.034181212542136352}, + {0.37240344960018007, 0.058825895404124327, -0.034326082298148763, 0.034326279386808767}, + {0.37825644502765804, 0.058624562307977773, -0.034469973142643717, 0.034470170550444823}, + {0.38409855319057434, 0.058422766526147363, -0.03461268560136338, 0.034612883327047483}, + {0.38992990532142097, 0.058220513505668127, -0.034754217290792977, 0.034754415321793852}, + {0.39575063059375398, 0.05801780868503105, -0.034894565940225275, 0.034894764239482401}, + {0.40156085616214704, 0.057814657494018276, -0.03503372931001919, 0.035033927940266274}, + {0.40736070720124001, 0.057611065353540297, -0.035171705448888313, 0.035171904338383853}, + {0.4131503069438725, 0.057407037675476467, -0.035308492379018008, 0.035308691557174809}, + {0.41892977671837622, 0.057202579862518456, -0.035444088269723228, 0.035444287757987747}, + {0.42469923598499343, 0.056997697308016092, -0.035578491508343524, 0.035578691227958814}, + {0.43045880237149153, 0.056792395395827006, -0.035711700465431095, 0.035711900476218861}, + {0.43620859170796128, 0.056586679500167769, -0.035843713674373889, 0.035843913939326233}, + {0.44194871806084007, 0.056380554985469206, -0.035974529877171646, 0.035974730395279764}, + {0.44767929376617083, 0.056174027206233115, -0.03610414774041687, 0.03610434852883454}, + {0.4534004294621154, 0.055967101506893213, -0.03623256627180349, 0.036232767283625456}, + {0.45911223412075247, 0.055759783221677191, -0.036359784294300257, 0.036359985575213083}, + {0.46481481507916128, 0.055552077674473045, -0.036485801029562559, 0.036486002581040788}, + {0.4705082780698277, 0.05534399017869672, -0.036610615645340183, 0.036610817362391579}, + {0.47619272725037393, 0.055135526037163179, -0.03673422733447701, 0.036734429349538522}, + {0.48186826523263576, 0.054926690541960241, -0.036856635668703508, 0.036856837894684913}, + {0.4875349931111142, 0.054717488974324033, -0.036977839996291118, 0.03697804245405395}, + {0.49319301049079095, 0.054507926604518198, -0.03709783998197852, 0.03709804266030825}, + {0.49884241551435443, 0.054298008691714819, -0.037216635244778196, 0.037216838133808938}, + {0.50448330488882931, 0.054087740483878079, -0.037334225590723524, 0.037334428672331832}, + {0.51011577391163043, 0.053877127217650617, -0.03745061083963197, 0.037450814166237578}, + {0.51573991649606121, 0.05366617411824226, -0.037565790987116376, 0.037565994500125821}, + {0.52135582519626289, 0.053454886399320903, -0.037679766043299701, 0.037679969759291554}, + {0.52696359123163061, 0.053243269262905971, -0.037792536160930146, 0.037792740086595333}, + {0.53256330451071188, 0.053031327899264465, -0.03790410152377427, 0.03790430566444819}, + {0.53815505365459648, 0.052819067486809618, -0.038014462353729239, 0.038014666645756479}, + {0.54373892601981022, 0.052606493192000336, -0.038123619089797056, 0.038123823596776407}, + {0.54931500772072983, 0.052393610169245541, -0.038231572188673235, 0.038231776857400757}, + {0.55488338365152834, 0.052180423560808042, -0.038338322083169674, 0.038338526967978848}, + {0.56044413750765365, 0.051966938496712668, -0.038443869460336122, 0.038444074490149249}, + {0.56599735180686217, 0.051753160094656178, -0.038548214960697047, 0.038548420174945142}, + {0.57154310790982343, 0.051539093459918098, -0.038651359300783728, 0.03865156469212977}, + {0.57708148604027976, 0.051324743685275835, -0.038753303313278475, 0.038753508873542009}, + {0.58261256530481087, 0.051110115850920082, -0.038854047827516947, 0.03885425352703703}, + {0.58813642371217223, 0.050895215024373719, -0.038953593863930047, 0.038953799742317441}, + {0.5936531381922483, 0.050680046260411657, -0.039051942369723924, 0.039052148393501462}, + {0.59916278461461325, 0.050464614600983865, -0.039149094525704253, 0.039149300709067691}, + {0.604665437806714, 0.050248925075139519, -0.039245051348704707, 0.039245257704883296}, + {0.6101611715716837, 0.050032982698953607, -0.0393398141884157, 0.039340020678765242}, + {0.61565005870579248, 0.049816792475455521, -0.039433384175209085, 0.039433590782709749}, + {0.62113217101555085, 0.049600359394559604, -0.03952576270311331, 0.039525969464876687}, + {0.62660757933446276, 0.049383688432997709, -0.039616951154548996, 0.039617158083062654}, + {0.63207635353944003, 0.049166784554253246, -0.039706950965687993, 0.039707158028885303}, + {0.63753856256690566, 0.048949652708497771, -0.039795763632842041, 0.039795970824713367}, + {0.64299427442854662, 0.048732297832528811, -0.039883390715416744, 0.039883598022195044}, + {0.6484435562267814, 0.048514724849710394, -0.039969833795173493, 0.039970041252137822}, + {0.65388647416990864, 0.048296938669914323, -0.040055094626822219, 0.040055302188632791}, + {0.65932309358695185, 0.048078944189463915, -0.040139174802027611, 0.040139382466283624}, + {0.66475347894221959, 0.047860746291079587, -0.040222076108791416, 0.040222283926755475}, + {0.67017769384957893, 0.047642349843825635, -0.040303800408535762, 0.040304008347085751}, + {0.67559580108644557, 0.047423759703059412, -0.040384349527946228, 0.040384557567147539}, + {0.68100786260750534, 0.04720498071038158, -0.040463725309720977, 0.040463933515441863}, + {0.68641393955816521, 0.046986017693588657, -0.040541929862924375, 0.040542138140072737}, + {0.6918140922877416, 0.046766875466626529, -0.040618965050931266, 0.04061917344131355}, + {0.69720838036240385, 0.046547558829546018, -0.040694832939778602, 0.040695041420072353}, + {0.70259686257785448, 0.046328072568460166, -0.040769535599506079, 0.040769744177042211}, + {0.70797959697177659, 0.046108421455502847, -0.040843075211779323, 0.040843283874514524}, + {0.713356640836037, 0.045888610248788729, -0.040915453933365376, 0.040915662714278545}, + {0.71872805072865731, 0.045668643692375367, -0.04098667390549865, 0.040986882766290811}, + {0.7240938824855585, 0.045448526516226148, -0.041056737376074348, 0.041056946351043899}, + {0.72945419123208111, 0.045228263436175684, -0.041125646760486578, 0.041125855829079794}, + {0.73480903139429299, 0.045007859153894936, -0.041193404115758336, 0.041193613265126106}, + {0.74015845671006519, 0.044787318356860006, -0.041260012084225584, 0.04126022132843471}, + {0.74550252023997077, 0.044566645718320586, -0.041325472842150157, 0.041325682145079978}, + {0.75084127437794579, 0.04434584589727035, -0.041389788876953271, 0.041389998276851803}, + {0.7561747708617772, 0.044124923538419417, -0.041452962664242532, 0.0414531721519108}, + {0.7615030607833746, 0.043903883272166835, -0.041514996762563039, 0.041515206324923282}, + {0.76682619459887458, 0.043682729714575583, -0.041575893563667088, 0.041576103198573522}, + {0.77214422213852774, 0.043461467467348393, -0.041635655659335025, 0.041635865361911828}, + {0.77745719261643154, 0.043240101117804519, -0.04169428567302691, 0.041694495441968293}, + {0.78276515464006513, 0.043018635238858682, -0.041751786153220727, 0.041751996020681692}, + {0.78806815621966075, 0.042797074389000309, -0.041808159811494605, 0.041808369743455309}, + {0.79336624477739082, 0.04257542311227451, -0.041863409187297251, 0.041863619160778465}, + {0.79865946715640224, 0.042353685938264357, -0.041917537171844876, 0.041917747216542678}, + {0.80394786962967468, 0.042131867382073862, -0.041970546274862131, 0.041970756391756348}, + {0.80923149790872007, 0.041909971944312674, -0.042022439379057586, 0.042022649520310651}, + {0.81451039715212836, 0.041688004111081631, -0.042073219243027815, 0.042073429491929548}, + {0.8197846119739618, 0.041465968353959201, -0.042122888627521338, 0.042123098884367161}, + {0.82505418645198403, 0.041243869129989619, -0.042171450387905318, 0.042171660729135085}, + {0.8303191641357559, 0.041021710881671669, -0.042218907241819092, 0.042219117667153527}, + {0.83557958805458443, 0.040799498036948739, -0.042265262163675821, 0.042265472653354397}, + {0.84083550072531743, 0.040577235009199839, -0.042310518129338393, 0.042310728605943593}, + {0.84608694416002073, 0.040354926197231356, -0.042354677830226783, 0.042354888363629495}, + {0.8513339598734968, 0.040132575985270647, -0.042397744332585086, 0.042397954982744179}, + {0.85657658889068711, 0.0399101887429597, -0.042439720569605377, 0.042439931220499605}, + {0.86181487175393379, 0.03968776882535039, -0.042480609476143572, 0.042480820166178601}, + {0.86704884853012043, 0.039465320572900323, -0.042520414040601306, 0.042520624753858594}, + {0.87227855881768335, 0.039242848311469944, -0.042559137280574311, 0.042559348064557523}, + {0.87750404175349761, 0.039020356352320325, -0.042596782189850413, 0.042596993006779771}, + {0.88272533601965697, 0.038797848992111854, -0.042633351771030059, 0.042633562637742539}, + {0.88794247985011099, 0.038575330512904141, -0.042668849155491129, 0.042669060067023146}, + {0.89315551103721424, 0.038352805182156106, -0.042703277408084606, 0.042703488340093076}, + {0.89836446693814387, 0.038130277252727898, -0.042736639574177115, 0.042736850534225379}, + {0.90356938448121182, 0.037907750962882902, -0.042768938701613968, 0.042769149688378083}, + {0.90877030017207261, 0.037685230536290359, -0.04280017800384095, 0.042800389028249952}, + {0.91396725009981528, 0.037462720182030021, -0.042830360547373214, 0.042830571604946355}, + {0.91916026994296196, 0.037240224094596271, -0.042859489459649514, 0.042859700528750655}, + {0.92434939497535606, 0.037017746453903405, -0.042887567876241595, 0.042887778965944146}, + {0.92953466007195495, 0.036795291425292434, -0.042914599026733835, 0.042914810178968921}, + {0.93471609971451874, 0.036572863159537373, -0.04294058603031764, 0.042940797222241867}, + {0.93989374799721093, 0.036350465792853157, -0.042965532173728394, 0.042965743352460339}, + {0.94506763863209886, 0.036128103446903953, -0.04298944046417108, 0.042989651680960472}, + {0.95023780495456012, 0.035905780228812391, -0.04301231428308927, 0.043012525490154441}, + {0.95540427992860932, 0.035683500231168966, -0.043034156782982447, 0.043034368036973469}, + {0.96056709615211333, 0.035461267532042672, -0.043054971135463527, 0.043055182394935387}, + {0.96572628586195064, 0.035239086194992116, -0.043074760654249811, 0.043074971956068964}, + {0.9708818809390527, 0.035016960269076899, -0.04309352858196408, 0.043093739886712779}, + {0.9760339129133857, 0.034794893788870462, -0.043111278129532971, 0.043111489467083375}, + {0.98118241296883912, 0.034572890774472433, -0.043128012540550376, 0.043128223893534379}, + {0.98632741194803453, 0.034350955231522776, -0.043143735236785774, 0.043143946560314395}, + {0.99146894035705668, 0.034129091151215284, -0.043158449214773242, 0.043158660597409471}, + {0.99660702837010184, 0.033907302510312889, -0.043172158020882923, 0.043172369373912056}, + {1.0017417058340636, 0.033685593271162467, -0.043184864791971644, 0.04318507618987677}, + {1.0068730022730248, 0.033463967381710799, -0.043196572864627192, 0.043196784263133892}, + {1.0120009468926929, 0.033242428775521048, -0.043207285524183958, 0.043207496939823453}, + {1.0171255685847567, 0.03302098137178943, -0.043217006150190695, 0.043217217590484469}, + {1.0222468959311624, 0.032799629075362532, -0.043225737989627588, 0.043225949442992351}, + {1.0273649572083423, 0.032578375776755431, -0.043233484376423492, 0.043233695826878082}, + {1.0324797803913588, 0.032357225352169901, -0.043240248594235031, 0.0432404600598289}, + {1.0375913931579839, 0.032136181663513147, -0.0432460340123349, 0.043246245447581155}, + {1.0426998228927191, 0.031915248558417375, -0.043250843921073145, 0.043251055376068992}, + {1.0478050966907428, 0.031694429870259007, -0.043254681718313093, 0.043254893197704702}, + {1.0529072413618104, 0.031473729418179552, -0.043257550639332287, 0.043257762116223897}, + {1.0580062834340653, 0.031253151007105621, -0.043259454168762479, 0.043259665621869947}, + {1.0631022491578233, 0.031032698427770261, -0.043260395474957858, 0.043260606920779543}, + {1.0681951645092613, 0.030812375456734271, -0.043260378017931034, 0.043260589493207935}, + {1.0732850551940822, 0.030592185856408063, -0.043259405101162052, 0.043259616571594606}, + {1.0783719466510946, 0.030372133375073766, -0.043257480071037191, 0.043257691537095577}, + {1.0834558640557546, 0.030152221746908028, -0.043254606360482784, 0.043254817836192339}, + {1.0885368323236375, 0.029932454692004687, -0.043250787157327053, 0.043250998610924463}, + {1.0936148761138722, 0.029712835916398325, -0.043246025980295477, 0.043246237452462477}, + {1.0986900198325009, 0.02949336911208756, -0.043240326104370064, 0.043240537627406261}, + {1.1037622876358104, 0.029274057957059514, -0.043233690874361019, 0.043233902329489593}, + {1.1088317034335904, 0.029054906115313743, -0.04322612367251899, 0.043226335125687743}, + {1.1138982908923538, 0.028835917236886853, -0.043217627825117622, 0.043217839302215955}, + {1.1189620734385055, 0.028617094957877891, -0.043208206685437234, 0.043208418168515839}, + {1.1240230742614576, 0.028398442900472692, -0.043197863730689179, 0.04319807517514257}, + {1.1290813163167048, 0.028179964672970216, -0.043186602045295008, 0.043186813519466405}, + {1.1341368223288433, 0.027961663869807987, -0.043174425278975581, 0.043174636729259808}, + {1.1391896147945486, 0.027743544071588272, -0.043161336556975183, 0.043161548024339413}, + {1.1442397159855124, 0.027525608845103966, -0.043147339436513721, 0.043147550853110728}, + {1.1492871479513269, 0.027307861743365913, -0.043132437138590995, 0.043132648579094324}, + {1.1543319325223269, 0.027090306305629183, -0.043116633040206132, 0.043116844483539418}, + {1.1593740913123962, 0.026872946057420154, -0.04309993041315846, 0.043100141854418517}, + {1.1644136457217125, 0.026655784510564016, -0.043082332754987462, 0.043082544164718295}, + {1.1694506169394883, 0.026438825163212297, -0.043063843359209542, 0.043064054777552763}, + {1.1744850259466215, 0.02622207149987011, -0.043044465490907957, 0.043044676879385735}, + {1.1795168935183484, 0.026005526991424784, -0.043024202553380742, 0.043024413955640051}, + {1.1845462402268365, 0.025789195095173502, -0.043003057907761211, 0.043003269319421}, + {1.1895730864437331, 0.025573079254851611, -0.042981034909690685, 0.042981246316104338}, + {1.1945974523427025, 0.025357182900661523, -0.042958136772919264, 0.042958348173459282}, + {1.1996193579018946, 0.025141509449301033, -0.042934366936995871, 0.042934578267755319}, + {1.2046388229063949, 0.024926062303991962, -0.042909728685198045, 0.042909940053576345}, + {1.2096558669506376, 0.024710844854509827, -0.042884225327591952, 0.042884436688649774}, + {1.2146705094407741, 0.024495860477212389, -0.042857860295507533, 0.042858071627052297}, + {1.2196827695970187, 0.024281112535069337, -0.042830636748793061, 0.042830848071882921}, + {1.2246926664559461, 0.024066604377691619, -0.042802558065733354, 0.042802769354879089}, + {1.2297002188727715, 0.023852339341360815, -0.042773627650194324, 0.042773838950781906}, + {1.2347054455235817, 0.023638320749059319, -0.042743848660052486, 0.042744059937582957}, + {1.2397083649075464, 0.023424551910499768, -0.042713224484523304, 0.04271343578989778}, + {1.244708995349086, 0.023211036122155172, -0.042681758425717864, 0.04268196971564131}, + {1.2497073550000182, 0.022997776667289062, -0.042649453834326223, 0.042649665084285748}, + {1.2547034618416664, 0.022784776815985459, -0.042616313838960362, 0.042616525090949597}, + {1.2596973336869455, 0.022572039825179427, -0.0425823418711333, 0.042582553107880894}, + {1.2646889881824066, 0.022359568938687166, -0.042547541107566644, 0.042547752358060242}, + {1.2696784428102619, 0.022147367387236473, -0.042511914989148303, 0.04251212621340611}, + {1.2746657148903757, 0.021935438388497819, -0.042475466629107339, 0.042475677821802948}, + {1.2796508215822302, 0.021723785147114233, -0.042438199404242594, 0.042438410566841722}, + {1.2846337798868588, 0.021512410854732524, -0.042400116479959449, 0.042400327637873048}, + {1.2896146066487582, 0.021301318690033606, -0.042361221142273531, 0.042361432294732937}, + {1.2945933185577663, 0.02109051181876382, -0.042321516757353239, 0.042321727928555708}, + {1.299569932150922, 0.020879993393765506, -0.042281006492249529, 0.042281217645791902}, + {1.3045444638142849, 0.020669766555008066, -0.042239693560307878, 0.042239904709162279}, + {1.3095169297847469, 0.020459834429618587, -0.04219758126607781, 0.042197792398305067}, + {1.3144873461518063, 0.020250200131913566, -0.042154672736922416, 0.042154883849982505}, + {1.3194557288593196, 0.020040866763429373, -0.042110971382184281, 0.042111182479337428}, + {1.3244220937072295, 0.019831837412953586, -0.042066480248634908, 0.042066691355668095}, + {1.3293864563532725, 0.019623115156556049, -0.042021202685308401, 0.042021413773353611}, + {1.33434883231465, 0.01941470305762031, -0.041975141856123999, 0.041975352950746614}, + {1.3393092369696991, 0.019206604166874204, -0.041928300967060628, 0.041928512036170817}, + {1.344267685559511, 0.018998821522421744, -0.041880683326974033, 0.041880894338668262}, + {1.3492241931895534, 0.01879135814977375, -0.041832291800490251, 0.041832502870719712}, + {1.354178774831253, 0.018584217061879543, -0.041783130044627859, 0.041783341053103995}, + {1.3591314453235652, 0.01837740125915778, -0.041733200798910353, 0.041733411824529665}, + {1.3640822193745206, 0.018170913729527838, -0.041682507574776373, 0.041682718608742897}, + {1.3690311115627414, 0.017964757448441115, -0.041631053418500819, 0.041631264402514356}, + {1.3739781363389587, 0.017758935378912079, -0.041578841474567418, 0.04157905249416427}, + {1.3789233080274805, 0.017553450471549666, -0.041525874955566144, 0.041526085960111203}, + {1.3838666408276621, 0.017348305664588372, -0.041472156998661081, 0.0414723679750999}, + {1.3888081488153481, 0.017143503883919289, -0.041417690746155089, 0.041417901710570915}, + {1.3937478459442914, 0.016939048043121581, -0.041362479329356272, 0.041362690271261826}, + {1.3986857460475637, 0.016734941043493295, -0.041306525894044155, 0.041306736881463191}, + {1.4036218628389299, 0.016531185774082829, -0.041249833626507637, 0.04125004457681819}, + {1.4085562099142224, 0.016327785111719562, -0.041192405662172027, 0.041192616603312859}, + {1.4134888007526869, 0.016124741921045238, -0.041134244942813314, 0.041134455875962372}, + {1.4184196487183076, 0.015922059054544982, -0.041075354759761096, 0.041075565678418746}, + {1.4233487670611205, 0.015719739352577929, -0.04101573810433351, 0.041015949037463084}, + {1.4282761689185115, 0.015517785643408694, -0.040955398215443417, 0.040955609144615934}, +}}; + +inline constexpr std::array, 281> right = {{ + {-0.14084732125908495, 0.072795780352963274, 0.019747821526528039, -0.019747672825233435}, + {-0.1334322078965231, 0.072648558079311323, 0.019961291341424978, -0.019961141764082358}, + {-0.12605104983226534, 0.072500435543639316, 0.020174423101001, -0.020174272647664755}, + {-0.11870326879280674, 0.072351418067845411, 0.020387195029482878, -0.020387043712307099}, + {-0.11138829925859983, 0.072201510988708556, 0.02059958570048805, -0.020599433520667971}, + {-0.10410558812217852, 0.072050719657467788, 0.020811574093828909, -0.020811421053081463}, + {-0.096854594356919732, 0.071899049439403548, 0.021023139464641561, -0.021022985581177905}, + {-0.089634788696091761, 0.071746505713422393, 0.021234261439258658, -0.021234106750884656}, + {-0.082445653321844836, 0.071593093871643379, 0.021444920189389729, -0.021444764653169048}, + {-0.075286681563754732, 0.071438819318988198, 0.02165509589532965, -0.021654939553085215}, + {-0.068157377606650027, 0.071283687472773333, 0.02186476928898267, -0.021864612165416842}, + {-0.061057256207330657, 0.071127703762305136, 0.02207392148642982, -0.022073763562246079}, + {-0.053985842419919328, 0.070970873628478037, 0.022282533811498924, -0.022282375097956355}, + {-0.046942671329554764, 0.070813202523375404, 0.022490587985040961, -0.022490428474080466}, + {-0.03992728779408089, 0.070654695909873319, 0.022698066046157928, -0.022697905782933684}, + {-0.032939246193542659, 0.070495359261247267, 0.022904950354729937, -0.022904789347529445}, + {-0.025978110187147407, 0.070335198060782145, 0.023111223719082575, -0.023111061966510887}, + {-0.019043452477490119, 0.070174217801384928, 0.023316869132509997, -0.02331670664416173}, + {-0.012134854581772492, 0.07001242398520037, 0.023521869944591874, -0.02352170673525162}, + {-0.0052519066097874711, 0.069849822123230168, 0.023726209896160484, -0.023726045944262898}, + {0.0016057929515613245, 0.069686417734955028, 0.023929872876919289, -0.023929708215282872}, + {0.0084386374474052506, 0.069522216347959701, 0.024132843292230904, -0.024132677922804498}, + {0.015247012257973647, 0.069357223497561585, 0.024335105815760386, -0.024334939760170368}, + {0.022031294995728534, 0.069191444726442397, 0.024536645298976881, -0.024536478536051894}, + {0.028791855696793378, 0.069024885584282436, 0.024737447004385571, -0.024737279591539856}, + {0.035529057006852161, 0.068857551627399691, 0.024937496503809237, -0.02493732840231493}, + {0.042243254361723603, 0.068689448418390128, 0.025136779639218485, -0.025136610862264479}, + {0.048934796162745375, 0.068520581525773339, 0.02533528257283749, -0.025335113193571013}, + {0.055604023947176229, 0.068350956523639789, 0.025532991715382806, -0.025532821647623983}, + {0.062251272553739356, 0.068180578991302559, 0.025729893772699252, -0.025729723072019858}, + {0.068876870283509639, 0.068009454512951956, 0.025925975816888609, -0.025925804503800703}, + {0.075481139056245872, 0.067837588677313543, 0.026121225118534645, -0.0261210531913415}, + {0.082064394562356202, 0.067664987077309446, 0.026315629178499795, -0.026315456626048221}, + {0.088626946410598262, 0.067491655309723647, 0.026509175945516484, -0.026509002790281705}, + {0.095169098271689456, 0.067317598974869747, 0.026701853519170142, -0.026701679786675814}, + {0.10169114801791729, 0.067142823676262897, 0.026893650254420021, -0.026893475901647351}, + {0.10819338785891913, 0.066967335020295018, 0.027084554871353365, -0.027084379919717943}, + {0.1146761044736892, 0.066791138615913057, 0.027274556210883785, -0.027274380699864213}, + {0.12113957913902512, 0.066614240074301431, 0.027463643555285879, -0.027463467481546381}, + {0.12758408785440972, 0.066436645008567108, 0.027651806245590552, -0.027651629600661497}, + {0.13400990146355091, 0.066258359033428915, 0.027839034005732913, -0.027838856803525595}, + {0.14041728577260354, 0.066079387764909625, 0.028025316795958193, -0.028025139079591921}, + {0.14680650166520243, 0.065899736820032195, 0.028210644816768632, -0.028210466566918044}, + {0.15317780521442564, 0.065719411816518664, 0.028395008434043894, -0.028394829668341517}, + {0.15953144779174827, 0.065538418372493815, 0.028578398419636511, -0.028578219100784889}, + {0.16586767617309089, 0.06535676210619043, 0.028760805581125225, -0.028760625760593964}, + {0.17218673264207146, 0.065174448635660145, 0.028942221073663347, -0.028942040719559262}, + {0.17848885509050111, 0.064991483578486053, 0.029122636358224877, -0.029122455503968362}, + {0.18477427711625533, 0.064807872551499862, 0.029302042902552113, -0.029301861521223792}, + {0.19104322811857094, 0.064623621170501952, 0.029480432579333436, -0.029480250768846628}, + {0.19729593339084844, 0.064438735049984971, 0.029657797443868351, -0.02965761512704005}, + {0.20353261421104191, 0.064253219802861722, 0.029834129859810959, -0.02983394704151731}, + {0.20975348792970755, 0.06406708104019472, 0.030009422043668642, -0.030009238804835494}, + {0.21595876805577202, 0.063880324370931219, 0.030183666900604516, -0.030183483177077888}, + {0.222148664340104, 0.063692955401640383, 0.030356857227101845, -0.030356673082267176}, + {0.22832338285692483, 0.063504979736254394, 0.030528986176004649, -0.03052880154498298}, + {0.23448312608316474, 0.063316402975812841, 0.030700046989141465, -0.030699861926795746}, + {0.24062809297578053, 0.063127230718210414, 0.030870033193912121, -0.03086984767774046}, + {0.24675847904712161, 0.062937468557948628, 0.031038938493286244, -0.031038752564294371}, + {0.25287447643839472, 0.062747122085889923, 0.031206756764450203, -0.031206570400122237}, + {0.25897627399128015, 0.062556196889016313, 0.031373482070357644, -0.031373295296715897}, + {0.26506405731774491, 0.062364698550190265, 0.03153910871891203, -0.031538921521951903}, + {0.27113800886812917, 0.062172632647919966, 0.031703631043897527, -0.031703443474098682}, + {0.27719830799752626, 0.061980004756127263, 0.031867043817739055, -0.031866855857976273}, + {0.28324513103052046, 0.061786820443919338, 0.032029341772312564, -0.032029153408337445}, + {0.28927865132433578, 0.061593085275363162, 0.032190519948776336, -0.03219033116040388}, + {0.29529903933041984, 0.06139880480926424, 0.032350573452546494, -0.032350384328299564}, + {0.30130646265452249, 0.061203984598947478, 0.03250949758797847, -0.032509308094185355}, + {0.30730108611531221, 0.061008630192042231, 0.032667287941386895, -0.032667098065183524}, + {0.31328307180156484, 0.060812747130270163, 0.032823940057333759, -0.032823749829356118}, + {0.31925257912796329, 0.060616340949236583, 0.032979449844820707, -0.032979259254387057}, + {0.3252097648895505, 0.060419417178224674, 0.03313381327728105, -0.033133622332675035}, + {0.33115478331488329, 0.060221981339993416, 0.033287026486672422, -0.033286835171672073}, + {0.33708778611789364, 0.060024038950578278, 0.033439085734535065, -0.033438894076953177}, + {0.3430089225485255, 0.05982559551909529, 0.033589987497809642, -0.033589795488020345}, + {0.34891833944216416, 0.059626656547548104, 0.033739728292080218, -0.033739535969493833}, + {0.3548161812678961, 0.059427227530638546, 0.033888305001523761, -0.033888112349466704}, + {0.36070259017561263, 0.059227313955579633, 0.034035714346854498, -0.034035521392040829}, + {0.36657770604203416, 0.059026921301912672, 0.034181953498094275, -0.034181760200202561}, + {0.37244166651562621, 0.058826055041326125, 0.034327019553398751, -0.034326825948608079}, + {0.37829460706048706, 0.05862472063747888, 0.034470909806485726, -0.034470715901596366}, + {0.38413666099919869, 0.058422923545825539, 0.034613621737503944, -0.034613427526393993}, + {0.38996795955469093, 0.058220669213444862, 0.034755152839746443, -0.034754958341392875}, + {0.39578863189114205, 0.058017963078872012, 0.034895500897814159, -0.034895306075582527}, + {0.40159880515392438, 0.057814810571932732, 0.035034663655623015, -0.03503446857652158}, + {0.40739860450864823, 0.057611217113580818, 0.035172639224709736, -0.035172443811940887}, + {0.41318815317929442, 0.057407188115738826, 0.035309425427482381, -0.035309229770945749}, + {0.41896757248550115, 0.057202728981141267, 0.035445020636373711, -0.035444824749847635}, + {0.42473698187897929, 0.056997845103180671, 0.03557942315170156, -0.035579226969738954}, + {0.43049649897912473, 0.056792541865756822, 0.035712631406771202, -0.035712434938277235}, + {0.43624623960780967, 0.056586824643128482, 0.035844643937837629, -0.035844447201115966}, + {0.44198631782340564, 0.056380698799768376, 0.035975459322598685, -0.035975262374216499}, + {0.44771684595403843, 0.05617416969021985, 0.036105076424152491, -0.036104879216324839}, + {0.45343793463009979, 0.055967242658957903, 0.036233494170023528, -0.036233296717728639}, + {0.45914969281603862, 0.05575992304025125, 0.036360711429367545, -0.036360513751650371}, + {0.46485222784144192, 0.0555522161580288, 0.036486727319014811, -0.036486529377775495}, + {0.47054564543144523, 0.055344127325747033, 0.036611541106937609, -0.036611342925128555}, + {0.47623004973644539, 0.05513566184626123, 0.036735152029884865, -0.036734953609571921}, + {0.48190554336119512, 0.054926825011699161, 0.036857559408983623, -0.036857360773226301}, + {0.48757222739322709, 0.054717622103336784, 0.036978762890802909, -0.036978564025823335}, + {0.4932302014306908, 0.054508058391477224, 0.037098761957401555, -0.037098562871980822}, + {0.49887956360956048, 0.054298139135331831, 0.037217556332661744, -0.037217357038951002}, + {0.50452041063026365, 0.054087869582903875, 0.037335145720915397, -0.037334946248389379}, + {0.51015283778374276, 0.053877254970874713, 0.037451530065659461, -0.037451330349838802}, + {0.51577693897693866, 0.053666300524492568, 0.037566709298959548, -0.037566509404362507}, + {0.52139280675774269, 0.053455011457463592, 0.037680683384273446, -0.037680483254981897}, + {0.52700053233941491, 0.053243392971845327, 0.037793452511768494, -0.037793252211250954}, + {0.53260020562447186, 0.053031450257942529, 0.037905016903446088, -0.037904816376417114}, + {0.5381919152280803, 0.052819188494205534, 0.038015376719868453, -0.038015176018747376}, + {0.54377574850094734, 0.052606612847131015, 0.038124532485241835, -0.038124331615435235}, + {0.54935179155173253, 0.052393728471164316, 0.038232484537317021, -0.038232283502435953}, + {0.5549201292689907, 0.052180540508605266, 0.038339233455588526, -0.038339032182150304}, + {0.56048084534264819, 0.051967054089515317, 0.038444779750234324, -0.038444578350446922}, + {0.56603402228504063, 0.051753274331626881, 0.038549124218678399, -0.038548922609050108}, + {0.57157974145150447, 0.05153920634025598, 0.038652267458857359, -0.03865206569859507}, + {0.57711808306054668, 0.051324855208215454, 0.038754210364377879, -0.038754008485559495}, + {0.58264912621359999, 0.05111022601573155, 0.038854953792575891, -0.038854751683293935}, + {0.58817294891435856, 0.050895323830362257, 0.038954498788173725, -0.038954296532288518}, + {0.59368962808773551, 0.050680153706917641, 0.039052846150078709, -0.039052643768189101}, + {0.59919923959841981, 0.050464720687382073, 0.03914999714901244, -0.039149794589634995}, + {0.60470185826905198, 0.050249029800839451, 0.039245952874433394, -0.039245750185869149}, + {0.61019755789804697, 0.050033086063398839, 0.039340714566176195, -0.039340511750687542}, + {0.61568641127703061, 0.04981689447812368, 0.039434283408787553, -0.039434080421840198}, + {0.62116849020795306, 0.049600460034962025, 0.039526660773063084, -0.039526457683217955}, + {0.62664386551983298, 0.049383787710679167, 0.039617848027896203, -0.039617644799468034}, + {0.63211260708517281, 0.049166882468791723, 0.039707846665401003, -0.039707643273639313}, + {0.63757478383605792, 0.048949749259504308, 0.039796658187555713, -0.039796454650006641}, + {0.64303046377991757, 0.048732393019647352, 0.039884284024341014, -0.039884080359064392}, + {0.64847971401497739, 0.048514818672617123, 0.03997072595180326, -0.039970522149389225}, + {0.65392260074541531, 0.048297031128317819, 0.040055985512143795, -0.040055781583982687}, + {0.65935918929620674, 0.048079035283104761, 0.040140064476037143, -0.040139860458741961}, + {0.66478954412767433, 0.047860836019730242, 0.040222964572543561, -0.040222760424430549}, + {0.67021372884976871, 0.047642438207290028, 0.040304687557542754, -0.040304483304886514}, + {0.67563180623605446, 0.047423846701172868, 0.040385235481440136, -0.040385031115535992}, + {0.68104383823743042, 0.047205066343010568, 0.040464610044892997, -0.040464405577391629}, + {0.68644988599557588, 0.046986101960630453, 0.040542813298737775, -0.040542608701723916}, + {0.69185000985614642, 0.04676695836800894, 0.040619847200546545, -0.040619642523450357}, + {0.69724426938170869, 0.046547640365227447, 0.040695713850829575, -0.040695509060131217}, + {0.70263272336442206, 0.046328152738429017, 0.040770415245073903, -0.040770210337388937}, + {0.7080154298384842, 0.046108500259777516, 0.040843953577098947, -0.040843748584525011}, + {0.71339244609233687, 0.045888687687417357, 0.04091633093459416, -0.040916125839259354}, + {0.71876382868062816, 0.045668719765435468, 0.040987549579011608, -0.040987344422615993}, + {0.72412963343596504, 0.045448601223824811, 0.041057611830544537, -0.041057406525668617}, + {0.72948991548043018, 0.045228336778448577, 0.041126519762042804, -0.041126314407408909}, + {0.73484472923687782, 0.045007931131006908, 0.041194275904435251, -0.041194070440522583}, + {0.74019412844003063, 0.044787388969004319, 0.041260882505884208, -0.04126067696829832}, + {0.7455381661473528, 0.044566714965718804, 0.041326341961329303, -0.041326136351384053}, + {0.75087689474973107, 0.044345913780172511, 0.041390656693243884, -0.041390450963403207}, + {0.75621036598194813, 0.044124990057103207, 0.041453829104364237, -0.041453623330707401}, + {0.76153863093296215, 0.043903948426937678, 0.041515861787150429, -0.041515655922623886}, + {0.7668617400560005, 0.043682793505766454, 0.041576757289474284, -0.041576551351907166}, + {0.7721797431784585, 0.043461529895319567, 0.041636517986826035, -0.041636311985591506}, + {0.77749268951162032, 0.043240162182943409, 0.041695146606656262, -0.04169494056353569}, + {0.78280062766020153, 0.04301869494157938, 0.041752645735372497, -0.041752439576426589}, + {0.78810360563171034, 0.042797132729743656, 0.041809018004141919, -0.041808811800025041}, + {0.79340167084564528, 0.042575480091507814, 0.041864265998183915, -0.041864059737722101}, + {0.79869487014251828, 0.042353741556481167, 0.04191839252939375, -0.041918186161705284}, + {0.80398324979271907, 0.042131921639793665, 0.041971400318436866, -0.041971193921633537}, + {0.80926685550521005, 0.041910024842080826, 0.042023292047682244, -0.042023085552235773}, + {0.81454573243607875, 0.041688055649469008, 0.0420740704915663, -0.042073863962885281}, + {0.81981992519691627, 0.041466018533562306, 0.042123738455011696, -0.042123531861942079}, + {0.8250894778630613, 0.041243917951429954, 0.042172298752891454, -0.042172092093260977}, + {0.83035443398169062, 0.041021758345595911, 0.042219754189112696, -0.04221954750399904}, + {0.83561483657976132, 0.040799544144028248, 0.042266107756225982, -0.042265901024732273}, + {0.8408707281718133, 0.040577279760130494, 0.042311362246308194, -0.042311155430870204}, + {0.84612215076763864, 0.040354969592733644, 0.042355520576474938, -0.04235531371488116}, + {0.85136914587980472, 0.040132618026089209, 0.042398585646475605, -0.042398378751532545}, + {0.8566117545310552, 0.039910229429863123, 0.042440560391601941, -0.042440353462908642}, + {0.8618500172615704, 0.039687808159131199, 0.042481447878935125, -0.042481240917742739}, + {0.8670839741361045, 0.039465358554374566, 0.042521250986633355, -0.042521043940561068}, + {0.87231366475100125, 0.039242884941477249, 0.042559972721437488, -0.042559765661534701}, + {0.87753912824108116, 0.039020391631723467, 0.042597616248618776, -0.042597409162052256}, + {0.88276040328640648, 0.03879788292179677, 0.042634184421197048, -0.042633977264918015}, + {0.88797752811894171, 0.038575363093779451, 0.042669680333848249, -0.042669473186591915}, + {0.8931905405290792, 0.038352836415153399, 0.042704107100341823, -0.042703899885172557}, + {0.89839947787206986, 0.038130307138801077, 0.042737467740011699, -0.042737260506331184}, + {0.9036043770743325, 0.037907779503008159, 0.042769765387191648, -0.042769558134640798}, + {0.90880527463965555, 0.037685257731466318, 0.042801003285843048, -0.042800795980571425}, + {0.91400220665529686, 0.037462746033276997, 0.042831184260345524, -0.04283097693491307}, + {0.91919520879797623, 0.037240248602956411, 0.04286031172843454, -0.042860104375103243}, + {0.92438431633976448, 0.037017769620440615, 0.042888388717124928, -0.042888181326625835}, + {0.92956956415387504, 0.036795313251091873, 0.042915418378989587, -0.04291521094890207}, + {0.93475098672035606, 0.036572883645705531, 0.042941403881554055, -0.042941196422330269}, + {0.93992861813168394, 0.036350484940517694, 0.042966348505248553, -0.0429661410350895}, + {0.94510249209827157, 0.036128121257213326, 0.042990255333791409, -0.042990047854811871}, + {0.95027264195386807, 0.035905796702935783, 0.04301312765840281, -0.043012920152179106}, + {0.95543910066088267, 0.035683515370296182, 0.04303496864000398, -0.043034761102853418}, + {0.96060190081561214, 0.035461281337383993, 0.043055781493425954, -0.04305557395036258}, + {0.96576107465338468, 0.035239098667777843, 0.043075569511401235, -0.043075361922521309}, + {0.9709166540536136, 0.035016971410557651, 0.043094335958006727, -0.043094128368453062}, + {0.97606867054476742, 0.034794903600316576, 0.043112083983895044, -0.04311187638835054}, + {0.98121715530926901, 0.034572899257174147, 0.043128816886945379, -0.043128609278714564}, + {0.98636213918829307, 0.034350962386789696, 0.0431445379607047, -0.043144330306882112}, + {0.9915036526865082, 0.03412909698037668, 0.043159250500346705, -0.043159042839566647}, + {0.99664172597671619, 0.033907307014717135, 0.043172957779916803, -0.043172750108711466}, + {1.0017763889044407, 0.03368559645217703, 0.043185663006088088, -0.043185455360639467}, + {1.006907670992421, 0.033463969240722091, 0.043197369611135177, -0.043197161932109408}, + {1.012035601445042, 0.033242429313934257, 0.043208080769897024, -0.043207873049854219}, + {1.0171602091526915, 0.033020980591028461, 0.043217799836418443, -0.043217592153643576}, + {1.0222815226960458, 0.032799626976869646, 0.043226530136964446, -0.043226322400851586}, + {1.0273995703502821, 0.032578372361991265, 0.043234274936508159, -0.043234067235691821}, + {1.0325143800892358, 0.032357220622613328, 0.043241037693833334, -0.04324082997526333}, + {1.0376259795894716, 0.032136175620661012, 0.043246821606827732, -0.043246613850303442}, + {1.0427343962343065, 0.031915241203784313, 0.043251630002203958, -0.043251422259874078}, + {1.0478396571177568, 0.031694421205377445, 0.043255466186286388, -0.043255258444601638}, + {1.0529417890484356, 0.031473719444599493, 0.043258333623696306, -0.04325812589077755}, + {1.0580408185533721, 0.031253139726394576, 0.043260235535370839, -0.043260027781557145}, + {1.0631367718817766, 0.031032685841512815, 0.043261175363101943, -0.04326096758646248}, + {1.0682296750087557, 0.030812361566532408, 0.043261156361899908, -0.043260948616188843}, + {1.0733195536389515, 0.030592170663880619, 0.043260181900347625, -0.04325997412204681}, + {1.0784064332101395, 0.030372116881856426, 0.0432582553600337, -0.043258047596792755}, + {1.0834903388967583, 0.030152203954653303, 0.043255380021070079, -0.043255172274416859}, + {1.0885712956133899, 0.029932435602381562, 0.043251559378017182, -0.043251351597019851}, + {1.093649328018186, 0.02971281553109216, 0.043246796604458099, -0.043246588841254791}, + {1.0987244605162356, 0.029493347432800278, 0.043241095125597616, -0.043240887353292901}, + {1.1037967172628877, 0.029274034985509081, 0.043234458343326315, -0.043234250589908318}, + {1.1088661221670149, 0.029054881853234162, 0.043226889601101735, -0.043226681822995251}, + {1.1139326988942333, 0.028835891686028063, 0.043218392231715676, -0.043218184487950596}, + {1.1189964708700677, 0.028617068120005494, 0.04320896955353952, -0.043208761805145192}, + {1.1240574612830738, 0.028398414777368104, 0.043198624956385955, -0.043198417177084086}, + {1.1291156930879027, 0.028179935266430312, 0.043187361732268732, -0.043187154008145476}, + {1.1341711890083268, 0.027961633181644928, 0.043175183372119189, -0.043174975622506127}, + {1.1392239715402204, 0.027743512103629547, 0.043162093169083836, -0.043161885407901272}, + {1.1442740629544894, 0.027525575599192317, 0.043148094446783393, -0.043147886713794818}, + {1.1493214852999545, 0.027307827221358889, 0.043133190552430548, -0.043132982856211449}, + {1.1543662604062024, 0.027090270509399307, 0.043117384878419263, -0.043117177169325026}, + {1.1594084098863824, 0.026872908988854881, 0.043100680732956489, -0.043100473049955693}, + {1.1644479551399642, 0.026655746171565201, 0.04308308146691768, -0.043082873791168402}, + {1.1694849173554558, 0.026438785555696342, 0.043064590528145569, -0.043064382842919663}, + {1.1745193175130781, 0.026222030625768014, 0.04304521109647088, -0.043045003407060231}, + {1.1795511763874065, 0.026005484852681664, 0.043024946600620104, -0.043024738916013099}, + {1.1845805145499568, 0.025789151693748603, 0.043003800394104681, -0.043003592733121189}, + {1.1896073523717579, 0.025573034592718376, 0.042981775770859883, -0.04298156811176531}, + {1.1946317100258548, 0.02535713697980721, 0.042958876053506956, -0.042958668400458373}, + {1.1996536074898041, 0.025141462271726728, 0.042935104652162326, -0.042934897037744897}, + {1.2046730645481161, 0.024926013871712435, 0.042910464833166732, -0.042910257178773337}, + {1.2096901007946617, 0.024710795169553615, 0.042884959885238175, -0.042884752270869575}, + {1.2147047356350498, 0.024495809541621373, 0.042858593188433332, -0.042858385561929301}, + {1.2197169882889609, 0.024281060350898727, 0.042831368116557085, -0.042831160498602819}, + {1.2247268777924647, 0.024066550947009928, 0.042803287900663747, -0.042803080293506858}, + {1.229734423000278, 0.023852284666249716, 0.042774355857187786, -0.042774148242260751}, + {1.2347396425880097, 0.023638264831613541, 0.042744575304555235, -0.042744367719638861}, + {1.2397425550543628, 0.023424494752826965, 0.042713949561734466, -0.042713742004282816}, + {1.2447431787233125, 0.023210977726375932, 0.042682481964089143, -0.042682274385049797}, + {1.2497415317462461, 0.022997717035536443, 0.042650175638018546, -0.042649968130271984}, + {1.2547376321040733, 0.02278471595040546, 0.042617034144751216, -0.042616826604590617}, + {1.2597314976093075, 0.022571977727930299, 0.042583060554373706, -0.042582853044157334}, + {1.2647231459081181, 0.02235950561193966, 0.042548258249941733, -0.04254805075483728}, + {1.2697125944823493, 0.022147302833173774, 0.042512630500566646, -0.04251242298619444}, + {1.2746998606515125, 0.021935372609315155, 0.042476180572627094, -0.042475973088406969}, + {1.2796849615747565, 0.021723718145019048, 0.042438911702771039, -0.042438704210287823}, + {1.2846679142527948, 0.021512342631944258, 0.042400827233668649, -0.042400619754982424}, + {1.2896487355298172, 0.021301249248783634, 0.04236193031586085, -0.042361722853633446}, + {1.2946274420953783, 0.021090441161295312, 0.042322224315711011, -0.042322016863519482}, + {1.2996040504862338, 0.020879921522333506, 0.042281712448488544, -0.042281504962646418}, + {1.3045785770881968, 0.020669693471878837, 0.042240397880240095, -0.042240190470327739}, + {1.3095510381379118, 0.020459760137070487, 0.042198283999100347, -0.042198076585318531}, + {1.3145214497246518, 0.020250124632236008, 0.042155373924072349, -0.042155166520834092}, + {1.3194898277920644, 0.020040790058923258, 0.042111670943351991, -0.042111463514344805}, + {1.3244561881398962, 0.019831759505931086, 0.042067178262623121, -0.042066970898255548}, + {1.3294205464257034, 0.019623036049340563, 0.042021899050582921, -0.04202169169556598}, + {1.334382918166527, 0.019414622752546087, 0.041975836657044463, -0.041975629283935327}, + {1.3393433187405519, 0.019206522666286779, 0.041928994113242871, -0.041928786768834776}, + {1.3443017633887435, 0.018998738828677364, 0.041881374776246892, -0.041881167448371094}, + {1.3492582672164459, 0.018791274265239619, 0.041832981852885281, -0.041832774491148833}, + {1.3542128451949889, 0.018584131988933376, 0.041783818298299656, -0.041783610990856881}, + {1.3591655121632435, 0.018377315000188056, 0.041733887579768571, -0.041733680259803362}, + {1.3641162828291664, 0.018170826286933783, 0.041683192716561734, -0.041682985399044727}, + {1.3690651717713327, 0.017964668824632102, 0.041631736949248967, -0.041631529629667645}, + {1.3740121934404319, 0.017758845576308101, 0.041579523426561182, -0.04157931610995462}, + {1.3789573621607507, 0.01755335949258096, 0.041526555289850359, -0.041526347996612437}, + {1.3839006921316415, 0.017348213511695375, 0.041472835695786435, -0.041472628431247341}, + {1.3888421974289535, 0.017143410559552638, 0.041418367829953735, -0.041418160548106049}, + {1.3937818920064724, 0.016938953549741953, 0.04136315480243663, -0.041362947551687887}, + {1.3987197896973085, 0.016734845383571388, 0.041307199795198833, -0.041306992539488441}, + {1.4036559042152865, 0.016531088950099186, 0.041250505829181078, -0.04125029858041606}, + {1.4085902491563165, 0.016327687126164545, 0.041193076214110898, -0.041192868977241663}, + {1.4135228379997289, 0.016124642776418965, 0.041134913962167695, -0.041134706738650217}, + {1.4184536841096236, 0.0159219587533573, 0.041076022162711397, -0.041075814887105273}, + {1.4233828007361584, 0.015719637897348308, 0.041016403918549528, -0.041016196665515842}, + {1.4283102010168576, 0.015517683036666084, 0.040956062396396944, -0.040955855196883606}, +}}; + +inline constexpr std::array, 16> force = {{ + {0.022997316706837613, 86.965688801928167}, + {0.023851879860738903, 131.18023500886301}, + {0.024710385829300824, 185.68333781640229}, + {0.025572620660114301, 309.43303430630766}, + {0.026438366973481182, 346.73845194767057}, + {0.027307403932652949, 346.73845194774657}, + {0.029054448982586897, 350.90237841326041}, + {0.032577919665348412, 366.38968062207243}, + {0.037017290950069205, 391.09833613120429}, + {0.041465512658364498, 415.32100551306803}, + {0.04588815350788536, 446.86098014388892}, + {0.050248466389995548, 476.85464906482719}, + {0.054507465043132433, 508.41922419868212}, + {0.058624096921555034, 548.70255419074226}, + {0.060221345430025809, 567.00029919244162}, + {0.061786172348818356, 589.48924401103636}, +}}; + +template +std::array interpolate(const std::array, N>& rows, double x) { + x = std::clamp(x, rows.front()[0], rows.back()[0]); + auto b = std::upper_bound( + rows.begin(), rows.end(), x, [](double v, const auto& r) { return v < r[0]; }); + if (b == rows.end()) + return rows.back(); + if (b == rows.begin()) + return rows.front(); + const auto& a = *(b - 1); + const double t = (x - a[0]) / ((*b)[0] - a[0]); + std::array result{}; + for (std::size_t i = 0; i < M; ++i) + result[i] = a[i] + t * ((*b)[i] - a[i]); + return result; +} + +inline SelfRightingCore::Axes compensation(const SelfRightingCore::Axes& q) { + SelfRightingCore::Axes result{}; + for (std::size_t side = 0; side < 2; ++side) { + const auto& rows = side == 0 ? left : right; + const double delta = side == 0 ? q[1] - q[0] : q[2] - q[3]; + if (delta < rows.front()[0] || delta > rows.back()[0]) + continue; + const auto r = interpolate(rows, delta); + double f = interpolate(force, r[1])[1]; + + if (r[1] < force.front()[0]) + f = force.front()[1] * std::exp(594.93986075435475 * (r[1] - force.front()[0])); + if (r[1] > force.back()[0]) + f = force.back()[1] + 14371.522210231198 * (r[1] - force.back()[0]); + result[side * 2] = -r[2] * f * 1; + result[side * 2 + 1] = -r[3] * f * 1; + } + return result; +} + +inline constexpr std::array, 35> support = {{ + {41, 0.83382442560508974, -0.0055135279541929094, -1.198293523756444, 0.84373039730738231, + 0.0058233014432751418, 1.2642428931686212, -0.83379738421200666, 0.0055136923915455327, + 1.1983292215367314, -0.84374371808644544, -0.0058233908339921925, -1.2642623004455178}, + {43, 0.86876419703306451, -0.0056901168399292601, -1.2366793892926164, 0.88058115769007461, + 0.0059986973788284104, 1.3023123500133238, -0.86873833253793553, 0.0056902739986597339, + 1.236713506062026, -0.88059368167796304, -0.0059987805323125087, -1.3023304022996562}, + {45, 0.90361357013790666, -0.0058484569942507392, -1.2711012597255156, 0.91727223199979113, + 0.0061556507224835855, 1.3363759018836241, -0.90358882085604664, 0.0058486069920427042, + 1.2711338219761892, -0.91728401872610832, -0.0061557278718815667, -1.336392650506113}, + {47, 0.93834188227788495, -0.0059893514769981948, -1.3017335495324467, 0.95378005248572539, + 0.0062949806545198328, 1.3666115077048957, -0.93831818503191389, 0.0059894945025895652, + 1.3017645985513582, -0.95379116014676635, -0.0062950520818283451, -1.366627013803523}, + {49, 0.97292149294804509, -0.0061136424212016038, -1.3287591608013294, 0.99008299686287327, + 0.006417542464109971, 1.3932049670972724, -0.97289878428157006, 0.0061137787166209989, + 1.3287887484336736, -0.99009348177893364, -0.0064176084801914612, -1.3932192980847802}, + {51, 1.0073272693429669, -0.0062221845453791502, -1.3523637331380733, 1.0261610446973128, + 0.0065242017210920267, 1.4163443115555729, -1.0073054864380426, 0.0062223143837414341, + 1.3523919184477151, -1.0261709609542258, -0.0065242626501316181, -1.4163575383419058}, + {53, 1.0415361937983481, -0.006315825720193402, -1.3727314188582973, 1.0619954889099641, + 0.0066158154399253988, 1.4362157148117838, -1.0415152776584169, 0.0063159493812747769, + 1.3727582634581736, -1.0620048856495312, -0.006615871626092229, -1.4362279116886256}, + {55, 1.0755269573013704, -0.0063953934437144255, -1.3900419465606717, 1.0975688029124173, + 0.0066932184239476555, 1.4530005224355769, -1.0755068450385776, 0.0063955112405977843, + 1.390067518691751, -1.0975777326595673, -0.0066932701892064017, -1.4530117592359559}, + {57, 1.1092798137635542, -0.0064616849915211105, -1.4044684832748011, 1.1328643440105743, + 0.006757214541242007, 1.4668733623524055, -1.1092604474707835, 0.0064617972350689001, + 1.4044928496836917, -1.1328728537511255, -0.0067572622097777923, -1.4668837098552601}, + {59, 1.1427762767471372, -0.0065154616568641188, -1.416176378528089, 1.1678663077784035, + 0.0068085706028675474, 1.4780008092629693, -1.1427576005961642, 0.006515568653497894, + 1.4161996055550574, -1.1678744422410903, -0.0068086144922041569, -1.4780103360378938}, + {61, 1.1759989641158333, -0.0065574451024730251, -1.4253223759297384, 1.2025595950498553, + 0.0068480130432924113, 1.4865406696018546, -1.1759809243429979, 0.0065575471541734487, + 1.4253445286760424, -1.2025673968378905, -0.0068480534554105077, -1.4865494414451064}, + {63, 1.2089314513981722, -0.0065883156974422263, -1.4320542491352839, 1.2369297236142218, + 0.0068762264038291221, 1.4926416486660266, -1.2089139962525792, 0.0065884130906468386, + 1.4320753908458883, -1.2369372334472897, -0.0068762636273866917, -1.492649728302986}, + {65, 1.2415581504661433, -0.0066087122019276023, -1.4365107346714368, 1.2709627563982362, + 0.0068938531350148935, 1.4964433081132711, -1.2415412301997053, 0.0066088052186330515, + 1.4365309261821224, -1.2709700132078339, -0.0068938874431268005, -1.4964507548653749}, + {67, 1.2738642088119463, -0.0066192324266189928, -1.4388216758831618, 1.3046452426797306, + 0.0069014943515632865, 1.4980762298555383, -1.2738477755075068, 0.0066193213377196969, + 1.4388409754628739, -1.3046522838596959, -0.0069015260040217186, -1.4980830996624839}, + {69, 1.3058354255182993, -0.0066204345827713951, -1.4391083128306725, 1.3379641699335849, + 0.0068997112245838821, 1.4976623218698748, -1.3058194329050588, 0.0066205196413982037, + 1.4391267760083259, -1.3379710315247761, -0.0068997404634174586, -1.4976686672452111}, + {71, 1.3374581808592994, -0.0066128390407982351, -1.4374836681230185, 1.3709069240041767, + 0.0068890268534183008, 1.4953152169949746, -1.3374425841999882, 0.0066129204912002137, + 1.4375013475542708, -1.3709136409359792, -0.0068890539006716267, -1.4953210871171407}, + {73, 1.368719377204848, -0.0065969303864044093, -1.4340529917666878, 1.4034612562111373, + 0.0068699283151119062, 1.4911407283269007, -1.3687041330805865, 0.0065970084550670549, + 1.4340699373405208, -1.4034678624581449, -0.0068699533861682482, -1.4911461691258232}, + {75, 1.3996063893779997, -0.006573159621048319, -1.4289142376577857, 1.4356152560586823, + 0.006842868931489221, 1.4852373342200131, -1.3995914555159361, 0.0065732345255753444, + 1.4289304964165694, -1.4356217849166575, -0.0068428922239682777, -1.485242388561403}, + {77, 1.4301070225580108, -0.0065419463957105961, -1.4221585515583612, 1.4673573284893977, + 0.0068082705451111281, 1.4776966734020291, -1.4300923575941149, 0.00654201834482393, + 1.4221741679310007, -1.4673638127294177, -0.0068082922410894753, -1.477701381242813}, + {79, 1.4602094768407374, -0.0065036812535446131, -1.4138707566146991, 1.4986761748723292, + 0.0067665257574844873, 1.4686040362565933, -1.4601950400992825, 0.0065037504337617236, + 1.4138857724232956, -1.4986826470072405, -0.006766546032377363, -1.4686084348003448}, + {81, 1.4899023170150683, -0.0064587278081340571, -1.4041298264522162, 1.5295607772192534, + 0.0067180001628308617, 1.4580388426321811, -1.4898880683334932, 0.0064587944059724123, + 1.4041442810652782, -1.5295672696897729, -0.0067180191698490432, -1.4580429665378425}, + {83, 1.5191744471622011, -0.0064074248329326622, -1.3930093390412956, 1.5600003848244282, + 0.0066630344219831272, 1.4460750997535854, -1.5191603466009964, 0.0064074890193666079, + 1.3930232695469336, -1.5600069302475206, -0.0066630523143373921, -1.4460789813131036}, + {85, 1.5480150887380375, -0.0063500882463429775, -1.3805779077258329, 1.5899845032583264, + 0.0066019462874322699, 1.4327818364190392, -1.5480010963668522, 0.0063501501790241832, + 1.3805913490450017, -1.5899911346960864, -0.006601963202790273, -1.4327855057749694}, + {87, 1.5764137622070391, -0.0062870129635683725, -1.3668995863103106, 1.6195028851260713, + 0.0065350324685020666, 1.4182235109427843, -1.57639983783691, 0.0062870727957076156, + 1.3669125713855479, -1.6195096362841415, -0.006535048535649679, -1.418226996183769}, + {89, 1.6043602711815832, -0.0062184746663085377, -1.352034247799061, 1.6485455223046868, + 0.0064625703855014649, 1.402460391983773, -1.6043463741337651, 0.0062185325400143654, + 1.3520468076455572, -1.6485524277483343, -0.0064625857287836652, -1.402463719397673}, + {91, 1.6318446890429064, -0.0061447313992601948, -1.3360379361149199, 1.6771026395174387, + 0.0063848198061933203, 1.3855489123182352, -1.6318307777888132, 0.0061447874522002621, + 1.3360501000803104, -1.6771097351240409, -0.0063848345388528571, -1.3855521065586629}, + {93, 1.6588573473415649, -0.0060660250800204096, -1.3189631917942535, 1.7051646892587631, + 0.0063020243401545883, 1.367541996035726, -1.6588433792297241, 0.0060660794320988032, + 1.3189749876318935, -1.705172012340765, -0.0063020385643319798, -1.3675450801819977}, + {95, 1.6853888260433962, -0.0059825828713222506, -1.3008593520869027, 1.7327223473001356, + 0.0062144128332164428, 1.3484893599247094, -1.685374756943067, 0.0059826356557657334, + 1.3008708061490459, -1.7327299370612641, -0.0062144266532726533, -1.3484923557571804}, + {97, 1.711429944978339, -0.0058946184733343898, -1.2817728271952156, 1.759766509423933, + 0.0061222006508820925, 1.3284377908373557, -1.7114157289502447, 0.0058946698011652643, + 1.2817839645018481, -1.7597744072519494, -0.0061222141600758562, -1.3284407188929315}, + {99, 1.7369717566622331, -0.0058023332649703718, -1.2617473536113977, 1.7862882887270148, + 0.006025590828517835, 1.3074313999705822, -1.7369573454596372, 0.0058023832427700484, + 1.261758198030094, -1.7862965386781711, -0.0060256041156669937, -1.3074342797514404}, + {101, 1.7620055399034453, -0.0057059173919071782, -1.2408242263406422, 1.81227901366144, + 0.0059247751660329584, 1.2855118558219303, -1.761990882539477, 0.0057059661262570671, + 1.240834800728674, -1.8122876628774709, -0.0059247883177349081, -1.2855147058599137}, + {103, 1.786522794253939, -0.0056055507258179205, -1.2190425113955961, 1.8377302267344637, + 0.0058199352004351113, 1.2627185972879928, -1.7865078365212996, 0.0056055983232994322, + 1.2190528376621046, -1.8377393259345722, -0.0058199482966259097, -1.2627214353311267}, + {105, 1.8105152351741496, -0.005501403790297843, -1.19643923998769, 1.8626336837612547, + 0.0057112430962291683, 1.2390890284597056, -1.8104999190364859, 0.0055014503397288195, + 1.1964493392269127, -1.8626432878863195, -0.0057112562168448733, -1.2390918715499133}, + {107, 1.8339747895366845, -0.0053936385335795478, -1.1730495859940682, 1.8869813537309224, + 0.0055988624692027145, 1.2146586964645323, -1.8339590523140483, 0.0053936841348800613, + 1.1730594787073834, -1.8869915226358418, -0.0055988756919589378, -1.214661561099728}, + {109, 1.856893591487222, -0.0052824091190117883, -1.1489070279724345, 1.9107654192695824, + 0.0054829491413777021, 1.1894614538565662, -1.8568773652609913, 0.0052824538609996807, + 1.1489167340772433, -1.9107762185300614, -0.0054829625395491632, -1.1894643561016771}, +}}; + +inline SelfRightingCore::Axes + support_compensation(const SelfRightingCore::Axes& q, const std::array& gravity) { + SelfRightingCore::Axes effort{}; + for (std::size_t side = 0; side < 2; ++side) { + const double delta = side == 0 ? q[1] - q[0] : q[2] - q[3]; + double beta = geometry.back()[0]; + for (std::size_t i = 1; i < geometry.size(); ++i) { + const auto& a = geometry[i - 1]; + const auto& b = geometry[i]; + const double da = side == 0 ? a[2] - a[1] : a[3] - a[4]; + const double db = side == 0 ? b[2] - b[1] : b[3] - b[4]; + if (delta <= db) { + beta = a[0] + std::clamp((delta - da) / (db - da), 0., 1.) * (b[0] - a[0]); + break; + } + } + const auto coefficients = interpolate(support, beta); + auto b = + std::upper_bound(geometry.begin(), geometry.end(), beta, [](double v, const auto& r) { + return v < r[0]; + }); + if (b == geometry.end()) + b = geometry.end() - 1; + if (b == geometry.begin()) + ++b; + const auto& a = *(b - 1); + const double t = (beta - a[0]) / ((*b)[0] - a[0]); + const double zero = a[side * 2 + 1] + t * ((*b)[side * 2 + 1] - a[side * 2 + 1]); + const double angle = (zero - q[side * 2]) * (side == 0 ? 1. : -1.); + for (std::size_t axis = side * 2; axis < side * 2 + 2; ++axis) { + const double x = coefficients[1 + axis * 3], y = coefficients[2 + axis * 3], + z = coefficients[3 + axis * 3]; + const double vx = std::cos(angle) * x - std::sin(angle) * z; + const double vz = std::sin(angle) * x + std::cos(angle) * z; + effort[axis] = -9.81 * (gravity[0] * vx + gravity[1] * y + gravity[2] * vz); + } + } + return effort; +} + +} // namespace rmcs_core::controller::chassis::wheel_leg::recovery_v202 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/recovery_profile.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/recovery_profile.hpp new file mode 100644 index 000000000..00b376a31 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/recovery_profile.hpp @@ -0,0 +1,403 @@ +#pragma once +#include "../joint_coordinates.hpp" +#include "../self_righting.hpp" +#include "../support_observer.hpp" + +namespace rmcs_core::controller::chassis::wheel_leg { +inline constexpr std::string_view kRecoveryModelSpecSha256 = + "8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812"; +inline JointReferenceRecoveryConfig make_recovery_config() { + JointReferenceRecoveryConfig config; + config.fold = Eigen::Vector4d{ + -0.25197165154877471, -0.11799796974413002, 0.25197032674734215, 0.11796511585937414}; + config.insertion_fold = Eigen::Vector4d{ + -0.14375041390283763, -0.22623551337777834, 0.14374835281990206, 0.22619916469284207}; + config.thrust = Eigen::Vector4d{ + -0.74951799031281752, 0.37956728120704325, 0.74951877198684191, -0.37959303715815201}; + config.support = Eigen::Vector4d{ + -0.40505537676965336, 0.057993679319272018, 0.40505537676965336, -0.058022700299903765}; + for (int hip : {0, 2}) + config.insertion_fold.segment<2>(hip).array() += + config.fold.segment<2>(hip).mean() - config.insertion_fold.segment<2>(hip).mean(); + config.nominal = config.fold; + config.rl_nominal = Eigen::Map{kNominalPosition.data()}; + const SelfRightingCore::Axes support{ + config.support[0], config.support[1], config.support[2], config.support[3]}; + const auto weight = recovery_v202::support_compensation( + support, {0, -0.0045574624965667261, -0.99998961471386916}); + for (int i = 0; i < 4; ++i) + config.support[i] += weight[i] / 60.; + config.dt = .005; + config.blend_seconds = 0.; + config.dynamic_takeover = true; + config.release_motion_on_takeover = true; + config.dynamic_capture_height_min = .20; + config.dynamic_capture_height_max = .29; + config.prepare_speed_rad_s = 2.; + config.insert_speed_rad_s = 2.; + config.orbit_speed_rad_s = 2.; + return config; +} +inline constexpr std::array kRecoveryCaptureHingeMax{ + 0.37826080054777944, 0.37829033092311448}; +inline std::array recovery_leg_geometry() { + std::array result; + result[0].hip_origin_b_m = Eigen::Vector3f{0.0f, 0.1838f, -0.00083697f}; + result[0].hip_axis_b = Eigen::Vector3f{-3.673165765e-06f, 0.9999894031f, -0.004603656943f}; + result[0].wheel_axis_b = Eigen::Vector3f{-3.673165765e-06f, 0.9999894031f, -0.004603656943f}; + result[0].delta_rad = { + -0.1558250067f, -0.1408902232f, -0.1260936504f, -0.1114306083f, -0.09689662141f, + -0.0824874075f, -0.06819886764f, -0.05402707667f, -0.0399682743f, -0.02601885664f, + -0.01217536839f, 0.0015655047f, 0.01520694274f, 0.02875199835f, 0.0422036029f, + 0.0555645723f, 0.06883761263f, 0.08202532528f, 0.09513021197f, 0.1081546793f, + 0.1211010434f, 0.1339715336f, 0.1467682971f, 0.159493402f, 0.1721488414f, + 0.1847365361f, 0.1972583386f, 0.2097160355f, 0.2221113505f, 0.2344459472f, + 0.2467214316f, 0.2589393546f, 0.2711012143f, 0.283208458f, 0.2952624848f, + 0.3072646472f, 0.3192162529f, 0.3311185669f, 0.3429728133f, 0.3547801766f, + 0.3665418034f, 0.378258804f, 0.3899322537f, 0.4015631941f, 0.4131526348f, + 0.4247015539f, 0.4362109f, 0.4476815926f, 0.4591145238f, 0.4705105588f, + 0.4818705372f, 0.4931952739f, 0.50448556f, 0.5157421635f, 0.5269658303f, + 0.5381572851f, 0.5493172317f, 0.5604463542f, 0.5715453175f, 0.582614768f, + 0.5936553342f, 0.6046676273f, 0.6156522419f, 0.6266097564f, 0.6375407337f, + 0.6484457216f, 0.6593252534f, 0.6701798482f, 0.6810100118f, 0.6918162364f, + 0.7025990018f, 0.7133587754f, 0.7240960125f, 0.7348111571f, 0.7455046417f, + 0.7561768883f, 0.7668283082f, 0.7774593025f, 0.7880702626f, 0.7986615702f, + 0.8092335977f, 0.8197867088f, 0.830321258f, 0.8408375919f, 0.8513360485f, + 0.861816958f, 0.8722806428f, 0.8827274179f, 0.8931575909f, 0.9035714626f, + 0.9139693266f, 0.92435147f, 0.9347181734f, 0.9450697111f, 0.9554063514f, + 0.9657283564f, 0.9760359828f, 0.9863294812f, 0.9966090972f, 1.006875071f, + 1.017127637f, 1.027367026f, 1.037593462f, 1.047807166f, 1.058008353f, + 1.068197235f, 1.078374018f, 1.088538904f, 1.098692093f, 1.108833778f, + 1.118964149f, 1.129083394f, 1.139191694f, 1.149289229f, 1.159376174f, + 1.169452702f, 1.179518981f, 1.189575176f, 1.199621451f, 1.209657962f, + 1.219684868f, 1.229702321f, 1.23971047f, 1.249709464f, 1.259699446f, + 1.269680559f, 1.279652942f, 1.289616731f, 1.299572061f, 1.309519063f, + 1.319457867f, 1.329388599f, 1.339311385f, 1.349226346f, 1.359133604f, + 1.369033276f, 1.378925478f, 1.388810325f, 1.398687928f, 1.408558399f, + 1.418421844f, 1.428278371f, 1.438128085f}; + result[0].wheel_center_b_m = { + {-0.01709390656f, 0.1980677068f, -0.1599238948f}, + {-0.0184889416f, 0.1980599797f, -0.1616012244f}, + {-0.01989856095f, 0.198052309f, -0.1632663164f}, + {-0.02132265727f, 0.198044695f, -0.1649190441f}, + {-0.0227611221f, 0.1980371386f, -0.1665592815f}, + {-0.0242138459f, 0.1980296402f, -0.1681869038f}, + {-0.02568071804f, 0.1980222003f, -0.169801787f}, + {-0.02716162681f, 0.1980148197f, -0.1714038081f}, + {-0.02865645943f, 0.1980074987f, -0.1729928452f}, + {-0.03016510208f, 0.198000238f, -0.1745687772f}, + {-0.03168743985f, 0.1979930382f, -0.1761314841f}, + {-0.03322335681f, 0.1979858997f, -0.1776808469f}, + {-0.03477273601f, 0.1979788232f, -0.1792167476f}, + {-0.03633545944f, 0.1979718092f, -0.1807390692f}, + {-0.03791140811f, 0.1979648581f, -0.1822476959f}, + {-0.03950046199f, 0.1979579706f, -0.1837425127f}, + {-0.04110250008f, 0.1979511471f, -0.1852234058f}, + {-0.04271740037f, 0.1979443882f, -0.1866902625f}, + {-0.04434503988f, 0.1979376944f, -0.1881429709f}, + {-0.04598529466f, 0.1979310661f, -0.1895814205f}, + {-0.0476380398f, 0.197924504f, -0.1910055018f}, + {-0.04930314944f, 0.1979180085f, -0.1924151062f}, + {-0.05098049677f, 0.1979115801f, -0.1938101265f}, + {-0.05266995405f, 0.1979052192f, -0.1951904564f}, + {-0.05437139263f, 0.1978989265f, -0.1965559908f}, + {-0.05608468293f, 0.1978927022f, -0.1979066256f}, + {-0.05780969449f, 0.197886547f, -0.1992422582f}, + {-0.05954629593f, 0.1978804613f, -0.2005627866f}, + {-0.06129435501f, 0.1978744456f, -0.2018681104f}, + {-0.0630537386f, 0.1978685003f, -0.2031581302f}, + {-0.06482431273f, 0.1978626258f, -0.2044327477f}, + {-0.06660594255f, 0.1978568226f, -0.2056918658f}, + {-0.06839849238f, 0.1978510912f, -0.2069353887f}, + {-0.07020182572f, 0.197845432f, -0.2081632217f}, + {-0.07201580524f, 0.1978398455f, -0.2093752712f}, + {-0.0738402928f, 0.1978343319f, -0.210571445f}, + {-0.07567514944f, 0.1978288919f, -0.211751652f}, + {-0.07752023545f, 0.1978235257f, -0.2129158022f}, + {-0.07937541031f, 0.1978182338f, -0.2140638071f}, + {-0.08124053275f, 0.1978130166f, -0.2151955792f}, + {-0.08311546071f, 0.1978078745f, -0.2163110323f}, + {-0.08500005144f, 0.1978028079f, -0.2174100815f}, + {-0.08689416139f, 0.1977978171f, -0.2184926431f}, + {-0.08879764634f, 0.1977929026f, -0.2195586346f}, + {-0.09071036132f, 0.1977880647f, -0.2206079748f}, + {-0.09263216067f, 0.1977833038f, -0.2216405839f}, + {-0.09456289804f, 0.1977786203f, -0.2226563833f}, + {-0.0965024264f, 0.1977740145f, -0.2236552954f}, + {-0.09845059804f, 0.1977694867f, -0.2246372444f}, + {-0.1004072646f, 0.1977650374f, -0.2256021553f}, + {-0.1023722771f, 0.1977606668f, -0.2265499548f}, + {-0.1043454858f, 0.1977563752f, -0.2274805706f}, + {-0.1063267406f, 0.1977521631f, -0.2283939319f}, + {-0.1083158905f, 0.1977480307f, -0.2292899691f}, + {-0.110312784f, 0.1977439784f, -0.230168614f}, + {-0.1123172691f, 0.1977400063f, -0.2310297996f}, + {-0.1143291931f, 0.197736115f, -0.2318734604f}, + {-0.1163484028f, 0.1977323046f, -0.2326995322f}, + {-0.1183747445f, 0.1977285754f, -0.233507952f}, + {-0.1204080638f, 0.1977249278f, -0.2342986582f}, + {-0.1224482058f, 0.1977213619f, -0.2350715907f}, + {-0.1244950153f, 0.1977178781f, -0.2358266906f}, + {-0.1265483363f, 0.1977144767f, -0.2365639004f}, + {-0.1286080125f, 0.1977111579f, -0.2372831639f}, + {-0.130673887f, 0.1977079219f, -0.2379844263f}, + {-0.1327458025f, 0.197704769f, -0.2386676344f}, + {-0.1348236012f, 0.1977016994f, -0.2393327359f}, + {-0.1369071249f, 0.1976987134f, -0.2399796804f}, + {-0.1389962149f, 0.1976958112f, -0.2406084184f}, + {-0.1410907121f, 0.197692993f, -0.2412189022f}, + {-0.143190457f, 0.1976902591f, -0.2418110853f}, + {-0.1452952897f, 0.1976876096f, -0.2423849225f}, + {-0.14740505f, 0.1976850447f, -0.2429403701f}, + {-0.1495195771f, 0.1976825647f, -0.2434773859f}, + {-0.15163871f, 0.1976801697f, -0.2439959289f}, + {-0.1537622874f, 0.1976778599f, -0.2444959597f}, + {-0.1558901475f, 0.1976756355f, -0.2449774402f}, + {-0.1580221283f, 0.1976734966f, -0.2454403337f}, + {-0.1601580673f, 0.1976714435f, -0.245884605f}, + {-0.1622978021f, 0.1976694762f, -0.2463102202f}, + {-0.1644411696f, 0.197667595f, -0.2467171469f}, + {-0.1665880065f, 0.1976657999f, -0.2471053541f}, + {-0.1687381495f, 0.1976640911f, -0.2474748123f}, + {-0.1708914347f, 0.1976624688f, -0.2478254934f}, + {-0.1730476981f, 0.197660933f, -0.2481573706f}, + {-0.1752067757f, 0.1976594839f, -0.2484704186f}, + {-0.1773685028f, 0.1976581215f, -0.2487646136f}, + {-0.179532715f, 0.1976568461f, -0.2490399333f}, + {-0.1816992475f, 0.1976556576f, -0.2492963566f}, + {-0.1838679351f, 0.1976545563f, -0.249533864f}, + {-0.1860386128f, 0.197653542f, -0.2497524374f}, + {-0.1882111152f, 0.197652615f, -0.2499520602f}, + {-0.1903852769f, 0.1976517754f, -0.2501327172f}, + {-0.1925609324f, 0.1976510231f, -0.2502943947f}, + {-0.194737916f, 0.1976503582f, -0.2504370802f}, + {-0.1969160618f, 0.1976497808f, -0.250560763f}, + {-0.199095204f, 0.1976492909f, -0.2506654335f}, + {-0.2012751767f, 0.1976488886f, -0.250751084f}, + {-0.2034558138f, 0.1976485739f, -0.2508177078f}, + {-0.2056369493f, 0.1976483468f, -0.2508652998f}, + {-0.207818417f, 0.1976482073f, -0.2508938565f}, + {-0.2100000509f, 0.1976481554f, -0.2509033756f}, + {-0.2121816847f, 0.1976481912f, -0.2508938565f}, + {-0.2143631525f, 0.1976483147f, -0.2508652998f}, + {-0.2165442879f, 0.1976485258f, -0.2508177078f}, + {-0.218724925f, 0.1976488245f, -0.250751084f}, + {-0.2209048977f, 0.1976492108f, -0.2506654335f}, + {-0.2230840399f, 0.1976496847f, -0.250560763f}, + {-0.2252621858f, 0.1976502461f, -0.2504370802f}, + {-0.2274391693f, 0.1976508949f, -0.2502943947f}, + {-0.2296148248f, 0.1976516313f, -0.2501327172f}, + {-0.2317889866f, 0.197652455f, -0.2499520602f}, + {-0.233961489f, 0.197653366f, -0.2497524374f}, + {-0.2361321667f, 0.1976543643f, -0.249533864f}, + {-0.2383008543f, 0.1976554497f, -0.2492963566f}, + {-0.2404673868f, 0.1976566223f, -0.2490399333f}, + {-0.242631599f, 0.1976578818f, -0.2487646136f}, + {-0.2447933261f, 0.1976592283f, -0.2484704186f}, + {-0.2469524037f, 0.1976606615f, -0.2481573706f}, + {-0.2491086672f, 0.1976621814f, -0.2478254934f}, + {-0.2512619524f, 0.197663788f, -0.2474748123f}, + {-0.2534120953f, 0.1976654809f, -0.2471053541f}, + {-0.2555589323f, 0.1976672603f, -0.2467171469f}, + {-0.2577022998f, 0.1976691258f, -0.2463102202f}, + {-0.2598420346f, 0.1976710773f, -0.245884605f}, + {-0.2619779737f, 0.1976731147f, -0.2454403337f}, + {-0.2641099545f, 0.1976752379f, -0.2449774402f}, + {-0.2662378146f, 0.1976774467f, -0.2444959597f}, + {-0.2683613919f, 0.1976797409f, -0.2439959289f}, + {-0.2704805249f, 0.1976821204f, -0.2434773859f}, + {-0.272595052f, 0.1976845848f, -0.2429403701f}, + {-0.2747048123f, 0.1976871342f, -0.2423849225f}, + {-0.276809645f, 0.1976897683f, -0.2418110853f}, + {-0.27890939f, 0.1976924868f, -0.2412189022f}, + {-0.2810038872f, 0.1976952896f, -0.2406084184f}, + {-0.2830929772f, 0.1976981764f, -0.2399796804f}, + {-0.2851765009f, 0.1977011471f, -0.2393327359f}, + {-0.2872542997f, 0.1977042014f, -0.2386676344f}, + {-0.2893262152f, 0.1977073391f, -0.2379844263f}, + {-0.2913920897f, 0.1977105599f, -0.2372831639f}, + {-0.2934517659f, 0.1977138636f, -0.2365639004f}, + {-0.295505087f, 0.19771725f, -0.2358266906f}, + {-0.2975518965f, 0.1977207187f, -0.2350715907f}, + }; + result[1].hip_origin_b_m = Eigen::Vector3f{0.0f, -0.1828f, 0.00083241f}; + result[1].hip_axis_b = Eigen::Vector3f{3.673165765e-06f, -0.9999894031f, 0.004603656943f}; + result[1].wheel_axis_b = Eigen::Vector3f{3.673165765e-06f, -0.9999894031f, 0.004603656943f}; + result[1].delta_rad = { + -1.438157734f, -1.428308038f, -1.418451527f, -1.408588099f, -1.398717646f, + -1.38884006f, -1.37895523f, -1.369063046f, -1.359163392f, -1.349256152f, + -1.339341209f, -1.329418442f, -1.319487728f, -1.309548943f, -1.29960196f, + -1.289646649f, -1.279682879f, -1.269710516f, -1.259729423f, -1.249739461f, + -1.239740487f, -1.229732359f, -1.219714927f, -1.209688042f, -1.199651552f, + -1.189605299f, -1.179549126f, -1.169482869f, -1.159406364f, -1.149319441f, + -1.139221929f, -1.129113653f, -1.118994432f, -1.108864085f, -1.098722424f, + -1.08856926f, -1.078404399f, -1.068227642f, -1.058038786f, -1.047837625f, + -1.037623948f, -1.027397539f, -1.017158177f, -1.006905639f, -0.9966396939f, + -0.9863601067f, -0.9760666375f, -0.9657590409f, -0.955437066f, -0.9451004565f, + -0.9347489499f, -0.9243822783f, -0.9140001671f, -0.9036023359f, -0.8931884976f, + -0.8827583585f, -0.8723116179f, -0.8618479682f, -0.8513670944f, -0.8408686742f, + -0.8303523774f, -0.8198178657f, -0.8092647931f, -0.7986928046f, -0.7881015368f, + -0.7774906172f, -0.7668596641f, -0.7562082863f, -0.7455360825f, -0.7348426415f, + -0.7241275414f, -0.7133903496f, -0.7026306223f, -0.691847904f, -0.6810417274f, + -0.6702116129f, -0.6593570681f, -0.6484775873f, -0.6375726515f, -0.6266417274f, + -0.6156842671f, -0.6046997079f, -0.5936874713f, -0.5826469629f, -0.5715775714f, + -0.5604786683f, -0.5493496074f, -0.5381897237f, -0.5269983333f, -0.5157747322f, + -0.5045181959f, -0.4932279785f, -0.4819033121f, -0.4705434056f, -0.4591474442f, + -0.4477145883f, -0.4362439727f, -0.4247347055f, -0.4131858671f, -0.4015965091f, + -0.3899656533f, -0.3782922904f, -0.3665753787f, -0.354813843f, -0.3430065732f, + -0.3311524225f, -0.3192502066f, -0.3072987017f, -0.2952966426f, -0.2832427218f, + -0.2711355869f, -0.258973839f, -0.2467560307f, -0.234480664f, -0.2221461883f, + -0.2097509977f, -0.1972934285f, -0.1847717574f, -0.1721841977f, -0.1595288973f, + -0.1468039353f, -0.1340073189f, -0.12113698f, -0.1081907718f, -0.09516646489f, + -0.08206174353f, -0.06887420122f, -0.05560133647f, -0.04224054809f, -0.02878913023f, + -0.0152442672f, -0.001603027888f, 0.01213764007f, 0.02598091652f, 0.0399301154f, + 0.05398869174f, 0.06816024909f, 0.08244854742f, 0.09685751152f, 0.11139124f, + 0.1260540146f, 0.1408503105f, 0.1557848071f}; + result[1].wheel_center_b_m = { + {-0.2975517863f, -0.1988789634f, -0.233264101f}, + {-0.2955049768f, -0.1988824321f, -0.2340192009f}, + {-0.2934516557f, -0.1988858185f, -0.2347564107f}, + {-0.2913919795f, -0.1988891222f, -0.2354756742f}, + {-0.289326105f, -0.198892343f, -0.2361769366f}, + {-0.2872541895f, -0.1988954807f, -0.2368601447f}, + {-0.2851763907f, -0.198898535f, -0.2375252462f}, + {-0.283092867f, -0.1989015057f, -0.2381721907f}, + {-0.281003777f, -0.1989043925f, -0.2388009287f}, + {-0.2789092798f, -0.1989071953f, -0.2394114125f}, + {-0.2768095348f, -0.1989099138f, -0.2400035956f}, + {-0.2747047021f, -0.1989125479f, -0.2405774328f}, + {-0.2725949418f, -0.1989150972f, -0.2411328804f}, + {-0.2704804147f, -0.1989175617f, -0.2416698962f}, + {-0.2683612818f, -0.1989199412f, -0.2421884392f}, + {-0.2662377044f, -0.1989222354f, -0.24268847f}, + {-0.2641098443f, -0.1989244441f, -0.2431699505f}, + {-0.2619778635f, -0.1989265673f, -0.243632844f}, + {-0.2598419244f, -0.1989286048f, -0.2440771153f}, + {-0.2577021896f, -0.1989305563f, -0.2445027305f}, + {-0.2555588221f, -0.1989324218f, -0.2449096572f}, + {-0.2534119852f, -0.1989342011f, -0.2452978644f}, + {-0.2512618422f, -0.1989358941f, -0.2456673226f}, + {-0.249108557f, -0.1989375006f, -0.2460180037f}, + {-0.2469522935f, -0.1989390206f, -0.2463498809f}, + {-0.244793216f, -0.1989404538f, -0.2466629289f}, + {-0.2426314888f, -0.1989418003f, -0.2469571239f}, + {-0.2404672766f, -0.1989430598f, -0.2472324436f}, + {-0.2383007441f, -0.1989442324f, -0.2474888669f}, + {-0.2361320565f, -0.1989453178f, -0.2477263743f}, + {-0.2339613788f, -0.1989463161f, -0.2479449477f}, + {-0.2317888764f, -0.1989472271f, -0.2481445705f}, + {-0.2296147146f, -0.1989480508f, -0.2483252275f}, + {-0.2274390591f, -0.1989487871f, -0.2484869049f}, + {-0.2252620756f, -0.198949436f, -0.2486295905f}, + {-0.2230839297f, -0.1989499974f, -0.2487532732f}, + {-0.2209047875f, -0.1989504713f, -0.2488579438f}, + {-0.2187248149f, -0.1989508576f, -0.2489435943f}, + {-0.2165441778f, -0.1989511563f, -0.2490102181f}, + {-0.2143630423f, -0.1989513674f, -0.2490578101f}, + {-0.2121815745f, -0.1989514908f, -0.2490863668f}, + {-0.2099999407f, -0.1989515267f, -0.2490958859f}, + {-0.2078183068f, -0.1989514748f, -0.2490863668f}, + {-0.2056368391f, -0.1989513353f, -0.2490578101f}, + {-0.2034557036f, -0.1989511082f, -0.2490102181f}, + {-0.2012750665f, -0.1989507935f, -0.2489435943f}, + {-0.1990950938f, -0.1989503912f, -0.2488579438f}, + {-0.1969159516f, -0.1989499013f, -0.2487532732f}, + {-0.1947378058f, -0.1989493239f, -0.2486295905f}, + {-0.1925608222f, -0.198948659f, -0.2484869049f}, + {-0.1903851668f, -0.1989479067f, -0.2483252275f}, + {-0.188211005f, -0.198947067f, -0.2481445705f}, + {-0.1860385026f, -0.1989461401f, -0.2479449477f}, + {-0.1838678249f, -0.1989451258f, -0.2477263743f}, + {-0.1816991373f, -0.1989440245f, -0.2474888669f}, + {-0.1795326049f, -0.198942836f, -0.2472324436f}, + {-0.1773683927f, -0.1989415606f, -0.2469571239f}, + {-0.1752066655f, -0.1989401982f, -0.2466629289f}, + {-0.1730475879f, -0.1989387491f, -0.2463498809f}, + {-0.1708913245f, -0.1989372133f, -0.2460180037f}, + {-0.1687380393f, -0.198935591f, -0.2456673226f}, + {-0.1665878963f, -0.1989338822f, -0.2452978644f}, + {-0.1644410594f, -0.1989320871f, -0.2449096572f}, + {-0.1622976919f, -0.1989302059f, -0.2445027305f}, + {-0.1601579572f, -0.1989282386f, -0.2440771153f}, + {-0.1580220181f, -0.1989261855f, -0.243632844f}, + {-0.1558900373f, -0.1989240466f, -0.2431699505f}, + {-0.1537621772f, -0.1989218222f, -0.24268847f}, + {-0.1516385998f, -0.1989195124f, -0.2421884392f}, + {-0.1495194669f, -0.1989171174f, -0.2416698962f}, + {-0.1474049398f, -0.1989146374f, -0.2411328804f}, + {-0.1452951795f, -0.1989120725f, -0.2405774328f}, + {-0.1431903468f, -0.198909423f, -0.2400035956f}, + {-0.1410906019f, -0.1989066891f, -0.2394114125f}, + {-0.1389961047f, -0.1989038709f, -0.2388009287f}, + {-0.1369070147f, -0.1989009687f, -0.2381721907f}, + {-0.134823491f, -0.1988979827f, -0.2375252462f}, + {-0.1327456923f, -0.1988949131f, -0.2368601447f}, + {-0.1306737768f, -0.1988917602f, -0.2361769366f}, + {-0.1286079023f, -0.1988885242f, -0.2354756742f}, + {-0.1265482261f, -0.1988852054f, -0.2347564107f}, + {-0.1244949051f, -0.198881804f, -0.2340192009f}, + {-0.1224480956f, -0.1988783202f, -0.233264101f}, + {-0.1204079536f, -0.1988747543f, -0.2324911685f}, + {-0.1183746343f, -0.1988711067f, -0.2317004623f}, + {-0.1163482926f, -0.1988673775f, -0.2308920425f}, + {-0.1143290829f, -0.1988635671f, -0.2300659707f}, + {-0.1123171589f, -0.1988596758f, -0.2292223099f}, + {-0.1103126738f, -0.1988557037f, -0.2283611242f}, + {-0.1083157803f, -0.1988516514f, -0.2274824794f}, + {-0.1063266304f, -0.198847519f, -0.2265864422f}, + {-0.1043453756f, -0.1988433069f, -0.2256730809f}, + {-0.1023721669f, -0.1988390153f, -0.2247424651f}, + {-0.1004071544f, -0.1988346447f, -0.2237946656f}, + {-0.09845048784f, -0.1988301954f, -0.2228297547f}, + {-0.0965023162f, -0.1988256676f, -0.2218478057f}, + {-0.09456278784f, -0.1988210618f, -0.2208488936f}, + {-0.09263205047f, -0.1988163783f, -0.2198330942f}, + {-0.09071025112f, -0.1988116174f, -0.2188004851f}, + {-0.08879753614f, -0.1988067795f, -0.2177511449f}, + {-0.0868940512f, -0.198801865f, -0.2166851534f}, + {-0.08499994124f, -0.1987968742f, -0.2156025918f}, + {-0.08311535052f, -0.1987918076f, -0.2145035426f}, + {-0.08124042255f, -0.1987866655f, -0.2133880895f}, + {-0.07937530012f, -0.1987814483f, -0.2122563174f}, + {-0.07752012526f, -0.1987761564f, -0.2111083125f}, + {-0.07567503925f, -0.1987707902f, -0.2099441623f}, + {-0.0738401826f, -0.1987653502f, -0.2087639553f}, + {-0.07201569505f, -0.1987598366f, -0.2075677815f}, + {-0.07020171553f, -0.1987542501f, -0.206355732f}, + {-0.06839838219f, -0.1987485909f, -0.205127899f}, + {-0.06660583235f, -0.1987428595f, -0.2038843761f}, + {-0.06482420253f, -0.1987370563f, -0.202625258f}, + {-0.06305362841f, -0.1987311818f, -0.2013506405f}, + {-0.06129424482f, -0.1987252365f, -0.2000606207f}, + {-0.05954618574f, -0.1987192207f, -0.1987552969f}, + {-0.0578095843f, -0.198713135f, -0.1974347685f}, + {-0.05608457274f, -0.1987069799f, -0.1960991359f}, + {-0.05437128243f, -0.1987007556f, -0.1947485011f}, + {-0.05266984385f, -0.1986944629f, -0.1933829667f}, + {-0.05098038657f, -0.198688102f, -0.1920026368f}, + {-0.04930303924f, -0.1986816736f, -0.1906076165f}, + {-0.0476379296f, -0.1986751781f, -0.1891980121f}, + {-0.04598518446f, -0.198668616f, -0.1877739308f}, + {-0.04434492968f, -0.1986619877f, -0.1863354812f}, + {-0.04271729017f, -0.1986552939f, -0.1848827728f}, + {-0.04110238988f, -0.198648535f, -0.1834159161f}, + {-0.03950035179f, -0.1986417115f, -0.181935023f}, + {-0.03791129791f, -0.198634824f, -0.1804402062f}, + {-0.03633534925f, -0.1986278729f, -0.1789315795f}, + {-0.03477262581f, -0.1986208589f, -0.1774092579f}, + {-0.03322324662f, -0.1986137823f, -0.1758733572f}, + {-0.03168732965f, -0.1986066439f, -0.1743239944f}, + {-0.03016499188f, -0.1985994441f, -0.1727612875f}, + {-0.02865634924f, -0.1985921834f, -0.1711853555f}, + {-0.02716151661f, -0.1985848624f, -0.1695963184f}, + {-0.02568060784f, -0.1985774818f, -0.1679942973f}, + {-0.0242137357f, -0.1985700419f, -0.1663794141f}, + {-0.0227610119f, -0.1985625435f, -0.1647517918f}, + {-0.02132254707f, -0.198554987f, -0.1631115543f}, + {-0.01989845076f, -0.1985473731f, -0.1614588267f}, + {-0.01848883141f, -0.1985397024f, -0.1597937347f}, + {-0.01709379637f, -0.1985319753f, -0.1581164051f}, + }; + return result; +} +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/features.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/features.hpp new file mode 100644 index 000000000..e315cdce4 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/features.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include + +namespace rmcs_core::controller::chassis { + +template +struct BasicWheelLegFeatures { + std::array command{}, gyro{}, gravity{}; + Scalar height = 0; + std::array position{}, velocity{}, previous_action{}; + std::array auxiliary{1, 0, 0, 0, 0, 0, 0}; +}; + +using WheelLegFeatures = BasicWheelLegFeatures; +using StandingReferenceFeatures = BasicWheelLegFeatures; + +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/local_odometry.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/local_odometry.hpp new file mode 100644 index 000000000..24180f175 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/local_odometry.hpp @@ -0,0 +1,197 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../model/prone_odometry.hpp" +#include "features.hpp" + +namespace rmcs_core::controller::chassis { + +struct LocalStandingState { + bool source_active = false; + bool wheel_fault = false; + bool reference_active = false; + bool valid = false; + std::array source_origin{}, source_last{}; + std::array source_speed{}, anchor_m{}, filtered_m{}, displacement_m{}; + float capture_age_s = 0.F; + std::optional source_tick, last_tick; +}; + +class LocalStandingOdometry { +public: + void reset() { state_ = {}; } + + void sample( + std::uint64_t tick, const std::array& wheels, + const std::array& nominal, StandingReferenceFeatures& features, bool enabled) { + if (state_.last_tick && tick < *state_.last_tick) + throw std::invalid_argument("Policy clock moved backwards; reset local odometry"); + const bool requested = enabled && std::isfinite(features.command[0]) && std::isfinite(features.command[1]) + && std::isfinite(features.command[2]) && std::abs(features.command[0]) < .001F + && std::abs(features.command[1]) < .001F && std::abs(features.command[2]) < .001F; + const auto local = local_wheel_angles_(tick, wheels, features, requested); + std::array q{}; + std::array velocity{}; + for (std::size_t i = 0; i < 6; ++i) { + q[i] = i < 4 ? features.position[i] + nominal[i] : local[i - 4]; + velocity[i] = 10.F * features.velocity[i]; + } + const standing_reference::Vector3 gravity = features.gravity; + const standing_reference::Vector3 gyro{2.F * features.gyro[0], 2.F * features.gyro[1], 2.F * features.gyro[2]}; + const float magnitude = standing_reference::norm(gravity); + const bool imu_valid = standing_reference::finite(gravity) + && standing_reference::finite(gyro) && magnitude > .5F + && magnitude < 1.5F; + const float gz = gravity[2] / std::max(magnitude, 1e-9F); + const auto& table = standing_reference::friction_geometry110(); + auto projected = q; + bool projection_valid = true; + bool standing = imu_valid && gz < static_cast(-.9659258262890683); + for (std::size_t side = 0; side < 2; ++side) { + const auto& geometry = table[side]; + const std::size_t hip = side * 2; + const float difference = q[hip + 1] - q[hip]; + const float delta = std::atan2(std::sin(difference), std::cos(difference)); + const float lower = geometry.knots.front().delta_rad; + const float upper = geometry.knots.back().delta_rad; + const float bounded = std::clamp(delta, lower, upper); + const float overshoot = std::abs(delta - bounded); + projection_valid &= + std::isfinite(q[hip]) && std::isfinite(q[hip + 1]) && overshoot <= .012F; + if (overshoot > 0.F) { + const float margin = 16.F * std::numeric_limits::epsilon() + * (1.F + std::max(std::abs(q[hip]), std::abs(q[hip + 1]))); + const float interior = std::clamp(delta, lower + margin, upper - margin); + const float error = delta - interior; + projected[hip] += .5F * error; + projected[hip + 1] -= .5F * error; + } + // EncoderLegGeometry uses the original sensor legs, a 1 mrad + // validity margin, and the clamped physical table for support height. + const auto center = wheel_center_(geometry, q[hip], bounded); + standing &= std::isfinite(delta) && delta >= lower - .001F && delta <= upper + .001F + && -center[2] + .06F > .20F; + } + const auto coordinate = + standing_reference::wheel_angle_coordinate(table, projected, gravity); + const bool valid = standing && projection_valid && coordinate.valid && !state_.wheel_fault + && std::all_of(velocity.begin(), velocity.end(), [](float value) { + return std::isfinite(value); + }); + const bool active_request = requested && state_.source_active; + const bool eligible = active_request && valid; + const bool fresh = !state_.last_tick || *state_.last_tick != tick; + const bool reset_reference = !active_request || (imu_valid && gz > -.5F); + if (reset_reference) { + state_.reference_active = false; + state_.capture_age_s = 0.F; + state_.filtered_m = {}; + } + if (fresh && !state_.reference_active && !reset_reference) { + state_.capture_age_s = eligible ? state_.capture_age_s + .01F : 0.F; + if (eligible && state_.capture_age_s + 1e-7F >= .05F) { + state_.anchor_m = coordinate.coordinate_m; + state_.reference_active = true; + state_.filtered_m = {}; + } + } + state_.valid = active_request && valid && state_.reference_active && !reset_reference; + if (state_.valid && fresh) { + constexpr float alpha = static_cast(.01 / (.05 + .01)); + for (std::size_t side = 0; side < 2; ++side) { + const float limit = .2F; + const float delta = std::clamp( + coordinate.coordinate_m[side] - state_.anchor_m[side], -limit, limit); + state_.filtered_m[side] += alpha * (delta - state_.filtered_m[side]); + } + } + state_.displacement_m = state_.valid ? state_.filtered_m : std::array{}; + state_.last_tick = tick; + features.position[4] = 5.F * state_.displacement_m[0]; + features.position[5] = 5.F * state_.displacement_m[1]; + features.auxiliary[3] = state_.valid ? 1.F : 0.F; + } + + const LocalStandingState& state() const { return state_; } + +private: + std::array local_wheel_angles_( + std::uint64_t tick, const std::array& wheel, const StandingReferenceFeatures& features, + bool requested) { + const std::array speed{10.F * features.velocity[4], 10.F * features.velocity[5]}; + const bool finite = std::isfinite(wheel[0]) && std::isfinite(wheel[1]) + && std::isfinite(speed[0]) && std::isfinite(speed[1]); + const bool start = requested && !state_.source_active && finite; + if (start) + state_.source_origin = wheel; + if (!requested) + state_.wheel_fault = false; + const bool fresh = requested && state_.source_active + && (!state_.source_tick || *state_.source_tick != tick); + if (requested && !finite) + state_.wheel_fault = true; + if (fresh && state_.source_tick) { + const float seconds = + static_cast(std::max(tick - *state_.source_tick, 1)) * .01F; + for (std::size_t side = 0; side < 2; ++side) { + const float bound = + .02F + + 3.F * seconds + * std::max(std::abs(speed[side]), std::abs(state_.source_speed[side])); + state_.wheel_fault |= std::abs(wheel[side] - state_.source_last[side]) > bound; + } + } + state_.source_active = requested && (state_.source_active || start); + if (requested && finite && (!state_.source_tick || *state_.source_tick != tick)) { + state_.source_last = wheel; + state_.source_speed = speed; + state_.source_tick = tick; + } + std::array result{}; + if (state_.source_active && !state_.wheel_fault && finite) + for (std::size_t side = 0; side < 2; ++side) + result[side] = static_cast(wheel[side] - state_.source_origin[side]); + return result; + } + + static standing_reference::Vector3 + wheel_center_(const standing_reference::SideGeometry& geometry, float hip, float delta) { + const auto upper_iterator = std::upper_bound( + geometry.knots.begin(), geometry.knots.end(), delta, + [](float value, const auto& knot) { return value < knot.delta_rad; }); + const auto index = std::clamp( + upper_iterator - geometry.knots.begin(), 1, geometry.knots.size() - 1); + const auto& lower = geometry.knots[index - 1]; + const auto& upper = geometry.knots[index]; + const float blend = + std::clamp((delta - lower.delta_rad) / (upper.delta_rad - lower.delta_rad), 0.F, 1.F); + standing_reference::Vector3 value{}; + for (std::size_t j = 0; j < 3; ++j) + value[j] = lower.center_m[j] + blend * (upper.center_m[j] - lower.center_m[j]) + - geometry.hip_origin_m[j]; + const auto& axis = geometry.hip_axis; + const standing_reference::Vector3 cross{ + axis[1] * value[2] - axis[2] * value[1], axis[2] * value[0] - axis[0] * value[2], + axis[0] * value[1] - axis[1] * value[0]}; + const float dot = axis[0] * value[0] + axis[1] * value[1] + axis[2] * value[2]; + const float cosine = std::cos(hip - geometry.hip_reference_rad); + const float sine = std::sin(hip - geometry.hip_reference_rad); + standing_reference::Vector3 center{}; + for (std::size_t j = 0; j < 3; ++j) + center[j] = cosine * value[j] + sine * cross[j] + (1.F - cosine) * axis[j] * dot + + geometry.hip_origin_m[j]; + return center; + } + + LocalStandingState state_; +}; + +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/parameters.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/parameters.hpp new file mode 100644 index 000000000..792a3b37a --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/parameters.hpp @@ -0,0 +1,56 @@ +#pragma once +#include "../joint_coordinates.hpp" +#include +namespace rmcs_core::controller::chassis { +inline constexpr std::string_view kModelSpecSha256 = + "8a9d759525c48955e8965b2e14b9e158c1d8fc9f3d68f33e4024a3c94bf8f812"; +struct PolicyParameters { + std::string_view name, observation_version, observation_sha; + double policy_hz, leg_scale, leg_bound, wheel_bound, leg_kp, leg_kd, gas_gain; + double forward_limit, yaw_limit; + LegBounds winding_bounds; + std::string_view model_spec_sha256 = kModelSpecSha256; + bool jump_control = false; +}; +inline bool wheelbipe_policy(const PolicyParameters& p) { + return p.name == "wheelbipe" || p.jump_control; +} +inline JointVector decoded_action_target( + const JointVector& decoded, const JointVector& q, const LegBounds& bounds) { + LegVector desired{}, current{}; + for (int i = 0; i < 4; ++i) { + desired[i] = kNominalPosition[i] + decoded[i]; + current[i] = q[i]; + } + const auto legs = paired_targets(desired, current, bounds); + auto target = decoded; + std::copy(legs.begin(), legs.end(), target.begin()); + return target; +} +inline JointVector + action_target(const PolicyParameters& p, const JointVector& raw, const JointVector& q) { + JointVector target; + LegVector desired{}, current{}; + for (int i = 0; i < 4; ++i) { + desired[i] = + kNominalPosition[i] + std::clamp(raw[i] * p.leg_scale, -p.leg_bound, p.leg_bound); + current[i] = q[i]; + } + const auto legs = paired_targets(desired, current, p.winding_bounds); + std::copy(legs.begin(), legs.end(), target.begin()); + for (int i = 4; i < 6; ++i) + target[i] = std::clamp(raw[i] * 10., -90., 90.); + return target; +} +inline JointVector executed_history(const PolicyParameters& p, const JointVector& raw) { + if (wheelbipe_policy(p)) + return raw; + auto result = raw; + for (int i = 0; i < 6; ++i) { + const double scale = i < 4 ? p.leg_scale : 10.; + const double bound = i < 4 ? p.leg_bound : p.wheel_bound; + result[i] = std::clamp(raw[i] * scale, -bound, bound) / scale; + } + return result; +} +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/prone_motion.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/prone_motion.hpp new file mode 100644 index 000000000..e0c023a65 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/prone_motion.hpp @@ -0,0 +1,115 @@ +#pragma once +#include "local_odometry.hpp" +#include "parameters.hpp" +#include +namespace rmcs_core::controller::chassis { +inline constexpr LegBounds kProneWindingBounds{ + {{-.14062820701680934, 1.3692123767724917}, {-.1406283965709381, 1.3692125038958158}}}; +inline constexpr PolicyParameters kProneMotion{ + "prone_motion", + "wheel-leg-prone-motion-v4-odom-v2", + "6613ab1b2a538fc31558b668117dbcb77bed4f84e93133d9c087c5752ec6c967", + 100., + .25, + 4., + 90., + 160., + 2.5, + 1., + 2., + 10.471975511965978, + kProneWindingBounds}; +inline std::array measured_policy_history(const std::array& position) { + constexpr JointVector frozen_nominal{-.42, .13742282595395358, .42, -.1374155762580851, 0., 0.}; + std::array result{}; + for (int side = 0; side < 2; ++side) { + const int hip = 2 * side; + const float sign = side == 0 ? 1.F : -1.F; + const float first = + static_cast(position[hip]) - static_cast(kNominalPosition[hip]); + const float second = + static_cast(position[hip + 1]) - static_cast(kNominalPosition[hip + 1]); + const float nominal_delta = + static_cast(frozen_nominal[hip + 1] - frozen_nominal[hip]); + const auto& bounds = kProneWindingBounds[side]; + const float delta = std::clamp( + sign * (nominal_delta + second - first), static_cast(bounds[0]), + static_cast(bounds[1])); + const float offset_delta = sign * delta - nominal_delta; + const float mean = .5F * (first + second); + const float common = std::atan2(std::sin(mean), std::cos(mean)); + result[hip] = static_cast((common - .5F * offset_delta) / .25F); + result[hip + 1] = static_cast((common + .5F * offset_delta) / .25F); + } + return result; +} + +class ProneMotion { +public: + void reset() { odometry_.reset(); } + std::optional sample( + std::uint64_t policy_tick, const JointVector& position, const JointVector& velocity, + const std::array& gyro, const std::array& gravity, + const std::array& command, double height, const JointVector& applied_action, + bool active) { + StandingReferenceFeatures features; + std::array q{}; + + for (std::size_t i = 0; i < 3; ++i) { + features.command[i] = static_cast(command[i]); + features.gyro[i] = .5F * static_cast(gyro[i]); + features.gravity[i] = static_cast(gravity[i]); + } + features.height = 5.F * static_cast(height); + for (std::size_t i = 0; i < 6; ++i) { + q[i] = i < 4 ? static_cast(position[i]) : 0.F; + features.velocity[i] = .1F * static_cast(velocity[i]); + features.previous_action[i] = static_cast(applied_action[i]); + if (i < 4) { + const float delta = q[i] - static_cast(kNominalPosition[i]); + features.position[i] = std::atan2(std::sin(delta), std::cos(delta)); + } + } + bool finite_inputs = + std::isfinite(features.height) + && std::all_of(q.begin(), q.end(), [](float value) { return std::isfinite(value); }); + for (auto* values : {&features.command, &features.gyro, &features.gravity}) + for (auto& value : *values) { + finite_inputs &= std::isfinite(value); + value = std::clamp(value, -100.F, 100.F); + } + for (auto* values : {&features.position, &features.velocity, &features.previous_action}) + for (auto& value : *values) { + finite_inputs &= std::isfinite(value); + value = std::clamp(value, -100.F, 100.F); + } + features.height = std::clamp(features.height, -100.F, 100.F); + { + std::array nominal{}; + for (std::size_t i = 0; i < nominal.size(); ++i) + nominal[i] = static_cast(kNominalPosition[i]); + odometry_.sample(policy_tick, {position[4], position[5]}, nominal, features, active); + } + if (!finite_inputs + || !std::all_of(features.position.begin(), features.position.end(), [](float value) { + return std::isfinite(value); + })) + return std::nullopt; + WheelLegFeatures physical; + physical.command = command; + physical.height = height; + physical.gyro = gyro; + physical.gravity = gravity; + physical.velocity = velocity; + physical.previous_action = applied_action; + for (std::size_t i = 0; i < 6; ++i) + physical.position[i] = features.position[i]; + for (std::size_t i = 0; i < 7; ++i) + physical.auxiliary[i] = features.auxiliary[i]; + return physical; + } + +private: + LocalStandingOdometry odometry_; +}; +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/selection.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/selection.hpp new file mode 100644 index 000000000..1d330c290 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/selection.hpp @@ -0,0 +1,16 @@ +#pragma once +#include "prone_motion.hpp" +#include "wheelbipe.hpp" +#include "wheelbipe_jump.hpp" +#include +namespace rmcs_core::controller::chassis { +inline const PolicyParameters& policy_parameters(std::string_view name) { + if (name == kProneMotion.name) + return kProneMotion; + if (name == kWheelbipe.name) + return kWheelbipe; + if (name == kWheelbipeJump.name) + return kWheelbipeJump; + throw std::invalid_argument("Unsupported wheel-leg policy: " + std::string{name}); +} +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/wheelbipe.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/wheelbipe.hpp new file mode 100644 index 000000000..07c0faa7c --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/wheelbipe.hpp @@ -0,0 +1,56 @@ +#pragma once +#include "features.hpp" +#include "parameters.hpp" +#include +#include +namespace rmcs_core::controller::chassis { +inline constexpr PolicyParameters kWheelbipe{ + "wheelbipe", + "wheelbipe-v202-foundation-v2", + "bd8397a8cb057c5d7859ba79ffbc1310c7c75d2a9ddcfdb7ea844d02db001e69", + 50., + .5, + 1e100, + 1e100, + 60., + 2., + 0., + 2.7, + 10.471975511965978, + {{{-.1408932677945942, 1.4282761689185115}, {-.14084732125908495, 1.4283102010168576}}}}; +class Wheelbipe { +public: + void reset() {} + std::optional sample( + std::uint64_t, const JointVector& position, const JointVector& velocity, + const std::array& gyro, const std::array& gravity, + const std::array& command, double height, const JointVector& previous_action, + bool) { + WheelLegFeatures features; + for (std::size_t i = 0; i < 3; ++i) { + if (!std::isfinite(command[i]) || !std::isfinite(gyro[i]) || !std::isfinite(gravity[i])) + return std::nullopt; + features.command[i] = command[i]; + features.gyro[i] = gyro[i]; + features.gravity[i] = gravity[i]; + } + if (!std::isfinite(height)) + return std::nullopt; + features.height = height; + const auto nominal = paired_targets( + {kNominalPosition[0], kNominalPosition[1], kNominalPosition[2], kNominalPosition[3]}, + {position[0], position[1], position[2], position[3]}, kWheelbipe.winding_bounds); + for (std::size_t i = 0; i < position.size(); ++i) { + const auto j = i; + if (!std::isfinite(position[j]) || !std::isfinite(velocity[j]) + || !std::isfinite(previous_action[j])) + return std::nullopt; + if (i < 4) + features.position[i] = position[j] - nominal[j]; + features.velocity[i] = velocity[j]; + features.previous_action[i] = previous_action[j]; + } + return features; + } +}; +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/wheelbipe_jump.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/wheelbipe_jump.hpp new file mode 100644 index 000000000..c63b84eb4 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/policy/wheelbipe_jump.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include "../model/jump_v4.hpp" +#include "wheelbipe.hpp" + +namespace rmcs_core::controller::chassis { + +inline constexpr PolicyParameters kWheelbipeJump = [] { + auto p = kWheelbipe; + p.name = "wheelbipe-jump-v4"; + p.observation_version = "wheelbipe-v202-foundation-jump-v4"; + p.observation_sha = wheelbipe_jump_v4::kObservationSha256; + p.model_spec_sha256 = wheelbipe_jump_v4::kModelSpecSha256; + p.leg_kp = wheelbipe_jump_v4::kLegKp; + p.leg_kd = wheelbipe_jump_v4::kLegKd; + p.winding_bounds = wheelbipe_jump_v4::kBounds; + p.jump_control = true; + return p; +}(); + +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/recovery.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/recovery.cpp new file mode 100644 index 000000000..bf5ab9ba4 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/recovery.cpp @@ -0,0 +1,761 @@ +#include "recovery.hpp" + +#include "model/body_geometry.hpp" + +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis::wheel_leg { +namespace { +constexpr float kPi = std::numbers::pi_v; +constexpr float kTau = 2.0f * kPi; +constexpr float kPushOffClearance = 0.03f; +// Reserve margin between the conditional wheel plane and the real support surface. +constexpr float kCaptureClearance = 0.025f; +constexpr float radians(double degrees) { + return static_cast(degrees * std::numbers::pi / 180.0); +} +float wrapped(float value) { return std::atan2(std::sin(value), std::cos(value)); } +bool positive_finite(double value) { return std::isfinite(value) && value > 0.0; } +} // namespace + +std::string_view recovery_phase_name(JointReferenceRecoveryPhase phase) { + constexpr std::array names{"SELECT", "FOLD", "PLANT", "ORBIT", + "THRUST", "SIDE", "CAPTURE", "PREPARE", + "BLEND", "RL", "FAILED"}; + const auto index = static_cast(phase); + return index < names.size() ? names[index] : "UNKNOWN"; +} + +JointReferenceRecoveryController::JointReferenceRecoveryController( + const JointReferenceRecoveryConfig& config) + : config_(config) { + const std::array values{ + config.dt, + config.dynamic_capture_height_min, + config.dynamic_capture_height_max, + config.stable_seconds, + config.max_script_seconds, + config.prepare_speed_rad_s, + config.insert_speed_rad_s, + config.push_speed_rad_s, + config.orbit_speed_rad_s, + config.capture_speed_rad_s, + config.side_speed_rad_s, + config.side_angle_rad, + config.plant_angle_rad, + config.orbit_turns, + config.handover_height_min, + config.handover_height_max, + config.handover_tilt_rad, + config.handover_gyro_max, + config.reroute_tilt_rad, + config.reroute_support_lost_seconds, + config.pitch_kp_nm_rad, + config.pitch_kd_nm_s_rad, + config.wheel_damping_nm_s_rad, + config.prepare_tilt_limit_deg, + config.capture_tilt_limit_deg}; + if (!std::all_of(values.begin(), values.end(), positive_finite) + || (config.dt != 0.005 && config.dt != 0.001) || !std::isfinite(config.blend_seconds) + || (config.blend_seconds != 0.0 && config.blend_seconds != 0.2) + || (config.dynamic_takeover && config.blend_seconds != 0.0) + || config.dynamic_capture_height_min < 0.20 + || config.dynamic_capture_height_min > config.handover_height_min + || config.dynamic_capture_height_min >= config.dynamic_capture_height_max + || config.dynamic_capture_height_max > config.handover_height_max + || config.stable_seconds < 1.0 || config.max_script_seconds > 8.0 || config.max_reroutes < 0 + || config.max_reroutes > 1 || config.handover_height_min >= config.handover_height_max + || config.handover_tilt_rad >= config.reroute_tilt_rad + || config.reroute_tilt_rad >= std::numbers::pi + || config.prepare_tilt_limit_deg > config.capture_tilt_limit_deg + || config.capture_tilt_limit_deg >= 90.0 || !config.nominal.allFinite() + || !config.fold.allFinite() || !config.insertion_fold.allFinite() + || !config.thrust.allFinite() || !config.support.allFinite() + || !config.rl_nominal.allFinite() || !config.root_axis_signs.allFinite() + || !config.wheel_axis_signs.allFinite()) + throw std::invalid_argument("Invalid Joint reference recovery configuration"); + for (int i = 0; i < 4; ++i) + if (std::abs(config.root_axis_signs[i]) != 1.0 + || config.root_axis_signs[i] != config.root_axis_signs[i ^ 1]) + throw std::invalid_argument( + "Joint reference recovery roots require signed coupled pairs"); + for (int i = 0; i < 2; ++i) + if (std::abs(config.wheel_axis_signs[i]) != 1.0) + throw std::invalid_argument( + "Joint reference recovery requires signed wheel motor axes"); + publish_(); +} + +std::uint64_t JointReferenceRecoveryController::ticks_(double seconds) const { + return static_cast(std::ceil(seconds / config_.dt - 1e-9)); +} + +bool JointReferenceRecoveryController::reached_( + std::uint64_t ticks, double elapsed, double seconds) const { + return config_.dt == 0.005 ? ticks >= ticks_(seconds) : elapsed + 1e-12 >= seconds; +} + +bool JointReferenceRecoveryController::phase_reached_( + const StepContext& step, double seconds) const { + return config_.dt == 0.005 ? step.elapsed >= static_cast(seconds) + : step.elapsed_seconds + 1e-12 >= seconds; +} + +float JointReferenceRecoveryController::phase_elapsed_() const { + return config_.dt == 0.005 ? static_cast(phase_ticks_) * static_cast(config_.dt) + : static_cast(durations_.phase); +} + +void JointReferenceRecoveryController::reset_fsm_(const Vector6f& q, bool active) { + enabled_ = active; + phase_ = active ? JointReferenceRecoveryPhase::kSelect : JointReferenceRecoveryPhase::kRl; + motion_released_ = !active; + targets_ = continuous_q_ = canonical_q_ = last_raw_q_ = q; + targets_.tail<2>().setZero(); + route_ = JointReferenceRecoveryRoute::kUpright; + age_ticks_ = phase_ticks_ = ready_ticks_ = stable_ticks_ = plant_ticks_ = unsupported_ticks_ = + 0; + durations_ = {}; + reroute_count_ = failure_code_ = 0; + continuous_pitch_ = last_pitch_ = orbit_angle_ = plant_pitch_ = 0.0f; + pitch_initialized_ = wheel_balance_active_ = false; + push_off_required_ = false; + grounded_start_ = false; + orbit_start_.setZero(); + thrust_anchor_.setZero(); + fold_anchor_.setZero(); + insert_anchor_.setZero(); + side_start_.setZero(); + side_direction_.setZero(); + side_fold_.setZero(); + side_lower_leg_ = 0; +} + +void JointReferenceRecoveryController::reset( + const JointReferenceRecoveryVector6& q, bool active, double initial_release_seconds) { + if (!q.allFinite() || !std::isfinite(initial_release_seconds) || initial_release_seconds < 0.0 + || initial_release_seconds >= config_.max_script_seconds) + throw std::invalid_argument( + "Joint reference recovery reset requires finite angles and bounded release"); + membership_ = active; + initial_release_seconds_ = initial_release_seconds; + elapsed_ticks_ = 0; + elapsed_seconds_ = 0.0; + release_finished_ = !active || initial_release_seconds == 0.0; + reset_fsm_(q.cast(), active && release_finished_); + publish_(); +} + +JointReferenceRecoveryController::Vector4f JointReferenceRecoveryController::paired_delta_( + const Vector4f& goal, const Vector4f& reference) { + Vector4f difference = goal - reference; + for (int side = 0; side < 2; ++side) { + // torch.round uses ties-to-even. nearbyint has the same default rounding. + const float turns = std::nearbyint(difference[2 * side] / kTau); + difference.segment<2>(2 * side).array() -= turns * kTau; + } + return difference; +} + +Eigen::Vector4d JointReferenceRecoveryController::paired_delta( + const Eigen::Vector4d& goal, const Eigen::Vector4d& reference) { + return paired_delta_(goal.cast(), reference.cast()).cast(); +} + +JointReferenceRecoveryController::Vector4f JointReferenceRecoveryController::shape_at_heading_( + const Vector4f& shape, const Vector4f& reference) { + Vector4f result = shape; + for (int hip : {0, 2}) + result.segment<2>(hip).array() += + (reference.segment<2>(hip).sum() - shape.segment<2>(hip).sum()) / 2.0f; + return result; +} + +bool JointReferenceRecoveryController::folded_() const { + for (int hip : {0, 2}) { + const float delta = continuous_q_[hip + 1] - continuous_q_[hip]; + const double folded_delta = config_.insertion_fold[hip + 1] - config_.insertion_fold[hip]; + // Bound measured relative-drive error, not just each drive's tracking + // error: spring deflection must not turn the insertion into a long-leg sweep. + if (std::abs(delta - folded_delta) > 0.24) + return false; + } + return true; +} + +void JointReferenceRecoveryController::enter_(JointReferenceRecoveryPhase phase) { + phase_ = phase; + phase_ticks_ = 0; + durations_.phase = 0.0; + if (phase == JointReferenceRecoveryPhase::kRl && config_.release_motion_on_takeover) + motion_released_ = true; +} + +void JointReferenceRecoveryController::fail_(int code) { + if (!enabled_ || phase_ == JointReferenceRecoveryPhase::kFailed) + return; + enter_(JointReferenceRecoveryPhase::kFailed); + failure_code_ = code; + motion_released_ = false; +} + +void JointReferenceRecoveryController::start_upright_(bool grounded) { + route_ = JointReferenceRecoveryRoute::kUpright; + grounded_start_ = grounded; + push_off_required_ = grounded; + orbit_angle_ = 0.0f; + unsupported_ticks_ = 0; + durations_.unsupported = 0.0; + fold_anchor_ = + shape_at_heading_(config_.insertion_fold.cast(), continuous_q_.head<4>()); + enter_(grounded ? JointReferenceRecoveryPhase::kFold : JointReferenceRecoveryPhase::kCapture); +} + +void JointReferenceRecoveryController::start_thrust_(const Vector4f& reference) { + Vector4f goal = config_.thrust.cast(); + if (push_off_required_) { + // Extend at the engaged leg heading. Mixing a large common rotation + // into this stroke delays knee extension and keeps the shell grounded. + goal = shape_at_heading_(config_.support.cast(), reference); + } + thrust_anchor_ = reference + paired_delta_(goal, reference); + plant_pitch_ = continuous_pitch_; + enter_(JointReferenceRecoveryPhase::kThrust); +} + +void JointReferenceRecoveryController::publish_() { + command_.targets = targets_.cast(); + command_.continuous_q = continuous_q_.cast(); + command_.phase = phase_; + command_.route = route_; + command_.failed = phase_ == JointReferenceRecoveryPhase::kFailed; + command_.release_finished = release_finished_; + command_.pure_rl = + (!membership_ || release_finished_) && phase_ == JointReferenceRecoveryPhase::kRl; + command_.blending = enabled_ && phase_ == JointReferenceRecoveryPhase::kBlend; + command_.scripted = enabled_ && phase_ < JointReferenceRecoveryPhase::kBlend; + command_.blend = + command_.pure_rl ? 1.0 + : command_.blending && config_.blend_seconds > 0.0 + ? static_cast(std::clamp( + phase_elapsed_() / static_cast(config_.blend_seconds), 0.0f, 1.0f)) + : 0.0; + command_.motion_released = !membership_ || (release_finished_ && motion_released_); + command_.motion_hold = membership_ && !command_.motion_released; + command_.wheel_balance_active = wheel_balance_active_; + command_.failure_code = failure_code_; + command_.age_ticks = age_ticks_; + command_.phase_ticks = phase_ticks_; + command_.stable_ticks = stable_ticks_; + command_.reroute_count = reroute_count_; +} + +const JointReferenceRecoveryCommand& + JointReferenceRecoveryController::update(const JointReferenceRecoveryFeedback& feedback) { + return update(feedback, config_.dt); +} + +const JointReferenceRecoveryCommand& JointReferenceRecoveryController::update( + const JointReferenceRecoveryFeedback& feedback, double elapsed_seconds) { + const bool valid = std::isfinite(elapsed_seconds) && elapsed_seconds > 0.0 + && elapsed_seconds <= 0.02 && feedback.q.allFinite() && feedback.dq.allFinite() + && feedback.gravity.allFinite() && feedback.gyro.allFinite() + && std::isfinite(feedback.estimated_height) && feedback.gravity.norm() > 0.5; + if (!valid) { + // Invalid sensing also terminates a zero-effort release episode. + enabled_ = membership_; + fail_(1); + publish_(); + return command_; + } + double dt = config_.dt == 0.005 ? config_.dt : elapsed_seconds; + elapsed_seconds_ += dt; + const Vector6f raw_q = feedback.q.cast(); + canonical_q_ = raw_q; + for (int i = 0; i < 6; ++i) + continuous_q_[i] += wrapped(raw_q[i] - last_raw_q_[i]); + last_raw_q_ = raw_q; + if (membership_ && !release_finished_ + && (config_.dt == 0.005 ? static_cast(elapsed_ticks_) * config_.dt + : elapsed_seconds_ + 1e-12) + >= initial_release_seconds_) { + if (config_.dt == 0.001) { + // The portion spent releasing cannot also advance the active script. + // Preserve route selection in this update, even at an exact boundary. + dt = std::clamp(elapsed_seconds_ - initial_release_seconds_, 0.0, dt); + if (dt <= 1e-12) + dt = 0.0; + } + // Accumulated encoder winding survives passive initial release. + reset_fsm_(continuous_q_, true); + release_finished_ = true; + } + if (membership_ && !release_finished_) { + ++elapsed_ticks_; + publish_(); + return command_; + } + Eigen::Vector3f gravity = feedback.gravity.cast(); + gravity /= std::max(gravity.norm(), 1e-6f); + const Eigen::Vector3f gyro = feedback.gyro.cast(); + const Vector6f dq = feedback.dq.cast(); + const float height = static_cast(feedback.estimated_height); + const float tilt = std::acos(std::clamp(-gravity.z(), -1.0f, 1.0f)); + const float pitch = std::atan2(gravity.x(), -gravity.z()); + continuous_pitch_ = + pitch_initialized_ ? continuous_pitch_ + wrapped(pitch - last_pitch_) : pitch; + last_pitch_ = pitch; + pitch_initialized_ = true; + if (enabled_ && phase_ != JointReferenceRecoveryPhase::kFailed + && phase_ != JointReferenceRecoveryPhase::kRl) { + ++age_ticks_; + durations_.age += dt; + } + if (enabled_ && phase_ != JointReferenceRecoveryPhase::kFailed) { + ++phase_ticks_; + durations_.phase += dt; + } + + const bool overturning = route_ == JointReferenceRecoveryRoute::kOrbitPositive + || route_ == JointReferenceRecoveryRoute::kOrbitNegative + || route_ == JointReferenceRecoveryRoute::kSide; + if (enabled_ && config_.dynamic_takeover && overturning + && phase_ > JointReferenceRecoveryPhase::kSelect + && phase_ < JointReferenceRecoveryPhase::kBlend && tilt < radians(30.0)) { + // IMU upright detection ends the overturning stroke before another + // reference step. Finish with the same fold/insertion as a prone start; + // never reselect the old revolution after this one-way route change. + start_upright_( + !feedback.support + || height < body_geometry::support_depth(gravity) + kPushOffClearance); + } + + const bool recapturable = phase_ == JointReferenceRecoveryPhase::kPrepare + || phase_ == JointReferenceRecoveryPhase::kCapture; + const bool unsupported = enabled_ && recapturable && !feedback.support + && tilt > static_cast(config_.reroute_tilt_rad); + unsupported_ticks_ = unsupported ? unsupported_ticks_ + 1 : 0; + durations_.unsupported = unsupported ? durations_.unsupported + dt : 0.0; + if (unsupported + && reached_( + unsupported_ticks_, durations_.unsupported, config_.reroute_support_lost_seconds)) { + if (grounded_start_ || route_ == JointReferenceRecoveryRoute::kUpright + || route_ == JointReferenceRecoveryRoute::kSide) { + // Upright insertion and side capture must never fall back to a + // sagittal revolution after losing their capture window. + fail_(2); + } else if (reroute_count_ < config_.max_reroutes) { + ++reroute_count_; + unsupported_ticks_ = 0; + durations_.unsupported = 0.0; + enter_(JointReferenceRecoveryPhase::kSelect); + } + } + const bool near_upright = tilt < radians(15.0) && height > 0.20f; + JointReferenceRecoveryRoute selected = + tilt < radians(70.0) ? JointReferenceRecoveryRoute::kPlant + : pitch >= 0.0f ? JointReferenceRecoveryRoute::kOrbitPositive + : JointReferenceRecoveryRoute::kOrbitNegative; + if (tilt >= radians(70.0) && std::abs(gravity.y()) > 0.7f) + selected = JointReferenceRecoveryRoute::kSide; + if (near_upright) + selected = JointReferenceRecoveryRoute::kUpright; + if (phase_ == JointReferenceRecoveryPhase::kSelect) { + route_ = selected; + push_off_required_ = config_.dynamic_takeover && tilt < radians(15.0) + && (route_ == JointReferenceRecoveryRoute::kUpright + || route_ == JointReferenceRecoveryRoute::kPlant) + && height < body_geometry::support_depth(gravity) + kPushOffClearance; + grounded_start_ = push_off_required_; + if (push_off_required_) + fold_anchor_ = + shape_at_heading_(config_.insertion_fold.cast(), continuous_q_.head<4>()); + if (route_ == JointReferenceRecoveryRoute::kSide) { + side_lower_leg_ = gravity.y() >= 0.0f ? 0 : 1; + side_direction_.setZero(); + // A backward-canted side fall needs a rearward stroke. A forward + // stroke amplifies its pitch before it can produce the roll impulse. + const float direction = config_.dynamic_takeover && gravity.x() < -0.1f ? -1.0f : 1.0f; + side_direction_.segment<2>(2 * side_lower_leg_).setConstant(direction); + // Preserve each leg's measured heading while setting its length. + // Rotating to an absolute fold pose can pitch a canted side fall + // onto its back before the grounded leg has delivered its stroke. + side_fold_ = config_.thrust.cast(); + for (int side = 0; side < 2; ++side) { + const int hip = 2 * side; + const float offset = + (continuous_q_.segment<2>(hip).sum() - side_fold_.segment<2>(hip).sum()) / 2.0f; + side_fold_.segment<2>(hip).array() += offset; + } + } + enter_( + near_upright && !push_off_required_ ? JointReferenceRecoveryPhase::kPrepare + : JointReferenceRecoveryPhase::kFold); + } + // Transition predicates share one snapshot. A newly entered phase cannot + // consume another trajectory step; wheel balancing uses the resulting phase. + const StepContext step{ + phase_, + selected, + gravity, + gyro, + dq, + height, + tilt, + pitch, + static_cast(dt), + phase_elapsed_(), + durations_.phase, + near_upright}; + const auto goal = update_goal_(step, feedback.support); + const auto reference = advance_reference_(step, goal); + update_transitions_(step, reference, feedback, dt); + update_wheels_(step, feedback.support); + ++elapsed_ticks_; + publish_(); + return command_; +} + +JointReferenceRecoveryController::JointGoal + JointReferenceRecoveryController::update_goal_(const StepContext& step, bool supported) { + const auto phase = step.phase; + const auto& gravity = step.gravity; + const float tilt = step.tilt; + const float pitch = step.pitch; + const float elapsed = step.elapsed; + const bool folding = phase == JointReferenceRecoveryPhase::kFold; + const bool planting = phase == JointReferenceRecoveryPhase::kPlant; + const bool orbiting = phase == JointReferenceRecoveryPhase::kOrbit; + const bool thrusting = phase == JointReferenceRecoveryPhase::kThrust; + const bool side = phase == JointReferenceRecoveryPhase::kSide; + const bool preparing = phase == JointReferenceRecoveryPhase::kPrepare; + const bool capturing = phase == JointReferenceRecoveryPhase::kCapture; + const bool blending = phase == JointReferenceRecoveryPhase::kBlend; + const Vector4f axes = config_.root_axis_signs.cast(); + Vector4f goal = config_.nominal.cast(); + if (folding) { + goal = push_off_required_ ? fold_anchor_ : config_.fold.cast(); + if (route_ == JointReferenceRecoveryRoute::kSide) { + if (config_.dynamic_takeover) { + goal = side_fold_; + } else { + const int upper = 1 - side_lower_leg_; + goal.segment<2>(2 * upper) = config_.thrust.segment<2>(2 * upper).cast(); + } + } + } + if ((preparing || capturing || blending) && (supported || (grounded_start_ && capturing)) + && tilt < radians(85.0)) { + const float extension = grounded_start_ + ? std::clamp((tilt - radians(30.0)) / radians(25.0), 0.0f, 1.0f) + : std::clamp((tilt - radians(12.0)) / radians(43.0), 0.0f, 1.0f); + goal = config_.nominal.cast() + + extension * (config_.support - config_.nominal).cast() + - pitch * axes.cwiseInverse(); + } + if (planting) { + goal = + push_off_required_ ? config_.insertion_fold.cast() : config_.fold.cast(); + if (push_off_required_) { + // Insertion is relative to the now-upright body. An accumulated + // IMU revolution from the overturning stroke must not add a new turn. + goal = insert_anchor_ - pitch * axes.cwiseInverse(); + } else if (std::abs(gravity.y()) < 0.5f) { + const float sign = pitch > 0.0f ? 1.0f : pitch < 0.0f ? -1.0f : 0.0f; + const float offset = std::clamp( + -pitch - static_cast(config_.plant_angle_rad) * sign, -2.2f, 2.2f); + goal += offset * axes.cwiseInverse(); + } + } + if (orbiting) { + orbit_angle_ += static_cast(config_.orbit_speed_rad_s) * step.dt; + const float direction = + route_ == JointReferenceRecoveryRoute::kOrbitPositive ? 1.0f : -1.0f; + goal = orbit_start_ + direction * orbit_angle_ * axes.cwiseInverse(); + } + if (thrusting) + goal = thrust_anchor_ - (continuous_pitch_ - plant_pitch_) * axes.cwiseInverse(); + if (side) { + // A single grounded-leg stroke; returning it here rolls the body onto its back. + const float stroke = std::min( + elapsed * static_cast(config_.side_speed_rad_s), + static_cast(config_.side_angle_rad)); + goal = side_start_ + side_direction_.cwiseProduct(axes.cwiseInverse()) * stroke; + } + float speed = static_cast(config_.prepare_speed_rad_s); + if (planting && push_off_required_) { + // Brake over the last 100 mm of wheel-plane approach. A fast + // suspended stroke must not hit the floor at its free-space speed. + const float approach = std::clamp( + (body_geometry::support_depth(step.gravity) - step.height) / 0.10f, 0.0f, 1.0f); + const float insertion_speed = static_cast(config_.insert_speed_rad_s); + speed = std::lerp( + std::min(insertion_speed, static_cast(config_.push_speed_rad_s)), + insertion_speed, approach); + } + if (preparing || thrusting) + speed = static_cast(config_.push_speed_rad_s); + if (capturing) + speed = static_cast( + grounded_start_ ? config_.insert_speed_rad_s : config_.capture_speed_rad_s); + if (orbiting) + speed = static_cast(config_.orbit_speed_rad_s); + if (side) + speed = static_cast(config_.side_speed_rad_s); + return { + goal, speed, + orbiting || thrusting || side || (planting && push_off_required_) + ? Winding::kContinuous + : Winding::kCoupledNearest}; +} + +JointReferenceRecoveryController::ReferenceStep + JointReferenceRecoveryController::advance_reference_( + const StepContext& step, const JointGoal& goal) { + Vector4f reference = targets_.head<4>(); + const Vector4f delta = goal.winding == Winding::kContinuous + ? Vector4f(goal.position - reference) + : paired_delta_(goal.position, reference); + const float fraction = + std::min(goal.speed * step.dt / std::max(delta.cwiseAbs().maxCoeff(), 1e-6f), 1.0f); + if ((enabled_ && phase_ < JointReferenceRecoveryPhase::kBlend) + || (enabled_ && step.phase == JointReferenceRecoveryPhase::kBlend)) + reference += delta * fraction; + targets_.head<4>() = reference; + const float remaining = delta.cwiseAbs().maxCoeff() * (1.0f - fraction); + const float measured_error = + paired_delta_(reference, continuous_q_.head<4>()).cwiseAbs().maxCoeff(); + return {reference, remaining, measured_error}; +} + +void JointReferenceRecoveryController::update_transitions_( + const StepContext& step, const ReferenceStep& reference_step, + const JointReferenceRecoveryFeedback& feedback, double dt) { + const auto phase = step.phase; + const auto& gyro = step.gyro; + const auto& dq = step.dq; + const float height = step.height; + const float tilt = step.tilt; + const auto& reference = reference_step.position; + const float remaining = reference_step.remaining; + const float measured_error = reference_step.measured_error; + const bool folding = phase == JointReferenceRecoveryPhase::kFold; + const bool planting = phase == JointReferenceRecoveryPhase::kPlant; + const bool orbiting = phase == JointReferenceRecoveryPhase::kOrbit; + const bool thrusting = phase == JointReferenceRecoveryPhase::kThrust; + const bool side = phase == JointReferenceRecoveryPhase::kSide; + const bool preparing = phase == JointReferenceRecoveryPhase::kPrepare; + const bool capturing = phase == JointReferenceRecoveryPhase::kCapture; + const bool blending = phase == JointReferenceRecoveryPhase::kBlend; + // Start wheel-assisted capture before the planting arc carries a supported + // leg pose past the actor's entry window. Keep this transition for one tick. + const bool plant_capture = + config_.dynamic_takeover + && (route_ == JointReferenceRecoveryRoute::kPlant + || (push_off_required_ && route_ == JointReferenceRecoveryRoute::kUpright)) + && (folding || planting) && (!push_off_required_ || !folding) + && (!push_off_required_ || remaining < 0.15f) && feedback.support && height > 0.22f + && tilt < radians(40.0) && gyro.norm() < 8.0f; + if (plant_capture) { + if (push_off_required_) + start_thrust_(reference); + else + enter_(JointReferenceRecoveryPhase::kCapture); + } + // A moving recovery must continue as soon as the measured fold is reached. + // Holding an unbalanced body here loses the impulse needed by the next stroke. + const double fold_min_seconds = config_.dynamic_takeover ? 0.06 : 0.3; + if (!plant_capture && folding && remaining < 0.02f && measured_error < 0.5f + && phase_reached_(step, fold_min_seconds)) { + if (route_ == JointReferenceRecoveryRoute::kOrbitPositive + || route_ == JointReferenceRecoveryRoute::kOrbitNegative) { + orbit_start_ = reference; + orbit_angle_ = 0.0f; + enter_(JointReferenceRecoveryPhase::kOrbit); + } else if (route_ == JointReferenceRecoveryRoute::kSide) { + side_start_ = reference; + enter_(JointReferenceRecoveryPhase::kSide); + } else { + if (push_off_required_) { + Vector4f delta = paired_delta_(config_.insertion_fold.cast(), reference); + for (int hip : {0, 2}) { + const float axis = static_cast(config_.root_axis_signs[hip]); + // Near half a turn, choose one physical stroke direction + // for both legs rather than letting IMU noise choose a turn. + if (delta.segment<2>(hip).mean() * axis > kPi - radians(20.0)) + delta.segment<2>(hip).array() -= kTau / axis; + } + insert_anchor_ = reference + delta; + } + enter_(JointReferenceRecoveryPhase::kPlant); + } + } + const bool side_upright = side && tilt < radians(65.0); + if (side_upright) + enter_(JointReferenceRecoveryPhase::kCapture); + if (side && !side_upright && phase_reached_(step, 2.5)) + fail_(2); + plant_ticks_ = planting && feedback.support ? plant_ticks_ + 1 : 0; + durations_.plant = planting && feedback.support ? durations_.plant + dt : 0.0; + const bool planted = !plant_capture && planting && remaining < 0.15f && measured_error < 0.5f + && reached_(plant_ticks_, durations_.plant, 0.06); + if (planted) { + if (push_off_required_) + start_thrust_(reference); + else + enter_(JointReferenceRecoveryPhase::kPrepare); + } + if (!plant_capture && planting && !planted && phase_reached_(step, 2.5)) + fail_(3); + const bool orbit_contact = orbiting && feedback.support && tilt < radians(145.0) + && height > 0.12f && orbit_angle_ > 3.7f; + if (orbit_contact) + start_thrust_(reference); + // A shell-supported upright recovery needs an extension before the low + // capture reference. Otherwise the actor inherits the floor contact. + // Ending the stroke uses geometry: the push can exceed the support + // observer's acceleration window. Actor transfer still requires support. + const float push_clearance = feedback.support ? kPushOffClearance : kCaptureClearance; + const bool inserted_low = (folding || planting) && folded_() && feedback.support; + if ((thrusting || inserted_low) && push_off_required_ && tilt < radians(45.0) + && height > body_geometry::support_depth(step.gravity) + push_clearance) { + push_off_required_ = false; + enter_(JointReferenceRecoveryPhase::kCapture); + } + const bool captured = (orbiting && tilt < radians(30.0) && height > 0.22f && feedback.support) + || (thrusting && !push_off_required_ && tilt < radians(65.0) + && height > 0.24f && gyro.norm() < 8.0f && feedback.support); + if (captured) + enter_(JointReferenceRecoveryPhase::kCapture); + if (orbiting && !orbit_contact && !captured + && orbit_angle_ >= static_cast(2.0 * std::numbers::pi * config_.orbit_turns)) + fail_(4); + if (capturing && !grounded_start_ && tilt < radians(20.0) && feedback.support && height > 0.22f) + enter_(JointReferenceRecoveryPhase::kPrepare); + const bool stable = feedback.support && feedback.body_clear + && tilt < static_cast(config_.handover_tilt_rad) + && gyro.norm() < static_cast(config_.handover_gyro_max) + && height > static_cast(config_.handover_height_min) + && height < static_cast(config_.handover_height_max) + && dq.tail<2>().cwiseAbs().maxCoeff() * 0.06f < 0.25f + && dq.head<4>().cwiseAbs().maxCoeff() < 2.0f; + const bool ready = preparing && stable && remaining < 0.02f && measured_error < 0.12f; + ready_ticks_ = ready ? ready_ticks_ + 1 : 0; + durations_.ready = ready ? durations_.ready + dt : 0.0; + // A valid measured capture ends the current script, even before PREPARE. + // Use the current phase so an earlier failure cannot be cleared here. + // Dynamic capture uses conditional support and the caller's motion bounds. + // Bidirectional probes remain part of static handover; their acceleration + // test cannot require a balancing wheel to settle before a moving capture. + // Acquire several actual sensor frames before trusting a cold IMU/filter + // baseline. This is startup sensing time, not a standing-stability dwell. + const bool captured_for_rl = + !plant_capture && enabled_ && phase_ < JointReferenceRecoveryPhase::kBlend + && reached_(age_ticks_, durations_.age, 0.06) && config_.dynamic_takeover + && feedback.rl_capture_ready && feedback.support; + if (captured_for_rl + || (!config_.dynamic_takeover && preparing + && reached_(ready_ticks_, durations_.ready, 0.1))) + enter_( + config_.blend_seconds == 0.0 ? JointReferenceRecoveryPhase::kRl + : JointReferenceRecoveryPhase::kBlend); + if (blending && reached_(phase_ticks_, durations_.phase, config_.blend_seconds)) + enter_(JointReferenceRecoveryPhase::kRl); + const bool holding_rl = + enabled_ && phase == JointReferenceRecoveryPhase::kRl && !motion_released_; + stable_ticks_ = holding_rl && stable ? stable_ticks_ + 1 : 0; + durations_.stable = holding_rl && stable ? durations_.stable + dt : 0.0; + motion_released_ = + motion_released_ + || (holding_rl && reached_(stable_ticks_, durations_.stable, config_.stable_seconds)); + if (enabled_ + && (phase_ == JointReferenceRecoveryPhase::kBlend + || (phase_ == JointReferenceRecoveryPhase::kRl && !motion_released_)) + && tilt > radians(45.0)) + fail_(5); + if (enabled_ && phase_ != JointReferenceRecoveryPhase::kRl + && reached_(age_ticks_, durations_.age, config_.max_script_seconds)) + fail_(6); + if (membership_ && phase_ != JointReferenceRecoveryPhase::kRl + && (config_.dt == 0.005 ? static_cast(elapsed_ticks_) * config_.dt + : elapsed_seconds_ + 1e-12) + >= config_.max_script_seconds) + fail_(6); +} + +void JointReferenceRecoveryController::update_wheels_(const StepContext& step, bool supported) { + const auto phase = step.phase; + const auto& gyro = step.gyro; + const auto& dq = step.dq; + const float tilt = step.tilt; + const float pitch = step.pitch; + // RecoveryTraining updates wheels after the FSM transition. Entering BLEND + // recomputes once; subsequent BLEND ticks retain that velocity reference. + const bool is_capture = phase_ == JointReferenceRecoveryPhase::kCapture; + const bool is_prepare = phase_ == JointReferenceRecoveryPhase::kPrepare; + const bool is_push_off = phase_ == JointReferenceRecoveryPhase::kThrust && push_off_required_; + const bool is_insertion = phase_ == JointReferenceRecoveryPhase::kPlant && push_off_required_; + const bool is_blend = enabled_ && phase_ == JointReferenceRecoveryPhase::kBlend; + const float balance_limit = + radians(is_capture ? config_.capture_tilt_limit_deg : config_.prepare_tilt_limit_deg); + // Keep pitch damping while the grounded start acquires wheel load or its + // lift impulse changes acceleration. Actor transfer still requires support. + // Grounded capture must retain the reaction torque through transient unloading. + const bool eligible = + enabled_ && release_finished_ + && (supported || is_insertion || is_push_off || (grounded_start_ && is_capture)) + && tilt < balance_limit + && (is_prepare || is_capture || is_push_off || is_insertion + || (is_blend && phase == JointReferenceRecoveryPhase::kPrepare)) + && config_.wheel_balance_enabled; + const bool retained = is_blend && wheel_balance_active_ && supported && tilt < balance_limit; + Eigen::Vector2f desired = + retained ? Eigen::Vector2f(targets_.tail<2>()) : Eigen::Vector2f::Zero(); + if (eligible) { + const float effort = static_cast(config_.pitch_kp_nm_rad) * pitch + + static_cast(config_.pitch_kd_nm_s_rad) * gyro.y(); + desired = effort * config_.wheel_axis_signs.cast() / 0.6f + + (1.0f - static_cast(config_.wheel_damping_nm_s_rad) / 0.6f) * dq.tail<2>(); + } + wheel_balance_active_ = eligible || retained; + targets_.tail<2>() = desired; +} + +JointReferenceRecoveryVector6 JointReferenceRecoveryController::torques( + const JointReferenceRecoveryFeedback& feedback, + const JointReferenceRecoveryVector6& rl_torque) const { + if (!membership_) + return rl_torque; + if (!release_finished_ || command_.failed || !feedback.q.allFinite() || !feedback.dq.allFinite() + || !feedback.wheel_probe_torque.allFinite() || !rl_torque.allFinite()) + return JointReferenceRecoveryVector6::Zero(); + const Vector6f q = project_feedback(feedback.q).cast(); + const Vector6f dq = feedback.dq.cast(); + Vector6f script; + script.head<4>() = 60.0f * (targets_.head<4>() - q.head<4>()) - 2.0f * dq.head<4>(); + script.tail<2>() = + 0.6f * (targets_.tail<2>() - dq.tail<2>()) + feedback.wheel_probe_torque.cast(); + script.head<4>() = script.head<4>().cwiseMax(-40.0f).cwiseMin(40.0f); + script.tail<2>() = script.tail<2>().cwiseMax(-4.5f).cwiseMin(4.5f); + Vector6f actor = rl_torque.cast(); + actor.head<4>() = actor.head<4>().cwiseMax(-40.0f).cwiseMin(40.0f); + actor.tail<2>() = actor.tail<2>().cwiseMax(-4.5f).cwiseMin(4.5f); + const float alpha = static_cast(command_.blend); + return ((1.0f - alpha) * script + alpha * actor).cast(); +} + +JointReferenceRecoveryVector6 JointReferenceRecoveryController::project_feedback( + const JointReferenceRecoveryVector6& input) const { + const Vector6f raw_q = input.cast(); + Vector6f q; + for (int i = 0; i < 6; ++i) { + const float turns = std::nearbyint((canonical_q_[i] - raw_q[i]) / kTau); + q[i] = turns == 0.0f ? raw_q[i] : raw_q[i] + turns * kTau; + } + return q.cast(); +} + +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/recovery.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/recovery.hpp new file mode 100644 index 000000000..6f524035d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/recovery.hpp @@ -0,0 +1,231 @@ +#pragma once + +#include +#include +#include + +#include + +namespace rmcs_core::controller::chassis::wheel_leg { + +using JointReferenceRecoveryVector6 = Eigen::Matrix; + +enum class JointReferenceRecoveryPhase : std::uint8_t { + kSelect = 0, + kFold = 1, + kPlant = 2, + kOrbit = 3, + kThrust = 4, + kSide = 5, + kCapture = 6, + kPrepare = 7, + kBlend = 8, + kRl = 9, + kFailed = 10, +}; +enum class JointReferenceRecoveryRoute : std::uint8_t { + kUpright = 0, + kPlant = 1, + kOrbitPositive = 2, + kOrbitNegative = 3, + kSide = 4, +}; +std::string_view recovery_phase_name(JointReferenceRecoveryPhase phase); + +struct JointReferenceRecoveryConfig { + // Script PREPARE/capture reference; dynamic deployment selects a low fold. + Eigen::Vector4d nominal = Eigen::Vector4d::Zero(); + Eigen::Vector4d fold = Eigen::Vector4d::Zero(); + Eigen::Vector4d insertion_fold = Eigen::Vector4d::Zero(); + Eigen::Vector4d thrust = Eigen::Vector4d::Zero(); + Eigen::Vector4d support = Eigen::Vector4d::Zero(); + JointReferenceRecoveryVector6 rl_nominal = JointReferenceRecoveryVector6::Zero(); + Eigen::Vector4d root_axis_signs{1.0, 1.0, -1.0, -1.0}; + Eigen::Vector2d wheel_axis_signs{1.0, -1.0}; + double dt = 0.005; + // Frozen simulation uses 200 ms; zero selects direct deployment takeover. + double blend_seconds = 0.2; + // End any script at the caller's bounded upright capture before stabilization. + bool dynamic_takeover = false; + double dynamic_capture_height_min = 0.27; + double dynamic_capture_height_max = 0.36; + // Deployment may accept driver commands as soon as the actor owns all axes. + bool release_motion_on_takeover = false; + double stable_seconds = 1.0; + double max_script_seconds = 8.0; + double prepare_speed_rad_s = 2.0; + double insert_speed_rad_s = 2.0; + double push_speed_rad_s = 2.0; + double orbit_speed_rad_s = 2.0; + double capture_speed_rad_s = 2.0; + double side_speed_rad_s = 2.0; + double side_angle_rad = std::numbers::pi / 2.0; + double plant_angle_rad = std::numbers::pi / 9.0; + double orbit_turns = 1.0; + double handover_height_min = 0.27; + double handover_height_max = 0.36; + double handover_tilt_rad = 8.0 * std::numbers::pi / 180.0; + double handover_gyro_max = 0.75; + double reroute_tilt_rad = 70.0 * std::numbers::pi / 180.0; + double reroute_support_lost_seconds = 0.15; + int max_reroutes = 1; + bool wheel_balance_enabled = true; + double pitch_kp_nm_rad = 8.0; + double pitch_kd_nm_s_rad = 1.5; + double wheel_damping_nm_s_rad = 0.2; + double prepare_tilt_limit_deg = 55.0; + double capture_tilt_limit_deg = 85.0; +}; + +struct JointReferenceRecoveryFeedback { + JointReferenceRecoveryVector6 q = JointReferenceRecoveryVector6::Zero(); + JointReferenceRecoveryVector6 dq = JointReferenceRecoveryVector6::Zero(); + Eigen::Vector3d gravity{0.0, 0.0, -1.0}; + Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); + double estimated_height = 0.0; + // Native plausible conditional support; never true simulator contacts. + bool support = false; + // Wheel-probe evidence does not require the static standing height. + bool support_confirmed = false; + bool body_clear = false; + // Caller validates the mechanism's upright pose/velocity envelope and sensing. + bool rl_capture_ready = false; + Eigen::Vector2d wheel_probe_torque = Eigen::Vector2d::Zero(); +}; + +struct JointReferenceRecoveryCommand { + JointReferenceRecoveryVector6 targets = JointReferenceRecoveryVector6::Zero(); + JointReferenceRecoveryVector6 continuous_q = JointReferenceRecoveryVector6::Zero(); + JointReferenceRecoveryPhase phase = JointReferenceRecoveryPhase::kRl; + JointReferenceRecoveryRoute route = JointReferenceRecoveryRoute::kUpright; + double blend = 1.0; + bool motion_released = true; + bool motion_hold = false; + bool scripted = false; + bool blending = false; + bool pure_rl = true; + bool failed = false; + bool release_finished = true; + bool wheel_balance_active = false; + int failure_code = 0; + std::uint64_t age_ticks = 0; + std::uint64_t phase_ticks = 0; + std::uint64_t stable_ticks = 0; + int reroute_count = 0; +}; + +class JointReferenceRecoveryController { +public: + explicit JointReferenceRecoveryController(const JointReferenceRecoveryConfig& config); + void reset( + const JointReferenceRecoveryVector6& q, bool active = true, + double initial_release_seconds = 0.0); + // Advance once at the configured period. Sensor freshness, drive readiness, + // geometry guards and support probing stay with the caller. + const JointReferenceRecoveryCommand& update(const JointReferenceRecoveryFeedback& feedback); + // The 1 kHz path integrates actual elapsed time; 200 Hz retains the frozen + // fixed-period arithmetic for comparison with the training implementation. + const JointReferenceRecoveryCommand& + update(const JointReferenceRecoveryFeedback& feedback, double elapsed_seconds); + const JointReferenceRecoveryCommand& command() const { return command_; } + JointReferenceRecoveryVector6 project_feedback(const JointReferenceRecoveryVector6& q) const; + // Reevaluate native PD on current feedback without advancing the reference. + // rl_torque is output-side actor PD effort; both paths are clipped before + // the optional 200 ms linear torque blend. Zero takes over in the ready tick. + // The scripted path uses feedback PD and the mechanism's wheel torque limits. + JointReferenceRecoveryVector6 torques( + const JointReferenceRecoveryFeedback& feedback, + const JointReferenceRecoveryVector6& rl_torque) const; + static Eigen::Vector4d + paired_delta(const Eigen::Vector4d& goal, const Eigen::Vector4d& reference); + +private: + using Vector4f = Eigen::Matrix; + using Vector6f = Eigen::Matrix; + struct StepContext { + JointReferenceRecoveryPhase phase; + JointReferenceRecoveryRoute selected_route; + Eigen::Vector3f gravity; + Eigen::Vector3f gyro; + Vector6f dq; + float height; + float tilt; + float pitch; + float dt; + float elapsed; + double elapsed_seconds; + bool near_upright; + }; + enum class Winding { kCoupledNearest, kContinuous }; + struct JointGoal { + Vector4f position; + float speed; + Winding winding; + }; + struct ReferenceStep { + Vector4f position; + float remaining; + float measured_error; + }; + struct Durations { + double age = 0.0; + double phase = 0.0; + double ready = 0.0; + double stable = 0.0; + double plant = 0.0; + double unsupported = 0.0; + }; + static Vector4f paired_delta_(const Vector4f& goal, const Vector4f& reference); + static Vector4f shape_at_heading_(const Vector4f& shape, const Vector4f& reference); + bool folded_() const; + JointGoal update_goal_(const StepContext& step, bool supported); + ReferenceStep advance_reference_(const StepContext& step, const JointGoal& goal); + void update_transitions_( + const StepContext& step, const ReferenceStep& reference, + const JointReferenceRecoveryFeedback& feedback, double dt); + void update_wheels_(const StepContext& step, bool supported); + bool reached_(std::uint64_t ticks, double elapsed, double seconds) const; + bool phase_reached_(const StepContext& step, double seconds) const; + float phase_elapsed_() const; + void enter_(JointReferenceRecoveryPhase phase); + void start_upright_(bool grounded); + void start_thrust_(const Vector4f& reference); + void fail_(int code); + void reset_fsm_(const Vector6f& q, bool active); + void publish_(); + std::uint64_t ticks_(double seconds) const; + JointReferenceRecoveryConfig config_; + JointReferenceRecoveryCommand command_; + Vector6f targets_ = Vector6f::Zero(); + Vector6f continuous_q_ = Vector6f::Zero(); + // External native encoder provider uses integer turns, independently of + // the FSM's trigonometric accumulation. Zero-turn PD must be an identity. + Vector6f canonical_q_ = Vector6f::Zero(); + Vector6f last_raw_q_ = Vector6f::Zero(); + Vector4f orbit_start_ = Vector4f::Zero(); + Vector4f thrust_anchor_ = Vector4f::Zero(); + Vector4f fold_anchor_ = Vector4f::Zero(); + Vector4f insert_anchor_ = Vector4f::Zero(); + Vector4f side_start_ = Vector4f::Zero(); + Vector4f side_direction_ = Vector4f::Zero(); + Vector4f side_fold_ = Vector4f::Zero(); + int side_lower_leg_ = 0; + JointReferenceRecoveryPhase phase_ = JointReferenceRecoveryPhase::kRl; + JointReferenceRecoveryRoute route_ = JointReferenceRecoveryRoute::kUpright; + std::uint64_t age_ticks_ = 0, phase_ticks_ = 0, ready_ticks_ = 0; + std::uint64_t stable_ticks_ = 0, plant_ticks_ = 0, unsupported_ticks_ = 0; + std::uint64_t elapsed_ticks_ = 0; + Durations durations_; + double elapsed_seconds_ = 0.0; + int reroute_count_ = 0, failure_code_ = 0; + bool enabled_ = false, membership_ = false, motion_released_ = true; + bool pitch_initialized_ = false, release_finished_ = true; + bool wheel_balance_active_ = false; + bool push_off_required_ = false; + bool grounded_start_ = false; + float continuous_pitch_ = 0.0f, last_pitch_ = 0.0f; + float orbit_angle_ = 0.0f, plant_pitch_ = 0.0f; + double initial_release_seconds_ = 0.0; +}; + +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/recovery_yaw.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/recovery_yaw.hpp new file mode 100644 index 000000000..57470c28e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/recovery_yaw.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include +#include +#include + +namespace rmcs_core::controller::chassis::wheel_leg { + +inline double recovery_yaw_error(double yaw) { return -std::remainder(yaw, std::numbers::pi); } + +inline bool recovery_yaw_aligned(double yaw) { + constexpr double tolerance = 10. * std::numbers::pi / 180.; + return std::isfinite(yaw) && std::abs(recovery_yaw_error(yaw)) <= tolerance; +} + +class RecoveryYawAlignment { +public: + void reset() { initialized_ = false; } + + double update(double yaw, double now) { + if (!std::isfinite(yaw) || !std::isfinite(now)) { + reset(); + return std::numeric_limits::quiet_NaN(); + } + if (!initialized_) { + error_ = recovery_yaw_error(yaw); + progress_error_ = error_; + progress_time_ = now; + initialized_ = true; + } else { + error_ -= std::remainder(yaw - last_yaw_, 2. * std::numbers::pi); + } + last_yaw_ = yaw; + constexpr double progress = std::numbers::pi / 180.; + constexpr double tolerance = 10. * std::numbers::pi / 180.; + if (std::abs(error_) <= tolerance + || std::abs(progress_error_) - std::abs(error_) >= progress) { + progress_error_ = error_; + progress_time_ = now; + } else if (now - progress_time_ >= .5) { + error_ += error_ > 0. ? -std::numbers::pi : std::numbers::pi; + progress_error_ = error_; + progress_time_ = now; + } + return error_; + } + +private: + bool initialized_ = false; + double last_yaw_ = 0., error_ = 0., progress_error_ = 0., progress_time_ = 0.; +}; + +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/self_righting.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/self_righting.hpp new file mode 100644 index 000000000..9d2b1206f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/self_righting.hpp @@ -0,0 +1,940 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis::wheel_leg { + +// All positions, speeds and torques are at the four ACTIVE output axes, in the +// authenticated CAD frame. The hardware adapter must apply its calibration. +// No ROS, simulator, wall clock, state teleport, or policy dependency. +class SelfRightingCore { +public: + using Axes = std::array; + using Row = std::array; // knee degrees, four zero-orbit positions + using SupportModel = Axes (*)(const Axes&, const std::array&); + enum Phase { + Disabled = 0, + Rise = 1, + Plant = 2, + Fold = 3, + Capture = 4, + Complete = 5, + Flip = 7, + Tuck = 8, + Clearance = 9, + Sweep = 10, + Thrust = 11, + Fault = 255 + }; + enum class Route { + None = 0, + Standing = 1, + ProneRear = 2, + ProneFront = 3, + ProneMixed = 4, + Inverted = 5, + LeftSide = 6, + RightSide = 7, + Sagittal = 8, + UnderBody = 9 + }; + struct PoseRouteSettings { + bool enabled = false; + double tuck_knee_deg = 45., support_knee_deg = 105.; + double landing_knee_deg = 48., landing_orbit_deg = 20.; + double support_orbit_deg = 95., side_stroke_deg = 80.; + double upright_dwell_seconds = .08, upright_gyro_rad_s = 2.; + double plant_wheel_assist_nm = .6; + }; + struct Feedback { + Axes position{}, velocity{}, spring_compensation{}; + std::array gravity{0., 0., -1.}, gyro{}; + std::array wheel_velocity{}; + }; + struct Output { + Axes position{}, velocity{}, torque{}; + Axes servo_velocity{}, servo_feedforward{}; + std::array wheel_torque{}; + Phase phase = Disabled; + int fault = 0; + }; + struct FlipWaypoint { + std::array knee_deg{}, orbit_deg{}; + double speed = 1.5, acceleration = 3., hold_seconds = 0.; + std::array wheel_velocity{}; // physical left/right output coordinates + }; + struct Settings { + double speed_limit = 3.14, cruise_speed = 1.5, acceleration = 3.; + double velocity_gain = 5., position_gain = 12., torque_limit = 40.; + double integral_gain = 50., integral_limit = 30.; + double fold_knee = 75., flip_knee = 105., stand_knee = 90.; + double stand_orbit = 0., timeout_seconds = 40.; + double flip_orbit = 0., sweep_orbit = 180.; + double balance_kp = 35., balance_kd = 5., balance_kv = 2.; + double flip_asymmetry = 0.; + double pitch_trim_gain = 0., yaw_rate_gain = .6; + double support_gain = 1.; + }; + + explicit SelfRightingCore( + std::vector table, Settings settings, SupportModel support = nullptr) + : table_(std::move(table)) + , settings_(settings) + , support_model_(support) { + if (table_.size() < 2 || !(settings_.speed_limit > 0.) || settings_.speed_limit > 3.14 + || !(settings_.cruise_speed > 0.) || settings_.cruise_speed >= settings_.speed_limit + || !(settings_.acceleration > 0.)) + throw std::invalid_argument("Invalid recovery table or speed envelope"); + for (std::size_t i = 0; i < table_.size(); ++i) { + for (double value : table_[i]) + if (!std::isfinite(value)) + throw std::invalid_argument("Nonfinite recovery table"); + if (i && table_[i][0] <= table_[i - 1][0]) + throw std::invalid_argument("Recovery knee table must increase"); + if (i) + for (std::size_t side = 0; side < 2; ++side) { + const auto plus = side == 0 ? 2 : 3, minus = side == 0 ? 1 : 4; + if (table_[i][plus] - table_[i][minus] + <= table_[i - 1][plus] - table_[i - 1][minus]) + throw std::invalid_argument("Recovery motor-pair geometry must increase"); + } + } + for (double value : + {settings_.speed_limit, settings_.cruise_speed, settings_.acceleration, + settings_.velocity_gain, settings_.position_gain, settings_.torque_limit, + settings_.integral_gain, settings_.integral_limit, settings_.fold_knee, + settings_.flip_knee, settings_.stand_knee, settings_.stand_orbit, + settings_.flip_orbit, settings_.sweep_orbit, settings_.flip_asymmetry, + settings_.balance_kp, settings_.balance_kd, settings_.balance_kv, + settings_.pitch_trim_gain, settings_.yaw_rate_gain, settings_.timeout_seconds, + settings_.support_gain}) + if (!std::isfinite(value)) + throw std::invalid_argument("Nonfinite recovery setting"); + if (settings_.velocity_gain <= 0. || settings_.position_gain <= 0. + || settings_.integral_gain < 0. || settings_.integral_limit < 0. + || settings_.torque_limit <= 0. || settings_.torque_limit > 40. + || settings_.timeout_seconds <= 0. || settings_.support_gain < 0. + || settings_.support_gain > 1. + || (settings_.flip_asymmetry != 0. + && (settings_.flip_asymmetry < table_.front()[0] + || settings_.flip_asymmetry > table_.back()[0]))) + throw std::invalid_argument("Invalid recovery gains, torque or timeout"); + for (double knee : {settings_.fold_knee, settings_.flip_knee, settings_.stand_knee}) + if (knee < table_.front()[0] || knee > table_.back()[0]) + throw std::invalid_argument("Recovery pose lies outside its geometry table"); + } + + void set_flip_program(std::vector program, bool relative) { + if (phase_ != Disabled || program.size() > 64) + throw std::invalid_argument("Configure a finite flip program while disabled"); + for (const auto& p : program) { + for (double v : + {p.knee_deg[0], p.knee_deg[1], p.orbit_deg[0], p.orbit_deg[1], p.speed, + p.acceleration, p.hold_seconds, p.wheel_velocity[0], p.wheel_velocity[1]}) + if (!std::isfinite(v)) + throw std::invalid_argument("Nonfinite flip waypoint"); + if (p.speed <= 0. || p.speed >= settings_.speed_limit || p.acceleration <= 0. + || p.hold_seconds < 0. || p.hold_seconds > 5. || std::abs(p.wheel_velocity[0]) > 55. + || std::abs(p.wheel_velocity[1]) > 55.) + throw std::invalid_argument("Flip waypoint exceeds its speed or time envelope"); + for (double knee : p.knee_deg) + if (knee < table_.front()[0] || knee > table_.back()[0]) + throw std::invalid_argument("Flip waypoint exceeds knee geometry"); + } + flip_program_ = std::move(program); + program_relative_ = relative; + } + void configure_capture(double value, double distance_gain) { + if (phase_ != Disabled || !std::isfinite(value) || value < 10. || value > 55. + || !std::isfinite(distance_gain) || distance_gain < 0. || distance_gain > 5.) + throw std::invalid_argument( + "Configure wheel speed limit between 10 and 55 rad/s while disabled"); + balance_wheel_speed_limit_ = value; + balance_distance_gain_ = distance_gain; + } + void configure_speed_protection(bool fault_enabled, bool braking_enabled) { + if (phase_ != Disabled) + throw std::invalid_argument("Configure speed protection while disabled"); + speed_fault_enabled_ = fault_enabled; + speed_braking_enabled_ = braking_enabled; + } + void configure_flip_handoff(double tilt_deg, double gyro, double dwell) { + if (phase_ != Disabled || !std::isfinite(tilt_deg) || !std::isfinite(gyro) + || !std::isfinite(dwell) || tilt_deg < 1. || tilt_deg > 60. || gyro <= 0. || gyro > 3. + || dwell < 0. || dwell > 1.) + throw std::invalid_argument("Invalid flip handoff conditions"); + flip_capture_gravity_z_ = -std::cos(tilt_deg * std::numbers::pi / 180.); + flip_capture_gyro_ = gyro; + flip_capture_dwell_ = dwell; + } + void set_flip_fold_first(bool value) { + if (phase_ != Disabled) + throw std::invalid_argument("Configure flip preparation while disabled"); + flip_fold_first_ = value; + } + void configure_flip_completion(double orbit_limit_deg, bool balance_when_upright) { + if (phase_ != Disabled || !std::isfinite(orbit_limit_deg) || orbit_limit_deg < 0. + || orbit_limit_deg > 180.) + throw std::invalid_argument( + "Configure flip handoff orbit limit between zero and 180 degrees while disabled"); + flip_handoff_orbit_limit_ = orbit_limit_deg * std::numbers::pi / 180.; + flip_balance_when_upright_ = balance_when_upright; + } + void configure_flip_stall(double dwell_seconds) { + if (phase_ != Disabled || !std::isfinite(dwell_seconds) || dwell_seconds < 0. + || (dwell_seconds > 0. && dwell_seconds < .05) || dwell_seconds > 2.) + throw std::invalid_argument( + "Configure flip stall dwell as zero or 0.05..2 seconds while disabled"); + flip_stall_dwell_ = dwell_seconds; + } + int stalled_waypoints() const { return stalled_waypoints_; } + void configure_flip_first_orbit_hold(unsigned mask) { + if (phase_ != Disabled || mask > 3) + throw std::invalid_argument( + "Configure first-waypoint orbit hold mask between zero and three while disabled"); + flip_first_orbit_hold_mask_ = mask; + } + + void configure_pose_routes(PoseRouteSettings settings) { + if (phase_ != Disabled) + throw std::invalid_argument("Configure pose routes while disabled"); + for (double value : + {settings.tuck_knee_deg, settings.support_knee_deg, settings.support_orbit_deg, + settings.side_stroke_deg, settings.upright_dwell_seconds, settings.upright_gyro_rad_s, + settings.plant_wheel_assist_nm, settings.landing_knee_deg, + settings.landing_orbit_deg}) + if (!std::isfinite(value)) + throw std::invalid_argument("Nonfinite pose-route setting"); + if (settings.tuck_knee_deg < table_.front()[0] + || settings.support_knee_deg > table_.back()[0] + || settings.tuck_knee_deg >= settings.support_knee_deg + || settings.landing_knee_deg < table_.front()[0] + || settings.landing_knee_deg >= settings.support_knee_deg + || settings.landing_orbit_deg < 5. || settings.landing_orbit_deg > 45. + || settings.support_orbit_deg < 60. || settings.support_orbit_deg > 120. + || settings.side_stroke_deg < 10. || settings.side_stroke_deg > 90. + || settings.upright_dwell_seconds < .04 || settings.upright_dwell_seconds > 1. + || settings.upright_gyro_rad_s <= 0. || settings.upright_gyro_rad_s > 3. + || settings.plant_wheel_assist_nm < 0. || settings.plant_wheel_assist_nm > 1.5) + throw std::invalid_argument("Invalid pose-route geometry or capture envelope"); + pose_routes_ = settings; + } + Route route() const { return route_; } + std::size_t route_waypoint() const { return route_index_; } + + void reset() { + phase_ = Disabled; + fault_ = 0; + elapsed_ = stable_ = segment_time_ = wheel_distance_ = orbit_trim_ = 0.; + segment_done_ = true; + flip_step_ = 0; + arrival_time_ = 0.; + completed_once_ = false; + reference_.fill(0.); + reference_velocity_.fill(0.); + integral_.fill(0.); + from_.fill(0.); + to_.fill(0.); + initial_orbit_.fill(0.); + support_blend_ = 0.; + distance_ = duration_ = ramp_ = peak_ = posture_lost_time_ = program_hold_time_ = 0.; + program_active_ = preparing_program_ = false; + program_index_ = 0; + lower_side_ = 0; + tracking_speed_ = settings_.cruise_speed; + segment_acceleration_ = flip_capture_time_ = flip_stall_time_ = 0.; + flip_stall_error_ = 0.; + stalled_waypoints_ = 0; + route_ = Route::None; + route_active_ = false; + route_count_ = route_index_ = 0; + route_upright_time_ = route_end_wait_ = 0.; + route_plant_orbit_.fill(0.); + route_plan_.fill({}); + } + + Output step(const Feedback& f, double dt, bool enabled, bool fresh = true) { + Output out; + out.position = f.position; + if (!enabled) { + reset(); + return out; + } + bool finite = std::isfinite(dt) && dt > 0. && dt <= .01; + for (const auto& a : {f.position, f.velocity, f.spring_compensation}) + for (double value : a) + finite = finite && std::isfinite(value); + for (double v : f.gravity) + finite = finite && std::isfinite(v); + for (double v : f.gyro) + finite = finite && std::isfinite(v); + for (double v : f.wheel_velocity) + finite = finite && std::isfinite(v); + const double gravity_norm = std::sqrt( + f.gravity[0] * f.gravity[0] + f.gravity[1] * f.gravity[1] + + f.gravity[2] * f.gravity[2]); + finite = finite && gravity_norm > .95 && gravity_norm < 1.05; + if (!finite || !fresh) + fail(!finite ? 1 : 2); + if (pose_routes_.enabled && finite) + for (std::size_t side = 0; side < 2; ++side) { + const auto plus = side == 0 ? 1 : 2, minus = side == 0 ? 0 : 3; + const double delta = f.position[plus] - f.position[minus]; + if (delta < table_.front()[plus + 1] - table_.front()[minus + 1] - .03 + || delta > table_.back()[plus + 1] - table_.back()[minus + 1] + .03) + fail(7); + } + for (double v : f.velocity) + if (speed_fault_enabled_ && std::abs(v) > settings_.speed_limit + 1e-6) + fail(3); + if (phase_ == Fault) { + out.position.fill(0.); + out.phase = Fault; + out.fault = fault_; + return out; + } + const bool capture = phase_ == Capture || phase_ == Complete; + posture_lost_time_ = capture && f.gravity[2] > -.75 ? posture_lost_time_ + dt : 0.; + if (posture_lost_time_ > .1) { + fail(6); + out.phase = Fault; + out.fault = fault_; + return out; + } + elapsed_ += dt; + if (elapsed_ > settings_.timeout_seconds && !completed_once_) + fail(4); + if (phase_ == Fault) { + out.phase = Fault; + out.fault = fault_; + return out; + } + if (phase_ == Disabled) { + reference_ = f.position; + for (std::size_t s = 0; s < 2; ++s) + initial_orbit_[s] = orbit(f.position, s); + const double stand_angle = settings_.stand_orbit * std::numbers::pi / 180.; + const Axes standing = pose( + settings_.stand_knee, + {nearest(stand_angle, initial_orbit_[0]), nearest(stand_angle, initial_orbit_[1])}); + bool already_standing = f.gravity[2] < -.99; + for (std::size_t i = 0; i < 4; ++i) + already_standing = already_standing && std::abs(standing[i] - f.position[i]) < .08; + if (already_standing) { + route_ = Route::Standing; + enter(Capture, standing, f); + support_blend_ = 1.; + } else if (pose_routes_.enabled) + start_pose_route(f); + else if (f.gravity[2] < -.75) + enter(Fold, pose(settings_.fold_knee, initial_orbit_), f); + else { + phase_ = Flip; + flip_step_ = 0; + program_active_ = !flip_program_.empty(); + lower_side_ = f.gravity[1] > 0. ? 0 : 1; + if (program_active_ && flip_fold_first_) { + preparing_program_ = true; + begin(pose(settings_.fold_knee, initial_orbit_), reference_); + } else if (program_active_) + start_program_waypoint(f); + else + begin(pose(settings_.fold_knee, initial_orbit_), reference_); + } + } + const bool was_routing = route_active_; + if (was_routing) + update_pose_route(f, dt); + if (phase_ == Fault) { + out.phase = Fault; + out.fault = fault_; + return out; + } + bool handoff_orbits_ready = true; + if (flip_handoff_orbit_limit_ > 0.) + for (std::size_t side = 0; side < 2; ++side) + handoff_orbits_ready = + handoff_orbits_ready + && std::abs(std::remainder(orbit(f.position, side), 2. * std::numbers::pi)) + <= flip_handoff_orbit_limit_; + const bool flip_quiet = phase_ == Flip && handoff_orbits_ready + && f.gravity[2] < flip_capture_gravity_z_ + && std::hypot(f.gyro[0], f.gyro[1]) < flip_capture_gyro_; + flip_capture_time_ = flip_quiet ? flip_capture_time_ + dt : 0.; + if (flip_quiet && flip_capture_time_ >= flip_capture_dwell_) { + for (std::size_t s = 0; s < 2; ++s) + initial_orbit_[s] = orbit(f.position, s); + program_active_ = false; + enter(Fold, pose(settings_.fold_knee, initial_orbit_), f); + } + if (!was_routing && segment_done_ && phase_ == Flip && program_active_) { + if (preparing_program_) { + preparing_program_ = false; + start_program_waypoint(f); + } else { + program_hold_time_ += dt; + if (program_hold_time_ >= flip_program_[program_index_].hold_seconds) { + if (++program_index_ < flip_program_.size()) + start_program_waypoint(f); + else { + program_active_ = false; + tracking_speed_ = settings_.cruise_speed; + } + } + } + } else if (!was_routing && segment_done_ && phase_ == Flip) { + ++flip_step_; + if (flip_step_ > 8) { + fail(8); + out.phase = Fault; + out.fault = fault_; + return out; + } + std::array angles{}; + double target = flip_step_ % 2 ? settings_.flip_orbit : settings_.sweep_orbit; + if (flip_step_ == 3 || flip_step_ == 4) + target = flip_step_ == 3 ? 90. : -90.; + for (std::size_t s = 0; s < 2; ++s) + angles[s] = nearest(target * std::numbers::pi / 180., orbit(f.position, s)); + Axes goal = pose(settings_.flip_knee, angles); + if (settings_.flip_asymmetry != 0. && flip_step_ >= 5) { + const std::size_t s = f.gravity[1] < 0. ? 0 : 1; // shorten the upper leg + const Axes folded = pose(settings_.flip_asymmetry, angles); + goal[s * 2] = folded[s * 2]; + goal[s * 2 + 1] = folded[s * 2 + 1]; + } + if (settings_.flip_asymmetry != 0. && std::abs(f.gravity[1]) > .9 && flip_step_ >= 3) { + const std::size_t lower = f.gravity[1] > 0. ? 0 : 1; + goal[lower * 2] = reference_[lower * 2]; + goal[lower * 2 + 1] = reference_[lower * 2 + 1]; + } + begin(goal, reference_); + } else if (!was_routing && segment_done_ && phase_ == Fold) { + const double a = settings_.stand_orbit * std::numbers::pi / 180.; + enter( + Plant, + pose( + settings_.fold_knee, + {nearest(a, orbit(f.position, 0)), nearest(a, orbit(f.position, 1))}), + f); + } else if (!was_routing && segment_done_ && phase_ == Plant) { + const double a = settings_.stand_orbit * std::numbers::pi / 180.; + enter( + Rise, + pose( + settings_.stand_knee, + {nearest(a, orbit(f.position, 0)), nearest(a, orbit(f.position, 1))}), + f); + } else if (!was_routing && segment_done_ && phase_ == Rise) + phase_ = Capture; + + advance(dt, f); + if (phase_ == Fault) { + out.phase = Fault; + out.fault = fault_; + return out; + } + out.position = reference_; + out.velocity = reference_velocity_; + out.phase = phase_; + if (phase_ == Flip && program_active_) + for (std::size_t i = 0; i < 2; ++i) + out.wheel_torque[i] = std::clamp( + .5 * (flip_program_[program_index_].wheel_velocity[i] - f.wheel_velocity[i]), + -4.5, 4.5); + bool legs_below = true; + if (pose_routes_.enabled) + for (std::size_t side = 0; side < 2; ++side) { + const double angle = orbit(f.position, side); + const double downward = + std::sin(angle) * f.gravity[0] - std::cos(angle) * f.gravity[2]; + legs_below = legs_below && downward > .65; + } + if (route_active_ && flip_route() && legs_below && (phase_ == Thrust || phase_ == Sweep)) + for (std::size_t side = 0; side < 2; ++side) + out.wheel_torque[side] = std::clamp(-.25 * f.wheel_velocity[side], -4.5, 4.5); + if ((phase_ == Plant || phase_ == Rise || phase_ == Capture || phase_ == Complete + || (phase_ == Flip && flip_balance_when_upright_)) + && f.gravity[2] < -.8 && legs_below) { + const double pitch = std::atan2(f.gravity[0], -f.gravity[2]); + const double trim_rate = + (phase_ == Capture || phase_ == Complete) && std::abs(pitch) < .35 + ? std::clamp(settings_.pitch_trim_gain * pitch, -.4, .4) + : 0.; + orbit_trim_ = std::clamp(orbit_trim_ + trim_rate * dt, -.4, .4); + for (std::size_t i = 0; i < 4; ++i) { + const double sign = i < 2 ? -1. : 1.; + out.position[i] += sign * orbit_trim_; + out.velocity[i] = std::clamp( + out.velocity[i] + sign * trim_rate, -settings_.cruise_speed, + settings_.cruise_speed); + } + const double velocity = .03 * (f.wheel_velocity[0] - f.wheel_velocity[1]); + // Airborne wheel rotation during the flip is not chassis odometry. + if (phase_ == Capture || phase_ == Complete) + wheel_distance_ += velocity * dt; + else + wheel_distance_ = 0.; + const double velocity_gain = (phase_ == Capture || phase_ == Complete) + ? settings_.balance_kv + : std::min(2., settings_.balance_kv); + double plant_assist = 0.; + if (pose_routes_.enabled && phase_ == Plant) + plant_assist = .5 * pose_routes_.plant_wheel_assist_nm + * (std::sin(orbit(f.position, 0)) + std::sin(orbit(f.position, 1))); + const double common = std::clamp( + settings_.balance_kp * pitch + settings_.balance_kd * f.gyro[1] + + velocity_gain * velocity + balance_distance_gain_ * wheel_distance_ + + plant_assist, + -4.5, 4.5); + const double headroom = 4.5 - std::abs(common); + const double yaw = std::clamp(settings_.yaw_rate_gain * f.gyro[2], -headroom, headroom); + out.wheel_torque = {common + yaw, -common + yaw}; + for (std::size_t i = 0; i < 2; ++i) + if (speed_braking_enabled_ + && std::abs(f.wheel_velocity[i]) > balance_wheel_speed_limit_ + && out.wheel_torque[i] * f.wheel_velocity[i] > 0.) + out.wheel_torque[i] = std::clamp(-.15 * f.wheel_velocity[i], -4.5, 4.5); + bool quiet = (phase_ == Capture || phase_ == Complete) && segment_done_ + && f.gravity[2] < -.9903 && std::hypot(f.gyro[0], f.gyro[1], f.gyro[2]) < .35 + && std::abs(f.wheel_velocity[0]) < 2. && std::abs(f.wheel_velocity[1]) < 2.; + for (double velocity : f.velocity) + quiet = quiet && std::abs(velocity) < .6; + for (std::size_t axis = 0; axis < 4; ++axis) + quiet = quiet && std::abs(out.position[axis] - f.position[axis]) < .08; + stable_ = quiet ? stable_ + dt : 0.; + if (phase_ == Complete && !quiet) + out.phase = phase_ = Capture; + if ((phase_ == Capture || phase_ == Complete) && stable_ >= .5) { + out.phase = phase_ = Complete; + completed_once_ = true; + } + } else { + stable_ = 0.; + if (phase_ == Complete) + out.phase = phase_ = Capture; + } + Axes support{}; + if (support_model_ && (phase_ == Rise || phase_ == Capture || phase_ == Complete) + && f.gravity[2] < -.75) { + support_blend_ = std::min(1., support_blend_ + dt / .5); + support = support_model_(f.position, f.gravity); + } else + support_blend_ = 0.; + for (std::size_t i = 0; i < 4; ++i) { + const double desired_velocity = std::clamp( + out.velocity[i] + settings_.position_gain * (out.position[i] - f.position[i]), + -tracking_speed_, tracking_speed_); + const double error = desired_velocity - f.velocity[i]; + const double previous_integral = integral_[i]; + integral_[i] = std::clamp( + integral_[i] + settings_.integral_gain * error * dt, -settings_.integral_limit, + settings_.integral_limit); + const double feedforward = + f.spring_compensation[i] + settings_.support_gain * support_blend_ * support[i]; + double torque = settings_.velocity_gain * error + integral_[i] + feedforward; + if (std::abs(torque) > settings_.torque_limit && torque * error > 0.) { + integral_[i] = previous_integral; + torque = settings_.velocity_gain * error + integral_[i] + feedforward; + } + if (speed_braking_enabled_ && std::abs(f.velocity[i]) > settings_.speed_limit - .12 + && torque * f.velocity[i] > 0.) { + integral_[i] = previous_integral; + out.servo_velocity[i] = 0.; + out.servo_feedforward[i] = 0.; + torque = -settings_.velocity_gain * f.velocity[i]; + } else { + out.servo_velocity[i] = desired_velocity; + out.servo_feedforward[i] = std::clamp( + integral_[i] + feedforward, -settings_.torque_limit, settings_.torque_limit); + } + out.torque[i] = std::clamp(torque, -settings_.torque_limit, settings_.torque_limit); + } + for (std::size_t side = 0; side < 2; ++side) { + const std::size_t plus = side == 0 ? 1 : 2, minus = side == 0 ? 0 : 3; + const double delta = f.position[plus] - f.position[minus]; + const double low = table_.front()[plus + 1] - table_.front()[minus + 1]; + const double high = table_.back()[plus + 1] - table_.back()[minus + 1]; + const double relative = out.torque[plus] - out.torque[minus]; + const double factor = + std::clamp((relative > 0. ? high - delta : delta - low) / .03, 0., 1.); + const double removed = (1. - factor) * relative * .5; + out.torque[plus] -= removed; + out.torque[minus] += removed; + out.servo_feedforward[plus] -= removed; + out.servo_feedforward[minus] += removed; + } + for (double& value : out.servo_feedforward) + value = std::clamp(value, -settings_.torque_limit, settings_.torque_limit); + return out; + } + +private: + struct RouteWaypoint { + Phase phase = Disabled; + Axes position{}; + }; + + Axes leg_pose(const std::array& knees, const std::array& angles) const { + Axes result{}; + for (std::size_t side = 0; side < 2; ++side) { + const auto row = interpolate(knees[side]); + for (std::size_t joint = 0; joint < 2; ++joint) + result[side * 2 + joint] = + row[side * 2 + joint + 1] - (side == 0 ? 1. : -1.) * angles[side]; + } + return result; + } + + bool flip_route() const { + return route_ == Route::Inverted || route_ == Route::LeftSide || route_ == Route::RightSide + || route_ == Route::Sagittal; + } + + void append_route( + Phase phase, const std::array& knees, const std::array& angles) { + route_plan_[route_count_++] = {phase, leg_pose(knees, angles)}; + } + + void start_pose_route(const Feedback& f) { + constexpr double pi = std::numbers::pi; + std::array initial{}, overhead{}, support{}; + std::array front{}, below{}; + for (std::size_t side = 0; side < 2; ++side) { + initial[side] = orbit(f.position, side); + front[side] = std::sin(initial[side]) >= 0.; + below[side] = std::cos(initial[side]) > .5; + } + if (std::abs(f.gravity[1]) > .7 && std::abs(f.gravity[1]) > std::abs(f.gravity[0]) + && std::abs(f.gravity[1]) > std::abs(f.gravity[2])) + route_ = f.gravity[1] > 0. ? Route::LeftSide : Route::RightSide; + else if (f.gravity[2] > .25) + route_ = Route::Inverted; + else if (f.gravity[2] >= -.75) + route_ = Route::Sagittal; + else if (below[0] && below[1]) + route_ = Route::UnderBody; + else if (front[0] && front[1] && !below[0] && !below[1]) + route_ = Route::ProneFront; + else if (!front[0] && !front[1] && !below[0] && !below[1]) + route_ = Route::ProneRear; + else + route_ = Route::ProneMixed; + + route_count_ = route_index_ = 0; + route_upright_time_ = route_end_wait_ = 0.; + const double tuck = pose_routes_.tuck_knee_deg, extended = pose_routes_.support_knee_deg; + const std::array short_legs{tuck, tuck}, long_legs{extended, extended}; + if (route_ == Route::LeftSide || route_ == Route::RightSide) { + const std::size_t lower = route_ == Route::LeftSide ? 0 : 1; + auto knees = short_legs; + knees[lower] = extended; + append_route(Tuck, knees, initial); + support = initial; + const double direction = f.gravity[0] < -.1 ? -1. : 1.; + support[lower] += direction * pose_routes_.side_stroke_deg * pi / 180.; + append_route(Sweep, knees, support); + } else if (flip_route()) { + const std::array flip_legs{settings_.flip_knee, settings_.flip_knee}; + append_route(Tuck, short_legs, initial); + const double ground = std::atan2(f.gravity[0], -f.gravity[2]); + for (std::size_t side = 0; side < 2; ++side) + support[side] = nearest(ground, initial[side]); + append_route(Clearance, short_legs, support); + append_route(Thrust, flip_legs, support); + const double direction = f.gravity[0] > .1 ? -1. : 1.; + auto lifting = support; + for (double& angle : lifting) + angle += direction * pi / 2.; + append_route(Sweep, long_legs, lifting); + for (double& angle : support) + angle += direction * pi; + append_route(Sweep, long_legs, support); + } else if (route_ == Route::UnderBody) { + for (std::size_t side = 0; side < 2; ++side) + route_plant_orbit_[side] = + nearest(settings_.stand_orbit * pi / 180., initial[side]); + append_route(Thrust, long_legs, initial); + append_route( + Plant, {pose_routes_.landing_knee_deg, pose_routes_.landing_knee_deg}, + route_plant_orbit_); + } else { + std::array travel_knees = short_legs; + const double support_angle = pose_routes_.support_orbit_deg * pi / 180.; + const double direction = route_ == Route::ProneFront ? 1. : -1.; + for (std::size_t side = 0; side < 2; ++side) { + if (below[side]) { + travel_knees[side] = extended; + overhead[side] = support[side] = initial[side]; + route_plant_orbit_[side] = + nearest(settings_.stand_orbit * pi / 180., initial[side]); + } else { + overhead[side] = nearest(front[side] ? pi : -pi, initial[side]); + support[side] = overhead[side] + direction * (pi - support_angle); + route_plant_orbit_[side] = support[side] + direction * support_angle + + settings_.stand_orbit * pi / 180.; + } + } + append_route(Tuck, travel_knees, initial); + append_route(Clearance, travel_knees, overhead); + append_route(Sweep, travel_knees, support); + append_route(Thrust, long_legs, support); + auto landing = route_plant_orbit_; + for (std::size_t side = 0; side < 2; ++side) + if (!below[side]) + landing[side] -= direction * pose_routes_.landing_orbit_deg * pi / 180.; + append_route(Plant, long_legs, landing); + append_route( + Plant, {pose_routes_.landing_knee_deg, pose_routes_.landing_knee_deg}, + route_plant_orbit_); + } + route_active_ = true; + phase_ = route_plan_[0].phase; + begin(route_plan_[0].position, reference_); + } + + void update_pose_route(const Feedback& f, double dt) { + const bool upright = + f.gravity[2] < -.85 + && std::hypot(f.gyro[0], f.gyro[1], f.gyro[2]) < pose_routes_.upright_gyro_rad_s; + route_upright_time_ = upright ? route_upright_time_ + dt : 0.; + if (flip_route() && route_upright_time_ >= pose_routes_.upright_dwell_seconds) { + integral_.fill(0.); + start_pose_route(f); + return; + } + if (!segment_done_) + return; + if (route_index_ + 1 < route_count_) { + ++route_index_; + phase_ = route_plan_[route_index_].phase; + begin(route_plan_[route_index_].position, reference_); + return; + } + if (flip_route()) { + route_end_wait_ += dt; + if (route_end_wait_ > 1.) + fail(8); + } else if (route_upright_time_ >= pose_routes_.upright_dwell_seconds) { + route_active_ = false; // Plant -> Rise on the following update. + } else { + route_end_wait_ += dt; + if (route_end_wait_ > 1.) + fail(6); + } + } + + static double nearest(double value, double anchor) { + return value + + 2. * std::numbers::pi * std::round((anchor - value) / (2. * std::numbers::pi)); + } + Row interpolate(double knee) const { + knee = std::clamp(knee, table_.front()[0], table_.back()[0]); + auto upper = + std::upper_bound(table_.begin(), table_.end(), knee, [](double value, const Row& row) { + return value < row[0]; + }); + if (upper == table_.end()) + return table_.back(); + if (upper == table_.begin()) + return table_.front(); + const Row& a = *(upper - 1); + const Row& b = *upper; + Row result{}; + const double t = (knee - a[0]) / (b[0] - a[0]); + for (std::size_t j = 0; j < 5; ++j) + result[j] = a[j] + t * (b[j] - a[j]); + return result; + } + double orbit(const Axes& q, std::size_t side) const { + const double sign = side == 0 ? 1. : -1.; + const auto hip = side * 2, knee = hip + 1; + const double relative = sign * (q[knee] - q[hip]); + double shape = table_.back()[0]; + for (std::size_t i = 1; i < table_.size(); ++i) { + const auto& a = table_[i - 1]; + const auto& b = table_[i]; + const double low = sign * (a[knee + 1] - a[hip + 1]); + const double high = sign * (b[knee + 1] - b[hip + 1]); + if (relative <= high) { + shape = std::lerp(a[0], b[0], std::clamp((relative - low) / (high - low), 0., 1.)); + break; + } + } + const auto row = interpolate(shape); + return .5 * sign * (row[hip + 1] + row[knee + 1] - q[hip] - q[knee]); + } + Axes pose(double knee, const std::array& angles) const { + const Row row = interpolate(knee); + Axes q{}; + for (std::size_t s = 0; s < 2; ++s) + for (std::size_t j = 0; j < 2; ++j) + q[s * 2 + j] = row[s * 2 + j + 1] - (s == 0 ? 1. : -1.) * angles[s]; + return q; + } + void + begin(const Axes& goal, const Axes& measured, double speed = 0., double acceleration = 0.) { + tracking_speed_ = speed > 0. ? speed : settings_.cruise_speed; + const double a = acceleration > 0. ? acceleration : settings_.acceleration; + from_ = measured; + to_ = goal; + distance_ = 0.; + for (std::size_t i = 0; i < 4; ++i) + distance_ = std::max(distance_, std::abs(to_[i] - from_[i])); + segment_acceleration_ = a; + ramp_ = std::min(tracking_speed_ / a, std::sqrt(distance_ / a)); + peak_ = a * ramp_; + duration_ = peak_ > 1e-9 ? distance_ / peak_ + ramp_ : 0.; + segment_time_ = 0.; + segment_done_ = distance_ < 1e-8; + arrival_time_ = flip_stall_time_ = flip_stall_error_ = 0.; + } + void start_program_waypoint(const Feedback& f) { + const auto& p = flip_program_[program_index_]; + Axes goal{}; + for (std::size_t index = 0; index < 2; ++index) { + const std::size_t side = + program_relative_ ? (index == 0 ? lower_side_ : 1 - lower_side_) : index; + const bool hold_initial = + program_index_ == 0 && (flip_first_orbit_hold_mask_ & (1u << index)); + const double angle = hold_initial ? initial_orbit_[side] + : nearest( + p.orbit_deg[index] * std::numbers::pi / 180., + orbit(f.position, side)); + const Row row = interpolate(p.knee_deg[index]); + for (std::size_t j = 0; j < 2; ++j) + goal[side * 2 + j] = row[side * 2 + j + 1] - (side == 0 ? 1. : -1.) * angle; + } + begin(goal, reference_, p.speed, p.acceleration); + program_hold_time_ = 0.; + } + void enter(Phase phase, const Axes& goal, const Feedback& f) { + (void)f; + phase_ = phase; + begin(goal, reference_); + } + void advance(double dt, const Feedback& f) { + reference_velocity_.fill(0.); + if (segment_done_) + return; + segment_time_ += dt; + const double t = std::min(segment_time_, duration_); + double distance = 0., velocity = 0.; + if (t < ramp_) { + velocity = segment_acceleration_ * t; + distance = .5 * velocity * t; + } else if (t <= duration_ - ramp_) { + velocity = peak_; + distance = .5 * peak_ * ramp_ + peak_ * (t - ramp_); + } else { + const double remaining = duration_ - t; + velocity = segment_acceleration_ * remaining; + distance = distance_ - .5 * segment_acceleration_ * remaining * remaining; + } + for (std::size_t i = 0; i < 4; ++i) { + const double direction = distance_ > 0. ? (to_[i] - from_[i]) / distance_ : 0.; + reference_[i] = from_[i] + direction * distance; + reference_velocity_[i] = direction * velocity; + } + if (t >= duration_) { + reference_ = to_; + bool arrived = true; + for (std::size_t i = 0; i < 4; ++i) + arrived = arrived && std::abs(to_[i] - f.position[i]) < .04; + arrival_time_ = arrived ? arrival_time_ + dt : 0.; + segment_done_ = arrival_time_ >= .04; + bool stopped = std::hypot(f.gyro[0], f.gyro[1], f.gyro[2]) < .5; + for (double value : f.velocity) + stopped = stopped && std::abs(value) < .6; + double error = 0.; + for (std::size_t axis = 0; axis < 4; ++axis) + error = std::max(error, std::abs(to_[axis] - f.position[axis])); + if (phase_ == Flip && program_active_ && !preparing_program_ && !arrived && stopped) { + if (flip_stall_time_ == 0. || error < flip_stall_error_ - .01) { + flip_stall_time_ = dt; + flip_stall_error_ = error; + } else + flip_stall_time_ += dt; + } else + flip_stall_time_ = 0.; + if (flip_stall_dwell_ > 0. && flip_stall_time_ >= flip_stall_dwell_) { + reference_ = from_ = to_ = f.position; + reference_velocity_.fill(0.); + integral_.fill(0.); + segment_done_ = true; + ++stalled_waypoints_; + flip_stall_time_ = 0.; + return; + } + if (segment_time_ > duration_ + 5.) + fail(5); + } + } + void fail(int reason) { + if (phase_ != Fault) { + phase_ = Fault; + fault_ = reason; + } + } + std::vector table_; + Settings settings_; + SupportModel support_model_ = nullptr; + double support_blend_ = 0.; + Phase phase_ = Disabled; + int fault_ = 0, flip_step_ = 0; + double elapsed_ = 0., stable_ = 0., segment_time_ = 0., wheel_distance_ = 0., orbit_trim_ = 0.; + double distance_ = 0., duration_ = 0., ramp_ = 0., peak_ = 0.; + double arrival_time_ = 0.; + double posture_lost_time_ = 0.; + bool segment_done_ = true, completed_once_ = false; + std::vector flip_program_; + bool program_relative_ = true, program_active_ = false; + bool flip_fold_first_ = false, preparing_program_ = false; + std::size_t program_index_ = 0, lower_side_ = 0; + double program_hold_time_ = 0., tracking_speed_ = 0., segment_acceleration_ = 0.; + double balance_wheel_speed_limit_ = 20., balance_distance_gain_ = .98; + bool speed_fault_enabled_ = true, speed_braking_enabled_ = true; + double flip_capture_gravity_z_ = -.85, flip_capture_gyro_ = 2., flip_capture_dwell_ = 0., + flip_capture_time_ = 0.; + double flip_handoff_orbit_limit_ = 0.; + bool flip_balance_when_upright_ = false; + double flip_stall_dwell_ = 0., flip_stall_time_ = 0., flip_stall_error_ = 0.; + int stalled_waypoints_ = 0; + unsigned flip_first_orbit_hold_mask_ = 0; + PoseRouteSettings pose_routes_; + Route route_ = Route::None; + std::array route_plan_{}; + std::size_t route_count_ = 0, route_index_ = 0; + bool route_active_ = false; + double route_upright_time_ = 0., route_end_wait_ = 0.; + std::array route_plant_orbit_{}; + Axes reference_{}, reference_velocity_{}, from_{}, to_{}, integral_{}; + std::array initial_orbit_{}; +}; + +} // namespace rmcs_core::controller::chassis::wheel_leg + +#include "model/recovery_geometry.hpp" + +namespace rmcs_core::controller::chassis::wheel_leg { +inline SelfRightingCore make_self_righting() { + SelfRightingCore::Settings settings; + settings.timeout_seconds = 30.; + settings.flip_knee = 55.; + settings.balance_kv = 5.2; + settings.stand_knee = recovery_v202::stand_knee; + settings.stand_orbit = recovery_v202::stand_orbit; + SelfRightingCore core{recovery_v202::geometry, settings, recovery_v202::support_compensation}; + core.configure_capture(40., .98); + SelfRightingCore::PoseRouteSettings routes; + routes.enabled = true; + core.configure_pose_routes(routes); + return core; +} +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/support_observer.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/support_observer.cpp new file mode 100644 index 000000000..60135323a --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/support_observer.cpp @@ -0,0 +1,216 @@ +#include "support_observer.hpp" + +#include +#include +#include +#include +#include + +#include + +namespace rmcs_core::controller::chassis::wheel_leg { +ClosedChainSupportObserver::ClosedChainSupportObserver( + std::array geometry, double period_seconds) + : geometry_{std::move(geometry)} { + if (!std::isfinite(period_seconds) + || (std::abs(period_seconds - 0.001) > 1e-12 && std::abs(period_seconds - 0.005) > 1e-12)) + throw std::invalid_argument("Closed-chain observation period must be 1 ms or 5 ms"); + period_seconds_ = std::abs(period_seconds - 0.001) <= 1e-12 ? 0.001 : 0.005; + reset(); +} + +void ClosedChainSupportObserver::reset() { + initialized_ = geometry_valid_ = plausible_ = confirmed_ = false; + winding_.setZero(); + previous_velocity_.setZero(); + previous_gyro_.setZero(); + previous_pulse_.setZero(); + evidence_ = {}; + bad_samples_ = {}; + bad_elapsed_seconds_ = {}; + for (auto& side : evidence_age_) + side.fill(std::numeric_limits::infinity()); + for (auto& side : evidence_elapsed_seconds_) + side.fill(std::numeric_limits::infinity()); + lost_seconds_ = 0.0f; + lost_elapsed_seconds_ = 0.0; + probe_elapsed_seconds_ = 0.0; + reference_tick_ = 0; +} + +JointReferenceRecoveryFeedback ClosedChainSupportObserver::observe( + const JointReferenceRecoveryVector6& raw_q, const JointReferenceRecoveryVector6& dq, + const Eigen::Vector3d& gravity, const Eigen::Vector3d& gyro, + const Eigen::Vector3d& acceleration, JointReferenceRecoveryPhase phase) { + return observe(raw_q, dq, gravity, gyro, acceleration, phase, period_seconds_); +} + +JointReferenceRecoveryFeedback ClosedChainSupportObserver::observe( + const JointReferenceRecoveryVector6& raw_q, const JointReferenceRecoveryVector6& dq, + const Eigen::Vector3d& gravity, const Eigen::Vector3d& gyro, + const Eigen::Vector3d& acceleration, JointReferenceRecoveryPhase phase, + double elapsed_seconds) { + if (!std::isfinite(elapsed_seconds) || elapsed_seconds <= 0.0 || elapsed_seconds > 0.020) { + reset(); + JointReferenceRecoveryFeedback feedback; + feedback.q.setConstant(std::numeric_limits::quiet_NaN()); + return feedback; + } + const bool fixed_period = period_seconds_ == 0.005; + const float dt = fixed_period ? 0.005f : static_cast(elapsed_seconds); + constexpr float tau = 2.0f * std::numbers::pi_v; + const Eigen::Matrix raw = raw_q.cast(); + if (!initialized_) { + q_ = raw; + for (int side = 0; side < 2; ++side) { + const auto& table = geometry_[side]; + const int hip = 2 * side, auxiliary = hip + 1; + const float midpoint = (table.delta_rad.front() + table.delta_rad.back()) / 2.0f; + const float difference = raw[auxiliary] - raw[hip]; + if (std::isfinite(difference)) + winding_[auxiliary] = + static_cast(std::nearbyint((midpoint - difference) / tau)); + } + initialized_ = true; + } else { + for (int axis = 0; axis < 6; ++axis) + if (std::isfinite(raw[axis]) && std::isfinite(last_raw_[axis])) + winding_[axis] -= + static_cast(std::nearbyint((raw[axis] - last_raw_[axis]) / tau)); + } + for (int axis = 0; axis < 6; ++axis) + if (std::isfinite(q_[axis])) + q_[axis] = winding_[axis] == 0 ? raw[axis] : raw[axis] + winding_[axis] * tau; + last_raw_ = raw; + const Eigen::Vector3f g = gravity.cast(), omega = gyro.cast(); + geometry_valid_ = q_.allFinite() && g.allFinite() && std::abs(g.squaredNorm() - 1.0f) <= 1e-3f; + const bool gravity_valid = geometry_valid_; + bool height_valid = geometry_valid_; + for (int side = 0; side < 2; ++side) { + const auto& table = geometry_[side]; + const int hip = 2 * side; + const float delta = q_[hip + 1] - q_[hip]; + const bool in_domain = std::isfinite(delta) && delta >= table.delta_rad.front() - 1e-6f + && delta <= table.delta_rad.back() + 1e-6f; + geometry_valid_ &= in_domain; + const auto found = std::lower_bound(table.delta_rad.begin(), table.delta_rad.end(), delta); + const auto upper = + std::clamp(found - table.delta_rad.begin(), 1, table.delta_rad.size() - 1); + const auto lower = upper - 1; + const float blend = std::clamp( + (delta - table.delta_rad[lower]) / (table.delta_rad[upper] - table.delta_rad[lower]), + 0.0f, 1.0f); + const Eigen::Vector3f point = + table.wheel_center_b_m[lower] + + blend * (table.wheel_center_b_m[upper] - table.wheel_center_b_m[lower]); + const Eigen::Vector3f vector = point - table.hip_origin_b_m; + const float angle = q_[hip] - table.hip_reference_rad; + const Eigen::Vector3f rotated = + std::cos(angle) * vector + std::sin(angle) * table.hip_axis_b.cross(vector) + + (1.0f - std::cos(angle)) * table.hip_axis_b * table.hip_axis_b.dot(vector); + const Eigen::Vector3f center = + in_domain ? Eigen::Vector3f{table.hip_origin_b_m + rotated} : Eigen::Vector3f::Zero(); + const float axle_g = g.dot(table.wheel_axis_b); + heights_[side] = center.dot(g) + + table.wheel_radius_m * std::sqrt(std::max(0.0f, 1.0f - axle_g * axle_g)); + if (!gravity_valid) + heights_[side] = 0.0f; + height_valid &= in_domain && std::isfinite(heights_[side]) && heights_[side] > 0.0f; + } + const float acceleration_norm = acceleration.cast().norm(); + // Allow a small roll/asymmetric-leg capture window. These are conditional + // wheel-plane heights, not proof that both wheels touch the ground. + plausible_ = height_valid && heights_.minCoeff() > 0.12f + && std::abs(heights_[0] - heights_[1]) < 0.045f && acceleration_norm > 6.0f + && acceleration_norm < 16.0f && omega.norm() < 8.0f; + const Eigen::Vector2f velocity = dq.tail<2>().cast(); + const Eigen::Vector2f wheel_acceleration = (velocity - previous_velocity_) / dt; + const float gyro_acceleration = (omega - previous_gyro_).norm() / dt; + if (fixed_period) + lost_seconds_ = plausible_ ? 0.0f : lost_seconds_ + dt; + else + lost_elapsed_seconds_ = plausible_ ? 0.0 : lost_elapsed_seconds_ + elapsed_seconds; + confirmed_ = plausible_; + for (int side = 0; side < 2; ++side) { + const bool tested = plausible_ && std::abs(previous_pulse_[side]) >= 0.14f; + const bool loaded = + tested && std::abs(wheel_acceleration[side]) < 100.0f && gyro_acceleration < 90.0f; + for (int direction = 0; direction < 2; ++direction) { + const bool test_direction = + tested && (previous_pulse_[side] > 0.0f) == (direction != 0); + if (fixed_period) { + // Retain the frozen 200 Hz float and sample-count arithmetic. + evidence_age_[side][direction] += dt; + if (test_direction) { + bad_samples_[side][direction] = loaded ? 0 : bad_samples_[side][direction] + 1; + if (loaded) { + evidence_[side][direction] = true; + evidence_age_[side][direction] = 0.0f; + } + } + evidence_[side][direction] &= + bad_samples_[side][direction] < 2 && lost_seconds_ < 0.05f; + confirmed_ &= evidence_[side][direction] && evidence_age_[side][direction] < 0.6f; + } else { + // The 1 kHz path measures bad response, loss and expiry in time. + constexpr double epsilon = 1e-12; + evidence_elapsed_seconds_[side][direction] += elapsed_seconds; + if (test_direction) { + bad_elapsed_seconds_[side][direction] = + loaded ? 0.0 : bad_elapsed_seconds_[side][direction] + elapsed_seconds; + if (loaded) { + evidence_[side][direction] = true; + evidence_elapsed_seconds_[side][direction] = 0.0; + } + } + evidence_[side][direction] &= + bad_elapsed_seconds_[side][direction] + epsilon < 0.010 + && lost_elapsed_seconds_ + epsilon < 0.050; + confirmed_ &= evidence_[side][direction] + && evidence_elapsed_seconds_[side][direction] + epsilon < 0.600; + } + } + } + previous_velocity_ = velocity; + previous_gyro_ = omega; + previous_pulse_.setZero(); + JointReferenceRecoveryFeedback feedback; + feedback.q = q_.cast(); + feedback.dq = dq; + feedback.gravity = gravity; + feedback.gyro = gyro; + feedback.estimated_height = heights_.minCoeff(); + feedback.support = plausible_; + feedback.support_confirmed = confirmed_; + feedback.body_clear = (confirmed_ || phase == JointReferenceRecoveryPhase::kBlend + || phase == JointReferenceRecoveryPhase::kRl) + && plausible_ && heights_.minCoeff() > 0.27f; + return feedback; +} + +Eigen::Vector2d ClosedChainSupportObserver::probe_command(bool scripted_and_released) { + return probe_command(scripted_and_released, period_seconds_); +} + +Eigen::Vector2d + ClosedChainSupportObserver::probe_command(bool scripted_and_released, double elapsed_seconds) { + if (!std::isfinite(elapsed_seconds) || elapsed_seconds <= 0.0 || elapsed_seconds > 0.020) { + reset(); + return Eigen::Vector2d::Zero(); + } + previous_pulse_.setZero(); + if (scripted_and_released && plausible_ && !confirmed_) { + const auto stage = + period_seconds_ == 0.005 + ? (reference_tick_ / 3) % 4 + : static_cast(std::floor((probe_elapsed_seconds_ + 1e-12) / 0.015)) + % 4; + previous_pulse_[stage / 2] = stage % 2 == 0 ? 0.18f : -0.18f; + } + if (period_seconds_ == 0.005) + ++reference_tick_; + else + probe_elapsed_seconds_ = std::fmod(probe_elapsed_seconds_ + elapsed_seconds, 0.060); + return previous_pulse_.cast(); +} +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/support_observer.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/support_observer.hpp new file mode 100644 index 000000000..dfa287372 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/support_observer.hpp @@ -0,0 +1,70 @@ +#pragma once + +#include "recovery.hpp" + +#include +#include + +#include + +namespace rmcs_core::controller::chassis::wheel_leg { + +struct ClosedChainLegGeometry { + std::vector delta_rad; + std::vector wheel_center_b_m; + Eigen::Vector3f hip_origin_b_m; + Eigen::Vector3f hip_axis_b; + Eigen::Vector3f wheel_axis_b; + float hip_reference_rad = 0.0f; + float wheel_radius_m = 0.06f; +}; + +// Ported from dev/wheel_leg_rl @ 0fcc9b45; sensor bundle was unused. +// Native conditional FK and wheel-response probe. No contact, force, root +// position or simulator truth is accepted here. The caller supplies validated +// body-frame IMU and calibrated actuator feedback. +class ClosedChainSupportObserver { +public: + explicit ClosedChainSupportObserver( + std::array geometry, double period_seconds = 0.005); + void reset(); + JointReferenceRecoveryFeedback observe( + const JointReferenceRecoveryVector6& q, const JointReferenceRecoveryVector6& dq, + const Eigen::Vector3d& gravity, const Eigen::Vector3d& gyro, + const Eigen::Vector3d& specific_acceleration, JointReferenceRecoveryPhase phase); + // The 1 kHz path uses actual elapsed time; 200 Hz preserves fixed-step + // arithmetic. An invalid interval clears evidence and returns nonfinite q. + JointReferenceRecoveryFeedback observe( + const JointReferenceRecoveryVector6& q, const JointReferenceRecoveryVector6& dq, + const Eigen::Vector3d& gravity, const Eigen::Vector3d& gyro, + const Eigen::Vector3d& specific_acceleration, JointReferenceRecoveryPhase phase, + double elapsed_seconds); + Eigen::Vector2d probe_command(bool scripted_and_released); + Eigen::Vector2d probe_command(bool scripted_and_released, double elapsed_seconds); + bool geometry_valid() const { return geometry_valid_; } + bool support_confirmed() const { return confirmed_; } + Eigen::Vector2d height_candidates() const { return heights_.cast(); } + +private: + std::array geometry_; + Eigen::Matrix q_ = Eigen::Matrix::Zero(); + Eigen::Matrix last_raw_ = Eigen::Matrix::Zero(); + Eigen::Matrix winding_ = Eigen::Matrix::Zero(); + Eigen::Vector2f previous_velocity_ = Eigen::Vector2f::Zero(); + Eigen::Vector3f previous_gyro_ = Eigen::Vector3f::Zero(); + Eigen::Vector2f previous_pulse_ = Eigen::Vector2f::Zero(); + Eigen::Vector2f heights_ = Eigen::Vector2f::Zero(); + std::array, 2> evidence_{}; + std::array, 2> evidence_age_{}; + std::array, 2> bad_samples_{}; + std::array, 2> evidence_elapsed_seconds_{}; + std::array, 2> bad_elapsed_seconds_{}; + double period_seconds_ = 0.005; + std::uint64_t reference_tick_ = 0; + float lost_seconds_ = 0.0f; + double lost_elapsed_seconds_ = 0.0; + double probe_elapsed_seconds_ = 0.0; + bool initialized_ = false, geometry_valid_ = false, plausible_ = false, confirmed_ = false; +}; + +} // namespace rmcs_core::controller::chassis::wheel_leg diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_chassis_controller.cpp new file mode 100644 index 000000000..f19daf811 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_chassis_controller.cpp @@ -0,0 +1,255 @@ +#include "wheel_leg/jump_trigger.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" +namespace rmcs_core::controller::chassis { +class WheelLegResetSource final : public rmcs_executor::Component { +public: + WheelLegResetSource() { + register_input("/remote/switch/left", left_); + register_input("/remote/switch/right", right_); + register_input("/wheel_leg/dr16_fresh", fresh_); + register_output("/chassis/reset_count", reset_, std::size_t{0}); + } + + void update() override { + using rmcs_msgs::Switch; + const bool reset = !*fresh_ || *left_ == Switch::UNKNOWN || *right_ == Switch::UNKNOWN + || (*left_ == Switch::DOWN && *right_ == Switch::DOWN); + if (reset && !reset_active_) { + ++*reset_; + } + reset_active_ = reset; + } + +private: + InputInterface left_, right_; + InputInterface fresh_; + OutputInterface reset_; + bool reset_active_ = false; +}; + +class WheelLegChassisController final + : public rmcs_executor::Component + , public rclcpp::Node + , public rmcs_utility::NodeMixin { +public: + WheelLegChassisController() + : Node{get_component_name(), node::options()} { + register_input("/remote/joystick/right", right_); + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); + register_input("/wheel_leg/dr16_fresh", remote_fresh_); + register_input("/predefined/timestamp", timestamp_); + register_input("/chassis/reset_count", reset_count_); + register_output( + "/chassis/control_velocity", command_, + rmcs_description::BaseLink::DirectionVector{0., 0., 0.}); + register_output("/chassis/control_height", height_, .305); + register_output("/chassis/control_enable", enable_, false); + reset_source_ = + create_partner_component(get_component_name() + "_reset"); + register_output("/chassis/jump/request_count", jump_request_, std::size_t{0}); + register_output("/chassis/control_mode", mode_, rmcs_msgs::ChassisMode::AUTO); + vx_max_ = get_parameter_or("vx_max", 2.); + vx_bias_ = get_parameter_or("vx_bias", 0.); + vy_max_ = get_parameter_or("vy_max", 0.); + spin_rate_ = get_parameter_or("spin_yaw_rate", 1.); + yaw_stick_rate_ = get_parameter_or("yaw_stick_rate", std::numbers::pi); + deadzone_ = get_parameter_or("deadzone", .05); + height_min_ = get_parameter_or("command_height_min", .23); + height_max_ = get_parameter_or("command_height_max", .42); + nominal_height_ = get_parameter_or("default_command_height", .305); + height_rate_ = get_parameter_or("height_rate", .1); + yaw_max_ = get_parameter_or("yaw_rate_max", 1.); + following_velocity_controller_.kp = get_parameter_or("following_velocity_kp", 8.0); + following_velocity_controller_.ki = get_parameter_or("following_velocity_ki", 0.0); + following_velocity_controller_.kd = get_parameter_or("following_velocity_kd", 0.0); + following_velocity_controller_.output_max = yaw_max_; + following_velocity_controller_.output_min = -yaw_max_; + yaw_invert_ = get_parameter_or("angular_z_invert", false); + arm_dwell_ = get_parameter_or("arm_dwell_seconds", 0.); + for (double value : + {vx_max_, yaw_max_, spin_rate_, yaw_stick_rate_, height_min_, height_max_, + height_rate_}) + if (!std::isfinite(value) || value <= 0.) + throw std::invalid_argument("Invalid chassis command limits"); + if (!std::isfinite(vx_bias_) || std::abs(vx_bias_) > vx_max_) + throw std::invalid_argument("Invalid chassis velocity bias"); + if (height_max_ < height_min_ || nominal_height_ < height_min_ + || nominal_height_ > height_max_ || !std::isfinite(deadzone_) || deadzone_ < 0. + || deadzone_ >= 1. || !std::isfinite(arm_dwell_) || arm_dwell_ < 0.) + throw std::invalid_argument("Invalid height, deadzone or arming dwell"); + *height_ = nominal_height_; + } + void update() override { + using rmcs_msgs::Switch; + const auto l = *switch_left_, r = *switch_right_; + const bool connected = *remote_fresh_; + const bool down = connected && l == Switch::DOWN && r == Switch::DOWN; + const bool middle = l == Switch::MIDDLE && r == Switch::MIDDLE; + const bool unknown = !connected || l == Switch::UNKNOWN || r == Switch::UNKNOWN; + const auto now = *timestamp_; + if (unknown || down) { + disarm(); + pending_ = down; + arm_started_.reset(); + } else { + if (middle && pending_) { + if (!arm_started_) + arm_started_ = now; + if (std::chrono::duration(now - *arm_started_).count() >= arm_dwell_) { + armed_ = true; + pending_ = false; + } + } else if (!middle) + arm_started_.reset(); + if (!armed_ && (l == Switch::UP || r == Switch::UP)) + pending_ = false; + if (armed_) { + if (wheel_leg::jump_requested( + was_armed_, keyboard_->ctrl, last_keyboard_.ctrl, + last_left_ == Switch::MIDDLE && last_right_ == Switch::MIDDLE, + l == Switch::MIDDLE, r == Switch::UP)) + ++*jump_request_; + const bool spin_edge = + l != Switch::DOWN && last_right_ == Switch::MIDDLE && r == Switch::DOWN; + if (spin_edge || (!last_keyboard_.c && keyboard_->c)) { + if (*mode_ == rmcs_msgs::ChassisMode::SPIN_FAST) + *mode_ = spin_edge ? rmcs_msgs::ChassisMode::STEP_DOWN + : rmcs_msgs::ChassisMode::AUTO; + else { + *mode_ = rmcs_msgs::ChassisMode::SPIN_FAST; + spin_forward_ = !spin_forward_; + } + following_velocity_controller_.reset(); + } else if (!last_keyboard_.z && keyboard_->z) { + *mode_ = *mode_ == rmcs_msgs::ChassisMode::STEP_DOWN + ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::STEP_DOWN; + following_velocity_controller_.reset(); + } + *enable_ = true; + if (*mode_ == rmcs_msgs::ChassisMode::SPIN_FAST) { + command_->vector = Eigen::Vector3d{ + 0., 0., (spin_forward_ ? 1. : -1.) * spin_rate_ * (yaw_invert_ ? -1. : 1.)}; + } else { + Eigen::Vector2d xy{channel(right_->x()) + keyboard_->w - keyboard_->s, 0.}; + if (xy.norm() > 1.) + xy.normalize(); + const auto rotated = + Eigen::Rotation2Dd{gimbal_yaw_angle_.ready() ? *gimbal_yaw_angle_ : 0.} + * xy; + double yaw = 0.; + if (*mode_ == rmcs_msgs::ChassisMode::STEP_DOWN) { + const double gimbal_yaw = + gimbal_yaw_angle_.ready() + ? std::remainder(*gimbal_yaw_angle_, 2. * std::numbers::pi) + : 0.; + yaw = following_velocity_controller_.update(gimbal_yaw) + * (yaw_invert_ ? -1. : 1.); + } else if (*mode_ == rmcs_msgs::ChassisMode::AUTO) { + const double yaw_stick = + std::clamp(channel(right_->y()) + keyboard_->a - keyboard_->d, -1., 1.); + yaw = yaw_stick * yaw_stick_rate_ * (yaw_invert_ ? -1. : 1.); + } + // Bias fades toward full stick so both directions still reach ±vx_max. + const double stick = rotated.x(); + const double vx = std::clamp( + stick * vx_max_ - vx_bias_ * (1. - std::abs(stick)), -vx_max_, vx_max_); + command_->vector = Eigen::Vector3d{vx, rotated.y() * vy_max_, yaw}; + } + double height = *height_; + const double dt = + last_height_update_ + ? std::clamp( + std::chrono::duration(now - *last_height_update_).count(), 0., + .1) + : 0.; + last_height_update_ = now; + if (!last_keyboard_.q && keyboard_->q) { + height_preset_ = height_preset_ == 1 ? 0 : 1; + height = height_preset_ == 1 ? height_max_ : nominal_height_; + } else if (!last_keyboard_.e && keyboard_->e) { + height_preset_ = height_preset_ == -1 ? 0 : -1; + height = height_preset_ == -1 ? height_min_ : nominal_height_; + } else { + const double knob = channel(*rotary_knob_); + height -= knob * height_rate_ * dt; + if (knob != 0.) + height_preset_ = 0; + } + *height_ = std::clamp(height, height_min_, height_max_); + } + } + last_right_ = r; + last_left_ = l; + last_keyboard_ = *keyboard_; + was_armed_ = armed_; + } + +private: + using Clock = std::chrono::steady_clock; + void disarm() { + armed_ = false; + *enable_ = false; + command_->vector.setZero(); + *height_ = nominal_height_; + *mode_ = rmcs_msgs::ChassisMode::AUTO; + spin_forward_ = true; + height_preset_ = 0; + last_height_update_.reset(); + following_velocity_controller_.reset(); + } + double channel(double value) const { + return !std::isfinite(value) || std::abs(value) < deadzone_ ? 0. + : std::clamp(value, -1., 1.); + } + InputInterface right_; + InputInterface gimbal_yaw_angle_; + InputInterface switch_right_, switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_; + InputInterface remote_fresh_; + InputInterface timestamp_; + OutputInterface command_; + OutputInterface height_; + OutputInterface enable_; + std::shared_ptr reset_source_; + InputInterface reset_count_; + OutputInterface jump_request_; + OutputInterface mode_; + double vx_max_, vx_bias_, vy_max_, yaw_max_, spin_rate_, yaw_stick_rate_, deadzone_, + height_min_, height_max_, nominal_height_, height_rate_, arm_dwell_; + bool yaw_invert_, armed_ = false, pending_ = false, spin_forward_ = true; + rmcs_msgs::Switch last_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_left_ = rmcs_msgs::Switch::UNKNOWN; + bool was_armed_ = false; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + std::optional arm_started_; + pid::PidCalculator following_velocity_controller_; + std::optional last_height_update_; + int height_preset_ = 0; +}; +} // namespace rmcs_core::controller::chassis +#include +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::WheelLegChassisController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_observation.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_observation.cpp new file mode 100644 index 000000000..86e63bbb2 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_observation.cpp @@ -0,0 +1,151 @@ +#include "wheel_leg/policy/selection.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace rmcs_core::controller::chassis { +class WheelLegObservation final + : public rmcs_executor::Component + , public rclcpp::Node + , public rmcs_utility::NodeMixin { +public: + WheelLegObservation() + : Node{get_component_name(), node::options()} + , policy_(policy_parameters(get_parameter("policy").as_string())) { + if (wheelbipe_policy(policy_)) + adapter_.emplace(); + register_input("/wheel_leg/state/position", q_); + register_input("/wheel_leg/state/velocity", dq_); + register_input("/wheel_leg/state/gravity", gravity_); + register_input("/wheel_leg/state/valid", valid_); + register_input("/wheel_leg/imu/angular_velocity", gyro_); + register_input("/wheel_leg/rl/input/enable", enable_); + register_input("/wheel_leg/rl/input/command", command_); + register_input("/wheel_leg/rl/input/height", height_); + register_input("/wheel_leg/rl/input/context", context_, false); + register_input("/wheel_leg/rl/applied_action", applied_); + register_input("/wheel_leg/rl/action_applied", action_applied_); + register_input("/wheel_leg/rl/state", state_); + register_input("/chassis/reset_count", reset_); + register_input("/predefined/update_count", tick_); + register_input("/predefined/update_rate", rate_); + register_input("/wheel_leg/rl/policy_rate", policy_rate_); + register_output("/wheel_leg/rl/features/command", feature_command_, std::vector(3)); + register_output("/wheel_leg/rl/features/height", feature_height_, 0.0); + register_output("/wheel_leg/rl/features/gyro", feature_gyro_, std::vector(3)); + register_output("/wheel_leg/rl/features/gravity", feature_gravity_, std::vector(3)); + register_output( + "/wheel_leg/rl/features/position", feature_position_, std::vector(6)); + register_output( + "/wheel_leg/rl/features/velocity", feature_velocity_, std::vector(6)); + register_output( + "/wheel_leg/rl/features/previous_action", feature_action_, std::vector(6)); + register_output( + "/wheel_leg/rl/features/auxiliary", feature_auxiliary_, std::vector(7)); + register_output("/wheel_leg/rl/input/frame_sequence", sequence_, std::size_t{0}); + } + void update() override { + if (*reset_ != last_reset_ || (!*enable_ && was_enabled_) + || (*state_ == 3 && last_state_ != 3)) { + std::visit([](auto& a) { a.reset(); }, adapter_); + last_tick_.reset(); + clear_features(); + *sequence_ = 0; + last_reset_ = *reset_; + } + was_enabled_ = *enable_; + last_state_ = *state_; + if (!*enable_ || !*valid_) + return; + if (!schedule_checked_) { + if (*rate_ != 1000.) + throw std::invalid_argument("Wheel-leg observation requires a 1000 Hz executor"); + schedule_checked_ = true; + } + if (!std::isfinite(*policy_rate_) || *policy_rate_ <= 0. || *policy_rate_ > 1000. + || std::fmod(1000., *policy_rate_) != 0.) + throw std::invalid_argument("Policy rate must divide the 1000 Hz executor rate"); + if (policy_.name == kProneMotion.name && *policy_rate_ != 100.) + throw std::invalid_argument( + "Standing-reference odometry currently requires 100 Hz sampling"); + const auto divisor = static_cast(1000. / *policy_rate_); + if (last_tick_ && *tick_ - *last_tick_ < divisor) + return; + last_tick_ = *tick_; + const auto history = + *action_applied_ + ? *applied_ + : (wheelbipe_policy(policy_) ? JointVector{} : measured_policy_history(*q_)); + const auto frame = std::visit( + [&](auto& a) { + return a.sample( + ++policy_tick_, *q_, *dq_, {gyro_->x(), gyro_->y(), gyro_->z()}, + {gravity_->x(), gravity_->y(), gravity_->z()}, + {command_->x(), command_->y(), command_->z()}, *height_, history, *state_ == 3); + }, + adapter_); + if (!frame) { + clear_features(); + *sequence_ = 0; + return; + } + std::copy(frame->command.begin(), frame->command.end(), feature_command_->begin()); + *feature_height_ = frame->height; + std::copy(frame->gyro.begin(), frame->gyro.end(), feature_gyro_->begin()); + std::copy(frame->gravity.begin(), frame->gravity.end(), feature_gravity_->begin()); + std::copy(frame->position.begin(), frame->position.end(), feature_position_->begin()); + std::copy(frame->velocity.begin(), frame->velocity.end(), feature_velocity_->begin()); + std::copy( + frame->previous_action.begin(), frame->previous_action.end(), feature_action_->begin()); + std::copy(frame->auxiliary.begin(), frame->auxiliary.end(), feature_auxiliary_->begin()); + if (policy_.jump_control) { + if (!context_.ready() || context_->size() != 7 + || !std::ranges::all_of(*context_, [](double v) { return std::isfinite(v); })) { + clear_features(); + *sequence_ = 0; + return; + } + std::copy(context_->begin(), context_->end(), feature_auxiliary_->begin()); + } + *sequence_ = policy_tick_; + } + +private: + void clear_features() { + for (auto* output : + {&feature_command_, &feature_gyro_, &feature_gravity_, &feature_position_, + &feature_velocity_, &feature_action_, &feature_auxiliary_}) + std::fill((**output).begin(), (**output).end(), 0.0); + *feature_height_ = 0.0; + } + + const PolicyParameters& policy_; + std::variant adapter_; + InputInterface q_, dq_, applied_; + InputInterface gravity_, gyro_, command_; + InputInterface height_, rate_, policy_rate_; + InputInterface> context_; + InputInterface enable_, valid_, action_applied_; + InputInterface state_; + InputInterface reset_, tick_; + OutputInterface> feature_command_, feature_gyro_, feature_gravity_; + OutputInterface> feature_position_, feature_velocity_, feature_action_, + feature_auxiliary_; + OutputInterface feature_height_; + OutputInterface sequence_; + std::optional last_tick_; + std::size_t policy_tick_ = 0, last_reset_ = 0; + int last_state_ = 0; + bool was_enabled_ = false, schedule_checked_ = false; +}; +} // namespace rmcs_core::controller::chassis +#include +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::WheelLegObservation, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_rl_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_rl_controller.cpp new file mode 100644 index 000000000..5b81e0ec9 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_rl_controller.cpp @@ -0,0 +1,586 @@ +#include "wheel_leg/capture.hpp" +#include "wheel_leg/joint_solver.hpp" +#include "wheel_leg/jump_command.hpp" +#include "wheel_leg/jump_control.hpp" +#include "wheel_leg/policy/selection.hpp" +#include "wheel_leg/recovery_yaw.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace rmcs_core::controller::chassis { +static_assert(wheel_leg::kRecoveryModelSpecSha256 == kModelSpecSha256); +class WheelLegRlController final + : public rmcs_executor::Component + , public rclcpp::Node + , public rmcs_utility::NodeMixin { +public: + WheelLegRlController() + : Node{get_component_name(), node::options()} + , policy_(policy_parameters(get_parameter("policy").as_string())) + , recovery_config_(recovery_configuration()) + , recovery_(recovery_config_) + , support_observer_(wheel_leg::recovery_leg_geometry(), recovery_config_.dt) { + register_input("/wheel_leg/state/position", q_); + register_input("/wheel_leg/state/velocity", dq_); + register_input("/wheel_leg/state/valid", valid_); + register_input("/wheel_leg/state/gravity", gravity_); + register_input("/wheel_leg/state/attitude", attitude_); + register_input("/wheel_leg/imu/angular_velocity", gyro_); + register_input("/wheel_leg/imu/acceleration", acceleration_); + register_input("/wheel_leg/dm_control_ready", drives_ready_); + register_input("/chassis/control_enable", requested_); + register_input("/chassis/control_velocity", command_); + register_input("/chassis/control_height", height_); + register_input("/chassis/control_mode", mode_); + register_input("/chassis/reset_count", reset_); + register_input("/chassis/jump/request_count", jump_request_, false); + register_input("/predefined/update_count", tick_); + register_input("/predefined/update_rate", rate_); + register_input("/wheel_leg/rl/valid", action_valid_); + register_input("/wheel_leg/rl/healthy", healthy_); + register_input("/wheel_leg/rl/action_seq", action_sequence_); + register_input("/wheel_leg/rl/action_steady_ns", action_stamp_); + register_input("/wheel_leg/rl/policy_rate", policy_rate_); + register_input("/wheel_leg/rl/model_spec_sha256", model_spec_); + register_input("/wheel_leg/rl/observation_contract_sha256", observation_sha_); + for (int i = 0; i < 6; ++i) { + const auto prefix = std::string{"/wheel_leg/"} + kMotorNames[i]; + register_input(prefix + "/max_torque", caps_[i]); + register_output(prefix + "/control_torque", effort_[i], 0.); + const auto raw = std::string{"/wheel_leg/rl/raw/"} + kMotorNames[i]; + register_input(raw, action_[i]); + register_input(raw + "/raw", raw_action_[i]); + register_input(raw + "/scale", action_scale_[i]); + register_input(raw + "/offset", action_offset_[i]); + if (policy_.jump_control) { + register_input(raw + "/clip_min", action_min_[i]); + register_input(raw + "/clip_max", action_max_[i]); + } + } + register_output("/wheel_leg/enable_request", enable_, false); + register_output("/wheel_leg/rl/input/enable", policy_enable_, false); + register_output("/wheel_leg/rl/input/command", policy_command_, Eigen::Vector3d::Zero()); + register_output("/wheel_leg/rl/input/height", policy_height_, .305); + register_output( + "/wheel_leg/rl/input/context", policy_context_, + std::vector{1., 0., 0., 0., 0., 0., 0.}); + register_output("/chassis/jump/active", jump_active_, false); + register_output("/chassis/jump/state", jump_state_, 0); + register_output("/chassis/jump/elapsed", jump_elapsed_, 0.); + register_output("/chassis/jump/target_apex", jump_apex_, .46); + register_output("/chassis/jump/reference_height", jump_height_, .305); + register_output("/chassis/jump/accepted_count", jump_accepted_, 0); + register_output("/wheel_leg/rl/pd_target", pd_target_, JointVector{}); + register_output("/wheel_leg/rl/pd_requested_effort", pd_requested_, JointVector{}); + register_output("/wheel_leg/rl/tuck_compensation", tuck_compensation_, JointVector{}); + register_output("/chassis/jump/tuck_gate_left", tuck_gate_left_, 0.); + register_output("/chassis/jump/tuck_gate_right", tuck_gate_right_, 0.); + register_output("/wheel_leg/rl/applied_action", applied_, JointVector{}); + register_output("/wheel_leg/rl/action_applied", action_applied_, false); + register_output("/wheel_leg/rl/state", state_, 0); + register_output("/wheel_leg/rl/fault_latched", fault_, false); + register_output("/wheel_leg/recovery/phase", recovery_phase_, 0); + register_output("/wheel_leg/recovery/fault", recovery_fault_, 0); + register_output("/wheel_leg/recovery/yaw_align_active", recovery_yaw_active_, false); + register_output("/wheel_leg/recovery/capture_ready", recovery_capture_ready_, false); + register_output("/wheel_leg/recovery/capture_blockers", recovery_capture_blockers_, 0); + timeout_ = get_parameter_or("inference_timeout", .06); + drive_loss_debounce_ = + std::chrono::duration(get_parameter_or("drive_loss_debounce", .1)); + if (!std::isfinite(drive_loss_debounce_.count()) || drive_loss_debounce_.count() < 0.) + throw std::invalid_argument("Invalid drive loss debounce"); + heading_hold_ = get_parameter_or("heading_hold_enabled", false); + history_source_ = get_parameter_or("action_history_source", "mapped"); + if (history_source_ != "raw" && history_source_ != "mapped") + throw std::invalid_argument("action_history_source must be raw or mapped"); + height_rate_ = get_parameter_or("height_reference_rate_max", .6666666666666666); + jump_enabled_ = get_parameter_or("jump.enabled", false); + jump_ = wheel_leg::JumpCommand(get_parameter_or("jump.target_apex", .46)); + if (jump_enabled_ != policy_.jump_control) + throw std::invalid_argument( + "Jump requires the explicit wheelbipe-jump-v4 policy profile"); + recovery_enabled_ = get_parameter_or("self_righting.enabled", false); + if (recovery_enabled_) + register_input("/gimbal/yaw/angle", recovery_yaw_); + const bool sim_only = get_parameter_or("self_righting.simulation_only", true); + const bool hardware_verified = + get_parameter_or("self_righting.hardware_mapping_verified", false); + simulation_guarded_ = recovery_enabled_ && (sim_only || !hardware_verified); + if (simulation_guarded_) + register_input("/wheel_leg/simulation", simulation_); + if (!std::isfinite(timeout_) || timeout_ <= 0. || !std::isfinite(height_rate_) + || height_rate_ <= 0.) + throw std::invalid_argument("Invalid inference timeout or height rate"); + } + void before_updating() override { + if (!std::isfinite(*policy_rate_) || *policy_rate_ <= 0. || *policy_rate_ > 1000. + || std::fmod(1000., *policy_rate_) != 0. || *model_spec_ != policy_.model_spec_sha256 + || *observation_sha_ != policy_.observation_sha) + throw std::invalid_argument("Policy model, cadence or observation contract mismatch"); + for (int i = 0; i < 6; ++i) + if (!std::isfinite(*action_scale_[i]) || *action_scale_[i] == 0. + || !std::isfinite(*action_offset_[i])) + throw std::invalid_argument( + "Wheel-leg action mapping must be invertible and finite"); + if (policy_.jump_control) { + if (!jump_request_.ready() || *policy_rate_ != policy_.policy_hz) + throw std::invalid_argument( + "Jump requires a request source and its trained policy cadence"); + for (int i = 0; i < 6; ++i) { + const bool leg = i < 4; + if (*action_scale_[i] != (leg ? .5 : 10.) || *action_offset_[i] != 0. + || (leg && (*action_min_[i] != -INFINITY || *action_max_[i] != INFINITY)) + || (!leg && (*action_min_[i] != -90. || *action_max_[i] != 90.))) + throw std::invalid_argument( + "Jump action scale, offset or clipping differs from training"); + } + } + if (simulation_guarded_ && (!simulation_.ready() || !*simulation_)) + throw std::invalid_argument( + "Self-righting requires an explicit /wheel_leg/simulation=true input until " + "hardware mapping is verified"); + } + void update() override { + if (!schedule_checked_) { + if (*rate_ != 1000.) + throw std::invalid_argument("Wheel-leg RL requires a 1000 Hz executor"); + schedule_checked_ = true; + } + if (*reset_ != last_reset_) { + reset_runtime(); + *fault_ = false; + last_reset_ = *reset_; + } + const auto jump_request = jump_request_.ready() ? *jump_request_ : 0; + const bool jump_requested = jump_request != last_jump_request_; + last_jump_request_ = jump_request; + if (!*requested_) { + if (*state_ != 1) + reset_runtime(); + *state_ = 1; + return; + } + if (*fault_) + return; + if (*state_ >= 3 && *policy_enable_) { + const auto now = Clock::now(); + const double since = + have_target_ + ? std::chrono::duration(now.time_since_epoch()).count() + - accepted_stamp_ * 1e-9 + : std::chrono::duration(now - wait_started_.value_or(now)).count(); + if (since > .5 && !action_starved_logged_) { + RCLCPP_WARN( + get_logger(), + "no policy action for %.2f s: state=%d valid=%d drives_ready=%d " + "action_valid=%.0f action_seq=%zu accepted_seq=%zu", + since, *state_, static_cast(*valid_), static_cast(*drives_ready_), + *action_valid_, *action_sequence_, accepted_sequence_); + action_starved_logged_ = true; + } else if (since <= .5) + action_starved_logged_ = false; + } + *enable_ = true; + *recovery_yaw_active_ = recovery_enabled_ && *state_ != 3; + if (*state_ <= 1) + *state_ = 2; + if (*state_ == 2) { + if (!*valid_ || !*drives_ready_) { + drive_loss_started_.reset(); + return; + } + if (recovery_enabled_ && !wheel_leg::recovery_yaw_aligned(*recovery_yaw_)) + return; + *state_ = recovery_enabled_ ? 4 : 3; + last_pd_.reset(); + last_command_tick_.reset(); + } else if (!*valid_) { + fail("wheel-leg state estimate invalid"); + return; + } else if (!*drives_ready_) { + jump_.reset(); + publish_jump(); + clear_tuck_effort(); + if (policy_.jump_control) { + for (auto& output : effort_) + *output = 0.; + *action_applied_ = false; + } + const auto now = Clock::now(); + if (!drive_loss_started_) + drive_loss_started_ = now; + if (std::chrono::duration(now - *drive_loss_started_) < drive_loss_debounce_) + return; + fail("DM drive readiness lost"); + return; + } else { + drive_loss_started_.reset(); + } + // Run inference throughout recovery, as a shadow policy. Only the script + // owns the actuators until measured dynamic capture accepts a fresh action. + if (!*policy_enable_) { + *policy_enable_ = true; + wait_started_ = Clock::now(); + accepted_sequence_ = *action_sequence_; + } + update_jump(jump_requested); + update_command(); + if (*fault_) + return; + if (*healthy_ <= 0.) { + fail("inference worker failed"); + return; + } + if (*action_valid_ > 0. && *action_sequence_ != accepted_sequence_) { + JointVector raw; + for (int i = 0; i < 6; ++i) { + raw[i] = *action_[i]; + if (!std::isfinite(raw[i])) { + fail("non-finite policy action"); + return; + } + } + decoded_ = raw; + target_ = decoded_action_target(raw, *q_, target_bounds()); + for (int i = 0; i < 6; ++i) + pending_history_[i] = (raw[i] - *action_offset_[i]) / *action_scale_[i]; + if (history_source_ == "raw") + for (int i = 0; i < 6; ++i) + pending_history_[i] = *raw_action_[i]; + if (!std::ranges::all_of(pending_history_, [](double v) { return std::isfinite(v); })) { + fail("invalid policy action feedback"); + return; + } + if (!std::ranges::all_of(target_, [](double v) { return std::isfinite(v); })) { + fail("invalid action target"); + return; + } + accepted_sequence_ = *action_sequence_; + accepted_stamp_ = *action_stamp_; + have_target_ = true; + } + const auto now = Clock::now(); + const double age = have_target_ + ? std::chrono::duration(now.time_since_epoch()).count() + - accepted_stamp_ * 1e-9 + : std::chrono::duration(now - *wait_started_).count(); + if (age > timeout_ || age < 0.) { + fail("policy result timeout"); + return; + } + if (*state_ == 3 && recovery_started_ && -gravity_->z() < std::cos(std::numbers::pi / 4.)) { + *recovery_fault_ = 5; + fail("self-righting lost upright after RL takeover"); + return; + } + const bool pd_due = !last_pd_ || *tick_ - *last_pd_ >= 5; + if (!pd_due) + return; + if (*state_ == 4) { + if (!acceleration_->allFinite()) { + fail("invalid self-righting acceleration"); + return; + } + auto feedback = support_observer_.observe( + Eigen::Map{q_->data()}, + Eigen::Map{dq_->data()}, *gravity_, + *gyro_, *acceleration_, + recovery_started_ ? recovery_.command().phase + : wheel_leg::JointReferenceRecoveryPhase::kSelect); + if (!feedback.q.allFinite()) { + fail("invalid self-righting feedback"); + return; + } + if (!recovery_started_) { + recovery_.reset(feedback.q); + recovery_started_ = true; + } + *recovery_capture_blockers_ = wheel_leg::recovery_capture_blockers( + feedback, *acceleration_, recovery_config_.nominal); + *recovery_capture_ready_ = *recovery_capture_blockers_ == 0; + feedback.rl_capture_ready = *recovery_capture_ready_ && have_target_; + const auto& recovery_command = recovery_.update(feedback); + *recovery_phase_ = static_cast(recovery_command.phase); + *recovery_fault_ = recovery_command.failure_code; + if (recovery_command.failed) { + fail("self-righting script failed"); + return; + } + feedback.wheel_probe_torque = support_observer_.probe_command( + recovery_command.scripted && recovery_command.release_finished); + if (!recovery_command.pure_rl) { + const auto native_torque = + recovery_.torques(feedback, wheel_leg::JointReferenceRecoveryVector6::Zero()); + JointVector torque; + std::copy_n(native_torque.data(), 6, torque.begin()); + if (!write_effort(torque)) + return; + last_pd_ = *tick_; + return; + } + // The actor owns all six axes in this very PD tick. Its next command + // update also accepts driver motion, without a static standing dwell. + *state_ = 3; + *recovery_yaw_active_ = false; + last_command_tick_.reset(); + update_command(); + if (*fault_) + return; + RCLCPP_INFO(get_logger(), "Self-righting handover: policy owns all six axes"); + } + if (have_target_) { + if (policy_.jump_control) + target_ = decoded_action_target(decoded_, *q_, target_bounds()); + auto torque = JointSolver::pd( + target_, *q_, *dq_, policy_.leg_kp, policy_.leg_kd, policy_.gas_gain); + *pd_target_ = target_; + *pd_requested_ = torque; + clear_tuck_effort(); + if (policy_.jump_control) { + const auto extra = wheel_leg::jump_tuck_effort( + {(*q_)[0], (*q_)[1], (*q_)[2], (*q_)[3]}, + {target_[0], target_[1], target_[2], target_[3]}, + {torque[0], torque[1], torque[2], torque[3]}, + jump_.state(jump_now()) == wheel_leg::JumpCommand::FLIGHT_REFERENCE); + for (int i = 0; i < 4; ++i) { + (*tuck_compensation_)[i] = extra.torque[i]; + torque[i] += extra.torque[i]; + } + *tuck_gate_left_ = extra.gate[0]; + *tuck_gate_right_ = extra.gate[1]; + } + if (!write_effort(torque)) + return; + *applied_ = pending_history_; + *action_applied_ = true; + last_pd_ = *tick_; + } + } + +private: + using Clock = std::chrono::steady_clock; + wheel_leg::JointReferenceRecoveryConfig recovery_configuration() { + auto config = wheel_leg::make_recovery_config(); + config.prepare_speed_rad_s = get_parameter_or("self_righting.fold_plant_speed_rad_s", 2.); + config.insert_speed_rad_s = get_parameter_or("self_righting.insert_speed_rad_s", 2.); + config.orbit_speed_rad_s = get_parameter_or("self_righting.orbit_speed_rad_s", 2.); + config.push_speed_rad_s = get_parameter_or("self_righting.push_speed_rad_s", 2.); + config.capture_speed_rad_s = get_parameter_or("self_righting.capture_speed_rad_s", 2.); + config.side_speed_rad_s = get_parameter_or("self_righting.side_speed_rad_s", 2.); + if (config.prepare_speed_rad_s > 2. || config.insert_speed_rad_s > 2. + || config.orbit_speed_rad_s > 2. || config.push_speed_rad_s > 2. + || config.capture_speed_rad_s > 2. || config.side_speed_rad_s > 2.) + throw std::invalid_argument("Recovery reference speed exceeds 2 rad/s"); + return config; + } + void reset_runtime() { + jump_.reset(); + jump_velocity_ = 0.; + *jump_accepted_ = 0; + last_jump_request_ = jump_request_.ready() ? *jump_request_ : 0; + publish_jump(); + clear_tuck_effort(); + *pd_target_ = *pd_requested_ = JointVector{}; + for (auto& output : effort_) + *output = 0.; + *enable_ = false; + *recovery_yaw_active_ = false; + *policy_enable_ = false; + *applied_ = JointVector{}; + *action_applied_ = false; + *policy_command_ = Eigen::Vector3d::Zero(); + *policy_height_ = .305; + *state_ = 1; + *recovery_phase_ = 0; + *recovery_fault_ = 0; + *recovery_capture_ready_ = false; + *recovery_capture_blockers_ = 0; + have_target_ = false; + accepted_sequence_ = *action_sequence_; + accepted_stamp_ = 0; + last_pd_.reset(); + last_command_tick_.reset(); + wait_started_.reset(); + drive_loss_started_.reset(); + action_starved_logged_ = false; + heading_.reset(); + recovery_.reset(wheel_leg::JointReferenceRecoveryVector6::Zero(), false); + support_observer_.reset(); + recovery_started_ = false; + target_ = decoded_ = pending_history_ = JointVector{}; + } + void fail(const char* reason) { + RCLCPP_ERROR(get_logger(), "Wheel-leg RL stopped: %s", reason); + const int phase = *recovery_phase_, fault = *recovery_fault_; + const bool capture_ready = *recovery_capture_ready_; + const int capture_blockers = *recovery_capture_blockers_; + reset_runtime(); + *fault_ = true; + *recovery_phase_ = phase; + *recovery_fault_ = fault; + *recovery_capture_ready_ = capture_ready; + *recovery_capture_blockers_ = capture_blockers; + } + bool write_effort(const JointVector& effort) { + JointVector caps; + for (int i = 0; i < 6; ++i) + caps[i] = *caps_[i]; + if (!std::ranges::all_of(effort, [](double v) { return std::isfinite(v); }) + || !std::ranges::all_of(caps, [](double v) { return std::isfinite(v) && v > 0.; })) { + fail("invalid actuator effort or limits"); + return false; + } + const auto mechanical = + policy_.jump_control + ? LegBounds{{{wheelbipe_jump_v4::kMechanicalMin, wheelbipe_jump_v4::kMechanicalMax}, {wheelbipe_jump_v4::kMechanicalMin, wheelbipe_jump_v4::kMechanicalMax}}} + : kMechanicalBounds; + const auto api = JointSolver::constrain(effort, *q_, caps, .03, mechanical); + for (int i = 0; i < 6; ++i) + *effort_[i] = api[i]; + return true; + } + void update_command() { + const std::size_t divisor = static_cast(1000. / *policy_rate_); + if (last_command_tick_ && *tick_ - *last_command_tick_ < divisor) + return; + last_command_tick_ = *tick_; + if (*state_ == 4) { + *policy_command_ = Eigen::Vector3d::Zero(); + *policy_height_ = .305; + return; + } + if (!command_->vector.allFinite() || !std::isfinite(*height_)) { + fail("invalid chassis command"); + return; + } + const bool spin = rmcs_msgs::is_spining(*mode_); + double vx = + spin ? 0. + : std::clamp(command_->vector.x(), -policy_.forward_limit, policy_.forward_limit); + double yaw = command_->vector.z(); + if (heading_hold_ && !spin && vx != 0. && yaw == 0.) { + if (!heading_) + heading_ = attitude_->z(); + yaw = std::clamp( + 5. * std::remainder(*heading_ - attitude_->z(), 2. * std::numbers::pi), + -policy_.yaw_limit, policy_.yaw_limit); + } else + heading_.reset(); + *policy_command_ = + Eigen::Vector3d{vx, 0., std::clamp(yaw, -policy_.yaw_limit, policy_.yaw_limit)}; + if (jump_.active()) { + *policy_command_ = Eigen::Vector3d{jump_velocity_, 0., 0.}; + *policy_height_ = jump_.height(jump_now()); + return; + } + const double step = height_rate_ / *policy_rate_; + *policy_height_ += + std::clamp(std::clamp(*height_, .23, .42) - *policy_height_, -step, step); + } + double jump_now() const { return static_cast(*tick_) / *rate_; } + LegBounds target_bounds() const { + return policy_.jump_control ? wheel_leg::jump_target_bounds(jump_.active()) + : policy_.winding_bounds; + } + void clear_tuck_effort() { + *tuck_compensation_ = JointVector{}; + *tuck_gate_left_ = *tuck_gate_right_ = 0.; + } + void publish_jump() { + const double now = jump_now(); + const auto context = jump_.context(now); + policy_context_->assign(context.begin(), context.end()); + *jump_active_ = jump_.active(); + *jump_state_ = jump_.state(now); + *jump_elapsed_ = jump_.elapsed(now); + *jump_apex_ = jump_.apex(); + *jump_height_ = jump_.height(now); + } + void update_jump(bool requested) { + const double now = jump_now(); + const bool busy = jump_.active(); + jump_.update(now); + if (*state_ != 3) + jump_.reset(); + if (requested && jump_enabled_ && !busy && *state_ == 3 && have_target_ + && *action_valid_ > 0. && *healthy_ > 0. && *valid_ && *drives_ready_ + && command_->vector.allFinite() + && std::abs(command_->vector.x()) <= wheelbipe_jump_v4::kMovingVxMax + && std::abs(command_->vector.y()) < 1e-6 && std::abs(command_->vector.z()) < 1e-6 + && !rmcs_msgs::is_spining(*mode_) && gravity_->allFinite() + && gravity_->z() < -std::cos(std::numbers::pi / 15.) && gyro_->allFinite() + && gyro_->norm() < .8 && jump_.request(now)) { + jump_velocity_ = command_->vector.x(); + ++*jump_accepted_; + last_command_tick_.reset(); + } + if (busy && !jump_.active()) + last_command_tick_.reset(); + publish_jump(); + } + const PolicyParameters& policy_; + std::string history_source_; + InputInterface q_, dq_; + InputInterface gravity_, gyro_, attitude_, acceleration_; + InputInterface command_; + InputInterface mode_; + InputInterface height_, rate_, action_valid_, healthy_, policy_rate_; + InputInterface recovery_yaw_; + InputInterface valid_, drives_ready_, requested_, simulation_; + InputInterface reset_, tick_, action_sequence_; + InputInterface jump_request_; + InputInterface action_stamp_; + InputInterface model_spec_, observation_sha_; + std::array, 6> caps_, action_, raw_action_, action_scale_, + action_offset_; + std::array, 6> action_min_, action_max_; + std::array, 6> effort_; + OutputInterface enable_, policy_enable_, fault_, action_applied_; + OutputInterface applied_; + OutputInterface policy_command_; + OutputInterface policy_height_; + OutputInterface> policy_context_; + OutputInterface jump_active_; + OutputInterface jump_state_, jump_accepted_; + OutputInterface jump_elapsed_, jump_apex_, jump_height_, tuck_gate_left_, + tuck_gate_right_; + OutputInterface pd_target_, pd_requested_, tuck_compensation_; + wheel_leg::JumpCommand jump_; + std::size_t last_jump_request_ = 0; + double jump_velocity_ = 0.; + bool jump_enabled_ = false; + OutputInterface state_, recovery_phase_, recovery_fault_, recovery_capture_blockers_; + OutputInterface recovery_capture_ready_; + OutputInterface recovery_yaw_active_; + const wheel_leg::JointReferenceRecoveryConfig recovery_config_; + wheel_leg::JointReferenceRecoveryController recovery_; + wheel_leg::ClosedChainSupportObserver support_observer_; + bool recovery_started_ = false; + JointVector target_{}, decoded_{}, pending_history_{}; + std::optional last_pd_, last_command_tick_; + std::optional wait_started_, drive_loss_started_; + std::optional heading_; + std::size_t last_reset_ = 0, accepted_sequence_ = 0; + std::uint64_t accepted_stamp_ = 0; + std::chrono::duration drive_loss_debounce_{.1}; + bool action_starved_logged_ = false; + double timeout_, height_rate_; + bool heading_hold_, recovery_enabled_, simulation_guarded_ = false, have_target_ = false, + schedule_checked_ = false; +}; +} // namespace rmcs_core::controller::chassis +#include +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::WheelLegRlController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_state_estimator.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_state_estimator.cpp new file mode 100644 index 000000000..5c1c005fd --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_state_estimator.cpp @@ -0,0 +1,197 @@ +#include "wheel_leg/encoder_unwrap.hpp" +#include "wheel_leg/joint_coordinates.hpp" +#include "wheel_leg/leg_kinematics.hpp" +#include "wheel_leg/model/mechanics.hpp" +#include "wheel_leg/policy/selection.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace rmcs_core::controller::chassis { +class WheelLegStateEstimator final + : public rmcs_executor::Component + , public rclcpp::Node + , public rmcs_utility::NodeMixin { +public: + WheelLegStateEstimator() + : Node{get_component_name(), node::options()} + , model_( + wheel_leg::mechanics::kThighLength, wheel_leg::mechanics::kShankLength, + wheel_leg::LegLinkage::knee_limit_min(), wheel_leg::LegLinkage::knee_limit_max(), + linkage_) { + for (std::size_t i = 0; i < 6; ++i) { + const auto prefix = std::string{"/wheel_leg/"} + kMotorNames[i]; + register_input(prefix + "/angle", angle_[i]); + register_input(prefix + "/velocity", velocity_[i]); + if (i < 4) + register_input(prefix + "/angle_wrap_span", angle_wrap_span_[i]); + } + register_input("/wheel_leg/imu/quaternion", orientation_); + register_input("/wheel_leg/imu/angular_velocity", gyro_); + register_input("/wheel_leg/feedback_available", available_); + register_input("/chassis/reset_count", reset_); + register_output("/wheel_leg/state/position", q_, JointVector{}); + register_output("/wheel_leg/state/velocity", dq_, JointVector{}); + register_output("/wheel_leg/state/valid", valid_, false); + register_output("/wheel_leg/state/gravity", gravity_, Eigen::Vector3d::Zero()); + register_output("/wheel_leg/state/attitude", attitude_, Eigen::Vector3d::Zero()); + register_output("/wheel_leg/state/passive_knee", knee_, std::array{}); + register_output("/wheel_leg/state/passive_knee_rate", knee_rate_, std::array{}); + register_output("/wheel_leg/state/shank", shank_, std::array{}); + register_output("/wheel_leg/state/shank_rate", shank_rate_, std::array{}); + register_output("/wheel_leg/state/leg_length", length_, std::array{}); + register_output("/wheel_leg/state/forward_velocity", forward_velocity_, 0.); + register_output("/wheel_leg/state/distance", distance_output_, 0.); + register_output("/wheel_leg/state/com_offset", com_offset_, 0.); + register_output("/wheel_leg/state/base_height", height_, 0.); + register_output("/wheel_leg/state/model_offsets", model_offsets_, kMotorOffsets); + register_output( + "/wheel_leg/state/calibration_sha256", calibration_sha256_, + std::string{full_vehicle_calibration110::kCalibrationSha256}); + register_output( + "/wheel_leg/state/calibration_model_spec_sha256", calibration_model_spec_sha256_, + std::string{full_vehicle_calibration110::kModelSpecSha256}); + const auto policy = get_parameter_or("policy", "traditional"); + bounds_ = + policy == "traditional" ? kMechanicalBounds : policy_parameters(policy).winding_bounds; + dt_ = get_parameter_or("dt", .001); + velocity_sign_ = get_parameter_or("velocity_sign", 1.); + if (!std::isfinite(dt_) || dt_ <= 0. || (velocity_sign_ != 1. && velocity_sign_ != -1.)) + throw std::invalid_argument("Invalid state estimator timing or velocity sign"); + } + void before_updating() override { + for (const auto& span : angle_wrap_span_) + if (!std::isfinite(*span) || *span <= std::numbers::pi) + throw std::invalid_argument("Joint angle_wrap_span must be finite and exceed pi"); + } + void update() override { + if (*reset_ != last_reset_) { + initialized_ = false; + unwrap_ = {}; + distance_ = 0.; + last_reset_ = *reset_; + } + *valid_ = false; + JointVector api{}, speed{}; + for (int i = 0; i < 6; ++i) { + api[i] = *angle_[i]; + speed[i] = *velocity_[i]; + if (!std::isfinite(api[i]) || !std::isfinite(speed[i])) { + initialized_ = false; + return; + } + } + if (!*available_ || !gyro_->allFinite() || !orientation_->coeffs().allFinite() + || orientation_->norm() == 0.) { + initialized_ = false; + return; + } + const auto orientation = orientation_->normalized(); + // The drive reports position modulo 2 * P_MAX; use the configured span + // before the closed-chain relative-angle branch is selected below. + for (int i = 0; i < 4; ++i) { + unwrap_[i].update(api[i], *angle_wrap_span_[i]); + api[i] = unwrap_[i].value(); + } + auto q = model_position(api); + LegVector measured{}; + std::copy_n(q.begin(), 4, measured.begin()); + const auto continuous = continuous_legs(measured, bounds_); + std::copy(continuous.begin(), continuous.end(), q.begin()); + *q_ = q; + *dq_ = model_velocity(speed); + *gravity_ = orientation.conjugate() * -Eigen::Vector3d::UnitZ(); + const double yaw = std::atan2( + 2. * (orientation.w() * orientation.z() + orientation.x() * orientation.y()), + 1. - 2. * (orientation.y() * orientation.y() + orientation.z() * orientation.z())); + unwrapped_yaw_ = initialized_ + ? unwrapped_yaw_ + std::remainder(yaw - last_yaw_, 2. * std::numbers::pi) + : yaw; + last_yaw_ = yaw; + *attitude_ = Eigen::Vector3d{ + std::atan2(gravity_->x(), -gravity_->z()), std::atan2(gravity_->y(), -gravity_->z()), + unwrapped_yaw_}; + std::array shape; + for (int side = 0; side < 2; ++side) { + const auto calibration = wheel_leg::CanonicalCalibration::side(side); + shape[side] = model_.forward(api[2 * side], api[2 * side + 1], calibration); + const auto& s = shape[side]; + const double crank_velocity = + calibration.crank_sign * (speed[2 * side + 1] - speed[2 * side]); + (*knee_rate_)[side] = + s.beta == s.beta_raw + ? linkage_.knee_velocity_from_encoder_crank(s.crank_rel, crank_velocity) + : 0.; + (*shank_rate_)[side] = -(*knee_rate_)[side] - speed[2 * side] / calibration.hip_sign; + (*knee_)[side] = s.beta; + (*shank_)[side] = s.shank; + (*length_)[side] = s.leg_length; + } + const double beta = .5 * (shape[0].beta + shape[1].beta), + phi = .5 * (shape[0].shank + shape[1].shank); + double kx, kz, ax, az; + model_.fk(beta, phi, kx, kz, ax, az); + const double pitch = attitude_->x(); + *height_ = std::max( + wheel_leg::mechanics::kWheelRadius + shape[0].axle_x * gravity_->x() + + shape[0].axle_z * gravity_->z(), + wheel_leg::mechanics::kWheelRadius + shape[1].axle_x * gravity_->x() + + shape[1].axle_z * gravity_->z()); + const auto rot = [](double a, double x, double z) { + return std::array{std::cos(a) * x - std::sin(a) * z, std::sin(a) * x + std::cos(a) * z}; + }; + const auto thigh = + rot(wheel_leg::mechanics::kThighComAngle, kx / wheel_leg::mechanics::kThighLength, + kz / wheel_leg::mechanics::kThighLength); + const auto shank = rot( + wheel_leg::mechanics::kShankComAngle, (ax - kx) / wheel_leg::mechanics::kShankLength, + (az - kz) / wheel_leg::mechanics::kShankLength); + const double body_x = -ax + std::cos(pitch) * wheel_leg::mechanics::kBodyComForward + + std::sin(pitch) * wheel_leg::mechanics::kBodyComUp; + const double thigh_x = -ax + wheel_leg::mechanics::kThighComOffset * thigh[0], + shank_x = kx - ax + wheel_leg::mechanics::kShankComOffset * shank[0]; + *com_offset_ = + (wheel_leg::mechanics::kBodyMass * body_x + wheel_leg::mechanics::kThighMass * thigh_x + + wheel_leg::mechanics::kShankMass * shank_x) + / (wheel_leg::mechanics::kBodyMass + wheel_leg::mechanics::kThighMass + + wheel_leg::mechanics::kShankMass + wheel_leg::mechanics::kWheelMass); + *forward_velocity_ = + velocity_sign_ * wheel_leg::mechanics::kWheelRadius * forward_wheel_velocity(speed); + distance_ += *forward_velocity_ * dt_; + *distance_output_ = distance_; + initialized_ = true; + *valid_ = true; + } + +private: + std::array, 6> angle_, velocity_; + std::array, 4> angle_wrap_span_; + std::array unwrap_; + InputInterface orientation_; + InputInterface gyro_; + InputInterface available_; + InputInterface reset_; + OutputInterface q_, dq_; + OutputInterface valid_; + OutputInterface gravity_, attitude_; + OutputInterface> knee_, knee_rate_, shank_, shank_rate_, length_; + OutputInterface forward_velocity_, distance_output_, com_offset_, height_; + OutputInterface model_offsets_; + OutputInterface calibration_sha256_, calibration_model_spec_sha256_; + wheel_leg::LegLinkage linkage_; + wheel_leg::WheelLegLegModel model_; + double dt_ = 0.001, velocity_sign_ = 1., distance_ = 0., last_yaw_ = 0., unwrapped_yaw_ = 0.; + std::size_t last_reset_ = 0; + LegBounds bounds_ = kMechanicalBounds; + bool initialized_ = false; +}; +} // namespace rmcs_core::controller::chassis +#include +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::WheelLegStateEstimator, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/pitch_gravity_feedforward.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/pitch_gravity_feedforward.cpp new file mode 100644 index 000000000..eb98ed3cb --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/pitch_gravity_feedforward.cpp @@ -0,0 +1,44 @@ +#include +#include + +#include +#include +#include + +namespace rmcs_core::controller::gimbal { + +using namespace rmcs_description; + +class PitchGravityFeedforward + : public rmcs_executor::Component + , public rclcpp::Node { +public: + PitchGravityFeedforward() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + get_parameter_or("pitch_gravity_ff_gain", gain_, 0.); + get_parameter_or("pitch_gravity_ff_phase", phase_, 0.); + register_input("/tf", tf_); + register_output("/gimbal/pitch/torque_ff", torque_, 0.); + } + + void update() override { + const auto direction = + fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + const double pitch = std::asin(std::clamp(direction->z(), -1., 1.)); + *torque_ = gain_ * std::sin(pitch - phase_); + } + +private: + InputInterface tf_; + OutputInterface torque_; + double gain_ = 0., phase_ = 0.; +}; + +} // namespace rmcs_core::controller::gimbal + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::gimbal::PitchGravityFeedforward, rmcs_executor::Component) 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 dfd361313..7340aeec2 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 @@ -1,4 +1,6 @@ +#include #include +#include #include #include @@ -6,6 +8,7 @@ #include #include +#include "controller/chassis/wheel_leg/recovery_yaw.hpp" #include "controller/gimbal/two_axis_gimbal_solver.hpp" namespace rmcs_core::controller::gimbal { @@ -34,13 +37,22 @@ class SimpleGimbalController register_input("/auto_aim/should_control", auto_aim_should_control_, false); register_input("/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/wheel_leg/recovery/yaw_align_active", recovery_yaw_active_, false); + register_input("/gimbal/yaw/angle", recovery_yaw_, false); register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); - two_axis_gimbal_solver.enable_yaw_limit( - *this, get_parameter("yaw_upper_limit").as_double(), - get_parameter("yaw_lower_limit").as_double()); + const bool has_yaw_upper_limit = has_parameter("yaw_upper_limit"); + const bool has_yaw_lower_limit = has_parameter("yaw_lower_limit"); + if (has_yaw_upper_limit != has_yaw_lower_limit) + throw std::runtime_error( + "yaw_upper_limit and yaw_lower_limit must be configured together"); + + if (has_yaw_upper_limit) + two_axis_gimbal_solver.enable_yaw_limit( + *this, get_parameter("yaw_upper_limit").as_double(), + get_parameter("yaw_lower_limit").as_double()); } void update() override { @@ -55,8 +67,20 @@ class SimpleGimbalController using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + recovery_yaw_alignment_.reset(); return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled()); + } + + if (recovery_yaw_active_.ready() && *recovery_yaw_active_) { + two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled()); + const auto elapsed = std::chrono::steady_clock::now().time_since_epoch(); + const double now = std::chrono::duration(elapsed).count(); + const double error = + recovery_yaw_.ready() ? recovery_yaw_alignment_.update(*recovery_yaw_, now) : nan_; + return {error, nan_}; + } + recovery_yaw_alignment_.reset(); const auto auto_aim_requested = mouse_->right || switch_right == Switch::UP; const auto should_control = auto_aim_should_control_.ready() && *auto_aim_should_control_; @@ -95,8 +119,11 @@ class SimpleGimbalController InputInterface auto_aim_should_control_; InputInterface auto_aim_control_direction_; + InputInterface recovery_yaw_active_; + InputInterface recovery_yaw_; TwoAxisGimbalSolver two_axis_gimbal_solver; + chassis::wheel_leg::RecoveryYawAlignment recovery_yaw_alignment_; OutputInterface yaw_angle_error_, pitch_angle_error_; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 5c048d0ef..1938a223a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -40,6 +40,8 @@ class TwoAxisGimbalSolver { class SetDisabled : public Operation { PitchLink::DirectionVector update(TwoAxisGimbalSolver& super) const override { super.control_enabled_ = false; + super.control_direction_ = OdomImu::DirectionVector{Eigen::Vector3d::UnitX()}; + super.yaw_axis_filtered_ = OdomImu::DirectionVector{Eigen::Vector3d::UnitZ()}; return {}; } }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp index 2951f53f8..b6bf346ef 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp @@ -31,6 +31,7 @@ class PidCalculator { double update(double err) { if (!std::isfinite(err)) { + reset(); return nan; } else { double control = kp * err; diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp index 9780edd37..a6896f99f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp @@ -1,3 +1,5 @@ +#include + #include #include #include @@ -25,6 +27,14 @@ class PidController register_output(get_parameter("control").as_string(), control_); + if (has_parameter("reset_interface")) { + const auto reset_interface = get_parameter("reset_interface").as_string(); + if (!reset_interface.empty()) { + register_input(reset_interface, reset_count_); + reset_enabled_ = true; + } + } + get_parameter("integral_min", pid_calculator_.integral_min); get_parameter("integral_max", pid_calculator_.integral_max); @@ -36,6 +46,13 @@ class PidController } void update() override { + if (reset_enabled_ && reset_count_.ready() && *reset_count_ != last_reset_count_) { + last_reset_count_ = *reset_count_; + pid_calculator_.reset(); + *control_ = 0.0; + return; + } + auto err = *setpoint_ - *measurement_; *control_ = *feedforward_ + pid_calculator_.update(err); } @@ -46,6 +63,9 @@ class PidController PidCalculator pid_calculator_; OutputInterface control_; + InputInterface reset_count_; + std::size_t last_reset_count_ = 0; + bool reset_enabled_ = false; }; } // namespace rmcs_core::controller::pid diff --git a/rmcs_ws/src/rmcs_core/src/debug/value_collector.cpp b/rmcs_ws/src/rmcs_core/src/debug/value_collector.cpp index d5099e8fc..228ae442b 100644 --- a/rmcs_ws/src/rmcs_core/src/debug/value_collector.cpp +++ b/rmcs_ws/src/rmcs_core/src/debug/value_collector.cpp @@ -1,13 +1,23 @@ #include #include +#include #include +#include #include +#include +#include +#include +#include #include +#include #include #include #include #include +#include +#include +#include #include namespace rmcs_core::debug { @@ -44,6 +54,12 @@ class ValueCollector if (csv_path_.empty() || signal_names_.empty()) return; + if (write_interval_ < 1 || flush_interval_ < 1) { + node::error( + "write_interval ({}) and flush_interval ({}) must be positive", write_interval_, + flush_interval_); + return; + } for (const auto& name : signal_names_) { const auto it = output_map.find(name); @@ -51,33 +67,58 @@ class ValueCollector node::error("signal '{}' not found", name); continue; } - if (it->second.type.get() != typeid(double)) { - node::error("signal '{}' type is not double", name); + if (it->second.kind != rmcs_executor::InterfaceKind::Normal) { + node::error("signal '{}' is an event interface", name); continue; } - - auto unit = std::make_unique(); - unit->name = name; - register_input(name, unit->value); - units_.push_back(std::move(unit)); + if (!register_signal(name, it->second.type.get())) + node::error( + "signal '{}' has an unsupported type ({})", name, it->second.type.get().name()); } + if (units_.empty()) + return; + node::info("resolved {} of {} signals", units_.size(), signal_names_.size()); + } + + auto before_updating() -> void override { if (units_.empty()) return; + for (auto& unit : units_) { + if (!unit.dynamic_width) + continue; + const auto width = unit.width_probe(); + *unit.dynamic_width = width; + unit.columns = index_columns(unit.dynamic_prefix, width); + if (width == 0) + node::warn( + "signal '{}' is an empty vector and records no column", unit.dynamic_prefix); + } + + auto columns = std::size_t{0}; + for (const auto& unit : units_) + columns += unit.columns.size(); + if (columns == 0) + return; + csv_file_.open(csv_path_, std::ios::out | std::ios::trunc); if (!csv_file_.is_open()) { node::error("failed to open {}", csv_path_); return; } + csv_file_ << std::setprecision(10); csv_file_ << "index"; for (const auto& unit : units_) - csv_file_ << "," << unit->name; + for (const auto& column : unit.columns) + csv_file_ << "," << column; csv_file_ << "\n"; csv_file_.flush(); - node::info("collecting {} signals to {}", units_.size(), csv_path_); + node::info( + "collecting {} signals ({} columns) to {} every {} ticks", units_.size(), columns, + csv_path_, write_interval_); } auto update() -> void override { @@ -89,7 +130,7 @@ class ValueCollector csv_file_ << sample_count_++; for (const auto& unit : units_) - csv_file_ << "," << *unit->value; + unit.append(csv_file_); csv_file_ << "\n"; if (sample_count_ % flush_interval_ == 0) @@ -98,16 +139,152 @@ class ValueCollector private: struct SignalUnit { - std::string name; - InputInterface value; + std::vector columns; + std::function append; + // Only set for std::vector: resolved from the paired input. + std::string dynamic_prefix; + std::shared_ptr dynamic_width; + std::function width_probe; }; + static auto index_columns(const std::string& name, std::size_t width) + -> std::vector { + auto columns = std::vector{}; + columns.reserve(width); + for (auto i = std::size_t{0}; i < width; ++i) + columns.push_back(name + "[" + std::to_string(i) + "]"); + return columns; + } + + template + auto register_typed( + const std::string& name, const std::type_info& type, std::vector columns, + Writer writer) -> bool { + if (type != typeid(T)) + return false; + + auto holder = std::make_shared>(); + register_input(name, *holder); + const auto* const raw = holder.get(); + keepalive_.push_back(holder); + + auto unit = SignalUnit{}; + unit.columns = std::move(columns); + unit.append = [raw, writer](std::ostream& os) { writer(os, **raw); }; + units_.push_back(std::move(unit)); + return true; + } + + template + auto register_array(const std::string& name, const std::type_info& type) -> bool { + return register_typed>( + name, type, index_columns(name, N), + [](std::ostream& os, const std::array& value) { + for (const auto component : value) + os << "," << component; + }); + } + + auto register_vector(const std::string& name, const std::type_info& type) -> bool { + if (type != typeid(std::vector)) + return false; + + auto holder = std::make_shared>>(); + register_input(name, *holder); + const auto* const raw = holder.get(); + keepalive_.push_back(holder); + + auto width = std::make_shared(0); + auto unit = SignalUnit{}; + unit.dynamic_prefix = name; + unit.dynamic_width = width; + unit.width_probe = [raw] { return (**raw).size(); }; + // Pad short samples so a shrinking vector cannot corrupt the row. + unit.append = [raw, width](std::ostream& os) { + const auto& value = **raw; + for (auto i = std::size_t{0}; i < *width; ++i) + os << "," << (i < value.size() ? value[i] : 0.); + }; + units_.push_back(std::move(unit)); + return true; + } + + auto register_signal(const std::string& name, const std::type_info& type) -> bool { + if (register_typed(name, type, {name}, [](std::ostream& os, double value) { + os << "," << value; + })) + return true; + if (register_typed(name, type, {name}, [](std::ostream& os, int value) { + os << "," << value; + })) + return true; + if (register_typed(name, type, {name}, [](std::ostream& os, std::uint8_t v) { + os << "," << static_cast(v); + })) + return true; + if (register_typed(name, type, {name}, [](std::ostream& os, std::uint32_t v) { + os << "," << v; + })) + return true; + if (register_typed(name, type, {name}, [](std::ostream& os, std::uint64_t v) { + os << "," << v; + })) + return true; + if constexpr (!std::is_same_v) { + if (register_typed(name, type, {name}, [](std::ostream& os, std::size_t v) { + os << "," << v; + })) + return true; + } + if (register_typed(name, type, {name}, [](std::ostream& os, bool value) { + os << "," << (value ? 1 : 0); + })) + return true; + if (register_typed( + name, type, {name}, + [](std::ostream& os, std::chrono::steady_clock::time_point value) { + os << "," << std::chrono::duration(value.time_since_epoch()).count(); + })) + return true; + if (register_typed( + name, type, {name + "/x", name + "/y", name + "/z"}, + [](std::ostream& os, const Eigen::Vector3d& value) { + os << "," << value.x() << "," << value.y() << "," << value.z(); + })) + return true; + if (register_typed( + name, type, {name + "/x", name + "/y", name + "/z"}, + [](std::ostream& os, const rmcs_description::BaseLink::DirectionVector& value) { + os << "," << value.vector.x() << "," << value.vector.y() << "," + << value.vector.z(); + })) + return true; + if (register_typed( + name, type, {name + "/w", name + "/x", name + "/y", name + "/z"}, + [](std::ostream& os, const Eigen::Quaterniond& value) { + os << "," << value.w() << "," << value.x() << "," << value.y() << "," + << value.z(); + })) + return true; + if (register_array<2>(name, type)) + return true; + if (register_array<3>(name, type)) + return true; + if (register_array<4>(name, type)) + return true; + if (register_array<6>(name, type)) + return true; + return register_vector(name, type); + } + std::string csv_path_; std::vector signal_names_; int write_interval_ = 1; int flush_interval_ = 100; - std::vector> units_; + std::vector units_; + // Keeps every registered input alive; SignalUnit lambdas point into these. + std::vector> keepalive_; std::ofstream csv_file_; int tick_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp index 83c1b2e32..c29a2ca35 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp @@ -7,11 +7,14 @@ #include #include #include +#include +#include #include #include #include "filter/imu_ekf.hpp" +#include "hardware/device/imu_lever_arm.hpp" namespace rmcs_core::hardware::device { @@ -19,22 +22,48 @@ class Bmi088Ekf { public: struct Config { Eigen::Matrix3d body_to_sensor = Eigen::Matrix3d::Identity(); + // Body origin -> accelerometer, in body axes, metres. + Eigen::Vector3d sensor_position_body_m = Eigen::Vector3d::Zero(); + double angular_acceleration_filter_tau_s = 0.01; }; using TimePoint = rmcs_msgs::BoardClock::time_point; using Snapshot = rmcs_msgs::ImuSnapshot; + struct AccelerationSnapshot { + Eigen::Vector3d specific_force_body_mps2; + TimePoint timestamp; + }; + explicit Bmi088Ekf(Config config) - : config_(std::move(config)) {} + : config_(std::move(config)) + , lever_arm_(config_.sensor_position_body_m, config_.angular_acceleration_filter_tau_s) { + if (!config_.body_to_sensor.allFinite() + || std::abs(config_.body_to_sensor.determinant() - 1.0) > 1e-3 + || (config_.body_to_sensor.transpose() * config_.body_to_sensor + - Eigen::Matrix3d::Identity()) + .norm() + > 1e-3) + throw std::invalid_argument("IMU installation must be a proper rotation matrix"); + } Bmi088Ekf() : Bmi088Ekf(Config{}) {} - void push_accelerometer_sample( + bool push_accelerometer_sample( std::int16_t x, std::int16_t y, std::int16_t z, TimePoint sample_time) { const Eigen::Vector3d accel_g = config_.body_to_sensor.transpose() * convert_accelerometer(x, y, z); + if (lever_arm_.enabled()) { + if ((initialized_ && sample_time <= ekf_state_time_) + || (pending_accel_sample_ && sample_time <= pending_accel_sample_->sample_time)) + return false; + // Wait for bracketing gyro samples before publishing or correcting the EKF. + pending_accel_sample_ = {accel_g, sample_time}; + return true; + } + if (!initialized_) { if (ekf_.reset_from_accel(accel_g)) { ekf_state_time_ = sample_time; @@ -44,32 +73,73 @@ class Bmi088Ekf { ekf_state_time_, }; const auto guard = std::scoped_lock{mutex_}; + latest_acceleration_ = AccelerationSnapshot{accel_g * 9.80665, sample_time}; initialized_ = true; + return true; } - return; + return false; } - if (sample_time < ekf_state_time_) - return; + if (sample_time <= ekf_state_time_) + return false; - if (pending_accel_sample_ && sample_time < pending_accel_sample_->sample_time) - return; + if (pending_accel_sample_ && sample_time <= pending_accel_sample_->sample_time) + return false; + { + const auto guard = std::scoped_lock{mutex_}; + latest_acceleration_ = AccelerationSnapshot{accel_g * 9.80665, sample_time}; + } pending_accel_sample_ = {accel_g, sample_time}; + return true; } std::optional try_update_with_gyroscope_sample( std::int16_t x, std::int16_t y, std::int16_t z, TimePoint sample_time) { - const Eigen::Vector3d gyro_rad_per_sec = - config_.body_to_sensor.transpose() * convert_gyroscope(x, y, z); + const Eigen::Vector3d sensor_gyro = convert_gyroscope(x, y, z); + const Eigen::Vector3d gyro_rad_per_sec = config_.body_to_sensor.transpose() * sensor_gyro; + // Clipping occurs on the physical sensor axes, before the mounting rotation. + const bool gyro_saturated = is_gyro_saturated(sensor_gyro); + + if (lever_arm_.enabled()) { + if (gyro_saturated) + lever_arm_.reset(); + else if (!lever_arm_.observe(gyro_rad_per_sec, sample_time)) + return std::nullopt; + } - if (!initialized_) - return std::nullopt; + if (!initialized_) { + if (!lever_arm_.enabled() || !pending_accel_sample_) + return std::nullopt; + const auto& sample = *pending_accel_sample_; + const auto corrected = lever_arm_.correct(sample.accel_g * 9.80665, sample.sample_time); + if (!corrected || !ekf_.reset_from_accel(*corrected / 9.80665)) + return std::nullopt; + ekf_state_time_ = sample.sample_time; + const auto guard = std::scoped_lock{mutex_}; + latest_acceleration_ = AccelerationSnapshot{*corrected, sample.sample_time}; + latest_snapshot_ = {ekf_.quaternion(), gyro_rad_per_sec, ekf_state_time_}; + initialized_ = true; + pending_accel_sample_.reset(); + } if (sample_time < ekf_state_time_) return std::nullopt; - if (is_gyro_saturated(gyro_rad_per_sec)) + // Buffered frames after reconnect or a receive gap must not integrate the current gyro + // over an unobserved interval. Check before the pending accelerometer prediction, which + // otherwise advances the state time and hides the gap from this guard. + // TODO: Remove this once librmcs guarantees historical IMU frames are flushed on + // connection. + if (std::chrono::duration{sample_time - ekf_state_time_}.count() + > ImuLeverArm::kMaximumSampleInterval) { + ekf_state_time_ = sample_time; + if (pending_accel_sample_ && pending_accel_sample_->sample_time <= sample_time) + pending_accel_sample_ = std::nullopt; + return std::nullopt; + } + + if (gyro_saturated) ekf_.inflate_attitude_uncertainty_to_initial(); while (pending_accel_sample_) { @@ -83,8 +153,19 @@ class Bmi088Ekf { break; ekf_state_time_ = accel_sample_time; - const auto correction = ekf_.prepare_correction(pending_accel_sample_->accel_g); - if (!correction || correction->chi_square() >= 3.0) + Eigen::Vector3d accel_g = pending_accel_sample_->accel_g; + if (lever_arm_.enabled()) { + const auto corrected = lever_arm_.correct(accel_g * 9.80665, accel_sample_time); + if (!corrected) { + pending_accel_sample_.reset(); + break; + } + accel_g = *corrected / 9.80665; + const auto guard = std::scoped_lock{mutex_}; + latest_acceleration_ = AccelerationSnapshot{*corrected, accel_sample_time}; + } + const auto correction = ekf_.prepare_correction(accel_g); + if (!correction || correction->chi_square() >= 16.0) break; if (!ekf_.correct(*correction)) break; @@ -93,16 +174,6 @@ class Bmi088Ekf { break; } - // Guard against stale IMU frames that librmcs may deliver right after reconnect before the - // device-side buffer is drained. Integrating a frame with a large timestamp jump can inject - // a huge bogus gyro delta, so drop it instead of advancing the filter. - // TODO: Remove this once librmcs guarantees buffered historical IMU frames are flushed on - // connection. - if (std::chrono::duration{sample_time - ekf_state_time_}.count() > 1 / 1000.0) { - ekf_state_time_ = sample_time; - return std::nullopt; - } - if (!ekf_.predict( gyro_rad_per_sec, std::chrono::duration{sample_time - ekf_state_time_}.count())) @@ -121,6 +192,16 @@ class Bmi088Ekf { return snapshot; } + // Called on the receive thread after a control reset. Preserve physical attitude and mount. + void reset_motion_history() { + lever_arm_.reset(); + if (lever_arm_.enabled()) { + pending_accel_sample_.reset(); + const auto guard = std::scoped_lock{mutex_}; + latest_acceleration_.reset(); + } + } + [[nodiscard]] bool initialized() const noexcept { const auto guard = std::scoped_lock{mutex_}; return initialized_; @@ -133,6 +214,11 @@ class Bmi088Ekf { return latest_snapshot_; } + [[nodiscard]] std::optional acceleration_snapshot() const noexcept { + const auto guard = std::scoped_lock{mutex_}; + return latest_acceleration_; + } + private: [[nodiscard]] static Eigen::Vector3d convert_accelerometer(std::int16_t x, std::int16_t y, std::int16_t z) noexcept { @@ -153,6 +239,7 @@ class Bmi088Ekf { } const Config config_; + ImuLeverArm lever_arm_; mutable std::mutex mutex_; bool initialized_ = false; @@ -167,6 +254,7 @@ class Bmi088Ekf { std::optional pending_accel_sample_; Snapshot latest_snapshot_; + std::optional latest_acceleration_; }; } // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp index 1bc1a17ea..9ac339701 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp @@ -59,6 +59,7 @@ class DjiMotor { status_component.register_output(name_prefix + "/angle", angle_output_, 0.0); status_component.register_output(name_prefix + "/velocity", velocity_output_, 0.0); status_component.register_output(name_prefix + "/torque", torque_output_, 0.0); + status_component.register_output(name_prefix + "/temperature_c", temperature_output_, 0.0); status_component.register_output(name_prefix + "/max_torque", max_torque_output_, 0.0); command_component.register_input(name_prefix + "/control_torque", control_torque_, false); @@ -164,15 +165,20 @@ class DjiMotor { auto send_id() const noexcept -> std::uint32_t { return send_id(type_, id_); } bool match_then_store_status(std::uint32_t can_id, std::span can_data) { - if (can_id != recv_id()) + if (!matches_feedback(can_id, can_data)) return false; store_status(can_data); return true; } - void update_status() { - const auto feedback = - std::bit_cast(can_data_.load(std::memory_order::relaxed)); + bool matches_feedback(std::uint32_t can_id, std::span can_data) const { + return can_id == recv_id() && can_data.size() == 8; + } + + void update_status() { update_status(can_data_.load(std::memory_order_relaxed)); } + + void update_status(CanPacket8 packet) { + const auto feedback = std::bit_cast(packet); // Temperature unit: celsius temperature_ = static_cast(feedback.temperature); @@ -206,6 +212,7 @@ class DjiMotor { *angle_output_ = angle(); *velocity_output_ = velocity(); *torque_output_ = torque(); + *temperature_output_ = temperature(); } double control_torque() const { @@ -275,6 +282,7 @@ class DjiMotor { rmcs_executor::Component::OutputInterface angle_output_; rmcs_executor::Component::OutputInterface velocity_output_; rmcs_executor::Component::OutputInterface torque_output_; + rmcs_executor::Component::OutputInterface temperature_output_; rmcs_executor::Component::OutputInterface max_torque_output_; rmcs_executor::Component::InputInterface control_torque_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dm_joint_enable_sequence.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_joint_enable_sequence.hpp new file mode 100644 index 000000000..6bb4c4306 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_joint_enable_sequence.hpp @@ -0,0 +1,176 @@ +#pragma once + +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class DmJointEnableSequence { +public: + using Clock = std::chrono::steady_clock; + enum class Command { kNone, kClearError, kEnable, kDisable }; + enum class Phase { kDisabled, kClearing, kEnabling, kActive, kFailed }; + enum class Failure { kNone, kStartupTimeout, kRunningUnavailable }; + + // Fixed order: LH, LK, RH, RK. Freshness includes the driver's fault check. + struct Feedback { + bool fresh = false; + int status = 0; + Clock::time_point received_at{}; + }; + struct Step { + Command system = Command::kNone; + bool control_active = false; + std::uint8_t system_mask = 0; + }; + + Step update( + bool requested, bool controller_healthy, const std::array& feedback, + Clock::time_point now) { + if (!requested) { + if (phase_ != Phase::kDisabled) { + *this = DmJointEnableSequence{}; + next_system_ = now; + } + if (now < next_system_) + return {}; + const auto mask = side_mask_(); + advance_slot_(now); + return {Command::kDisable, false, mask}; + } + if (phase_ == Phase::kDisabled) { + *this = DmJointEnableSequence{}; + phase_ = Phase::kClearing; + started_ = now; + next_system_ = now; + } + if (phase_ == Phase::kFailed) + return {}; + + const bool ready = controller_healthy && all_ready_(feedback); + if (phase_ == Phase::kActive) { + if (!ready) { + phase_ = Phase::kFailed; + failure_ = Failure::kRunningUnavailable; + pending_mask_ = unavailable_mask_(feedback); + return {}; + } + return {Command::kNone, true, 0}; + } + remaining_ms_ = static_cast(std::chrono::duration_cast( + kStartupTimeout - (now - started_)) + .count()); + if (now - started_ >= kStartupTimeout) { + phase_ = Phase::kFailed; + failure_ = Failure::kStartupTimeout; + remaining_ms_ = 0; + return {}; + } + + if (phase_ == Phase::kClearing && now - started_ >= kClearDuration) { + phase_ = Phase::kEnabling; + next_side_ = 0; + next_system_ = now; + } + if (phase_ == Phase::kEnabling) { + pending_mask_ = 0; + for (std::size_t i = 0; i < feedback.size(); ++i) + if (!feedback[i].fresh || feedback[i].status != 1 || enable_attempts_[i] == 0 + || feedback[i].received_at <= last_enable_[i]) + pending_mask_ |= 1u << i; + if (ready && pending_mask_ == 0) { + if (!confirming_) { + confirming_ = true; + stable_since_ = now; + } + bool recent_samples = true; + for (const auto& motor : feedback) + recent_samples &= motor.received_at >= stable_since_ + kStableDuration; + if (now - stable_since_ >= kStableDuration && recent_samples) { + phase_ = Phase::kActive; + remaining_ms_ = 0; + return {Command::kNone, true, 0}; + } + } else { + confirming_ = false; + } + } + if (now < next_system_) + return {}; + const auto selected = side_mask_(); + advance_slot_(now); + if (phase_ == Phase::kClearing) + return {Command::kClearError, false, selected}; + + std::uint8_t retry_mask = 0; + for (std::size_t i = 0; i < feedback.size(); ++i) { + if (!(selected & pending_mask_ & (1u << i))) + continue; + // A motor fault needs explicit disarm/clear, not repeated FC. + if (feedback[i].status > 1) + continue; + if (enable_attempts_[i] != 0 && now - last_enable_[i] < kEnableRetryInterval) + continue; + retry_mask |= 1u << i; + last_enable_[i] = now; + ++enable_attempts_[i]; + } + return {retry_mask ? Command::kEnable : Command::kNone, false, retry_mask}; + } + + Phase phase() const { return phase_; } + Failure failure() const { return failure_; } + int remaining_ms() const { return remaining_ms_; } + std::uint8_t pending_mask() const { return pending_mask_; } + const std::array& enable_attempts() const { return enable_attempts_; } + const char* phase_name() const { + switch (phase_) { + case Phase::kDisabled: return "disabled"; + case Phase::kClearing: return "clearing"; + case Phase::kEnabling: return "enabling"; + case Phase::kActive: return "active"; + case Phase::kFailed: + return failure_ == Failure::kStartupTimeout ? "startup_timeout" : "running_unavailable"; + } + return "unknown"; + } + +private: + static std::uint8_t unavailable_mask_(const std::array& feedback) { + std::uint8_t mask = 0; + for (std::size_t i = 0; i < feedback.size(); ++i) + if (!feedback[i].fresh || feedback[i].status != 1) + mask |= 1u << i; + return mask; + } + static bool all_ready_(const std::array& feedback) { + return unavailable_mask_(feedback) == 0; + } + std::uint8_t side_mask_() const { return next_side_ == 0 ? 0x3 : 0xc; } + void advance_slot_(Clock::time_point now) { + // One motor per bus; the next side follows in another USB transfer. + next_system_ = + now + (next_side_ == 0 ? std::chrono::milliseconds{5} : std::chrono::milliseconds{16}); + next_side_ ^= 1; + } + + static constexpr auto kClearDuration = std::chrono::milliseconds{50}; + static constexpr auto kEnableRetryInterval = std::chrono::milliseconds{100}; + static constexpr auto kStableDuration = std::chrono::milliseconds{50}; + static constexpr auto kStartupTimeout = std::chrono::seconds{2}; + Phase phase_ = Phase::kDisabled; + Failure failure_ = Failure::kNone; + Clock::time_point started_{}; + Clock::time_point next_system_{}; + Clock::time_point stable_since_{}; + std::array last_enable_{}; + std::array enable_attempts_{}; + std::uint8_t pending_mask_ = 0xf; + unsigned next_side_ = 0; + int remaining_ms_ = 0; + bool confirming_ = false; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp new file mode 100644 index 000000000..b16771fc7 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp @@ -0,0 +1,326 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "hardware/device/can_packet.hpp" + +namespace rmcs_core::hardware::device { + +inline const char* dm_error_text(int code) noexcept { + switch (code) { + case 0x8: return "over-voltage"; + case 0x9: return "under-voltage"; + case 0xA: return "over-current"; + case 0xB: return "MOS over-temperature"; + case 0xC: return "coil over-temperature"; + case 0xD: return "communication lost"; + case 0xE: return "overload"; + default: return "unknown"; + } +} + +class DmMotor { +public: + enum class Type : uint8_t { kDM8009 }; + + struct Config { + explicit Config(Type motor_type, std::uint8_t id = 1) + : motor_type(motor_type) + , id(id) {} + + Config& set_id(std::uint8_t id) { return this->id = id, *this; } + Config& set_feedback_id(std::uint16_t feedback_id) { + return this->feedback_id = feedback_id, *this; + } + Config& set_reversed() { return reversed = true, *this; } + Config& set_reversed(bool value) { return reversed = value, *this; } + // API angle = sign * (raw_angle - angle_bias); the bias lives in the feedback frame. + Config& set_angle_bias(double angle_bias) { return this->angle_bias = angle_bias, *this; } + // MIT calibration range, must match the drive registers. + Config& set_limits(double position_max, double velocity_max, double torque_max) { + return this->position_max = position_max, this->velocity_max = velocity_max, + this->torque_max = torque_max, *this; + } + Config& set_control_torque_max(double control_torque_max) { + return this->control_torque_max = control_torque_max, *this; + } + + Type motor_type; + std::uint8_t id; + std::uint16_t feedback_id = 0; + bool reversed = false; + double angle_bias = 0.0; // rad + double position_max = std::numbers::pi; // [rad] + double velocity_max = 45.0; // [rad/s] + double torque_max = 54.0; // [Nm] + // Software bound; the drive's own current protection still applies between frames. + double control_torque_max = 40.0; + }; + + static constexpr std::uint8_t kCommandEnable = 0xFC; + static constexpr std::uint8_t kCommandDisable = 0xFD; + static constexpr std::uint8_t kCommandClearError = 0xFB; + + // control_required=false lets a gimbal-only bringup omit the leg command source. + DmMotor( + rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, + const std::string& name_prefix, bool control_required = true) { + status_component.register_output(name_prefix + "/angle", angle_output_, 0.0); + status_component.register_output(name_prefix + "/velocity", velocity_output_, 0.0); + status_component.register_output(name_prefix + "/torque", torque_output_, 0.0); + status_component.register_output( + name_prefix + "/temperature_mos", temperature_mos_output_, 0.0); + status_component.register_output( + name_prefix + "/temperature_rotor", temperature_rotor_output_, 0.0); + status_component.register_output(name_prefix + "/max_torque", max_torque_output_, 0.0); + // The 16-bit MIT position wraps every 2 * position_max; consumers need the span. + status_component.register_output( + name_prefix + "/angle_wrap_span", angle_wrap_span_output_, 2 * position_max_); + status_component.register_output(name_prefix + "/fault_code", fault_code_output_, 0); + status_component.register_output(name_prefix + "/status_code", status_code_output_, 0); + + command_component.register_input( + name_prefix + "/control_torque", control_torque_, control_required); + } + + DmMotor( + rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, + const std::string& name_prefix, const Config& config, bool control_required = true) + : DmMotor(status_component, command_component, name_prefix, control_required) { + configure(config); + } + + DmMotor(const DmMotor&) = delete; + DmMotor& operator=(const DmMotor&) = delete; + DmMotor(DmMotor&&) = delete; + DmMotor& operator=(DmMotor&&) = delete; + + ~DmMotor() = default; + + void configure(const Config& config) { + if (config.id == 0 || config.id > 15 || config.feedback_id > 0x7FF + || !std::isfinite(config.angle_bias) || !std::isfinite(config.position_max) + || !std::isfinite(config.velocity_max) || !std::isfinite(config.torque_max) + || !std::isfinite(config.control_torque_max) || config.position_max <= 0.0 + || config.velocity_max <= 0.0 || config.torque_max <= 0.0 + || config.control_torque_max < 0.0) + throw std::invalid_argument("Invalid DM motor CAN ID or MIT mapping range"); + + type_ = config.motor_type; + id_ = config.id; + feedback_id_ = config.feedback_id; + reversed_ = config.reversed; + angle_bias_ = config.angle_bias; + position_max_ = config.position_max; + velocity_max_ = config.velocity_max; + torque_max_ = config.torque_max; + control_torque_max_ = std::min(config.control_torque_max, config.torque_max); + + *max_torque_output_ = control_torque_max_; + *angle_wrap_span_output_ = 2 * position_max_; + fault_code_ = 0; + } + + CanPacket8 generate_command() const { return generate_command(control_torque()); } + + CanPacket8 generate_command(double torque) const { + if (std::isnan(torque) || std::isinf(torque)) + torque = 0.0; + torque = std::clamp(torque, -control_torque_max_, control_torque_max_); + const double sign = reversed_ ? -1.0 : 1.0; + auto packet = encode_torque_frame(sign * torque); + // Rounding a 12-bit torque can put the encoded value outside the bound. + if (control_torque_max_ > torque_max_ / 4095.0 + && std::abs(command_frame_torque(packet)) > control_torque_max_) + packet = encode_torque_frame( + sign * std::copysign(control_torque_max_ - 2.0 * torque_max_ / 4095.0, torque)); + return packet; + } + + double command_frame_torque(CanPacket8 packet) const { + const auto bytes = packet.as_bytes(); + const auto encoded = static_cast( + ((static_cast(bytes[6]) & 0x0F) << 8) + | static_cast(bytes[7])); + return (reversed_ ? -1.0 : 1.0) * uint_to_float(encoded, -torque_max_, torque_max_, 12); + } + + static bool command_frame_uses_pd(CanPacket8 packet) { + const auto bytes = packet.as_bytes(); + return (static_cast(bytes[3]) & 0x0F) != 0 + || static_cast(bytes[4]) != 0 || static_cast(bytes[5]) != 0 + || (static_cast(bytes[6]) & 0xF0) != 0; + } + + CanPacket8 enable_command() const { return system_command(kCommandEnable); } + CanPacket8 disable_command() const { return system_command(kCommandDisable); } + CanPacket8 clear_error_command() const { return system_command(kCommandClearError); } + + bool match_then_store_status(std::uint32_t can_id, std::span can_data) { + if (!matches_feedback(can_id, can_data)) + return false; + can_data_.store(CanPacket8{can_data}, std::memory_order_relaxed); + return true; + } + + bool matches_feedback(std::uint32_t can_id, std::span can_data) const { + if (can_id != feedback_id_) + return false; + if (can_data.size() != 8) + return false; + // The motor id occupies only the low 4 bits of D[0]. + const auto d0 = static_cast(can_data[0]); + return (d0 & 0x0F) == (id_ & 0x0F); + } + + void update_status() { update_status(can_data_.load(std::memory_order_relaxed)); } + + void update_status(CanPacket8 packet) { + const auto bytes = packet.as_bytes(); + + const auto d0 = static_cast(bytes[0]); + status_code_ = static_cast(d0 >> 4); + // 0 = disabled, 1 = enabled, 8..E = fault. + fault_code_ = status_code_ <= 1 ? 0 : status_code_; + + const auto pos_u = static_cast( + (static_cast(static_cast(bytes[1])) << 8) + | static_cast(bytes[2])); + const auto vel_u = static_cast( + (static_cast(static_cast(bytes[3])) << 4) + | (static_cast(bytes[4]) >> 4)); + const auto tff_u = static_cast( + ((static_cast(bytes[4]) & 0x0F) << 8) + | static_cast(bytes[5])); + + const double sign = reversed_ ? -1.0 : 1.0; + const double raw_angle = uint_to_float(pos_u, -position_max_, position_max_, 16); + const double raw_velocity = uint_to_float(vel_u, -velocity_max_, velocity_max_, 12); + const double raw_torque = uint_to_float(tff_u, -torque_max_, torque_max_, 12); + + raw_angle_ = raw_angle; + raw_position_u_ = pos_u; + angle_ = sign * (raw_angle - angle_bias_); + velocity_ = sign * raw_velocity; + torque_ = sign * raw_torque; + + temperature_mos_ = static_cast(static_cast(bytes[6])); + temperature_rotor_ = static_cast(static_cast(bytes[7])); + + *angle_output_ = angle_; + *velocity_output_ = velocity_; + *torque_output_ = torque_; + *temperature_mos_output_ = temperature_mos_; + *temperature_rotor_output_ = temperature_rotor_; + *fault_code_output_ = fault_code_; + *status_code_output_ = status_code_; + } + + double control_torque() const { return control_torque_.ready() ? *control_torque_ : 0.0; } + + std::uint8_t id() const noexcept { return id_; } + std::uint32_t send_id() const noexcept { return id_; } + std::uint32_t feedback_id() const noexcept { return feedback_id_; } + double angle() const { return angle_; } + double raw_angle() const { return raw_angle_; } + std::uint16_t raw_position() const { return raw_position_u_; } + double velocity() const { return velocity_; } + double torque() const { return torque_; } + double max_torque() const { return control_torque_max_; } + double angle_wrap_span() const { return 2 * position_max_; } + double temperature_mos() const { return temperature_mos_; } + double temperature_rotor() const { return temperature_rotor_; } + int fault_code() const { return fault_code_; } + int status_code() const { return status_code_; } + + static std::uint16_t float_to_uint(double x, double x_min, double x_max, int bits) { + const double span = x_max - x_min; + if (span <= 0.0) + return 0; + const double max_u = static_cast((std::uint32_t{1} << bits) - 1); + const double u = std::clamp((x - x_min) / span * max_u, 0.0, max_u); + return static_cast(std::round(u)); + } + + static double uint_to_float(std::uint16_t u, double x_min, double x_max, int bits) { + const double max_u = static_cast((std::uint32_t{1} << bits) - 1); + return x_min + static_cast(u) / max_u * (x_max - x_min); + } + + static CanPacket8 pack_mit( + std::uint16_t pos_u, std::uint16_t vel_u, std::uint16_t kp_u, std::uint16_t kd_u, + std::uint16_t tff_u) { + std::array bytes{}; + bytes[0] = static_cast((pos_u >> 8) & 0xFF); + bytes[1] = static_cast(pos_u & 0xFF); + bytes[2] = static_cast((vel_u >> 4) & 0xFF); + bytes[3] = static_cast(((vel_u & 0x0F) << 4) | ((kp_u >> 8) & 0x0F)); + bytes[4] = static_cast(kp_u & 0xFF); + bytes[5] = static_cast((kd_u >> 4) & 0xFF); + bytes[6] = static_cast(((kd_u & 0x0F) << 4) | ((tff_u >> 8) & 0x0F)); + bytes[7] = static_cast(tff_u & 0xFF); + return CanPacket8{std::span(bytes)}; + } + + static CanPacket8 system_command(std::uint8_t command) { + std::array bytes; + bytes.fill(static_cast(0xFF)); + bytes[7] = static_cast(command); + return CanPacket8{std::span(bytes)}; + } + +private: + CanPacket8 encode_torque_frame(double torque) const { + return pack_mit( + float_to_uint(0., -position_max_, position_max_, 16), + float_to_uint(0., -velocity_max_, velocity_max_, 12), 0, 0, + float_to_uint(torque, -torque_max_, torque_max_, 12)); + } + + Type type_ = Type::kDM8009; + std::uint8_t id_ = 1; + std::uint16_t feedback_id_ = 0; + bool reversed_ = false; + double angle_bias_ = 0.0; + double position_max_ = std::numbers::pi; + double velocity_max_ = 45.0; + double torque_max_ = 54.0; + double control_torque_max_ = 40.0; + + std::atomic can_data_{CanPacket8{0}}; + + double angle_ = 0.0; + double raw_angle_ = 0.0; + std::uint16_t raw_position_u_ = 0; + double velocity_ = 0.0; + double torque_ = 0.0; + double temperature_mos_ = 0.0; + double temperature_rotor_ = 0.0; + int fault_code_ = 0; + int status_code_ = 0; + + rmcs_executor::Component::OutputInterface angle_output_; + rmcs_executor::Component::OutputInterface velocity_output_; + rmcs_executor::Component::OutputInterface torque_output_; + rmcs_executor::Component::OutputInterface temperature_mos_output_; + rmcs_executor::Component::OutputInterface temperature_rotor_output_; + rmcs_executor::Component::OutputInterface max_torque_output_; + rmcs_executor::Component::OutputInterface angle_wrap_span_output_; + rmcs_executor::Component::OutputInterface fault_code_output_; + rmcs_executor::Component::OutputInterface status_code_output_; + + rmcs_executor::Component::InputInterface control_torque_; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index 51895755b..45bb281e8 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -140,6 +141,15 @@ class Dr16 { Eigen::Vector2d joystick_right() const { return to_eigen_vector(joystick_right_); } Eigen::Vector2d joystick_left() const { return to_eigen_vector(joystick_left_); } + std::array raw_channels() const { + const auto packet = + std::bit_cast(data_part1_.load(std::memory_order::relaxed)); + return {static_cast(packet.joystick_channel0), + static_cast(packet.joystick_channel1), + static_cast(packet.joystick_channel2), + static_cast(packet.joystick_channel3)}; + } + rmcs_msgs::Switch switch_right() const { return std::bit_cast(switch_right_); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/imu_lever_arm.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/imu_lever_arm.hpp new file mode 100644 index 000000000..b06e86f01 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/imu_lever_arm.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +namespace rmcs_core::hardware::device { + +// Transfers accelerometer specific force from a rigidly mounted sensor to the body origin. +// Position, force and angular velocity are all expressed in body axes. +class ImuLeverArm { +public: + using TimePoint = rmcs_msgs::BoardClock::time_point; + static constexpr double kMaximumSampleInterval = 1.0 / 1000.0; + + explicit ImuLeverArm(Eigen::Vector3d position, double filter_tau_s) + : position_(std::move(position)) + , filter_tau_s_(filter_tau_s) { + if (!position_.allFinite() || !std::isfinite(filter_tau_s_) || filter_tau_s_ < 0.0) + throw std::invalid_argument("IMU position must be finite and filter tau nonnegative"); + } + + bool enabled() const { return !position_.isZero(0.0); } + + void reset() { + previous_.reset(); + current_.reset(); + angular_acceleration_.setZero(); + } + + bool observe(const Eigen::Vector3d& omega, TimePoint time) { + if (!omega.allFinite() || (current_ && time <= current_->time)) + return false; + if (!current_) { + current_ = GyroSample{omega, time}; + return true; + } + const double dt = std::chrono::duration{time - current_->time}.count(); + if (dt > kMaximumSampleInterval) { + reset(); + current_ = GyroSample{omega, time}; + return true; + } + const Eigen::Vector3d derivative = (omega - current_->omega) / dt; + if (previous_) + angular_acceleration_ += + dt / (filter_tau_s_ + dt) * (derivative - angular_acceleration_); + else + angular_acceleration_ = derivative; + previous_ = current_; + current_ = GyroSample{omega, time}; + return true; + } + + std::optional + correct(const Eigen::Vector3d& specific_force, TimePoint time) const { + if (!enabled()) + return specific_force; + // Never extrapolate a stale angular derivative across an unobserved interval. + if (!previous_ || !current_ || time < previous_->time || time > current_->time) + return std::nullopt; + const double fraction = static_cast((time - previous_->time).count()) + / static_cast((current_->time - previous_->time).count()); + const Eigen::Vector3d omega = + previous_->omega + fraction * (current_->omega - previous_->omega); + const Eigen::Vector3d corrected = specific_force - angular_acceleration_.cross(position_) + - omega.cross(omega.cross(position_)); + return corrected.allFinite() ? std::optional{corrected} : std::nullopt; + } + +private: + struct GyroSample { + Eigen::Vector3d omega; + TimePoint time; + }; + + const Eigen::Vector3d position_; + const double filter_tau_s_; + std::optional previous_, current_; + Eigen::Vector3d angular_acceleration_ = Eigen::Vector3d::Zero(); +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp index 1e4b7c01a..5289c0636 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -9,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -95,7 +97,8 @@ class LkMotor { break; case Type::kMG4010Ei10: raw_angle_modulus_ = 1 << 16; - torque_constant = 0.07; + // Bench: 50 counts -> 0.12 Nm, 1150 -> 4.38 Nm, i.e. 0.0039 Nm/count (0.0242 Nm/A). + torque_constant = 0.0242; reduction_ratio = 10.0; max_torque_ = 4.5; break; @@ -150,15 +153,58 @@ class LkMotor { if (can_data.size() != 8) [[unlikely]] return; - const CanPacket8 can_packet{can_data}; + const auto command = std::to_integer(can_data[0]); + if (command != 0x9C && (command < 0xA0 || command > 0xA8)) + return; + + can_packet_.store(CanPacket8{can_data}, std::memory_order::relaxed); + } + + struct Status1 { + std::uint8_t command = 0; + std::int8_t temperature = 0; + std::int16_t voltage_centivolt = 0; + std::int16_t current_centiamp = 0; + std::uint8_t motor_state = 0x10; + std::uint8_t error_state = 0; + }; + + static std::optional parse_status1(std::span can_data) { + if (can_data.size() != 8) [[unlikely]] + return std::nullopt; + const struct [[gnu::packed]] { uint8_t command; - uint8_t placeholder[7]; - } feedback alignas(CanPacket8) = std::bit_cast(can_packet); + int8_t temperature; + int16_t voltage; + int16_t current; + uint8_t motor_state; + uint8_t error_state; + } feedback alignas(CanPacket8) = std::bit_cast(CanPacket8{can_data}); + + if (feedback.command != 0x9A && feedback.command != 0x9B) + return std::nullopt; + + return Status1{ + feedback.command, + feedback.temperature, + feedback.voltage, + feedback.current, + feedback.motor_state, + feedback.error_state, + }; + } + + bool store_error_status(std::span can_data) { + const auto status = parse_status1(can_data); + if (!status) + return false; - // Exclude non-motor status messages - if ((feedback.command & 0xF0) != 0x80) - can_packet_.store(can_packet, std::memory_order::relaxed); + motor_state_.store(status->motor_state, std::memory_order::relaxed); + error_state_.store(status->error_state, std::memory_order::relaxed); + bus_voltage_centivolt_.store(status->voltage_centivolt, std::memory_order::relaxed); + bus_current_centiamp_.store(status->current_centiamp, std::memory_order::relaxed); + return true; } void update_status() { @@ -227,6 +273,20 @@ class LkMotor { double max_torque() const { return max_torque_; } double temperature() const { return temperature_; } + std::uint8_t motor_state() const { return motor_state_.load(std::memory_order::relaxed); } + std::uint8_t error_state() const { return error_state_.load(std::memory_order::relaxed); } + bool running() const { return motor_state() == 0x00; } + double bus_voltage() const { + return static_cast( + bus_voltage_centivolt_.load(std::memory_order::relaxed)) + / 100.0; + } + double bus_current() const { + return static_cast( + bus_current_centiamp_.load(std::memory_order::relaxed)) + / 100.0; + } + /// @brief Switch the motor from the startup state (default state after power-on) to the /// shutdown state, clearing the motor's rotation count and previously received control /// commands. The LED changes from steady on to slow flashing. At this time, the motor can still @@ -253,9 +313,6 @@ class LkMotor { /// @brief Disable the motor, but do not clear the motor's running state. Sending control /// commands again can control the motor actions. constexpr static CanPacket8 generate_disable_command() { - // Note: instead of sending a real disable message here, a torque control message with - // torque set to 0 is sent, because the disable message does not cause the motor to feedback - // its status. const struct [[gnu::packed]] { uint8_t id = 0xA1; uint8_t placeholder0[3]{}; @@ -265,6 +322,22 @@ class LkMotor { return std::bit_cast(command); } + constexpr static CanPacket8 generate_error_status_request() { + const struct [[gnu::packed]] { + uint8_t id = 0x9A; + uint8_t placeholder[7]{}; + } request alignas(CanPacket8){}; + return std::bit_cast(request); + } + + constexpr static CanPacket8 generate_clear_error_command() { + const struct [[gnu::packed]] { + uint8_t id = 0x9B; + uint8_t placeholder[7]{}; + } command alignas(CanPacket8){}; + return std::bit_cast(command); + } + /// @brief This command reads the current motor's temperature, motor torque current (MF, MG) / /// motor output power (MS), speed, and encoder position. constexpr static CanPacket8 generate_status_request() { @@ -537,6 +610,10 @@ class LkMotor { // Status std::atomic can_packet_; + std::atomic motor_state_{0x10}; + std::atomic error_state_{0}; + std::atomic bus_voltage_centivolt_{0}; + std::atomic bus_current_centiamp_{0}; int64_t multi_turn_encoder_count_ = 0; int last_raw_angle_ = 0; @@ -562,4 +639,42 @@ class LkMotor { bool first_generate_auto_command_ = true; }; +/// @brief errorState bit meaning, see "read status 1 and error flags" in the LK protocol manual. +namespace lk_error_flag { +constexpr std::uint8_t kLowVoltage = 1u << 0; +constexpr std::uint8_t kHighVoltage = 1u << 1; +constexpr std::uint8_t kDriveOverTemperature = 1u << 2; +constexpr std::uint8_t kMotorOverTemperature = 1u << 3; +constexpr std::uint8_t kOverCurrent = 1u << 4; +constexpr std::uint8_t kShortCircuit = 1u << 5; +constexpr std::uint8_t kStall = 1u << 6; +// Raised by the motor's own CAN input timeout protection: no command arrived in time. +constexpr std::uint8_t kInputTimeout = 1u << 7; +} // namespace lk_error_flag + +/// @brief Format the errorState flags of a status 1 reply, e.g. "stall|input_timeout". +inline std::string lk_error_text(std::uint8_t error_state) { + if (error_state == 0) + return "none"; + static constexpr std::array, 8> names{{ + {lk_error_flag::kLowVoltage, "low_voltage"}, + {lk_error_flag::kHighVoltage, "high_voltage"}, + {lk_error_flag::kDriveOverTemperature, "drive_over_temperature"}, + {lk_error_flag::kMotorOverTemperature, "motor_over_temperature"}, + {lk_error_flag::kOverCurrent, "over_current"}, + {lk_error_flag::kShortCircuit, "short_circuit"}, + {lk_error_flag::kStall, "stall"}, + {lk_error_flag::kInputTimeout, "input_timeout"}, + }}; + auto text = std::string{}; + for (const auto& [flag, name] : names) { + if (!(error_state & flag)) + continue; + if (!text.empty()) + text += '|'; + text += name; + } + return text; +} + } // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor_drive.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor_drive.hpp new file mode 100644 index 000000000..9a7a9013c --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor_drive.hpp @@ -0,0 +1,133 @@ +#pragma once + +#include +#include + +namespace rmcs_core::hardware::device { + +class LkMotorDriveScheduler { +public: + using Clock = std::chrono::steady_clock; + + enum class Action { kNone, kClearError, kEnable, kDisable, kTorque, kErrorPoll }; + enum class Phase { kDisabled, kClearing, kEnabling, kActive }; + + /// @brief Latest status 1 reply of the motor (0x9A poll / 0x9B echo). + struct Feedback { + bool valid = false; + std::uint8_t motor_state = 0x10; + std::uint8_t error_state = 0; + Clock::time_point received_at{}; + }; + + /// @brief Advance one command cycle (the board loop runs at 1 kHz). + Action next(bool requested, const Feedback& feedback, Clock::time_point now) noexcept { + if (!requested) { + phase_ = Phase::kDisabled; + phase_started_ = now; + next_frame_ = now; + clear_confirmed_ = false; + saw_status_ = false; + return Action::kDisable; + } + + if (phase_ == Phase::kDisabled) { + enter_(Phase::kClearing, now); + clear_confirmed_ = false; + saw_status_ = false; + } + + if (phase_ == Phase::kClearing) { + const bool replied = feedback.valid && feedback.received_at >= phase_started_; + saw_status_ = saw_status_ || replied; + if (replied && feedback.error_state == 0) { + clear_confirmed_ = true; + enter_(Phase::kEnabling, now); + } else if (now - phase_started_ >= kClearTimeout + && (!saw_status_ || (replied && feedback.error_state == 0x80))) { + // An idle input timeout may persist until torque commands start. + // Do not let it prevent startup; other reported faults still block it. + enter_(Phase::kEnabling, now); + } else { + if (now < next_frame_) + return Action::kNone; + next_frame_ = now + kFrameInterval; + return Action::kClearError; + } + } + + if (phase_ == Phase::kEnabling) { + if (now - phase_started_ < kEnableDuration) { + if (now < next_frame_) + return Action::kNone; + next_frame_ = now + kFrameInterval; + return Action::kEnable; + } + enter_(Phase::kActive, now); + next_poll_ = now + kPollInterval; + } + + // Phase::kActive + // Ignore status cached during clearing/enabling. Start torque streaming first, + // then evaluate replies received in the active phase. + if (feedback.valid && feedback.received_at > phase_started_ && feedback.error_state != 0) { + if (now - last_rearm_ < kRearmCooldown) + return Action::kDisable; + last_rearm_ = now; + ++rearm_count_; + enter_(Phase::kClearing, now); + clear_confirmed_ = false; + saw_status_ = false; + return Action::kClearError; + } + if (now >= next_poll_) { + next_poll_ = now + kPollInterval; + return Action::kErrorPoll; + } + return Action::kTorque; + } + + Phase phase() const noexcept { return phase_; } + bool active() const noexcept { return phase_ == Phase::kActive; } + bool clear_confirmed() const noexcept { return clear_confirmed_; } + unsigned rearm_count() const noexcept { return rearm_count_; } + bool startup_input_timeout(const Feedback& feedback) const noexcept { + return feedback.error_state == 0x80 && rearm_count_ == 0 + && (!active() || feedback.received_at <= phase_started_); + } + + static const char* phase_name(Phase phase) noexcept { + switch (phase) { + case Phase::kDisabled: return "disabled"; + case Phase::kClearing: return "clearing"; + case Phase::kEnabling: return "enabling"; + case Phase::kActive: return "active"; + } + return "unknown"; + } + const char* phase_name() const noexcept { return phase_name(phase_); } + +private: + void enter_(Phase phase, Clock::time_point now) noexcept { + phase_ = phase; + phase_started_ = now; + next_frame_ = now; + } + + static constexpr auto kFrameInterval = std::chrono::milliseconds{10}; + static constexpr auto kClearTimeout = std::chrono::milliseconds{300}; + static constexpr auto kEnableDuration = std::chrono::milliseconds{100}; + static constexpr auto kPollInterval = std::chrono::milliseconds{100}; + static constexpr auto kRearmCooldown = std::chrono::milliseconds{500}; + + Phase phase_ = Phase::kDisabled; + Clock::time_point phase_started_{}; + Clock::time_point next_frame_{}; + Clock::time_point next_poll_{}; + Clock::time_point last_rearm_{}; + unsigned rearm_count_ = 0; + bool clear_confirmed_ = false; + bool saw_status_ = false; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp index 8ac884f08..be67ccdb6 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp @@ -28,9 +28,12 @@ class Supercap { status_component.register_output("/chassis/supercap/voltage", supercap_voltage_, 0.0); status_component.register_output("/chassis/supercap/enabled", supercap_enabled_, false); - command_component.register_input("/referee/chassis/output_status", chassis_output_status_); + // Both referee interfaces stay optional so a robot without a referee + // component can still monitor and drive the supercap. command_component.register_input( - "/chassis/supercap/charge_power_limit", supercap_charge_power_limit_); + "/referee/chassis/output_status", chassis_output_status_, false); + command_component.register_input( + "/chassis/supercap/charge_power_limit", supercap_charge_power_limit_, false); } void store_status(std::span can_data) { @@ -52,9 +55,11 @@ class Supercap { CanPacket8::Quarter generate_command() const { SupercapCommand command; - command.enabled = *chassis_output_status_; + command.enabled = chassis_output_status_.ready() && *chassis_output_status_; - const double power_limit = *supercap_charge_power_limit_; + const double power_limit = + supercap_charge_power_limit_.ready() ? *supercap_charge_power_limit_ + : std::numeric_limits::quiet_NaN(); if (std::isnan(power_limit)) command.power_limit = 0; else @@ -67,7 +72,9 @@ class Supercap { SupercapCommand command; command.enabled = false; - const double power_limit = *supercap_charge_power_limit_; + const double power_limit = + supercap_charge_power_limit_.ready() ? *supercap_charge_power_limit_ + : std::numeric_limits::quiet_NaN(); if (std::isnan(power_limit)) command.power_limit = 0; else diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/tof.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/tof.hpp new file mode 100644 index 000000000..ff0d18c91 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/tof.hpp @@ -0,0 +1,160 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace rmcs_core::hardware::device { +using rmcs_executor::Component; + +class Tof { +public: + using Clock = std::chrono::steady_clock; + + struct Config { + Config& set_id(uint8_t value) { return id = value, *this; } + Config& set_reversed() { return reverse_factor = -1.0, *this; } + Config& mode_to_inquired() { return mode_inquired = true, *this; } + + uint8_t id = 0; + double reverse_factor = 1.0; + bool mode_inquired = false; + std::chrono::milliseconds timeout{200}; + }; + + explicit Tof(Component& status_component, const std::string& name_prefix) { + status_component.register_output( + name_prefix + "/distance", distance_, std::numeric_limits::quiet_NaN()); + } + + explicit Tof(Component& status_component, const std::string& name_prefix, const Config& config) + : Tof(status_component, name_prefix) { + configure(config); + } + + Tof(const Tof&) = delete; + Tof& operator=(const Tof&) = delete; + + void configure(const Config& config) { + if (config.timeout.count() <= 0) + throw std::invalid_argument("TOF timeout must be positive"); + config_ = config; + } + + // UART Frame0: 16 bytes, little-endian uint24 distance, trailing sum check. + bool store_status( + const std::byte* uart_data, size_t uart_data_length, Clock::time_point now = Clock::now()) { + if (uart_data_length != package_length || uart_data == nullptr) + return false; + if (uart_data[0] != std::byte{0x57} || uart_data[1] != std::byte{0x00}) + return false; + + uint8_t checksum = 0; + for (size_t i = 0; i < package_length - 1; ++i) + checksum += std::to_integer(uart_data[i]); + if (checksum != std::to_integer(uart_data[package_length - 1])) + return false; + + Package package{}; + std::memcpy(&package, uart_data + 8, 6); + store_package(package, now); + return true; + } + + // Module publishes on 0x200 + id and takes queries on 0x402. + uint8_t id() const noexcept { return config_.id; } + uint32_t recv_id() const noexcept { return can_feedback_base_id + config_.id; } + + bool matches_feedback(uint32_t can_id, std::span can_data) const noexcept { + return can_id == recv_id() && can_data.size() == can_package_length; + } + + bool match_then_store_status( + uint32_t can_id, std::span can_data, + Clock::time_point now = Clock::now()) { + if (!matches_feedback(can_id, can_data)) + return false; + store_status(can_data, now); + return true; + } + + // CAN Frame0: distance[3], dis_status, signal_strength[2], reserved[2]. + bool store_status(std::span can_data, Clock::time_point now = Clock::now()) { + if (can_data.size() != can_package_length) + return false; + + Package package{}; + std::memcpy(&package, can_data.data(), 6); + store_package(package, now); + return true; + } + + void update_status(Clock::time_point now = Clock::now()) { + const auto package = package_.load(std::memory_order::relaxed); + const auto received = Clock::time_point{ + std::chrono::nanoseconds{last_received_ns_.load(std::memory_order::relaxed)}}; + distance_valid_ = + package.received && package.dis_status == 0 && now - received <= config_.timeout; + *distance_ = distance_valid_ ? package.calculate_distance() * config_.reverse_factor + : std::numeric_limits::quiet_NaN(); + } + + void update() { update_status(); } + + double get_reverse_factor() const { return config_.reverse_factor; } + bool is_mode_inquire() const { return config_.mode_inquired; } + double get_distance() const { return *distance_; } + bool get_valid() const { return distance_valid_; } + + static std::array generate_can_query(uint8_t id) { + return {std::byte{0xff}, std::byte{0xff}, std::byte{0xff}, std::byte{id}, + std::byte{0xff}, std::byte{0xff}, std::byte{0xff}, std::byte{0xff}}; + } + + static constexpr size_t package_length = 16; + static constexpr size_t can_package_length = 8; + static constexpr uint32_t can_feedback_base_id = 0x200; + static constexpr uint32_t can_query_id = 0x402; + +private: + struct alignas(uint64_t) Package { + uint8_t raw_distance[3]; + uint8_t dis_status; + uint8_t signal_strength[2]; + uint8_t reserved; + uint8_t received; + + double calculate_distance() const { + const uint32_t raw = raw_distance[0] | (raw_distance[1] << 8) | (raw_distance[2] << 16); + return static_cast(raw) / 1000.0; + } + }; + static_assert(sizeof(Package) == can_package_length); + static_assert(std::atomic::is_always_lock_free); + + void store_package(Package package, Clock::time_point now) { + package.received = 1; + last_received_ns_.store( + std::chrono::duration_cast(now.time_since_epoch()).count(), + std::memory_order::relaxed); + package_.store(package, std::memory_order::relaxed); + } + + Component::OutputInterface distance_; + + Config config_; + std::atomic package_{Package{}}; + std::atomic last_received_ns_{0}; + bool distance_valid_ = false; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/wheel_leg_sensor_snapshot.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/wheel_leg_sensor_snapshot.hpp new file mode 100644 index 000000000..bbbe38fab --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/wheel_leg_sensor_snapshot.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "hardware/device/board_clock_lifter.hpp" + +namespace rmcs_core::hardware::device { + +template +struct WheelLegSensorSnapshot { + T value{}; + std::uint64_t sequence = 0; + std::uint64_t steady_ns = 0; + std::uint32_t board_quarter_us = 0; +}; + +template +class WheelLegSensorMailbox { +public: + using Snapshot = WheelLegSensorSnapshot; + + void publish(T value, std::uint64_t steady_ns, std::uint32_t board_quarter_us = 0) noexcept { + slots_[producer_] = {std::move(value), ++sequence_, steady_ns, board_quarter_us}; + producer_ = exchange_.exchange(producer_ | kDirty, std::memory_order_acq_rel) & kIndex; + } + + const Snapshot& read() noexcept { + if ((exchange_.load(std::memory_order_acquire) & kDirty) != 0) + consumer_ = exchange_.exchange(consumer_, std::memory_order_acq_rel) & kIndex; + return latest(); + } + + const Snapshot& latest() const noexcept { return slots_[consumer_]; } + +private: + static constexpr unsigned kDirty = 4; + static constexpr unsigned kIndex = 3; + static_assert(std::atomic::is_always_lock_free); + + std::array slots_{}; + unsigned producer_ = 2; + unsigned consumer_ = 0; + std::atomic exchange_{1}; + std::uint64_t sequence_ = 0; +}; + +class WheelLegImuSampleClock { +public: + using TimePoint = BoardClockLifter::time_point; + + std::optional accelerometer_time(std::uint32_t raw_timestamp) noexcept { + if (!accept_newer(accelerometer_tick_, raw_timestamp)) + return std::nullopt; + return lifter_.advance_timebase(raw_timestamp); + } + + std::optional gyroscope_time(std::uint32_t raw_timestamp) noexcept { + const auto time = lifter_.lift_timestamp(raw_timestamp); + if (!time || !accept_newer(gyroscope_tick_, raw_timestamp)) + return std::nullopt; + return time; + } + +private: + static bool accept_newer(std::optional& previous, std::uint32_t raw) noexcept { + // Consecutive samples are less than half the 32-bit board-clock period apart. + if (previous && static_cast(raw - *previous) <= 0) + return false; + previous = raw; + return true; + } + + BoardClockLifter lifter_; + std::optional accelerometer_tick_, gyroscope_tick_; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/util/wheel_leg_drive.hpp b/rmcs_ws/src/rmcs_core/src/hardware/util/wheel_leg_drive.hpp new file mode 100644 index 000000000..29b023f05 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/util/wheel_leg_drive.hpp @@ -0,0 +1,198 @@ +#pragma once + +#include +#include +#include +#include + +#include + +namespace rmcs_core::hardware { + +template +bool wheel_leg_feedback_fresh(Now read_now, std::int64_t timeout_ns, const Stamps&... stamps) { + static_assert(sizeof...(Stamps) > 0); + const std::array captured{stamps.load(std::memory_order_relaxed)...}; + const auto now = read_now(); + for (const auto stamp : captured) { + if (stamp <= 0 || now < stamp || (timeout_ns > 0 && now - stamp >= timeout_ns)) + return false; + } + return true; +} + +template +bool wheel_leg_feedback_available(Now read_now, const Stamps&... stamps) { + static_assert(sizeof...(Stamps) > 0); + const std::array captured{stamps.load(std::memory_order_relaxed)...}; + const auto now = read_now(); + for (const auto stamp : captured) + if (stamp <= 0 || now < stamp) + return false; + return true; +} + +class WheelLegDmCommandScheduler { +public: + enum class Action { kNone, kClearError, kEnable, kDisable, kMit, kFeedbackPoll }; + + Action next(bool enable, bool entered_both_down = false) noexcept { + if (entered_both_down && !enable) { + enabled_ = false; + resend_remaining_ = kResendCycles; + heartbeat_cycles_ = 0; + } + if (enable != enabled_) { + enabled_ = enable; + resend_remaining_ = kResendCycles; + heartbeat_cycles_ = 0; + if (enable) + return Action::kClearError; + } + if (resend_remaining_ > 0) { + const bool neutralize = enabled_ && resend_remaining_ == kResendCycles - 1; + --resend_remaining_; + if (neutralize) + return Action::kFeedbackPoll; + return enabled_ ? Action::kEnable : Action::kDisable; + } + if (++heartbeat_cycles_ >= kHeartbeatCycles) { + heartbeat_cycles_ = 0; + return enabled_ ? Action::kEnable : Action::kDisable; + } + if (enabled_) + return Action::kMit; + return Action::kFeedbackPoll; + } + +private: + // Each side advances at 500 Hz in the 1 kHz board command loop. + static constexpr int kResendCycles = 50; + static constexpr int kHeartbeatCycles = 250; + int resend_remaining_ = kResendCycles; + int heartbeat_cycles_ = 0; + bool enabled_ = false; +}; + +class WheelLegDmFaultClearScheduler { +public: + bool next(bool requested) noexcept { + if (!requested) { + remaining_ = 0; + return false; + } + if (remaining_ > 0) { + --remaining_; + return false; + } + remaining_ = 49; + return true; + } + +private: + int remaining_ = 0; +}; + +struct WheelLegDmPairFeedback { + int hip_status = -1; + int knee_status = -1; + int hip_fault = -1; + int knee_fault = -1; + bool available = false; +}; + +constexpr bool wheel_leg_dm_pair_safe_to_arm(const WheelLegDmPairFeedback& pair) noexcept { + return pair.available && pair.hip_fault == 0 && pair.knee_fault == 0 + && (pair.hip_status == 0 || pair.hip_status == 1) + && (pair.knee_status == 0 || pair.knee_status == 1); +} + +constexpr bool wheel_leg_dm_pair_confirmed_disabled(const WheelLegDmPairFeedback& pair) noexcept { + return pair.available && pair.hip_fault == 0 && pair.knee_fault == 0 && pair.hip_status == 0 + && pair.knee_status == 0; +} + +constexpr bool wheel_leg_dm_pair_enabled(const WheelLegDmPairFeedback& pair) noexcept { + return pair.available && pair.hip_fault == 0 && pair.knee_fault == 0 && pair.hip_status == 1 + && pair.knee_status == 1; +} + +class WheelLegNormalActionGate { +public: + using Action = WheelLegDmCommandScheduler::Action; + + std::array apply( + std::array actions, const std::array& requested, + const std::array& pairs) noexcept { + for (std::size_t side = 0; side < actions.size(); ++side) { + if (!requested[side]) { + fault_clear_[side].next(false); + enable_retry_[side].next(false); + } else if (actions[side] == Action::kNone) { + continue; + } else if (!wheel_leg_dm_pair_safe_to_arm(pairs[side])) { + actions[side] = fault_clear_[side].next(true) ? Action::kClearError : Action::kNone; + enable_retry_[side].next(false); + } else { + fault_clear_[side].next(false); + if (actions[side] == Action::kMit && !wheel_leg_dm_pair_enabled(pairs[side])) + actions[side] = + enable_retry_[side].next(true) ? Action::kEnable : Action::kNone; + else + enable_retry_[side].next(false); + } + } + return actions; + } + + void reset() noexcept { + for (auto& scheduler : fault_clear_) + scheduler.next(false); + for (auto& scheduler : enable_retry_) + scheduler.next(false); + } + +private: + std::array fault_clear_; + std::array enable_retry_; +}; + +class WheelLegDmSideSchedulers { +public: + using Action = WheelLegDmCommandScheduler::Action; + + template + static void dispatch( + const std::array& actions, const std::array& axis_order, + Send&& send) { + for (const auto axis : axis_order) + if (const auto action = actions[axis / 2]; action != Action::kNone) + send(axis, action); + for (const auto axis : axis_order) + if (actions[axis / 2] == Action::kDisable) + send(axis, Action::kFeedbackPoll); + } + + std::array + next(const std::array& enable, bool entered_both_down = false) noexcept { + scheduled_side_ = 1 - scheduled_side_; + std::array actions{Action::kNone, Action::kNone}; + for (std::size_t side = 0; side < actions.size(); ++side) { + if (!enable[side] && (previous_enable_[side] || entered_both_down)) + actions[side] = schedulers_[side].next(false, entered_both_down); + if (side == scheduled_side_ && actions[side] == Action::kNone) + actions[side] = schedulers_[side].next(enable[side]); + } + previous_enable_ = enable; + return actions; + } + + std::size_t scheduled_side() const noexcept { return scheduled_side_; } + +private: + std::array schedulers_; + std::array previous_enable_{}; + std::size_t scheduled_side_ = 1; +}; + +} // namespace rmcs_core::hardware diff --git a/rmcs_ws/src/rmcs_core/src/hardware/wheel_leg.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheel_leg.cpp new file mode 100644 index 000000000..552fcfd5f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheel_leg.cpp @@ -0,0 +1,1032 @@ +#include +#include +#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_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dm_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/lk_motor_drive.hpp" +#include "hardware/device/remote_control.hpp" +#include "hardware/device/supercap.hpp" +#include "hardware/device/tof.hpp" +#include "hardware/device/wheel_leg_sensor_snapshot.hpp" +#include "hardware/util/wheel_leg_drive.hpp" + +namespace rmcs_core::hardware { + +namespace { + +using LkDriveClock = std::chrono::steady_clock; +using LkDriveAction = device::LkMotorDriveScheduler::Action; + +// Only 0x9A/0x9B echoes advance this stamp, so "replied after clearing" is checkable. +device::LkMotorDriveScheduler::Feedback + lk_feedback(const device::LkMotor& motor, const std::atomic& error_stamp_ns) { + device::LkMotorDriveScheduler::Feedback feedback; + const auto stamp = error_stamp_ns.load(std::memory_order::relaxed); + if (stamp <= 0) + return feedback; + feedback.valid = true; + feedback.motor_state = motor.motor_state(); + feedback.error_state = motor.error_state(); + feedback.received_at = LkDriveClock::time_point{std::chrono::nanoseconds{stamp}}; + return feedback; +} + +std::optional lk_command(device::LkMotor& motor, LkDriveAction action) { + switch (action) { + case LkDriveAction::kClearError: return device::LkMotor::generate_clear_error_command(); + case LkDriveAction::kEnable: return device::LkMotor::generate_startup_command(); + case LkDriveAction::kErrorPoll: return device::LkMotor::generate_error_status_request(); + case LkDriveAction::kDisable: return motor.generate_disable_command(); + case LkDriveAction::kTorque: return motor.generate_torque_command(); + case LkDriveAction::kNone: break; + } + return std::nullopt; +} + +void log_lk_fault_transition( + const rclcpp::Logger& logger, const char* name, std::uint8_t motor_state, + std::uint8_t error_state, std::uint8_t& previous, bool startup_input_timeout) { + // Idle input timeout is expected before clear/enable/torque streaming. Do not + // advertise it as an active drive fault, or log a spurious "error cleared". + if (startup_input_timeout && previous == 0) + return; + if (error_state == previous) + return; + if (error_state == 0) + RCLCPP_INFO(logger, "LK drive %s error cleared", name); + else + RCLCPP_ERROR( + logger, "LK drive %s error 0x%02X (%s), motor state 0x%02X", name, + static_cast(error_state), device::lk_error_text(error_state).c_str(), + static_cast(motor_state)); + previous = error_state; +} + +} // namespace + +class WheelLeg final + : public rmcs_executor::Component + , public rclcpp::Node + , public rmcs_utility::NodeMixin { +public: + WheelLeg() + : Node{get_component_name(), node::options()} + , command_(create_partner_component(get_component_name() + "_command", *this)) { + remote_control_ = std::make_unique(*this); + register_output("/tf", tf_); + bottom_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); + top_board_ = std::make_unique( + *this, *command_, get_parameter("serial_filter_top_board").as_string()); + using Srv = std_srvs::srv::Trigger; + status_service_ = create_service( + "/rmcs/service/robot_status", + [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { + response->success = true; + response->message = status_text(); + }); + } + void update() override { + bottom_board_->update(); + top_board_->update(); + remote_control_->update(); + } + +private: + std::string status_text() const { + std::ostringstream text; + text << std::format(" yaw_motor_zero_point: {}\n", bottom_board_->yaw_raw_angle()); + text << std::format(" pitch_motor_zero_point: {}\n", top_board_->pitch_raw_angle()); + text << top_board_->status_text(); + text << bottom_board_->status_text(); + return text.str(); + } + + class Command : public rmcs_executor::Component { + public: + explicit Command(WheelLeg& hardware) + : hardware_(hardware) {} + void update() override { + hardware_.bottom_board_->command_update(); + hardware_.top_board_->command_update(); + } + + private: + WheelLeg& hardware_; + }; + struct BottomBoard final : librmcs::board::RmcsBoardLite::Callback { + using Clock = std::chrono::steady_clock; + using Action = WheelLegDmCommandScheduler::Action; + BottomBoard(WheelLeg& status, rmcs_executor::Component& command, const std::string& serial) + : status_(status) + , leg_drive_enabled_(status.get_parameter_or("leg_drive_enabled", true)) + , wheel_motors_( + {status, command, "/wheel_leg/left_wheel"}, + {status, command, "/wheel_leg/right_wheel"}) + , hip_motors_( + {status, command, "/wheel_leg/left_hip_joint", leg_drive_enabled_}, + {status, command, "/wheel_leg/right_hip_joint", leg_drive_enabled_}) + , knee_motors_( + {status, command, "/wheel_leg/left_knee_joint", leg_drive_enabled_}, + {status, command, "/wheel_leg/right_knee_joint", leg_drive_enabled_}) + , gimbal_yaw_motor_(status, command, "/gimbal/yaw") + , feeder_motor_(status, command, "/gimbal/bullet_feeder") + , supercap_(status, command) + // TOFSense pairs moved from CAN0 to CAN1 (0x205/0x206). + , tof_left_(status, "/chassis/tof/left", device::Tof::Config{}.set_id(5)) + , tof_right_(status, "/chassis/tof/right", device::Tof::Config{}.set_id(6)) + // Chassis BMI088: mounted rotated 180 degrees about body Y, so the + // sensor axes read (-x, y, -z) relative to the body frame. + , bmi088_( + device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi, Eigen::Vector3d::UnitY()} + .toRotationMatrix(), + }) { + status.register_output( + "/wheel_leg/imu/quaternion", imu_quaternion_, Eigen::Quaterniond::Identity()); + status.register_output( + "/wheel_leg/imu/angular_velocity", imu_angular_velocity_, Eigen::Vector3d::Zero()); + status.register_output( + "/wheel_leg/imu/acceleration", imu_acceleration_, Eigen::Vector3d::Zero()); + status.register_output( + "/wheel_leg/imu/last_steady_ns", imu_last_output_, std::uint64_t{0}); + status.register_output( + "/wheel_leg/imu/acceleration_steady_ns", acceleration_last_output_, + std::uint64_t{0}); + status.register_output( + "/wheel_leg/imu/sequence", imu_sequence_output_, std::uint64_t{0}); + status.register_output( + "/wheel_leg/imu/acceleration_sequence", acceleration_sequence_output_, + std::uint64_t{0}); + status.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + return size; + }; + status.register_output("/wheel_leg/feedback_fresh", feedback_fresh_output_, false); + status.register_output( + "/wheel_leg/feedback_available", feedback_available_output_, false); + status.register_output("/wheel_leg/dm_control_ready", dm_control_ready_output_, false); + status.register_output("/wheel_leg/dr16_fresh", dr16_fresh_output_, false); + constexpr std::array names{"left_hip_joint", "left_knee_joint", "right_hip_joint", + "right_knee_joint", "left_wheel", "right_wheel"}; + for (std::size_t i = 0; i < names.size(); ++i) { + const auto prefix = std::string{"/wheel_leg/"} + names[i]; + status.register_output( + prefix + "/feedback_steady_ns", feedback_ns_outputs_[i], std::uint64_t{0}); + status.register_output( + prefix + "/feedback_sequence", feedback_sequence_outputs_[i], std::uint64_t{0}); + } + // Without the leg drive there is no leg command source, so both stay optional. + command.register_input( + "/wheel_leg/enable_request", enable_request_, leg_drive_enabled_); + command.register_input("/chassis/reset_count", reset_count_, leg_drive_enabled_); + const double timeout = status.get_parameter_or("feedback_timeout_s", .04); + if (!std::isfinite(timeout) || timeout < 0. || timeout > 1.) + throw std::invalid_argument("feedback_timeout_s must be finite and in [0,1]"); + timeout_ns_ = static_cast(timeout * 1e9); + if (timeout == 0.) + RCLCPP_INFO( + status.get_logger(), + "WheelLeg CAN feedback age check disabled; startup availability and " + "per-channel logging remain"); + else + RCLCPP_INFO( + status.get_logger(), "WheelLeg CAN feedback age warning threshold: %.1f ms", + timeout * 1e3); + if (!leg_drive_enabled_) + RCLCPP_INFO( + status.get_logger(), + "WheelLeg leg drive disabled: DM joints and wheels stay disabled, gimbal arms " + "from the remote alone"); + configure_motors(status); + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial, options); + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 115200}); + status.remote_control_->register_dr16(&dr16_); + } + void update() { + const std::lock_guard lock{status_mutex_}; + update_motor_status(); + gimbal_yaw_motor_.update_status(); + log_lk_fault_transition( + status_.get_logger(), "/gimbal/yaw", gimbal_yaw_motor_.motor_state(), + gimbal_yaw_motor_.error_state(), gimbal_yaw_last_error_, + gimbal_yaw_scheduler_.startup_input_timeout( + lk_feedback(gimbal_yaw_motor_, gimbal_yaw_error_ns_))); + gimbal_yaw_phase_.store(gimbal_yaw_scheduler_.phase(), std::memory_order::relaxed); + feeder_motor_.update_status(); + supercap_.update_status(); + tof_left_.update_status(); + tof_right_.update_status(); + constexpr std::array receive_order{2, 4, 3, 5, 0, 1}; + for (std::size_t i = 0; i < receive_order.size(); ++i) { + const auto& sample = motor_feedback_[receive_order[i]].latest(); + *feedback_ns_outputs_[i] = sample.steady_ns; + *feedback_sequence_outputs_[i] = sample.sequence; + } + publish_imu_feedback(); + const bool available = feedback_available(); + const bool fresh = feedback_fresh(); + *feedback_available_output_ = available; + *feedback_fresh_output_ = fresh; + update_feedback_warning(); + const bool ready = available && enabled_[0] && enabled_[1] && mit_sent_[0] + && mit_sent_[1] && wheel_leg_dm_pair_enabled(pair_feedback(0)) + && wheel_leg_dm_pair_enabled(pair_feedback(1)); + if (ready_ && !ready && requested_snapshot_.load(std::memory_order_relaxed)) + RCLCPP_WARN( + status_.get_logger(), "WheelLeg drive readiness lost: %s", + readiness_text().c_str()); + ready_ = ready; + *dm_control_ready_output_ = ready; + log_dm_drive_health(); + dr16_.update_status(); + *dr16_fresh_output_ = dr16_fresh(); + } + void command_update() { + const bool both_down = dr16_fresh() && dr16_.switch_left() == rmcs_msgs::Switch::DOWN + && dr16_.switch_right() == rmcs_msgs::Switch::DOWN; + const bool down_edge = both_down && !previous_both_down_; + previous_both_down_ = both_down; + if (down_edge || (reset_count_.ready() && *reset_count_ != last_reset_)) + reset_imu_motion_requested_.store(true, std::memory_order_release); + if (reset_count_.ready()) + last_reset_ = *reset_count_; + const std::array pairs{pair_feedback(0), pair_feedback(1)}; + const bool available = feedback_available(); + const bool enable_requested = enable_request_.ready() && *enable_request_; + const bool requested = + leg_drive_enabled_ && enable_requested && dr16_fresh() && available && !both_down; + requested_snapshot_.store(requested, std::memory_order_relaxed); + // Same as the legs minus leg feedback; without the leg drive it arms from the remote. + const bool gimbal_requested = + (leg_drive_enabled_ ? enable_requested : true) && dr16_fresh() && !both_down; + gimbal_drive_requested_.store(gimbal_requested, std::memory_order::relaxed); + const std::array requests{requested, requested}; + for (int side = 0; side < 2; ++side) + if (enabled_[side] != requested) + mit_sent_[side] = false; + auto actions = normal_action_gate_.apply( + command_schedulers_.next(requests, down_edge), requests, pairs); + const bool wheel_allowed = requested && mit_sent_[0] && mit_sent_[1] + && wheel_leg_dm_pair_enabled(pairs[0]) + && wheel_leg_dm_pair_enabled(pairs[1]); + auto builder = board_->start_transmit(); + // M3508 IDs 1/2 are the wheels and ID 3 is the feeder, all behind the + // single 0x200 frame on CAN0. This board stays the only 0x200 writer: + // a second producer would overwrite the other slots every cycle. + auto wheels = device::CanPacket8{ + wheel_motors_[0].generate_command( + wheel_allowed ? wheel_motors_[0].control_torque() : 0.), + wheel_motors_[1].generate_command( + wheel_allowed ? wheel_motors_[1].control_torque() : 0.), + device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}}; + wheels << feeder_motor_; + builder.can_transmit( + Spec::kCans.kCan0, {.can_id = 0x200, .can_data = wheels.as_bytes()}); + auto supercap_package = device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, supercap_.generate_command()}; + builder.can_transmit( + Spec::kCans.kCan0, {.can_id = 0x1FE, .can_data = supercap_package.as_bytes()}); + // Same sequence as the DM joints: clear -> run -> torque, zero torque while disabled. + const auto gimbal_action = gimbal_yaw_scheduler_.next( + gimbal_requested, lk_feedback(gimbal_yaw_motor_, gimbal_yaw_error_ns_), + LkDriveClock::now()); + if (auto packet = lk_command(gimbal_yaw_motor_, gimbal_action)) + builder.can_transmit( + Spec::kCans.kCan3, {.can_id = 0x141, .can_data = packet->as_bytes()}); + WheelLegDmSideSchedulers::dispatch( + actions, {0, 2, 1, 3}, [&](std::size_t axis, Action action) { + const auto side = axis / 2; + const auto& motor = axis % 2 == 0 ? hip_motors_[side] : knee_motors_[side]; + auto packet = joint_command(motor, action, available); + builder.can_transmit( + axis % 2 == 0 ? Spec::kCans.kCan1 : Spec::kCans.kCan2, + {.can_id = motor.send_id(), .can_data = packet.as_bytes()}); + }); + for (int side = 0; side < 2; ++side) { + if (requested && available && actions[side] == Action::kMit) + mit_sent_[side] = true; + enabled_[side] = requested; + } + } + // Called with status_mutex_ held. + [[nodiscard]] std::string readiness_text() const { + const auto now = now_ns(); + const auto age_ms = [now](const auto& stamp) { + const auto t = stamp.load(std::memory_order_relaxed); + return t <= 0 ? -1.0 : static_cast(now - t) / 1'000'000.0; + }; + auto text = std::ostringstream{}; + text << "dr16_fresh=" << dr16_fresh() + << " enable_request=" << requested_snapshot_.load(std::memory_order_relaxed) + << " available=" << feedback_available() << " enabled=[" + << static_cast(enabled_[0]) << ',' << static_cast(enabled_[1]) + << "] mit_sent=[" << static_cast(mit_sent_[0]) << ',' + << static_cast(mit_sent_[1]) << ']'; + constexpr std::array names{ + "left_hip_joint", "right_hip_joint", "left_knee_joint", "right_knee_joint"}; + constexpr std::array indices{2, 3, 4, 5}; + const std::array joints{ + &hip_motors_[0], &hip_motors_[1], &knee_motors_[0], &knee_motors_[1]}; + for (std::size_t i = 0; i < joints.size(); ++i) { + const auto& motor = *joints[i]; + const int fault = motor.fault_code(); + text << ", " << names[i] + << std::format( + " status=0x{:X} fault=0x{:X}({}) age={:.1f}ms torque={:.2f}", + motor.status_code(), fault, device::dm_error_text(fault), + age_ms(motor_last_ns_[indices[i]]), motor.torque()); + } + return text.str(); + } + [[nodiscard]] std::string status_text() const { + const std::lock_guard lock{status_mutex_}; + const auto now = now_ns(); + const auto age_ms = [now](const auto& stamp) { + const auto t = stamp.load(std::memory_order_relaxed); + return t <= 0 ? -1.0 : static_cast(now - t) / 1'000'000.0; + }; + auto text = std::ostringstream{}; + text << "WheelLeg status (feedback fresh: " << feedback_fresh() + << ", dm feedback fresh: " << motor_feedback_fresh() + << ", dr16 fresh: " << dr16_fresh() << ", dm enable requested [L,R]: [" + << static_cast(enabled_[0]) << ',' << static_cast(enabled_[1]) << ']' + << ", dm control ready: " << ready_ + << ", dm feedback available: " << feedback_available(); + if (timeout_ns_ == 0) + text << ", CAN age check: disabled"; + else + text << std::format(", CAN age warning: {:.1f} ms", timeout_ns_ / 1e6); + text << ", joint mode: torque):\n"; + text << " switches [left,right]: [" << static_cast(dr16_.switch_left()) << ',' + << static_cast(dr16_.switch_right()) << "] (down=2, middle=3)" + << std::format(", IMU age={:.1f} ms", age_ms(imu_last_ns_)) + << std::format(", dr16 age={:.1f} ms\n", age_ms(dr16_last_ns_)); + constexpr std::array names{ + "left_hip_joint", "right_hip_joint", "left_knee_joint", "right_knee_joint"}; + constexpr std::array indices{2, 3, 4, 5}; + const std::array joints{ + &hip_motors_[0], &hip_motors_[1], &knee_motors_[0], &knee_motors_[1]}; + for (std::size_t i = 0; i < joints.size(); ++i) { + const auto& motor = *joints[i]; + text << " " << names[i] << ": can_id=" << motor.send_id() + << " master_id=" << motor.feedback_id() + << std::format(" status=0x{:X}", motor.status_code()) + << std::format(" angle={:.4f}", motor.angle()) + << std::format(" vel={:.3f}", motor.velocity()) + << std::format(" torque={:.3f}", motor.torque()) + << std::format(" fault=0x{:X}", motor.fault_code()) + << std::format( + " feedback_age_ms={:.1f}\n", age_ms(motor_last_ns_[indices[i]])); + } + for (std::size_t i = 0; i < 2; ++i) + text << " wheel[" << i + << "]: can_id=" << static_cast(wheel_motors_[i].id()) + << std::format(" feedback_id=0x{:X}", wheel_motors_[i].recv_id()) + << std::format(" angle={:.4f}", wheel_motors_[i].angle()) + << std::format(" vel={:.3f}", wheel_motors_[i].velocity()) + << std::format(" torque={:.3f}", wheel_motors_[i].torque()) + << std::format(" feedback_age_ms={:.1f}\n", age_ms(motor_last_ns_[i])); + text << " feeder: can_id=" << static_cast(feeder_motor_.id()) + << std::format(" feedback_id=0x{:X}", feeder_motor_.recv_id()) + << std::format(" angle={:.4f}", feeder_motor_.angle()) + << std::format(" vel={:.3f}", feeder_motor_.velocity()) + << std::format(" torque={:.3f}", feeder_motor_.torque()) + << std::format(" feedback_age_ms={:.1f}\n", age_ms(feeder_last_ns_)); + text << " gimbal_yaw: can_id=0x141" + << std::format( + " motor_state=0x{:02X}", + static_cast(gimbal_yaw_motor_.motor_state())) + << std::format( + " error=0x{:02X}({})", + static_cast(gimbal_yaw_motor_.error_state()), + device::lk_error_text(gimbal_yaw_motor_.error_state())) + << std::format(" bus={:.1f}V", gimbal_yaw_motor_.bus_voltage()) << " drive=" + << device::LkMotorDriveScheduler::phase_name( + gimbal_yaw_phase_.load(std::memory_order::relaxed)) + << '\n'; + return text.str(); + } + [[nodiscard]] double yaw_angle() const { return gimbal_yaw_motor_.angle(); } + [[nodiscard]] std::int64_t yaw_raw_angle() const { + return gimbal_yaw_motor_.last_raw_angle(); + } + [[nodiscard]] bool gimbal_drive_requested() const { + return gimbal_drive_requested_.load(std::memory_order::relaxed); + } + + private: + mutable std::mutex status_mutex_; + static std::int64_t now_ns() { + return std::chrono::duration_cast( + Clock::now().time_since_epoch()) + .count(); + } + bool feedback_fresh() const { + return wheel_leg_feedback_fresh( + now_ns, timeout_ns_, imu_last_ns_, motor_last_ns_[0], motor_last_ns_[1], + motor_last_ns_[2], motor_last_ns_[3], motor_last_ns_[4], motor_last_ns_[5]); + } + bool feedback_available() const { + return wheel_leg_feedback_available( + now_ns, imu_last_ns_, motor_last_ns_[0], motor_last_ns_[1], motor_last_ns_[2], + motor_last_ns_[3], motor_last_ns_[4], motor_last_ns_[5]); + } + bool motor_feedback_available() const { + return wheel_leg_feedback_available( + now_ns, motor_last_ns_[0], motor_last_ns_[1], motor_last_ns_[2], motor_last_ns_[3], + motor_last_ns_[4], motor_last_ns_[5]); + } + bool motor_feedback_fresh() const { + return wheel_leg_feedback_fresh( + now_ns, timeout_ns_, motor_last_ns_[0], motor_last_ns_[1], motor_last_ns_[2], + motor_last_ns_[3], motor_last_ns_[4], motor_last_ns_[5]); + } + void update_feedback_warning() { + // Parked legs are not a fault in a gimbal-only configuration. + if (!leg_drive_enabled_) + return; + const auto now = now_ns(); + auto missing = std::string{}; + auto silent_motors = std::string{}; + for (std::size_t i = 0; i < kChannelNames.size(); ++i) { + const auto stamp = motor_last_ns_[i].load(std::memory_order_relaxed); + if (stamp <= 0) { + if (!missing.empty()) + missing += ", "; + missing += std::format("{} ({})", kChannelNames[i], kChannelBuses[i]); + } else if (now - stamp >= kChannelSilentNs) { + if (!silent_motors.empty()) + silent_motors += ", "; + silent_motors += std::format( + "{} {:.1f} ms ({})", kChannelNames[i], + static_cast(now - stamp) / 1'000'000.0, kChannelBuses[i]); + } + } + if (imu_last_ns_.load(std::memory_order_relaxed) <= 0) { + if (!missing.empty()) + missing += ", "; + missing += "IMU"; + } + const bool feedback_error = !missing.empty() || !silent_motors.empty(); + if (feedback_error + && (!feedback_error_active_ || now - last_feedback_error_ns_ >= 200'000'000)) { + RCLCPP_ERROR( + status_.get_logger(), + "WheelLeg feedback unavailable: no first sample [%s], silent motors [%s]", + missing.c_str(), silent_motors.c_str()); + last_feedback_error_ns_ = now; + } else if (feedback_error_active_ && !feedback_error) { + RCLCPP_INFO(status_.get_logger(), "WheelLeg feedback restored"); + } + feedback_error_active_ = feedback_error; + for (std::size_t i = 0; i < kChannelNames.size(); ++i) { + const auto stamp = motor_last_ns_[i].load(std::memory_order_relaxed); + const bool silent = stamp > 0 && now - stamp >= kChannelSilentNs; + if (silent && !channel_silent_[i]) { + channel_silent_since_[i] = stamp; + RCLCPP_WARN( + status_.get_logger(), "%s feedback silent for %.1f ms (%s)", + kChannelNames[i], static_cast(now - stamp) / 1'000'000.0, + kChannelBuses[i]); + } else if (!silent && channel_silent_[i]) { + const double outage_ms = + stamp > channel_silent_since_[i] + ? static_cast(stamp - channel_silent_since_[i]) / 1'000'000.0 + : 0.0; + RCLCPP_WARN( + status_.get_logger(), "%s feedback resumed after %.1f ms (%s)", + kChannelNames[i], outage_ms, kChannelBuses[i]); + } + channel_silent_[i] = silent; + } + const auto state = !motor_feedback_available() ? FeedbackWarningState::kMissing + : !motor_feedback_fresh() ? FeedbackWarningState::kStale + : FeedbackWarningState::kFresh; + if (state == feedback_warning_state_) + return; + if (state == FeedbackWarningState::kMissing) { + RCLCPP_WARN( + status_.get_logger(), + "WheelLeg CAN joint feedback has not completed its first sample; " + "control remains gated until all six motors report once; missing [%s]", + missing.c_str()); + } else if (state == FeedbackWarningState::kStale) { + auto stale = std::string{}; + for (std::size_t i = 0; i < kChannelNames.size(); ++i) { + const auto stamp = motor_last_ns_[i].load(std::memory_order_relaxed); + const double age = + stamp <= 0 ? -1.0 : static_cast(now - stamp) / 1'000'000.0; + if (stamp > 0 && now - stamp < timeout_ns_) + continue; + if (!stale.empty()) + stale += ", "; + stale += + std::format("{} {:.1f} ms ({})", kChannelNames[i], age, kChannelBuses[i]); + } + RCLCPP_WARN( + status_.get_logger(), + "WheelLeg CAN joint feedback is older than %.1f ms [%s]; continuing with " + "the last valid samples", + static_cast(timeout_ns_) / 1e6, stale.c_str()); + } + feedback_warning_state_ = state; + } + void log_dm_drive_health() { + const std::array joints{ + &hip_motors_[0], &hip_motors_[1], &knee_motors_[0], &knee_motors_[1]}; + static constexpr std::array names{ + "left_hip_joint", "right_hip_joint", "left_knee_joint", "right_knee_joint"}; + static constexpr std::array sides{0, 1, 0, 1}; + for (std::size_t i = 0; i < joints.size(); ++i) { + const auto& motor = *joints[i]; + const int status = motor.status_code(); + const int fault = motor.fault_code(); + if (dm_health_primed_ && fault != dm_fault_[i] && fault != 0) + RCLCPP_ERROR( + status_.get_logger(), + "DM drive %s fault 0x%X (%s), status 0x%X, T_MOS %.0f C, T_rotor %.0f C", + names[i], fault, device::dm_error_text(fault), status, + motor.temperature_mos(), motor.temperature_rotor()); + else if (dm_health_primed_ && fault != dm_fault_[i]) + RCLCPP_INFO( + status_.get_logger(), "DM drive %s fault cleared (status 0x%X)", names[i], + status); + if (dm_health_primed_ && status != dm_status_[i]) { + if (status == 1) + RCLCPP_INFO(status_.get_logger(), "DM drive %s enabled", names[i]); + else if (enabled_[sides[i]]) + RCLCPP_ERROR( + status_.get_logger(), + "DM drive %s left enabled state (status 0x%X) with no error code; the " + "board is still commanding it - check its power and CAN wiring", + names[i], status); + } + dm_status_[i] = status; + dm_fault_[i] = fault; + } + dm_health_primed_ = true; + } + bool dr16_fresh() const { + return wheel_leg_feedback_fresh(now_ns, 100'000'000, dr16_last_ns_); + } + WheelLegDmPairFeedback pair_feedback(std::size_t side) const { + return { + hip_motors_[side].status_code(), knee_motors_[side].status_code(), + hip_motors_[side].fault_code(), knee_motors_[side].fault_code(), + wheel_leg_feedback_available( + now_ns, motor_last_ns_[2 + side], motor_last_ns_[4 + side])}; + } + static device::CanPacket8 + joint_command(const device::DmMotor& motor, Action action, bool available) { + if (action == Action::kClearError) + return motor.clear_error_command(); + if (action == Action::kEnable) + return motor.enable_command(); + if (action == Action::kDisable) + return motor.disable_command(); + return motor.generate_command( + action == Action::kMit && available ? motor.control_torque() : 0.); + } + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() != 8) + return; + if (can == Spec::kCans.kCan0) { + // The supercap answers on CAN0 with its own status frame. + if (data.can_id == 0x300) + supercap_.store_status(data.can_data); + else if ( + !store_motor_feedback(wheel_motors_, 0, data) + && feeder_motor_.match_then_store_status(data.can_id, data.can_data)) + feeder_last_ns_.store(now_ns(), std::memory_order::relaxed); + } else if (can == Spec::kCans.kCan1) { + // Hip DM joints and the TOFSense pairs share CAN1. + // TOF feedback IDs are 0x205/0x206, hip feedback uses the DM master IDs. + if (!store_motor_feedback(hip_motors_, 2, data) + && !tof_left_.match_then_store_status(data.can_id, data.can_data)) + tof_right_.match_then_store_status(data.can_id, data.can_data); + } else if (can == Spec::kCans.kCan2) + store_motor_feedback(knee_motors_, 4, data); + else if (can == Spec::kCans.kCan3 && data.can_id == 0x141) { + if (gimbal_yaw_motor_.store_error_status(data.can_data)) + gimbal_yaw_error_ns_.store(now_ns(), std::memory_order::relaxed); + else + gimbal_yaw_motor_.store_status(data.can_data); + } + } + void configure_motors(WheelLeg& status) { + constexpr auto kMotorIds = std::array{1, 2}; + // Master ID 是反馈帧 ID;官方默认 0,与命令 CAN ID 可以不同。 + // 顺序:左髋、右髋、左膝、右膝。 + const auto feedback_ids = status.get_parameter("dm_master_ids").as_integer_array(); + if (feedback_ids.size() != 4 || !std::ranges::all_of(feedback_ids, [](std::int64_t id) { + return id >= 0 && id <= 0x7FF; + })) + throw std::invalid_argument("dm_master_ids must contain four CAN standard IDs"); + const auto position_max = status.get_parameter("dm_mit_position_max").as_double_array(); + const auto velocity_max = status.get_parameter("dm_mit_velocity_max").as_double_array(); + const auto torque_max = status.get_parameter("dm_mit_torque_max").as_double_array(); + const auto torque_limit = + status.get_parameter("dm_control_torque_max").as_double_array(); + const auto valid_ranges = [](const auto& values) { + return values.size() == 4 && std::ranges::all_of(values, [](double value) { + return std::isfinite(value) && value > 0.0; + }); + }; + if (!valid_ranges(position_max) || !valid_ranges(velocity_max) + || !valid_ranges(torque_max) || !valid_ranges(torque_limit)) + throw std::invalid_argument( + "DM MIT ranges must contain four positive finite values"); + + // The stock M3508 gearbox is removed; the external gearbox provides + // the full reduction from the motor encoder to the wheel. + for (auto&& [motor, id] : std::views::zip(wheel_motors_, kMotorIds)) + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, id} + .set_reversed() + .set_reduction_ratio(15.62) + .enable_multi_turn_angle()); + + const auto configure_joint = [&](device::DmMotor& motor, std::size_t index, + std::uint8_t id) { + motor.configure( + device::DmMotor::Config{device::DmMotor::Type::kDM8009, id} + .set_feedback_id(static_cast(feedback_ids[index])) + .set_limits(position_max[index], velocity_max[index], torque_max[index]) + .set_control_torque_max(torque_limit[index]) + .set_reversed()); + }; + for (std::size_t side = 0; side < kMotorIds.size(); ++side) { + configure_joint(hip_motors_[side], side, kMotorIds[side]); + configure_joint(knee_motors_[side], side + 2, kMotorIds[side]); + } + const auto yaw_zero_point = + status.get_parameter_or("yaw_motor_zero_point", 0); + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(yaw_zero_point))); + // Feeder M3508 drives through an external 27:1 reducer. + feeder_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 3}.set_reduction_ratio( + 27.)); + RCLCPP_INFO( + status_.get_logger(), "DM Master IDs [LH,RH,LK,RK]=[%u,%u,%u,%u]", + static_cast(feedback_ids[0]), static_cast(feedback_ids[1]), + static_cast(feedback_ids[2]), static_cast(feedback_ids[3])); + RCLCPP_INFO( + status_.get_logger(), + "Wheel M3508: kCan0, motor IDs [1,2], feedback IDs [0x201,0x202], command 0x200"); + RCLCPP_INFO( + status_.get_logger(), + "Gimbal LK MG4010: yaw kCan3/0x141, pitch kCan0/0x142, drive sequence clear " + "error (0x9B) -> run (0x88) -> torque (0xA1), zero torque while disabled"); + } + + void update_motor_status() { + const auto update = [&](auto& motors, std::size_t offset) { + for (std::size_t side = 0; side < std::size(motors); ++side) { + const auto index = offset + side; + auto& buffer = motor_feedback_[index]; + const auto previous_sequence = buffer.latest().sequence; + const auto& snapshot = buffer.read(); + if (snapshot.sequence != previous_sequence) { + motors[side].update_status(snapshot.value); + motor_last_ns_[index].store( + static_cast(snapshot.steady_ns), + std::memory_order_relaxed); + } + } + }; + update(wheel_motors_, 0); + update(hip_motors_, 2); + update(knee_motors_, 4); + } + + void publish_imu_feedback() { + const auto& imu = imu_feedback_.read(); + const bool changed = imu.sequence != *imu_sequence_output_; + *imu_sequence_output_ = imu.sequence; + *imu_last_output_ = imu.steady_ns; + imu_last_ns_.store(static_cast(imu.steady_ns), std::memory_order_relaxed); + if (imu.sequence != 0 && changed) { + *imu_quaternion_ = imu.value.orientation.normalized(); + *imu_angular_velocity_ = imu.value.gyro_body; + } + const auto& acceleration = acceleration_feedback_.read(); + *acceleration_sequence_output_ = acceleration.sequence; + *acceleration_last_output_ = acceleration.steady_ns; + if (acceleration.sequence != 0) + *imu_acceleration_ = acceleration.value.specific_force_body_mps2; + } + + template + bool store_motor_feedback(Motor (&motors)[N], std::size_t offset, const View::Can& data) { + for (std::size_t side = 0; side < N; ++side) { + if (!motors[side].matches_feedback(data.can_id, data.can_data)) + continue; + const auto index = offset + side; + motor_feedback_[index].publish( + device::CanPacket8{data.can_data}, static_cast(now_ns())); + return true; + } + return false; + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kDbus && data.uart_data.size() == 18) { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + dr16_last_ns_.store(now_ns(), std::memory_order::relaxed); + } else if (uart == Spec::kUarts.kUart0) { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, + data.uart_data.size()); + } + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + reset_imu_motion_if_requested(); + const auto timestamp = imu_sample_clock_.accelerometer_time(data.timestamp_quarter_us); + if (!timestamp + || !bmi088_.push_accelerometer_sample(data.x, data.y, data.z, *timestamp)) + return; + publish_acceleration_snapshot(); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + reset_imu_motion_if_requested(); + const auto timestamp = imu_sample_clock_.gyroscope_time(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + if (const auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp)) + imu_feedback_.publish( + *snapshot, static_cast(now_ns()), data.timestamp_quarter_us); + publish_acceleration_snapshot(); + } + + void reset_imu_motion_if_requested() { + if (reset_imu_motion_requested_.exchange(false, std::memory_order_acq_rel)) + bmi088_.reset_motion_history(); + } + + void publish_acceleration_snapshot() { + const auto acceleration = bmi088_.acceleration_snapshot(); + if (!acceleration + || (published_acceleration_time_ + && acceleration->timestamp <= *published_acceleration_time_)) + return; + acceleration_feedback_.publish( + *acceleration, static_cast(now_ns()), + static_cast(acceleration->timestamp.time_since_epoch().count())); + published_acceleration_time_ = acceleration->timestamp; + } + + WheelLeg& status_; + bool leg_drive_enabled_; + OutputInterface imu_quaternion_; + OutputInterface imu_angular_velocity_, imu_acceleration_; + OutputInterface acceleration_last_output_, imu_last_output_, + imu_sequence_output_, acceleration_sequence_output_; + OutputInterface feedback_fresh_output_, feedback_available_output_, + dm_control_ready_output_, dr16_fresh_output_; + std::array, 6> feedback_ns_outputs_, + feedback_sequence_outputs_; + InputInterface enable_request_; + InputInterface reset_count_; + std::size_t last_reset_ = 0; + device::DjiMotor wheel_motors_[2]; + device::DmMotor hip_motors_[2], knee_motors_[2]; + device::LkMotor gimbal_yaw_motor_; + device::LkMotorDriveScheduler gimbal_yaw_scheduler_; + std::atomic gimbal_yaw_phase_{ + device::LkMotorDriveScheduler::Phase::kDisabled}; + std::atomic gimbal_yaw_error_ns_{0}; + std::atomic gimbal_drive_requested_{false}; + std::uint8_t gimbal_yaw_last_error_ = 0; + device::DjiMotor feeder_motor_; + device::Supercap supercap_; + // TOFSense pairs on CAN1 in ACTIVE output: left 0x205, right 0x206. + device::Tof tof_left_, tof_right_; + device::Dr16 dr16_; + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + device::Bmi088Ekf bmi088_; + device::WheelLegImuSampleClock imu_sample_clock_; + std::optional published_acceleration_time_; + std::atomic reset_imu_motion_requested_{false}; + std::array, 6> motor_feedback_; + device::WheelLegSensorMailbox imu_feedback_; + device::WheelLegSensorMailbox + acceleration_feedback_; + // Indexed like motor_last_ns_: wheels (CAN0), hips (CAN1), knees (CAN2). + static constexpr std::array kChannelNames{ + "left_wheel", "right_wheel", "left_hip_joint", + "right_hip_joint", "left_knee_joint", "right_knee_joint"}; + static constexpr std::array kChannelBuses{"CAN0", "CAN0", "CAN1", + "CAN1", "CAN2", "CAN2"}; + static constexpr std::int64_t kChannelSilentNs = 100'000'000; + std::array channel_silent_{}; + std::array channel_silent_since_{}; + // Per DM drive, ordered {left hip, right hip, left knee, right knee}. + std::array dm_status_{}, dm_fault_{}; + bool dm_health_primed_ = false; + std::array, 6> motor_last_ns_{}; + std::atomic imu_last_ns_{0}, dr16_last_ns_{0}, feeder_last_ns_{0}; + std::int64_t timeout_ns_ = 40'000'000; + std::array enabled_{}, mit_sent_{}; + bool ready_ = false; + std::atomic requested_snapshot_{false}; + WheelLegDmSideSchedulers command_schedulers_; + WheelLegNormalActionGate normal_action_gate_; + enum class FeedbackWarningState { kFresh, kMissing, kStale }; + FeedbackWarningState feedback_warning_state_ = FeedbackWarningState::kFresh; + bool feedback_error_active_ = false; + std::int64_t last_feedback_error_ns_ = 0; + bool previous_both_down_ = false; + std::unique_ptr board_; + }; + + // Gimbal board: pitch LK4010i10 on CAN0 (0x142), both friction M3508 on + // CAN1 sharing one 0x200 frame, and the gimbal-mounted BMI088. + struct TopBoard final : librmcs::board::RmcsBoardLite::Callback { + TopBoard(WheelLeg& status, rmcs_executor::Component& command, const std::string& serial) + : status_(status) + , pitch_motor_(status, command, "/gimbal/pitch") + , left_friction_(status, command, "/gimbal/left_friction") + , right_friction_(status, command, "/gimbal/right_friction") + // Gimbal BMI088: raw (x, y, z) reads as (y, -x, z), i.e. +90 deg about body Z. + , bmi088_( + device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2, Eigen::Vector3d::UnitZ()} + .toRotationMatrix(), + }) { + status.register_output("/gimbal/yaw/velocity_imu", yaw_velocity_, 0.0); + status.register_output("/gimbal/pitch/velocity_imu", pitch_velocity_, 0.0); + const auto pitch_zero_point = + status.get_parameter_or("pitch_motor_zero_point", 0); + pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(pitch_zero_point))); + left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reversed().set_reduction_ratio( + 1.)); + right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1}.set_reduction_ratio( + 1.)); + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial, options); + } + + [[nodiscard]] std::int64_t pitch_raw_angle() const { return pitch_motor_.last_raw_angle(); } + [[nodiscard]] std::string status_text() const { + return std::format( + " gimbal_pitch: can_id=0x142 motor_state=0x{:02X} error=0x{:02X}({}) " + "bus={:.1f}V drive={}\n", + static_cast(pitch_motor_.motor_state()), + static_cast(pitch_motor_.error_state()), + device::lk_error_text(pitch_motor_.error_state()), pitch_motor_.bus_voltage(), + device::LkMotorDriveScheduler::phase_name( + pitch_phase_.load(std::memory_order::relaxed))); + } + + void update() { + pitch_motor_.update_status(); + log_lk_fault_transition( + status_.get_logger(), "/gimbal/pitch", pitch_motor_.motor_state(), + pitch_motor_.error_state(), pitch_last_error_, + pitch_scheduler_.startup_input_timeout(lk_feedback(pitch_motor_, pitch_error_ns_))); + pitch_phase_.store(pitch_scheduler_.phase(), std::memory_order::relaxed); + left_friction_.update_status(); + right_friction_.update_status(); + + const double pitch_angle = pitch_motor_.angle(); + if (auto snapshot = bmi088_.snapshot()) { + *yaw_velocity_ = snapshot->gyro_body.z(); + *pitch_velocity_ = snapshot->gyro_body.y(); + status_.tf_->set_transform( + snapshot->orientation.conjugate()); + } + status_.tf_->set_state( + pitch_angle); + if (status_.bottom_board_) + status_.tf_ + ->set_state( + status_.bottom_board_->yaw_angle()); + } + + void command_update() { + // Gate comes from the bottom board, which already folds in the both-down check. + const bool requested = + status_.bottom_board_ && status_.bottom_board_->gimbal_drive_requested(); + auto builder = board_->start_transmit(); + const auto pitch_action = pitch_scheduler_.next( + requested, lk_feedback(pitch_motor_, pitch_error_ns_), LkDriveClock::now()); + if (auto packet = lk_command(pitch_motor_, pitch_action)) + builder.can_transmit( + Spec::kCans.kCan0, {.can_id = 0x142, .can_data = packet->as_bytes()}); + // Left friction is ID 2 and right is ID 1; both share 0x200. + auto friction = device::CanPacket8{std::uint64_t{0}}; + friction << right_friction_ << left_friction_; + builder.can_transmit( + Spec::kCans.kCan1, {.can_id = 0x200, .can_data = friction.as_bytes()}); + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() != 8) + return; + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x142) { + if (pitch_motor_.store_error_status(data.can_data)) + pitch_error_ns_.store(now_ns(), std::memory_order::relaxed); + else + pitch_motor_.store_status(data.can_data); + } + } else if (can == Spec::kCans.kCan1) { + if (!left_friction_.match_then_store_status(data.can_id, data.can_data)) + right_friction_.match_then_store_status(data.can_id, data.can_data); + } + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + } + + static std::int64_t now_ns() { + return std::chrono::duration_cast( + LkDriveClock::now().time_since_epoch()) + .count(); + } + + WheelLeg& status_; + OutputInterface yaw_velocity_, pitch_velocity_; + device::LkMotor pitch_motor_; + device::LkMotorDriveScheduler pitch_scheduler_; + std::atomic pitch_phase_{ + device::LkMotorDriveScheduler::Phase::kDisabled}; + std::atomic pitch_error_ns_{0}; + std::uint8_t pitch_last_error_ = 0; + device::DjiMotor left_friction_, right_friction_; + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; + std::unique_ptr board_; + }; + + std::unique_ptr remote_control_; + std::shared_ptr command_; + OutputInterface tf_; + std::unique_ptr bottom_board_; + std::unique_ptr top_board_; + rclcpp::Service::SharedPtr status_service_; +}; +} // namespace rmcs_core::hardware +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::WheelLeg, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/test/wheel_leg_encoder_unwrap.cpp b/rmcs_ws/src/rmcs_core/test/wheel_leg_encoder_unwrap.cpp new file mode 100644 index 000000000..380113795 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/test/wheel_leg_encoder_unwrap.cpp @@ -0,0 +1,71 @@ +#include "controller/chassis/wheel_leg/encoder_unwrap.hpp" +#include "controller/chassis/wheel_leg/joint_coordinates.hpp" +#include +#include +#include +#include + +using namespace rmcs_core::controller::chassis; + +namespace { + +void require(bool condition, const char* message) { + if (condition) + return; + std::cerr << message << '\n'; + std::exit(1); +} + +bool close(double a, double b, double tolerance = 1e-9) { return std::abs(a - b) <= tolerance; } + +} // namespace + +int main() { + constexpr double span = 2 * std::numbers::pi; + + // The current drive configuration uses P_MAX = pi, so the feedback span is + // 2 pi. The raw angle crosses from the positive to the negative boundary. + WheelLegEncoderUnwrap unwrap; + require(unwrap.update(3.0, span), "first frame anchors"); + require(close(unwrap.value(), 3.0), "first value"); + require(!unwrap.update(3.1, span), "small step does not re-anchor"); + require(!unwrap.update(-3.1, span), "drive wrap does not re-anchor"); + require(close(unwrap.value(), 3.1 + span - 6.2), "drive wrap stays continuous"); + require(!unwrap.update(-3.0, span), "post-wrap step does not re-anchor"); + require(close(unwrap.value(), 3.2 + span - 6.2), "post-wrap angle stays continuous"); + + // The helper must remain span-driven for a drive using a non-2-pi range. + constexpr double nonstandard_span = 25.0; + WheelLegEncoderUnwrap nonstandard; + require(nonstandard.update(12.4, nonstandard_span), "nonstandard span anchors"); + require(!nonstandard.update(12.49, nonstandard_span), "nonstandard small step"); + require(!nonstandard.update(-12.49, nonstandard_span), "nonstandard wrap"); + require(close(nonstandard.value(), 12.51), "nonstandard span stays continuous"); + + // A 2 pi assumption is wrong for the nonstandard span and leaves a residual. + const double old_step = std::remainder(-12.5 - 12.5, 2 * std::numbers::pi); + require( + close(old_step, 8 * std::numbers::pi - 25.0, 1e-12), + "2 pi assumption residual is 8 pi - 25"); + require(!close(old_step, 0.0, 1e-6), "2 pi assumption is not exact"); + + // A step no joint can make between two 1 kHz frames re-anchors to the raw value. + WheelLegEncoderUnwrap reanchor; + require(reanchor.update(3.0, span), "first frame anchors before re-anchor test"); + require(reanchor.update(0.0, span), "impossible step re-anchors"); + require(close(reanchor.value(), 0.0), "re-anchored value is the raw value"); + require(!reanchor.update(std::numbers::pi / 4, span), "quarter turn is still a tracked step"); + + // continuous_legs only picks the relative branch; it must not add 2 pi to the + // absolute hip angle any more. + const LegBounds bounds{{{-0.14, 1.43}, {-0.14, 1.43}}}; + const LegVector measured{12.4, 12.9, -12.4, -12.9}; + const auto continuous = continuous_legs(measured, bounds); + require(close(continuous[0], 12.4), "left hip keeps its unwrapped value"); + require(close(continuous[1], 12.9), "left crank keeps its unwrapped value"); + require(close(continuous[2], -12.4), "right hip keeps its unwrapped value"); + require(close(continuous[3], -12.9), "right crank keeps its unwrapped value"); + + std::cout << "wheel leg encoder unwrap passed\n"; + return 0; +} diff --git a/rmcs_ws/src/rmcs_core/test/wheel_leg_state_estimator.cpp b/rmcs_ws/src/rmcs_core/test/wheel_leg_state_estimator.cpp new file mode 100644 index 000000000..b76c010be --- /dev/null +++ b/rmcs_ws/src/rmcs_core/test/wheel_leg_state_estimator.cpp @@ -0,0 +1,161 @@ +#include "controller/chassis/wheel_leg/joint_coordinates.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_executor { +class Executor { +public: + static void pair(const std::vector>& list) { + Component::OutputInfoMap info; + for (const auto& component : list) + for (const auto& output : component->output_list_) + if (!info.emplace(output.name, Component::OutputInfo{output.type, output.kind}) + .second) + throw std::runtime_error("Duplicate test output: " + output.name); + for (const auto& component : list) + component->before_pairing(info); + for (const auto& component : list) + for (auto& input : component->input_list_) { + bool found = false; + for (const auto& source : list) + for (auto& output : source->output_list_) + if (input.name == output.name) { + if (input.type != output.type || input.kind != output.kind) + throw std::runtime_error("Test binding type: " + input.name); + input.bind(input.binding, output.binding); + found = true; + } + if (!found && input.required) + throw std::runtime_error("Missing test input: " + input.name); + } + for (const auto& component : list) + component->before_updating(); + } + template + static const T& + read(const std::vector>& list, const std::string& name) { + for (const auto& component : list) + for (const auto& output : component->output_list_) + if (output.name == name && output.type == typeid(T)) + return *static_cast(output.binding); + throw std::runtime_error("Missing test result: " + name); + } +}; +} // namespace rmcs_executor + +using namespace rmcs_core::controller::chassis; + +class TestFeedback final : public rmcs_executor::Component { +public: + TestFeedback() { + for (std::size_t i = 0; i < 6; ++i) { + const auto prefix = std::string{"/wheel_leg/"} + kMotorNames[i]; + register_output(prefix + "/angle", angle[i], 0.); + register_output(prefix + "/velocity", velocity[i], 0.); + if (i < 4) + register_output(prefix + "/angle_wrap_span", wrap_span[i], 2 * std::numbers::pi); + } + register_output("/wheel_leg/imu/quaternion", orientation, Eigen::Quaterniond::Identity()); + register_output("/wheel_leg/imu/angular_velocity", gyro, Eigen::Vector3d::Zero()); + register_output("/wheel_leg/feedback_available", available, true); + register_output("/chassis/reset_count", reset, std::size_t{0}); + } + void update() override {} + + std::array, 6> angle, velocity; + std::array, 4> wrap_span; + OutputInterface orientation; + OutputInterface gyro; + OutputInterface available; + OutputInterface reset; +}; + +namespace { +void require(bool condition, const std::string& message) { + if (!condition) + throw std::runtime_error(message); +} +} // namespace + +int main(int argc, char** argv) { + using rmcs_executor::Component; + using rmcs_executor::Executor; + rclcpp::init(argc, argv); + try { + pluginlib::ClassLoader loader{"rmcs_executor", "rmcs_executor::Component"}; + Component::initializing_component_name = "state_estimator_feedback"; + auto feedback = std::make_shared(); + Component::initializing_component_name = "wheel_leg_state_estimator"; + auto estimator = + loader.createSharedInstance("rmcs_core::controller::chassis::WheelLegStateEstimator"); + const std::vector> components{feedback, estimator}; + Executor::pair(components); + + const auto set = [&](std::size_t index, double value) { *feedback->angle[index] = value; }; + // Left hip and knee are 0.5 rad apart in the API frame, so the model + // relative angle stays inside the 1 kHz chain bounds across the wrap. + set(0, 3.0); + set(1, 3.5); + set(2, 2.0); + set(3, 1.5); + const auto step = [&] { + feedback->update(); + estimator->update(); + return Executor::read(components, "/wheel_leg/state/position"); + }; + require(std::abs(step()[0] - (-1.4)) < 1e-9, "first left hip model angle"); + require(Executor::read(components, "/wheel_leg/state/valid"), "state is valid"); + + // The current drive wraps at +/- pi, so the raw value jumps by 2 pi while + // the physical angle only moves a small amount. + set(0, -3.1); + set(1, -2.6); + auto q = step(); + require( + std::abs(q[0] - (-(3.1 + 2 * std::numbers::pi - 6.2) + 1.6)) < 1e-9, + "left hip unwraps by the configured drive span"); + require( + std::abs(q[1] - (q[0] + (kMotorOffsets[1] - kMotorOffsets[0] - 0.5))) < 1e-9, + "left crank keeps the closed-chain offset"); + + set(0, -3.0); + set(1, -2.5); + q = step(); + require( + std::abs(q[0] - (-(3.2 + 2 * std::numbers::pi - 6.2) + 1.6)) < 1e-9, + "left hip stays continuous after the wrap"); + + // A drive restart re-anchors, and the state stays finite. + set(0, 0.5); + set(1, 1.0); + q = step(); + require(std::abs(q[0] - (-0.5 + 1.6)) < 1e-9, "left hip re-anchors to the raw value"); + require(Executor::read(components, "/wheel_leg/state/valid"), "state stays valid"); + + set(0, -3.1); + set(1, -2.6); + ++*feedback->reset; + q = step(); + require( + std::abs(q[0] - (3.1 + 1.6)) < 1e-9, + "double-down reset must discard the previous encoder winding"); + + std::cout << "wheel leg state estimator unwrap passed\n"; + rclcpp::shutdown(); + return 0; + } catch (const std::exception& error) { + std::cerr << error.what() << '\n'; + rclcpp::shutdown(); + return 1; + } +} diff --git a/rmcs_ws/src/rmcs_core/test/wheelbipe_jump_components.cpp b/rmcs_ws/src/rmcs_core/test/wheelbipe_jump_components.cpp new file mode 100644 index 000000000..f741a5e35 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/test/wheelbipe_jump_components.cpp @@ -0,0 +1,334 @@ +#include "controller/chassis/wheel_leg/joint_coordinates.hpp" +#include "controller/chassis/wheel_leg/model/jump_v4.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_executor { +class Executor { +public: + static std::shared_ptr partner(const std::shared_ptr& component) { + if (component->partner_component_list_.size() != 1) + throw std::runtime_error("Expected one chassis reset partner"); + return component->partner_component_list_.front(); + } + static void pair(const std::vector>& list) { + Component::OutputInfoMap info; + for (const auto& component : list) + for (const auto& output : component->output_list_) + if (!info.emplace(output.name, Component::OutputInfo{output.type, output.kind}) + .second) + throw std::runtime_error("Duplicate test output: " + output.name); + for (const auto& component : list) + component->before_pairing(info); + for (const auto& component : list) + for (auto& input : component->input_list_) { + bool found = false; + for (const auto& source : list) + for (auto& output : source->output_list_) + if (input.name == output.name) { + if (input.type != output.type || input.kind != output.kind) + throw std::runtime_error("Test binding type: " + input.name); + input.bind(input.binding, output.binding); + found = true; + } + if (!found && input.required) + throw std::runtime_error("Missing test input: " + input.name); + } + for (const auto& component : list) + component->before_updating(); + } + template + static const T& + read(const std::vector>& list, const std::string& name) { + for (const auto& component : list) + for (const auto& output : component->output_list_) + if (output.name == name && output.type == typeid(T)) + return *static_cast(output.binding); + throw std::runtime_error("Missing test result: " + name); + } +}; +} // namespace rmcs_executor + +using namespace rmcs_core::controller::chassis; + +class TestRemote final : public rmcs_executor::Component { +public: + TestRemote() { + register_output("/remote/joystick/right", joystick, Eigen::Vector2d::Zero()); + register_output("/remote/switch/left", left, rmcs_msgs::Switch::UNKNOWN); + register_output("/remote/switch/right", right, rmcs_msgs::Switch::UNKNOWN); + register_output("/remote/keyboard", keyboard, rmcs_msgs::Keyboard::zero()); + register_output("/remote/rotary_knob", knob, 0.); + register_output("/wheel_leg/dr16_fresh", fresh, false); + register_output("/predefined/timestamp", timestamp, std::chrono::steady_clock::now()); + } + void update() override {} + OutputInterface joystick; + OutputInterface left, right; + OutputInterface keyboard; + OutputInterface knob; + OutputInterface fresh; + OutputInterface timestamp; +}; + +class TestFeedback final : public rmcs_executor::Component { +public: + TestFeedback() { + register_output("/predefined/update_count", tick, std::size_t{0}); + register_output("/predefined/update_rate", rate, 1000.); + register_output("/chassis/reset_count", reset, std::size_t{0}); + register_output("/chassis/jump/request_count", request, std::size_t{0}); + register_output("/chassis/control_enable", enabled, false); + register_output("/chassis/control_height", height, .305); + register_output("/chassis/control_velocity", command); + command->vector.setZero(); + register_output("/chassis/control_mode", mode, rmcs_msgs::ChassisMode::AUTO); + register_output("/wheel_leg/state/position", q, kNominalPosition); + register_output("/wheel_leg/state/velocity", dq, JointVector{}); + register_output("/wheel_leg/state/valid", valid, true); + register_output("/wheel_leg/dm_control_ready", ready, true); + register_output("/wheel_leg/state/gravity", gravity, Eigen::Vector3d{0., 0., -1.}); + register_output("/wheel_leg/state/attitude", attitude, Eigen::Vector3d::Zero()); + register_output("/wheel_leg/imu/angular_velocity", gyro, Eigen::Vector3d::Zero()); + register_output("/wheel_leg/imu/acceleration", acceleration, Eigen::Vector3d{0., 0., 9.81}); + register_output("/wheel_leg/rl/valid", action_valid, 1.); + register_output("/wheel_leg/rl/healthy", healthy, 1.); + register_output("/wheel_leg/rl/action_seq", sequence, std::size_t{0}); + register_output("/wheel_leg/rl/action_steady_ns", stamp, std::uint64_t{0}); + register_output("/wheel_leg/rl/policy_rate", policy_rate, 50.); + register_output( + "/wheel_leg/rl/model_spec_sha256", model, + std::string{wheelbipe_jump_v4::kModelSpecSha256}); + register_output( + "/wheel_leg/rl/observation_contract_sha256", observation, + std::string{wheelbipe_jump_v4::kObservationSha256}); + for (int i = 0; i < 6; ++i) { + const auto prefix = std::string{"/wheel_leg/"} + kMotorNames[i]; + const auto action = std::string{"/wheel_leg/rl/raw/"} + kMotorNames[i]; + const double value = i == 0 || i == 3 ? .4 : i == 1 || i == 2 ? -.4 : 0.; + register_output(prefix + "/max_torque", caps[i], i < 4 ? 40. : 4.5); + register_output(action, mapped[i], i < 4 ? value * .5 : 0.); + register_output(action + "/raw", raw[i], value); + register_output(action + "/scale", scale[i], i < 4 ? .5 : 10.); + register_output(action + "/offset", offset[i], 0.); + register_output( + action + "/clip_min", low[i], + i < 4 ? -std::numeric_limits::infinity() : -90.); + register_output( + action + "/clip_max", high[i], + i < 4 ? std::numeric_limits::infinity() : 90.); + } + } + void update() override { + ++*tick; + ++*sequence; + *stamp = std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); + } + OutputInterface tick, reset, request, sequence; + OutputInterface stamp; + OutputInterface rate, height, action_valid, healthy, policy_rate; + OutputInterface enabled, valid, ready; + OutputInterface mode; + OutputInterface command; + OutputInterface q, dq; + OutputInterface gravity, attitude, gyro, acceleration; + OutputInterface model, observation; + std::array, 6> caps, mapped, raw, scale, offset, low, high; +}; + +void require(bool condition, const char* message) { + if (!condition) + throw std::runtime_error(message); +} + +int main(int argc, char** argv) { + using rmcs_executor::Component; + using rmcs_executor::Executor; + try { + rclcpp::init(argc, argv); + pluginlib::ClassLoader loader{"rmcs_executor", "rmcs_executor::Component"}; + if (argc > 1 && std::string_view{argv[1]} == "--model-check") { + Component::initializing_component_name = "rl_bridge"; + auto bridge = loader.createSharedInstance("rmcs_rl::RlBridge"); + std::cout + << "Native ONNX loading and metadata validation passed; no hardware instantiated\n"; + rclcpp::shutdown(); + return 0; + } + Component::initializing_component_name = "jump_test_feedback"; + auto feedback = std::make_shared(); + Component::initializing_component_name = "wheel_leg_rl_controller"; + auto controller = + loader.createSharedInstance("rmcs_core::controller::chassis::WheelLegRlController"); + Component::initializing_component_name = "wheel_leg_observation"; + auto observation = + loader.createSharedInstance("rmcs_core::controller::chassis::WheelLegObservation"); + const std::vector> components{feedback, controller, observation}; + Executor::pair(components); + const auto active = [&] { + return Executor::read(components, "/chassis/jump/active"); + }; + const auto effort = [&] { + return Executor::read(components, "/wheel_leg/rl/tuck_compensation"); + }; + const auto check_clear = [&] { + require( + !active() && effort() == JointVector{}, "Jump state or compensation survives stop"); + require( + Executor::read>(components, "/wheel_leg/rl/input/context")[0] + == 1., + "Jump context survives stop"); + for (const auto name : kMotorNames) + require( + Executor::read( + components, std::string{"/wheel_leg/"} + name + "/control_torque") + == 0., + "Motor effort survives stop"); + }; + const auto step = [&] { + feedback->update(); + controller->update(); + observation->update(); + }; + step(); + *feedback->enabled = true; + for (int i = 0; i < 40; ++i) + step(); + require(effort() == JointVector{}, "Ordinary compensation"); + ++*feedback->request; + bool saw_flight = false, saw_context = false; + int last_phase = -1, phase_ticks = 0; + for (int i = 0; i < 1200; ++i) { + step(); + if (i == 0) + require(active(), "Upright fresh request was rejected"); + if (i == 10) + ++*feedback->request; + const int phase = Executor::read(components, "/chassis/jump/state"); + phase_ticks = phase == last_phase ? phase_ticks + 1 : 1; + last_phase = phase; + if (phase == 3 && effort()[0] > 0.) + saw_flight = true; + if (phase != 3 && phase_ticks >= 5) + require(effort() == JointVector{}, "Compensation outside flight clock"); + if (Executor::read>( + components, "/wheel_leg/rl/features/auxiliary")[4] + == 1.) + saw_context = true; + for (const auto name : kMotorNames) + require( + std::abs( + Executor::read( + components, std::string{"/wheel_leg/"} + name + "/control_torque")) + <= 40., + "Final effort cap"); + } + require(saw_flight && saw_context, "Flight compensation or actor context not wired"); + require( + Executor::read(components, "/chassis/jump/accepted_count") == 1, + "Busy request repeats"); + *feedback->ready = false; + step(); + check_clear(); + *feedback->ready = true; + ++*feedback->request; + step(); + require(active(), "Fresh request after readiness returns"); + *feedback->healthy = 0.; + step(); + check_clear(); + require( + Executor::read(components, "/wheel_leg/rl/fault_latched"), + "Inference fault not latched"); + *feedback->enabled = false; + ++*feedback->reset; + *feedback->healthy = 1.; + step(); + check_clear(); + require(!Executor::read(components, "/wheel_leg/rl/fault_latched"), "Reset failed"); + *feedback->enabled = true; + for (int i = 0; i < 40; ++i) + step(); + ++*feedback->request; + step(); + require(active(), "Fresh request after reset"); + *feedback->enabled = false; + step(); + check_clear(); + bool rejected = false; + *feedback->model = "old-model"; + try { + controller->before_updating(); + } catch (const std::invalid_argument&) { + rejected = true; + } + require(rejected, "Wrong model identity accepted"); + + Component::initializing_component_name = "reset_test_remote"; + auto remote = std::make_shared(); + Component::initializing_component_name = "wheel_leg_chassis_controller"; + auto chassis = loader.createSharedInstance( + "rmcs_core::controller::chassis::WheelLegChassisController"); + auto reset_source = Executor::partner(chassis); + const std::vector> reset_components{ + remote, reset_source, chassis}; + Executor::pair(reset_components); + const auto reset_step = [&] { + reset_source->update(); + chassis->update(); + }; + const auto count = [&] { + return Executor::read(reset_components, "/chassis/reset_count"); + }; + reset_step(); + require(count() == 1, "Missing remote did not reset"); + *remote->fresh = true; + *remote->left = *remote->right = rmcs_msgs::Switch::MIDDLE; + reset_step(); + *remote->left = *remote->right = rmcs_msgs::Switch::DOWN; + reset_step(); + require(count() == 2, "Double-down after remote recovery did not reset"); + reset_step(); + require(count() == 2, "Holding double-down repeated reset"); + *remote->left = *remote->right = rmcs_msgs::Switch::MIDDLE; + reset_step(); + require( + Executor::read(reset_components, "/chassis/control_enable"), + "Double-middle did not rearm"); + *remote->left = *remote->right = rmcs_msgs::Switch::DOWN; + reset_step(); + require(count() == 3, "Double-down after arming did not reset"); + *remote->left = *remote->right = rmcs_msgs::Switch::MIDDLE; + reset_step(); + require( + Executor::read(reset_components, "/chassis/control_enable"), + "Double-middle after reset did not rearm"); + std::cout + << "Native components: request, actor context, flight compensation, busy edge, drive " + "loss, inference fault, reset, double-down rearm and disarm passed; no hardware " + "instantiated\n"; + rclcpp::shutdown(); + } catch (const std::exception& error) { + std::cerr << error.what() << '\n'; + return 1; + } +} diff --git a/rmcs_ws/src/rmcs_core/test/wheelbipe_jump_parity.py b/rmcs_ws/src/rmcs_core/test/wheelbipe_jump_parity.py new file mode 100755 index 000000000..619a32b22 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/test/wheelbipe_jump_parity.py @@ -0,0 +1,185 @@ +#!/usr/bin/env python3 +"""Compare compiled deployment control with the actual pinned Python training code.""" + +import argparse +import hashlib +import io +import json +import subprocess +import sys +import tarfile +import tempfile +from pathlib import Path + +import numpy as np +import torch + +ROOT = Path(__file__).resolve().parents[4] + + +def trace(binary, option, rows): + stream = io.StringIO() + np.savetxt(stream, rows, fmt="%.17g") + result = subprocess.run( + [str(binary), option], + input=stream.getvalue(), + text=True, + capture_output=True, + check=True, + ) + return np.loadtxt(io.StringIO(result.stdout), ndmin=2) + + +def compare(training_repo, revision, binary): + contract = json.loads( + ( + ROOT / "rmcs_ws/src/rmcs_bringup/config/contracts/wheelbipe_jump_v4.json" + ).read_text() + ) + commit = subprocess.check_output( + ["git", "-C", str(training_repo), "rev-parse", revision + "^{commit}"], + text=True, + ).strip() + assert commit == contract["training_source_commit"], "Wrong training source" + paths = [ + "src/wheeled_tasks", + "contracts", + "model/wheel_leg_v2.0.2/friction_v1/isaac/gas_spring_model.json", + "model/gas_spring_identified_symmetric_20261028/model.json", + ] + archive = subprocess.check_output( + ["git", "-C", str(training_repo), "archive", commit, *paths] + ) + with tempfile.TemporaryDirectory(prefix="wheelbipe-jump-parity-") as directory: + root = Path(directory) + with tarfile.open(fileobj=io.BytesIO(archive)) as bundle: + bundle.extractall(root, filter="data") + sys.path.insert(0, str(root / "src")) + from wheeled_tasks.chassis.gas_spring_compensation import GasSpringCompensation + from wheeled_tasks.wheelbipe_port.control import tuck_spring_effort + from wheeled_tasks.wheelbipe_port.mixed_curriculum import TASKS, TaskSchedule + from wheeled_tasks.wheelbipe_port.model import PairedTargets + + gas_path = "model/wheel_leg_v2.0.2/friction_v1/isaac/gas_spring_model.json" + gas = json.loads((root / gas_path).read_text()) + assert ( + hashlib.sha256((root / gas_path).read_bytes()).hexdigest() + == contract["gas_model_sha256"] + ) + settings = { + "enabled": True, + "model_file": gas_path, + "model_sha256": contract["gas_model_sha256"], + "gain": 1.0, + "force_extrapolation": "positive_c1_endpoint_trend_v1", + "physical_curve_outside_geometry": "hold_geometry_endpoint", + "relative_soft_limits": { + "minimum": contract["real_state_soft_limits_rad"][0], + "maximum": contract["real_state_soft_limits_rad"][1], + "margin": 0.03, + "projection": "outward_relative_torque_v1", + }, + } + spring = GasSpringCompensation( + settings, root=root, device="cpu", dtype=torch.float64 + ) + paired = PairedTargets(gas, "cpu", overtravel_rad=0.2) + generator = np.random.default_rng(119) + n = 4096 + requested = torch.tensor(generator.integers(0, 2, n), dtype=torch.bool) + flight = torch.tensor(generator.integers(0, 2, n), dtype=torch.bool) & requested + desired = torch.tensor(generator.uniform(-14.0, 14.0, (n, 4))) + q = torch.tensor(generator.uniform(-7.0, 7.0, (n, 4))) + relative = torch.tensor(generator.uniform(-0.35, 1.65, (n, 2))) + q[:, 1] = q[:, 0] + relative[:, 0] + q[:, 3] = q[:, 2] - relative[:, 1] + dq = torch.tensor(generator.uniform(-18.0, 18.0, (n, 4))) + target = paired(desired, q, extended_mask=requested) + pd = 80.0 * (target - q) - 1.5 * dq + holding, _ = spring.evaluate(q) + extra, gate = tuck_spring_effort( + q, + target, + pd, + flight, + holding, + contract["control"]["tuck_spring_compensation"], + ) + limited = (pd + extra).clamp(-40.0, 40.0).unflatten(-1, (2, 2)) + coefficients = q.new_tensor([[-1.0, 1.0], [1.0, -1.0]]) + actual_relative = (q.unflatten(-1, (2, 2)) * coefficients).sum(-1) + outward = (limited * coefficients).sum(-1) + low, high = contract["real_state_soft_limits_rad"] + blocked = ((actual_relative >= high - 0.03) & (outward > 0.0)) | ( + (actual_relative <= low + 0.03) & (outward < 0.0) + ) + final = ( + limited - coefficients * torch.where(blocked, outward / 2.0, 0.0)[..., None] + ).flatten(-2) + expected = torch.cat((target, pd, holding, extra, gate, final), -1).numpy() + rows = np.column_stack((requested, flight, desired, q, dq)) + actual = trace(binary, "--trace", rows) + error = np.max(np.abs(actual - expected), axis=0) + np.testing.assert_allclose(actual, expected, atol=1e-8, rtol=1e-10) + assert gate.max() == 1.0 and (gate > 0).sum() > 100 + assert np.max(np.abs(final.numpy())) <= 40.0 + spec = json.loads( + (root / "contracts/wheelbipe_foundation_jump_v4.json").read_text() + ) + count = 1024 + schedule = TaskSchedule(count, "cpu", 119, spec) + schedule.role.fill_(TASKS.index("jump")) + schedule.apex = torch.linspace(0.42, 0.54, count, dtype=torch.float64) + schedule.delay = torch.full((count,), 0.8, dtype=torch.float64) + elapsed = torch.linspace( + 0.0, + contract["request"]["outcome_seconds"] + - contract["request"]["nominal_request_delay_s"] + - 0.001, + count, + dtype=torch.float64, + ) + reference = schedule.reference(elapsed + schedule.delay) + context = schedule.context( + torch.tensor( + [[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float64 + ).expand(count, -1), + elapsed + schedule.delay, + ) + states = torch.where( + reference["crouch"], + 1, + torch.where(reference["push"], 2, torch.where(reference["flight"], 3, 4)), + ) + expected_clock = torch.cat( + (reference["height"][:, None], states[:, None], context), -1 + ).numpy() + actual_clock = trace( + binary, "--reference", np.column_stack((schedule.apex, elapsed)) + ) + np.testing.assert_allclose(actual_clock, expected_clock, atol=1e-12, rtol=1e-12) + return { + "training_commit": commit, + "control_samples": n, + "reference_samples": count, + "max_absolute_error": float(error.max()), + "max_reference_error": float(np.max(np.abs(actual_clock - expected_clock))), + "compensation_positive_cases": int((gate > 0).sum()), + "actual_limit_blocked_cases": int(blocked.sum()), + "physical_robot_tested": False, + "learned_behavior_tested": False, + } + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--training-repo", type=Path, required=True) + parser.add_argument("--revision", required=True) + parser.add_argument("--test-binary", type=Path, required=True) + args = parser.parse_args() + print( + json.dumps( + compare(args.training_repo, args.revision, args.test_binary.resolve()), + indent=2, + ) + ) diff --git a/rmcs_ws/src/rmcs_core/test/wheelbipe_jump_test.cpp b/rmcs_ws/src/rmcs_core/test/wheelbipe_jump_test.cpp new file mode 100644 index 000000000..3b1dbeb53 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/test/wheelbipe_jump_test.cpp @@ -0,0 +1,115 @@ +#include "controller/chassis/wheel_leg/joint_solver.hpp" +#include "controller/chassis/wheel_leg/jump_command.hpp" +#include "controller/chassis/wheel_leg/jump_control.hpp" +#include "controller/chassis/wheel_leg/jump_trigger.hpp" +#include "controller/chassis/wheel_leg/policy/selection.hpp" +#include +#include +#include +#include +#include + +using namespace rmcs_core::controller::chassis; +using namespace rmcs_core::controller::chassis::wheel_leg; + +void require(bool condition, const char* message) { + if (!condition) { + std::cerr << message << '\n'; + std::exit(1); + } +} + +int main(int argc, char** argv) { + std::cout << std::setprecision(17); + if (argc == 2 && std::string_view{argv[1]} == "--trace") { + bool requested, flight; + while (std::cin >> requested >> flight) { + LegVector desired{}, q{}, dq{}; + for (auto* row : {&desired, &q, &dq}) + for (auto& x : *row) + std::cin >> x; + require(bool(std::cin), "Truncated trace input"); + const auto target = paired_targets(desired, q, jump_target_bounds(requested)); + LegVector pd{}; + for (int i = 0; i < 4; ++i) + pd[i] = 80. * (target[i] - q[i]) - 1.5 * dq[i]; + const auto holding = jump_spring_holding(q); + const auto extra = jump_tuck_effort(q, target, pd, flight && requested); + JointVector effort{}, position{}, caps{40., 40., 40., 40., 4.5, 4.5}; + for (int i = 0; i < 4; ++i) { + effort[i] = pd[i] + extra.torque[i]; + position[i] = q[i]; + } + const LegBounds bounds{ + {{wheelbipe_jump_v4::kMechanicalMin, wheelbipe_jump_v4::kMechanicalMax}, + {wheelbipe_jump_v4::kMechanicalMin, wheelbipe_jump_v4::kMechanicalMax}}}; + const auto final = + motor_effort(JointSolver::constrain(effort, position, caps, .03, bounds)); + for (const auto& row : {target, pd, holding, extra.torque}) + for (const auto x : row) + std::cout << x << ' '; + for (const auto x : extra.gate) + std::cout << x << ' '; + for (int i = 0; i < 4; ++i) + std::cout << final[i] << ' '; + std::cout << '\n'; + } + return 0; + } + if (argc == 2 && std::string_view{argv[1]} == "--reference") { + double apex, t; + while (std::cin >> apex >> t) { + JumpCommand command{apex}; + command.request(0.); + command.update(t); + std::cout << command.height(t) << ' ' << command.state(t) << ' '; + for (const auto x : command.context(t)) + std::cout << x << ' '; + std::cout << '\n'; + } + return 0; + } + require(!jump_requested(false, true, false, false, true, true), "Disarmed trigger"); + require(jump_requested(true, true, false, false, true, false), "Ctrl rising trigger"); + require(!jump_requested(true, true, true, false, true, false), "Held Ctrl repeats"); + require(jump_requested(true, false, false, true, true, true), "Switch rising trigger"); + JumpCommand jump; + require(!jump.request(std::numeric_limits::quiet_NaN()), "Nonfinite request"); + require(jump.request(10.) && !jump.request(10.1), "Busy request replacement"); + require(jump.state(10.) == JumpCommand::CROUCH, "Initial phase"); + jump.update(9.); + require(!jump.active(), "Backward clock reset"); + jump.request(0.); + jump.update(wheelbipe_jump_v4::kRequestDuration); + require(!jump.active() && jump.context(6.)[0] == 1., "Request expiry"); + jump.request(0.); + jump.reset(); + require(!jump.active(), "Explicit reset"); + const LegVector q{0., .7, 0., -.7}, target{.1, .6, -.1, -.6}; + const LegVector inward{8., -8., -8., 8.}, braking{-8., 8., 8., -8.}; + const auto tuck = jump_tuck_effort(q, target, inward, true); + require(tuck.gate[0] == 1. && tuck.gate[1] == 1., "Inward intent gate"); + require( + tuck.torque[0] > 0. && tuck.torque[1] == -tuck.torque[0] && tuck.torque[2] < 0. + && tuck.torque[3] == -tuck.torque[2], + "Bilateral torque direction"); + require(jump_tuck_effort(q, target, braking, true).torque == LegVector{}, "Braking assistance"); + require( + jump_tuck_effort(q, target, inward, false).torque == LegVector{}, "Ordinary assistance"); + require(jump_tuck_effort(q, q, inward, true).torque == LegVector{}, "No relative intent"); + const auto extended = jump_target_bounds(true), ordinary = jump_target_bounds(false); + require(std::abs(extended[0][1] - ordinary[0][1] - .2) < 1e-14, "Target authority"); + const LegBounds bounds{{{-.14, 1.42}, {-.14, 1.42}}}; + const JointVector caps{40., 40., 40., 40., 4.5, 4.5}; + const auto protected_effort = motor_effort( + JointSolver::constrain( + {-40., 40., 40., -40., 0., 0.}, {0., 1.42, 0., -1.42, 0., 0.}, caps, .03, bounds)); + require(protected_effort == JointVector{}, "Actual angle outward protection"); + require( + policy_parameters("wheelbipe").leg_kp == 60. && policy_parameters("wheelbipe").leg_kd == 2. + && !policy_parameters("wheelbipe").jump_control + && policy_parameters("wheelbipe-jump-v4").leg_kp == 80., + "Legacy policy parameters changed"); + std::cout << "Jump request, reset, compensation direction, braking, target authority and " + "actual limits passed\n"; +} diff --git a/rmcs_ws/src/rmcs_core/tools/export_wheelbipe_jump.py b/rmcs_ws/src/rmcs_core/tools/export_wheelbipe_jump.py new file mode 100755 index 000000000..605630953 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/tools/export_wheelbipe_jump.py @@ -0,0 +1,265 @@ +#!/usr/bin/env python3 +"""Export a saved v4 actor with the exact RMCS control contract; never controls a learner.""" + +import argparse +import hashlib +import json +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[4] +CONTRACT = ROOT / "rmcs_ws/src/rmcs_bringup/config/contracts/wheelbipe_jump_v4.json" + + +def digest(path): + return hashlib.sha256(Path(path).read_bytes()).hexdigest() + + +def require(condition, message): + if not condition: + raise ValueError(message) + + +def validate_receipt(receipt, contract): + require( + receipt["arguments"]["training_profile"] == contract["training_profile"] + and receipt["deployment"]["training_profile"] == contract["training_profile"], + "Wrong training profile", + ) + require( + receipt["deployment"]["source_commit"] == contract["training_source_commit"], + "Training source commit differs", + ) + native = receipt["native"] + control = contract["control"] + require( + native["leg_kp_Nm_per_rad"] == control["leg_kp_nm_per_rad"] + and native["leg_kd_Nm_per_rad_s"] == control["leg_kd_nm_s_per_rad"], + "PD differs", + ) + require( + native["virtual_pair_target_overtravel_rad"] + == control["virtual_pair_overtravel_rad"] + and native["virtual_pair_target_overtravel_scope"] + == control["virtual_pair_overtravel_scope"], + "Target authority differs", + ) + require( + native["tuck_spring_compensation"] == control["tuck_spring_compensation"], + "Tuck compensation differs", + ) + require( + native["spring_model_sha256"] == contract["gas_model_sha256"], + "Spring model differs", + ) + require( + native["physical_measured_gas_spring"] + and not native["gas_cancellation_feedforward"], + "Physical spring or global cancellation differs", + ) + require( + native["policy_dt_s"] == 1.0 / contract["policy_hz"] + and native["physics_dt_s"] == 1.0 / contract["feedback_pd_hz"], + "Policy or feedback cadence differs", + ) + require( + receipt["arguments"]["iterations"] == receipt["arguments"]["course_updates"], + "Course and budget differ", + ) + require( + receipt["initial_optimizer_state_count"] == 0 + and not receipt["cold_resume"] + and not receipt["cross_course_continuation"], + "This profile requires a fresh v4 experiment", + ) + model_path = "model/wheel_leg_v2.0.2/friction_v1/isaac/model_spec.json" + require( + receipt["deployment"]["files_sha256"][model_path] + == contract["model_spec_sha256"], + "Physical model differs", + ) + + +def export(checkpoint, receipt_path, output, *, allow_pilot=False): + import numpy as np + import onnx + import onnxruntime as ort + import torch + from torch import nn + + contract = json.loads(CONTRACT.read_text()) + receipt = json.loads(Path(receipt_path).read_text()) + validate_receipt(receipt, contract) + checkpoint = Path(checkpoint).resolve() + checksum = digest(checkpoint) + data = torch.load(checkpoint, map_location="cpu", weights_only=False) + if data.get("format") == "wheelbipe-rsl-durable-v1": + metadata = json.loads(Path(str(checkpoint) + ".json").read_text()) + pins = metadata["checkpoint_pins"] + require( + len(pins) == 1 + and pins[0]["sha256"] == checksum + and pins[0]["bytes"] == checkpoint.stat().st_size, + "Checkpoint sidecar does not pin the payload", + ) + require( + data["session_sha256"] == metadata["session_sha256"] + and Path(pins[0]["path"]).parent + == Path(receipt["arguments"]["output"]) / "checkpoints", + "Checkpoint and receipt belong to different sessions", + ) + updates = data["completed_updates"] + require( + updates == metadata["completed_updates"] + and data["total_updates"] == receipt["arguments"]["iterations"], + "Checkpoint budget differs", + ) + require( + data["curriculum_state"]["profile_sha256"] + == receipt["effective_course_sha256"], + "Checkpoint course differs", + ) + else: + require( + checksum == receipt.get("checkpoint_sha256"), + "RSL checkpoint is not pinned by the completed receipt", + ) + updates = int(data["iter"]) + 1 + budget = receipt["arguments"]["iterations"] + require(0 < updates <= budget, "Checkpoint update count is outside its budget") + complete = receipt["status"] == "training_complete" and updates == budget + require( + allow_pilot or complete, + "Final export requires completion of the declared course budget; pilot needs --allow-pilot", + ) + state = data["model_state_dict"] + require( + not data.get("normalizers"), "Unexpected external observation normalization" + ) + require( + not any("obs_normalizer" in k for k in state), + "Unexpected external observation normalization", + ) + weights = sorted( + (int(k.split(".")[1]), k, v) + for k, v in state.items() + if k.startswith("actor.") and k.endswith(".weight") + ) + require( + [(v.shape[1], v.shape[0]) for _, _, v in weights] + == [(35, 256), (256, 128), (128, 64), (64, 6)], + "Unexpected actor topology", + ) + layers = [] + for i, (_, key, weight) in enumerate(weights): + layer = nn.Linear(weight.shape[1], weight.shape[0]) + layer.weight.data.copy_(weight) + layer.bias.data.copy_(state[key.removesuffix("weight") + "bias"]) + layers.append(layer) + if i < 3: + layers.append(nn.ELU()) + actor = nn.Sequential(*layers).eval() + require( + all(torch.isfinite(v).all() for v in actor.state_dict().values()), + "Actor contains nonfinite parameters", + ) + output = Path(output).resolve() + require( + not output.exists() and not output.with_suffix(".deployment.json").exists(), + "Refuse to overwrite an existing artifact", + ) + output.parent.mkdir(parents=True, exist_ok=True) + temporary = output.with_suffix(".pending.onnx") + require(not temporary.exists(), "A pending export already exists") + torch.onnx.export( + actor, + torch.zeros(1, 35), + str(temporary), + input_names=["obs"], + output_names=["actions"], + dynamic_axes={"obs": {0: "batch"}, "actions": {0: "batch"}}, + opset_version=17, + dynamo=False, + ) + model = onnx.load(str(temporary)) + props = { + "rmcs_model_spec_sha256": contract["model_spec_sha256"], + "leg_target_model_spec_sha256": contract["model_spec_sha256"], + "leg_winding_version": "paired_full_vehicle110_jump_v4", + "rmcs_observation_version": contract["observation_version"], + "rmcs_observation_contract_sha256": contract["observation_contract_sha256"], + "rmcs_obs_normalization": "none", + "policy_hz": "50", + "feedback_pd_hz": "200", + "wheelbipe_deployment_contract_sha256": contract["deployment_contract_sha256"], + "training_source_commit": contract["training_source_commit"], + "gas_spring_compensation_enabled": "false", + "tuck_spring_compensation": "model", + "leg_kp": "80", + "leg_kd": "1.5", + "leg_target_overtravel_rad": "0.2", + "leg_target_overtravel_scope": "jump-request", + "gas_model_sha256": contract["gas_model_sha256"], + "rmcs_checkpoint_sha256": checksum, + "completed_updates": str(updates), + "training_budget": str(budget), + "effective_course_sha256": receipt["effective_course_sha256"], + "behavior_qualified": "false", + "hardware_tested": "false", + "pilot": str(not complete).lower(), + } + onnx.helper.set_model_props(model, props) + onnx.checker.check_model(model) + onnx.save(model, str(temporary)) + sample = ( + np.random.default_rng(119).uniform(-5.0, 5.0, (1024, 35)).astype(np.float32) + ) + with torch.inference_mode(): + expected = actor(torch.from_numpy(sample)).numpy() + actual = ort.InferenceSession( + str(temporary), providers=["CPUExecutionProvider"] + ).run(None, {"obs": sample})[0] + error = float(np.max(np.abs(actual - expected))) + require( + np.allclose(actual, expected, atol=2e-5, rtol=2e-5), + f"ONNX parity failed: {error}", + ) + temporary.replace(output) + result = { + "schema": "wheelbipe-jump-v4-onnx-export-v1", + "checkpoint_sha256": checksum, + "onnx_sha256": digest(output), + "completed_updates": updates, + "budget_updates": budget, + "training_complete": complete, + "metadata": props, + "parity_samples": 1024, + "max_absolute_error": error, + "behavior_qualified": False, + "hardware_tested": False, + "learner_modified": False, + } + output.with_suffix(".deployment.json").write_text( + json.dumps(result, indent=2) + "\n" + ) + return result + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--checkpoint", type=Path, required=True) + parser.add_argument("--receipt", type=Path, required=True) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--allow-pilot", action="store_true") + args = parser.parse_args() + print( + json.dumps( + export( + args.checkpoint, args.receipt, args.output, allow_pilot=args.allow_pilot + ), + indent=2, + ) + ) + + +if __name__ == "__main__": + main() diff --git a/rmcs_ws/src/rmcs_core/tools/sync_wheelbipe_jump_contract.py b/rmcs_ws/src/rmcs_core/tools/sync_wheelbipe_jump_contract.py new file mode 100755 index 000000000..6e7c93718 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/tools/sync_wheelbipe_jump_contract.py @@ -0,0 +1,237 @@ +#!/usr/bin/env python3 +"""Generate the jump deployment contract and geometry from a frozen training commit.""" + +import argparse +import hashlib +import json +import subprocess +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[4] +CONTRACT = ROOT / "rmcs_ws/src/rmcs_bringup/config/contracts/wheelbipe_jump_v4.json" +HEADER = ( + ROOT / "rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/model/jump_v4.hpp" +) + + +def digest(data): + return hashlib.sha256(data).hexdigest() + + +def canonical(value): + return json.dumps(value, sort_keys=True, separators=(",", ":")).encode() + + +def generate(repo, revision): + commit = subprocess.check_output( + ["git", "-C", str(repo), "rev-parse", revision + "^{commit}"], text=True + ).strip() + pins = {} + + def read(name): + data = subprocess.check_output( + ["git", "-C", str(repo), "show", commit + ":" + name] + ) + pins[name] = digest(data) + return json.loads(data) + + bundle = "model/wheel_leg_v2.0.2/friction_v1/isaac/" + profile = read("contracts/wheelbipe_foundation_jump_v4.json") + gas = read(bundle + "gas_spring_model.json") + manifest = read(bundle + "manifest.json") + read(bundle + "model_spec.json") + read("model/gas_spring_identified_symmetric_20261028/model.json") + control = { + name: profile["control"][name] + for name in ( + "leg_kp_nm_per_rad", + "leg_kd_nm_s_per_rad", + "virtual_pair_overtravel_rad", + "virtual_pair_overtravel_scope", + "tuck_spring_compensation", + ) + } + j = profile["jump"] + bounds = [ + [gas["sides"][s]["delta_rad"][i] for i in (0, -1)] for s in ("left", "right") + ] + nominal = [ + manifest["nominal_joint_pos"][n] + for n in ( + "L_joint1", + "LL_joint1", + "R_joint1", + "RR_joint1", + "L_joint3", + "R_joint3", + ) + ] + request = { + "standing_height_m": j["standing_height_m"], + "crouch_height_m": j["crouch_height_m"], + "release_height_m": j["release_height_m"], + "crouch_seconds": j["crouch_seconds"], + "apex_range_m": [j["apex_start_m"][0], j["apex_final_m"][1]], + "nominal_request_delay_s": sum(j["request_delay_s"]) / 2, + "outcome_seconds": j["outcome_seconds"], + "moving_vx_max_m_s": j["moving_jump"]["maximum_vx_m_s"], + } + contract = { + "schema": "wheelbipe-jump-v4-deployment-v1", + "training_source_commit": commit, + "training_profile": "foundation-jump-v4", + "source_files_sha256": pins, + "model_spec_sha256": pins[bundle + "model_spec.json"], + "gas_model_sha256": pins[bundle + "gas_spring_model.json"], + "policy_hz": profile["policy_hz"], + "feedback_pd_hz": profile["physics_and_feedback_pd_hz"], + "control": control, + "request": request, + "nominal_position": nominal, + "winding_bounds_rad": bounds, + "real_state_soft_limits_rad": gas["soft_limit_active_difference_rad"], + "real_state_margin_rad": 0.03, + "leg_torque_cap_nm": 40.0, + "wheel_torque_cap_nm": 4.5, + "observation_version": "wheelbipe-v202-foundation-jump-v4", + "observation": { + "size": 35, + "action_size": 6, + "normalization": "none", + "joint_select": [0, 2, 1, 3, 4, 5], + "order": [ + "command", + "height", + "gyro", + "gravity", + "position", + "velocity", + "previous_action", + "auxiliary", + ], + "jump_context": "ordinary=[1,0,0,0,0,0,0]; request=[0,0,0,0,1,apex,elapsed_s]", + "command_inputs": "external request and elapsed time only; no contact or measured apex", + }, + "landing_buffer": { + "seconds": j["landing_buffer_duration_s"], + "scope": "training reward only; actor controls deployment landing", + }, + "behavior_qualified": False, + "hardware_tested": False, + } + contract["observation_contract_sha256"] = digest( + canonical( + { + k: contract[k] + for k in ( + "model_spec_sha256", + "observation_version", + "observation", + "request", + "nominal_position", + ) + } + ) + ) + contract["deployment_contract_sha256"] = digest(canonical(contract)) + + def number(value): + return format(value, ".17g") + + lines = [ + "#pragma once", + "", + "#include ", + "#include ", + '#include "gas_spring_types.hpp"', + "", + "// Generated by tools/sync_wheelbipe_jump_contract.py from the pinned training commit.", + "namespace rmcs_core::controller::chassis::wheelbipe_jump_v4 {", + ] + for key, value in ( + ("TrainingCommit", commit), + ("ModelSpecSha256", contract["model_spec_sha256"]), + ("GasModelSha256", contract["gas_model_sha256"]), + ("ObservationSha256", contract["observation_contract_sha256"]), + ("DeploymentSha256", contract["deployment_contract_sha256"]), + ): + lines.append(f'inline constexpr std::string_view k{key} = "{value}";') + constants = { + "LegKp": control["leg_kp_nm_per_rad"], + "LegKd": control["leg_kd_nm_s_per_rad"], + "Overtravel": control["virtual_pair_overtravel_rad"], + "StandingHeight": request["standing_height_m"], + "CrouchHeight": request["crouch_height_m"], + "ReleaseHeight": request["release_height_m"], + "CrouchSeconds": request["crouch_seconds"], + "RequestDuration": request["outcome_seconds"] + - request["nominal_request_delay_s"], + "MinimumApex": request["apex_range_m"][0], + "MaximumApex": request["apex_range_m"][1], + "MovingVxMax": request["moving_vx_max_m_s"], + "MechanicalMin": contract["real_state_soft_limits_rad"][0], + "MechanicalMax": contract["real_state_soft_limits_rad"][1], + "MechanicalMargin": 0.03, + "LowCompressionRate": gas["force_extension"]["low_compression_rate_per_m"], + "HighCompressionSlope": gas["force_extension"][ + "high_compression_slope_n_per_m" + ], + } + settings = control["tuck_spring_compensation"] + for field, name in ( + ("gain", "TuckGain"), + ("cap_nm", "TuckCap"), + ("error_on_rad", "ErrorOn"), + ("error_full_rad", "ErrorFull"), + ("inward_pd_full_nm", "InwardPdFull"), + ): + constants[name] = settings[field] + for name, value in constants.items(): + lines.append(f"inline constexpr double k{name} = {number(value)};") + lines.append( + "inline constexpr std::array, 2> kBounds{{" + + ",".join("{" + ",".join(map(number, row)) + "}" for row in bounds) + + "}};" + ) + lines.append( + "inline constexpr std::array kForce{{" + ) + for x, y in zip(gas["compression_knots_m"], gas["force_knots_n"]): + lines.append(" {" + number(x) + "," + number(y) + "},") + lines.append("}};") + for side, name in (("left", "Left"), ("right", "Right")): + row = gas["sides"][side] + lines.append( + f"inline constexpr std::array k{name}" + + "{{" + ) + for delta, beta, compression, jac in zip( + row["delta_rad"], + row["beta_deg"], + row["compression_m"], + row["length_jacobian_m_per_rad"], + ): + lines.append( + " {" + ",".join(map(number, (delta, beta, compression, *jac))) + "}," + ) + lines.append("}};") + lines.append("} // namespace rmcs_core::controller::chassis::wheelbipe_jump_v4") + return contract, "\n".join(lines) + "\n" + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--training-repo", type=Path, required=True) + parser.add_argument("--revision", required=True) + args = parser.parse_args() + contract, header = generate(args.training_repo, args.revision) + CONTRACT.parent.mkdir(parents=True, exist_ok=True) + HEADER.write_text(header) + CONTRACT.write_text(json.dumps(contract, indent=2) + "\n") + print(contract["deployment_contract_sha256"]) + + +if __name__ == "__main__": + main() diff --git a/rmcs_ws/src/rmcs_rl b/rmcs_ws/src/rmcs_rl new file mode 160000 index 000000000..26d29ce0a --- /dev/null +++ b/rmcs_ws/src/rmcs_rl @@ -0,0 +1 @@ +Subproject commit 26d29ce0ac1540130ed8862c8c154c5f17cb34ee