Reintegrate motor_command_collector (HULKs#2215)

Author: BenSampaolo

Cargo.lock

@@ -32,7 +32,9 @@ pub fn collect_hulk_cyclers(root: impl AsRef<Path>) -> Result<Cyclers, Error> {
                     "motion::booster_walking",
                     "motion::remote_control",
                     "motion::look_at",
+                    "motion::head_motion",
                     "motion::motion_selector",
+                    "motion::motor_commands_collector",
                 ],
                 execution_time_warning_threshold: Some(Duration::from_secs_f32(1.0 / 500.0)),
             },

crates/control/src/motion/motor_commands_collector.rs

@@ -11,13 +11,15 @@ booster = { workspace = true }
 color-eyre = { workspace = true }
 context_attribute = { workspace = true }
 coordinate_systems = { workspace = true }
+filtering = { workspace = true }
 framework = { workspace = true }
 hardware = { workspace = true }
 kinematics = { workspace = true }
 linear_algebra = { workspace = true }
 log = { workspace = true }
 path_serde = { workspace = true }
 projection = { workspace = true }
+proptest = { workspace = true }
 serde = { workspace = true }
 types = { workspace = true }
 walking_inference = { workspace = true }

crates/hulk_manifest/src/lib.rs

@@ -41,7 +41,7 @@ pub struct CycleContext {
 #[context]
 #[derive(Default)]
 pub struct MainOutputs {
-    pub target_joint_positions: MainOutput<Joints>,
+    pub walking_target_joint_positions: MainOutput<Joints>,
 }
 
 impl RLWalking {
@@ -77,16 +77,17 @@ impl RLWalking {
             .walking_inference_inputs
             .fill_if_subscribed(|| walking_inference_inputs.clone());
 
-        let target_joint_positions = context.common_motor_command_parameters.default_positions
-            + inference_output_positions * context.walking_parameters.control.action_scale;
+        let walking_target_joint_positions =
+            context.common_motor_command_parameters.default_positions
+                + inference_output_positions * context.walking_parameters.control.action_scale;
 
         self.smoothed_target_joint_positions = self.smoothed_target_joint_positions
             * context.walking_parameters.joint_position_smoothing_factor
-            + target_joint_positions
+            + walking_target_joint_positions
                 * (1.0 - context.walking_parameters.joint_position_smoothing_factor);
 
         Ok(MainOutputs {
-            target_joint_positions: self.smoothed_target_joint_positions.into(),
+            walking_target_joint_positions: self.smoothed_target_joint_positions.into(),
         })
     }
 }

crates/motion/Cargo.toml

@@ -4,11 +4,7 @@ use context_attribute::context;
 use framework::AdditionalOutput;
 use hardware::{LowCommandInterface, TimeInterface};
 use serde::{Deserialize, Serialize};
-use types::{
-    joints::{head::HeadJoints, Joints},
-    motion_command::{HeadMotion, MotionCommand},
-    parameters::MotorCommandParameters,
-};
+use types::{joints::Joints, parameters::MotorCommandParameters};
 
 #[derive(Deserialize, Serialize)]
 pub struct CommandSender {
@@ -26,14 +22,11 @@ pub struct CreationContext {
 pub struct CycleContext {
     low_command: AdditionalOutput<LowCommand, "low_command">,
 
-    target_joint_positions: Input<Joints, "target_joint_positions">,
-    look_at: Input<HeadJoints<f32>, "look_at">,
-    motion_command: Input<MotionCommand, "selected_motion_command">,
-
     walk_motor_command_parameters: Parameter<MotorCommandParameters, "common_motor_command">,
     _prepare_motor_command_parameters: Parameter<MotorCommandParameters, "prepare_motor_command">,
 
     hardware_interface: HardwareInterface,
+    collected_target_joint_positions: Input<Joints<f32>, "collected_target_joint_positions">,
 }
 
 #[context]
@@ -55,20 +48,8 @@ impl CommandSender {
         &mut self,
         mut context: CycleContext<impl LowCommandInterface + TimeInterface>,
     ) -> Result<MainOutputs> {
-        let look_at_head_joints = match context.motion_command.head_motion() {
-            Some(HeadMotion::LookAt { .. }) => Some(*context.look_at),
-            _ => None,
-        };
-
-        let target_joint_positions = Joints {
-            head: look_at_head_joints.unwrap_or(context.target_joint_positions.head),
-            left_arm: context.target_joint_positions.left_arm,
-            right_arm: context.target_joint_positions.right_arm,
-            left_leg: context.target_joint_positions.left_leg,
-            right_leg: context.target_joint_positions.right_leg,
-        };
         let walk_low_command = LowCommand::new(
-            &target_joint_positions,
+            context.collected_target_joint_positions,
             context.walk_motor_command_parameters,
             CommandType::Serial,
         );