New KeeperMotion (#1560)

* add the math and the motions left and right wideStance

* cleaned up

* Edit stiffnes in motions

* Rename WideStance to KeeperMotion

* finished Motion file and Bug Fixes

* Cleanup

* rm match stamant and added if statment

* renamend things

* fixed default.json

* fixed naming and added a newline

crates/control/src/behavior/defend.rs

@@ -11,8 +11,8 @@ use spl_network_messages::{GamePhase, SubState, Team};
 use types::{
     field_dimensions::{FieldDimensions, Side},
     filtered_game_controller_state::FilteredGameControllerState,
-    motion_command::{MotionCommand, WalkSpeed},
-    parameters::{RolePositionsParameters, WideStanceParameters},
+    motion_command::{JumpDirection, MotionCommand, WalkSpeed},
+    parameters::{KeeperMotionParameters, RolePositionsParameters},
     path_obstacles::PathObstacle,
     world_state::{BallState, WorldState},
 };
@@ -60,7 +60,7 @@ impl<'cycle> Defend<'cycle> {
         )
     }
 
-    pub fn wide_stance(&self, parameters: WideStanceParameters) -> Option<MotionCommand> {
+    pub fn keeper_motion(&self, parameters: KeeperMotionParameters) -> Option<MotionCommand> {
         let ball = self.world_state.ball?;
 
         let position = ball.ball_in_ground;
@@ -78,8 +78,24 @@ impl<'cycle> Defend<'cycle> {
         let horizontal_distance_to_intersection =
             position.y() - position.x() / velocity.x() * velocity.y();
 
-        if horizontal_distance_to_intersection.abs() < parameters.action_radius {
-            Some(MotionCommand::WideStance)
+        if (-parameters.action_radius_center..=parameters.action_radius_center)
+            .contains(&horizontal_distance_to_intersection)
+        {
+            Some(MotionCommand::KeeperMotion {
+                direction: JumpDirection::Center,
+            })
+        } else if (parameters.action_radius_center..parameters.action_radius_left)
+            .contains(&horizontal_distance_to_intersection)
+        {
+            Some(MotionCommand::KeeperMotion {
+                direction: JumpDirection::Left,
+            })
+        } else if (-parameters.action_radius_left..-parameters.action_radius_center)
+            .contains(&horizontal_distance_to_intersection)
+        {
+            Some(MotionCommand::KeeperMotion {
+                direction: JumpDirection::Right,
+            })
         } else {
             None
         }

crates/control/src/behavior/node.rs

@@ -16,8 +16,8 @@ use types::{
     filtered_game_state::FilteredGameState,
     motion_command::{MotionCommand, WalkSpeed},
     parameters::{
-        BehaviorParameters, InWalkKicksParameters, InterceptBallParameters, LostBallParameters,
-        WideStanceParameters,
+        BehaviorParameters, InWalkKicksParameters, InterceptBallParameters, KeeperMotionParameters,
+        LostBallParameters,
     },
     path_obstacles::PathObstacle,
     planned_path::PathSegment,
@@ -70,7 +70,7 @@ pub struct CycleContext {
     intercept_ball_parameters: Parameter<InterceptBallParameters, "behavior.intercept_ball">,
     maximum_step_size: Parameter<Step, "step_planner.max_step_size">,
     enable_pose_detection: Parameter<bool, "pose_detection.enable">,
-    wide_stance: Parameter<WideStanceParameters, "wide_stance">,
+    keeper_motion: Parameter<KeeperMotionParameters, "keeper_motion">,
     use_stand_head_unstiff_calibration:
         Parameter<bool, "calibration_controller.use_stand_head_unstiff_calibration">,
 
@@ -158,10 +158,8 @@ impl Behavior {
             }
         }
 
-        if matches!(world_state.robot.player_number, PlayerNumber::One)
-            && matches!(world_state.robot.role, Role::Keeper)
-        {
-            actions.push(Action::WideStance);
+        if matches!(world_state.robot.player_number, PlayerNumber::One) {
+            actions.push(Action::KeeperMotion);
         }
         actions.push(Action::InterceptBall);
 
@@ -294,7 +292,7 @@ impl Behavior {
                     Action::StandUp => stand_up::execute(world_state),
                     Action::NoGroundContact => no_ground_contact::execute(world_state),
                     Action::LookAround => look_around::execute(world_state),
-                    Action::WideStance => defend.wide_stance(context.wide_stance.clone()),
+                    Action::KeeperMotion => defend.keeper_motion(context.keeper_motion.clone()),
                     Action::InterceptBall => intercept_ball::execute(
                         world_state,
                         *context.intercept_ball_parameters,

crates/control/src/motion/dispatching_interpolator.rs

@@ -38,6 +38,10 @@ pub struct CycleContext {
     stand_up_front_positions: Input<Joints<f32>, "stand_up_front_positions">,
     stand_up_sitting_positions: Input<Joints<f32>, "stand_up_sitting_positions">,
     wide_stance_positions: Input<Joints<f32>, "wide_stance_positions">,
+    keeper_jump_left_motor_commands:
+        Input<MotorCommands<Joints<f32>>, "keeper_jump_left_motor_commands">,
+    keeper_jump_right_motor_commands:
+        Input<MotorCommands<Joints<f32>>, "keeper_jump_right_motor_commands">,
     walk_motor_commands: Input<MotorCommands<BodyJoints<f32>>, "walk_motor_commands">,
 
     initial_pose: Parameter<Joints<f32>, "initial_pose">,
@@ -105,6 +109,8 @@ impl DispatchingInterpolator {
                 MotionType::StandUpFront => *context.stand_up_front_positions,
                 MotionType::StandUpSitting => *context.stand_up_sitting_positions,
                 MotionType::WideStance => *context.wide_stance_positions,
+                MotionType::KeeperJumpLeft => context.keeper_jump_left_motor_commands.positions,
+                MotionType::KeeperJumpRight => context.keeper_jump_right_motor_commands.positions,
                 MotionType::CenterJump => *context.center_jump_positions,
                 MotionType::Unstiff => panic!("Dispatching Unstiff doesn't make sense"),
                 MotionType::Animation => context.animation_commands.positions,

crates/control/src/motion/keeper_jump_left.rs

@@ -0,0 +1,65 @@
+use color_eyre::Result;
+use context_attribute::context;
+use framework::MainOutput;
+use hardware::PathsInterface;
+use motionfile::{MotionFile, MotionInterpolator};
+use serde::{Deserialize, Serialize};
+use types::{
+    condition_input::ConditionInput,
+    cycle_time::CycleTime,
+    joints::{mirror::Mirror, Joints},
+    motion_selection::{MotionSafeExits, MotionSelection, MotionType},
+    motor_commands::MotorCommands,
+};
+
+#[derive(Deserialize, Serialize)]
+pub struct KeeperJumpLeft {
+    interpolator: MotionInterpolator<MotorCommands<Joints<f32>>>,
+}
+
+#[context]
+pub struct CreationContext {
+    hardware_interface: HardwareInterface,
+}
+
+#[context]
+pub struct CycleContext {
+    condition_input: Input<ConditionInput, "condition_input">,
+    cycle_time: Input<CycleTime, "cycle_time">,
+    motion_selection: Input<MotionSelection, "motion_selection">,
+
+    motion_safe_exits: CyclerState<MotionSafeExits, "motion_safe_exits">,
+}
+
+#[context]
+#[derive(Default)]
+pub struct MainOutputs {
+    pub keeper_jump_left_motor_commands: MainOutput<MotorCommands<Joints<f32>>>,
+}
+
+impl KeeperJumpLeft {
+    pub fn new(context: CreationContext<impl PathsInterface>) -> Result<Self> {
+        let paths = context.hardware_interface.get_paths();
+        Ok(Self {
+            interpolator: MotionFile::from_path(paths.motions.join("keeper_jump_right.json"))?
+                .try_into()?,
+        })
+    }
+
+    pub fn cycle(&mut self, context: CycleContext) -> Result<MainOutputs> {
+        let last_cycle_duration = context.cycle_time.last_cycle_duration;
+        let condition_input = context.condition_input;
+
+        if context.motion_selection.current_motion == MotionType::KeeperJumpLeft {
+            self.interpolator
+                .advance_by(last_cycle_duration, condition_input);
+        } else {
+            self.interpolator.reset();
+        }
+        context.motion_safe_exits[MotionType::KeeperJumpLeft] = self.interpolator.is_finished();
+
+        Ok(MainOutputs {
+            keeper_jump_left_motor_commands: self.interpolator.value().mirrored().into(),
+        })
+    }
+}

crates/control/src/motion/keeper_jump_right.rs

@@ -0,0 +1,65 @@
+use color_eyre::Result;
+use context_attribute::context;
+use framework::MainOutput;
+use hardware::PathsInterface;
+use motionfile::{MotionFile, MotionInterpolator};
+use serde::{Deserialize, Serialize};
+use types::{
+    condition_input::ConditionInput,
+    cycle_time::CycleTime,
+    joints::Joints,
+    motion_selection::{MotionSafeExits, MotionSelection, MotionType},
+    motor_commands::MotorCommands,
+};
+
+#[derive(Deserialize, Serialize)]
+pub struct KeeperJumpRight {
+    interpolator: MotionInterpolator<MotorCommands<Joints<f32>>>,
+}
+
+#[context]
+pub struct CreationContext {
+    hardware_interface: HardwareInterface,
+}
+
+#[context]
+pub struct CycleContext {
+    condition_input: Input<ConditionInput, "condition_input">,
+    cycle_time: Input<CycleTime, "cycle_time">,
+    motion_selection: Input<MotionSelection, "motion_selection">,
+
+    motion_safe_exits: CyclerState<MotionSafeExits, "motion_safe_exits">,
+}
+
+#[context]
+#[derive(Default)]
+pub struct MainOutputs {
+    pub keeper_jump_right_motor_commands: MainOutput<MotorCommands<Joints<f32>>>,
+}
+
+impl KeeperJumpRight {
+    pub fn new(context: CreationContext<impl PathsInterface>) -> Result<Self> {
+        let paths = context.hardware_interface.get_paths();
+        Ok(Self {
+            interpolator: MotionFile::from_path(paths.motions.join("keeper_jump_right.json"))?
+                .try_into()?,
+        })
+    }
+
+    pub fn cycle(&mut self, context: CycleContext) -> Result<MainOutputs> {
+        let last_cycle_duration = context.cycle_time.last_cycle_duration;
+        let condition_input = context.condition_input;
+
+        if context.motion_selection.current_motion == MotionType::KeeperJumpRight {
+            self.interpolator
+                .advance_by(last_cycle_duration, condition_input);
+        } else {
+            self.interpolator.reset();
+        }
+        context.motion_safe_exits[MotionType::KeeperJumpRight] = self.interpolator.is_finished();
+
+        Ok(MainOutputs {
+            keeper_jump_right_motor_commands: self.interpolator.value().into(),
+        })
+    }
+}

crates/control/src/motion/mod.rs

@@ -9,6 +9,8 @@ pub mod fall_protector;
 pub mod head_motion;
 pub mod jump_left;
 pub mod jump_right;
+pub mod keeper_jump_left;
+pub mod keeper_jump_right;
 pub mod look_around;
 pub mod look_at;
 pub mod motion_selector;

crates/control/src/motion/motion_selector.rs

@@ -87,7 +87,12 @@ fn motion_type_from_command(command: &MotionCommand) -> MotionType {
             Kind::FacingUp => MotionType::StandUpBack,
             Kind::Sitting => MotionType::StandUpSitting,
         },
-        MotionCommand::WideStance => MotionType::WideStance,
+        MotionCommand::KeeperMotion { direction } => match direction {
+            JumpDirection::Left => MotionType::KeeperJumpLeft,
+            JumpDirection::Right => MotionType::KeeperJumpRight,
+            JumpDirection::Center => MotionType::WideStance,
+        },
+
         MotionCommand::Unstiff => MotionType::Unstiff,
         MotionCommand::Animation { stiff: false } => MotionType::Animation,
         MotionCommand::Animation { stiff: true } => MotionType::AnimationStiff,
@@ -122,7 +127,12 @@ fn transition_motion(
             MotionType::Dispatching
         }
         (_, _, MotionType::FallProtection, _) => MotionType::FallProtection,
-        (_, _, MotionType::WideStance, _) => MotionType::WideStance,
+        (MotionType::Walk, _, MotionType::WideStance, _) => MotionType::WideStance,
+        (MotionType::Walk, _, MotionType::KeeperJumpRight, _) => MotionType::KeeperJumpRight,
+        (MotionType::Walk, _, MotionType::KeeperJumpLeft, _) => MotionType::KeeperJumpLeft,
+        (_, true, MotionType::WideStance, _) => MotionType::WideStance,
+        (_, true, MotionType::KeeperJumpRight, _) => MotionType::KeeperJumpRight,
+        (_, true, MotionType::KeeperJumpLeft, _) => MotionType::KeeperJumpLeft,
         (_, _, MotionType::CenterJump, _) => MotionType::CenterJump,
         (MotionType::ArmsUpStand, _, _, false) => MotionType::ArmsUpStand,
         (MotionType::Dispatching, true, _, _) => to,

crates/control/src/motion/motor_commands_collector.rs

@@ -42,6 +42,10 @@ pub struct CycleContext {
     stand_up_front_positions: Input<Joints<f32>, "stand_up_front_positions">,
     stand_up_sitting_positions: Input<Joints<f32>, "stand_up_sitting_positions">,
     wide_stance_positions: Input<Joints<f32>, "wide_stance_positions">,
+    keeper_jump_left_motor_commands:
+        Input<MotorCommands<Joints<f32>>, "keeper_jump_left_motor_commands">,
+    keeper_jump_right_motor_commands:
+        Input<MotorCommands<Joints<f32>>, "keeper_jump_right_motor_commands">,
     center_jump_positions: Input<Joints<f32>, "center_jump_positions">,
     walk_motor_commands: Input<MotorCommands<BodyJoints<f32>>, "walk_motor_commands">,
     cycle_time: Input<CycleTime, "cycle_time">,
@@ -86,6 +90,8 @@ impl MotorCommandCollector {
         let stand_up_front_positions = context.stand_up_front_positions;
         let stand_up_sitting_positions = context.stand_up_sitting_positions;
         let wide_stance_positions = context.wide_stance_positions;
+        let keeper_jump_left = context.keeper_jump_left_motor_commands;
+        let keeper_jump_right = context.keeper_jump_right_motor_commands;
         let walk = context.walk_motor_commands;
 
         let (positions, stiffnesses) = match motion_selection.current_motion {
@@ -207,6 +213,13 @@ impl MotorCommandCollector {
                     },
                 ),
             ),
+
+            MotionType::KeeperJumpLeft => {
+                (keeper_jump_left.positions, keeper_jump_left.stiffnesses)
+            }
+            MotionType::KeeperJumpRight => {
+                (keeper_jump_right.positions, keeper_jump_right.stiffnesses)
+            }
             MotionType::Unstiff => (measured_positions, Joints::fill(0.0)),
             MotionType::Walk => (
                 Joints::from_head_and_body(head_joints_command.positions, walk.positions),

crates/hulk_manifest/src/lib.rs

@@ -85,6 +85,8 @@ pub fn collect_hulk_cyclers() -> Result<Cyclers, Error> {
                     "control::motion::step_planner",
                     "control::motion::walk_manager",
                     "control::motion::wide_stance",
+                    "control::motion::keeper_jump_right",
+                    "control::motion::keeper_jump_left",
                     "control::motion::walking_engine",
                     "control::obstacle_filter",
                     "control::odometry",

crates/types/src/action.rs

@@ -38,7 +38,7 @@ pub enum Action {
     SitDown,
     Stand,
     StandUp,
-    WideStance,
+    KeeperMotion,
     SupportLeft,
     SupportRight,
     SupportStriker,

crates/types/src/motion_command.rs

@@ -63,7 +63,10 @@ pub enum MotionCommand {
     StandUp {
         kind: Kind,
     },
-    WideStance,
+    KeeperMotion {
+        direction: JumpDirection,
+    },
+
     #[default]
     Unstiff,
     Animation {
@@ -103,7 +106,7 @@ impl MotionCommand {
             | MotionCommand::FallProtection { .. }
             | MotionCommand::Jump { .. }
             | MotionCommand::StandUp { .. } => None,
-            MotionCommand::WideStance => None,
+            MotionCommand::KeeperMotion { .. } => None,
         }
     }