Walk to ball position as default behaviour

crates/control/src/lib.rs

@@ -1,6 +1,5 @@
 pub mod a_star;
 pub mod active_vision;
-pub mod ball_filter;
 pub mod ball_state_composer;
 pub mod behavior;
 pub mod button_filter;

crates/control/src/motion/booster_walking.rs

@@ -34,7 +34,7 @@ pub struct CycleContext {
 
     imu_state: Input<ImuState, "imu_state">,
     serial_motor_states: Input<Joints<MotorState>, "serial_motor_states">,
-    motion_command: Input<MotionCommand, "motion_command">,
+    motion_command: Input<MotionCommand, "motion_selection">,
     cycle_time: Input<CycleTime, "cycle_time">,
 }
 

crates/control/src/motion/motion_selector.rs

@@ -1,215 +1,42 @@
 use color_eyre::Result;
 use context_attribute::context;
 use framework::MainOutput;
-use hardware::SpeakerInterface;
 use serde::{Deserialize, Serialize};
-use types::{
-    audio::{Sound, SpeakerRequest},
-    fall_state::FallenKind,
-    motion_command::{JumpDirection, MotionCommand},
-    motion_selection::{MotionSafeExits, MotionSelection, MotionType},
-};
+use types::motion_command::MotionCommand;
 
 #[derive(Deserialize, Serialize)]
-pub struct MotionSelector {
-    last_motion: MotionType,
-    last_stand_up_count: u32,
-    was_standing_up: bool,
-}
+pub struct MotionSelector {}
 
 #[context]
 pub struct CreationContext {}
 
 #[context]
 pub struct CycleContext {
-    motion_command: Input<MotionCommand, "motion_command">,
-    has_ground_contact: Input<bool, "has_ground_contact">,
-
-    motion_safe_exits: CyclerState<MotionSafeExits, "motion_safe_exits">,
-    stand_up_count: CyclerState<u32, "stand_up_count">,
-
-    maximum_standup_attempts: Parameter<u32, "behavior.maximum_standup_attempts">,
-
-    hardware_interface: HardwareInterface,
+    motion_command_from_behavior: Input<MotionCommand, "WorldState", "motion_command">,
+    motion_command_from_remote_control: Input<MotionCommand, "motion_command">,
+    use_remote_control_for_motion_selection:
+        Parameter<bool, "motion.use_remote_control_for_motion_selection">,
 }
 
 #[context]
 #[derive(Default)]
 pub struct MainOutputs {
-    pub motion_selection: MainOutput<MotionSelection>,
+    pub motion_selection: MainOutput<MotionCommand>,
 }
 
 impl MotionSelector {
     pub fn new(_context: CreationContext) -> Result<Self> {
-        Ok(Self {
-            last_motion: MotionType::Unstiff,
-            last_stand_up_count: 0,
-            was_standing_up: false,
-        })
+        Ok(Self {})
     }
 
-    pub fn cycle(&mut self, context: CycleContext<impl SpeakerInterface>) -> Result<MainOutputs> {
-        let motion_safe_to_exit = context.motion_safe_exits[self.last_motion];
-        let requested_motion = motion_type_from_command(context.motion_command);
-
-        let current_motion = transition_motion(
-            self.last_motion,
-            requested_motion,
-            motion_safe_to_exit,
-            *context.has_ground_contact,
-        );
-
-        let switching_to_stand_up =
-            current_motion.is_dispatching() && requested_motion.is_standup_motion();
-
-        let current_stand_up_count = count_stand_up_attempts(
-            current_motion,
-            switching_to_stand_up,
-            self.was_standing_up,
-            self.last_stand_up_count,
-        );
-
-        if self.last_stand_up_count <= *context.maximum_standup_attempts
-            && current_stand_up_count > *context.maximum_standup_attempts
-        {
-            context
-                .hardware_interface
-                .write_to_speakers(SpeakerRequest::PlaySound { sound: Sound::Ouch });
-        }
-
-        self.last_stand_up_count = current_stand_up_count;
-
-        let dispatching_motion = if current_motion == MotionType::Dispatching {
-            if requested_motion == MotionType::Unstiff {
-                Some(MotionType::SitDown)
-            } else {
-                Some(requested_motion)
-            }
+    pub fn cycle(&mut self, context: CycleContext) -> Result<MainOutputs> {
+        let seleciton: MotionCommand = if *context.use_remote_control_for_motion_selection {
+            context.motion_command_from_remote_control.clone()
         } else {
-            None
+            context.motion_command_from_behavior.clone()
         };
-
-        *context.stand_up_count = self.last_stand_up_count;
-
-        self.last_motion = current_motion;
-        self.was_standing_up = switching_to_stand_up;
-
         Ok(MainOutputs {
-            motion_selection: MotionSelection {
-                current_motion,
-                dispatching_motion,
-            }
-            .into(),
+            motion_selection: seleciton.into(),
         })
     }
 }
-
-fn motion_type_from_command(command: &MotionCommand) -> MotionType {
-    match command {
-        MotionCommand::ArmsUpSquat => MotionType::ArmsUpSquat,
-        MotionCommand::ArmsUpStand { .. } => MotionType::ArmsUpStand,
-        MotionCommand::FallProtection { .. } => MotionType::FallProtection,
-        MotionCommand::Initial { .. } => MotionType::Initial,
-        MotionCommand::Jump { direction } => match direction {
-            JumpDirection::Left => MotionType::JumpLeft,
-            JumpDirection::Right => MotionType::JumpRight,
-            JumpDirection::Center => MotionType::CenterJump,
-        },
-        MotionCommand::Penalized => MotionType::Penalized,
-        MotionCommand::SitDown { .. } => MotionType::SitDown,
-        MotionCommand::Stand { .. } => MotionType::Stand,
-        MotionCommand::StandUp { kind, speed } => match kind {
-            FallenKind::FacingDown => MotionType::StandUpFront(*speed),
-            FallenKind::FacingUp => MotionType::StandUpBack,
-            FallenKind::Sitting => MotionType::StandUpSitting(*speed),
-        },
-        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,
-        MotionCommand::Walk { .. } => MotionType::Walk,
-        MotionCommand::InWalkKick { .. } => MotionType::Walk,
-        MotionCommand::WalkWithVelocity { .. } => MotionType::Walk,
-    }
-}
-
-fn transition_motion(
-    from: MotionType,
-    to: MotionType,
-    motion_safe_to_exit: bool,
-    has_ground_contact: bool,
-) -> MotionType {
-    match (from, motion_safe_to_exit, to, has_ground_contact) {
-        (MotionType::SitDown, true, MotionType::Unstiff, _) => MotionType::Unstiff,
-        (_, _, MotionType::Unstiff, false) => MotionType::Unstiff,
-        (MotionType::Dispatching, true, MotionType::Unstiff, true) => MotionType::SitDown,
-        (MotionType::StandUpFront(speed), _, MotionType::FallProtection, _) => {
-            MotionType::StandUpFront(speed)
-        }
-        (MotionType::StandUpBack, _, MotionType::FallProtection, _) => MotionType::StandUpBack,
-        (MotionType::WideStance, _, MotionType::FallProtection, _) => MotionType::WideStance,
-        (MotionType::JumpLeft, _, MotionType::FallProtection, _) => MotionType::JumpLeft,
-        (MotionType::JumpRight, _, MotionType::FallProtection, _) => MotionType::JumpRight,
-        (MotionType::CenterJump, _, MotionType::FallProtection, _) => MotionType::CenterJump,
-        (MotionType::StandUpSitting(speed), _, MotionType::FallProtection, _) => {
-            MotionType::StandUpSitting(speed)
-        }
-        (MotionType::ArmsUpStand, _, MotionType::FallProtection, _) => MotionType::ArmsUpStand,
-        (MotionType::StandUpFront(_), true, MotionType::StandUpFront(_), _) => {
-            MotionType::Dispatching
-        }
-        (MotionType::StandUpBack, true, MotionType::StandUpBack, _) => MotionType::Dispatching,
-        (MotionType::StandUpBack, true, MotionType::StandUpSitting(speed), _) => {
-            MotionType::StandUpSitting(speed)
-        }
-        (MotionType::StandUpSitting(_), true, MotionType::StandUpSitting(_), _) => {
-            MotionType::Dispatching
-        }
-        (_, _, MotionType::FallProtection, _) => MotionType::FallProtection,
-        (MotionType::Walk, _, MotionType::WideStance, _) => MotionType::WideStance,
-        (MotionType::Walk, _, MotionType::KeeperJumpRight, _) => MotionType::KeeperJumpRight,
-        (MotionType::Walk, _, MotionType::KeeperJumpLeft, _) => MotionType::KeeperJumpLeft,
-
-        (MotionType::WideStance, false, _, _) => MotionType::WideStance,
-        (MotionType::KeeperJumpRight, false, _, _) => MotionType::KeeperJumpRight,
-        (MotionType::KeeperJumpLeft, false, _, _) => MotionType::KeeperJumpLeft,
-        (_, true, MotionType::WideStance, _) => MotionType::WideStance,
-        (_, true, MotionType::KeeperJumpRight, _) => MotionType::KeeperJumpRight,
-        (_, true, MotionType::KeeperJumpLeft, _) => MotionType::KeeperJumpLeft,
-        (_, _, MotionType::CenterJump, _) => MotionType::CenterJump,
-        (MotionType::ArmsUpSquat, _, MotionType::JumpRight, _) => MotionType::JumpRight,
-        (MotionType::ArmsUpSquat, _, MotionType::JumpLeft, _) => MotionType::JumpLeft,
-        (MotionType::ArmsUpStand, _, _, false) => MotionType::ArmsUpStand,
-        (MotionType::Dispatching, true, _, _) => to,
-        (MotionType::Stand, _, MotionType::Walk, _) => MotionType::Walk,
-        (MotionType::Walk, _, MotionType::Stand, _) => MotionType::Stand,
-        (MotionType::Unstiff | MotionType::AnimationStiff, true, MotionType::Animation, _) => {
-            MotionType::Animation
-        }
-        (MotionType::Animation, true, MotionType::AnimationStiff, _) => MotionType::AnimationStiff,
-        (from, true, to, _) if from != to => MotionType::Dispatching,
-        _ => from,
-    }
-}
-
-fn count_stand_up_attempts(
-    current_motion: MotionType,
-    is_standing_up: bool,
-    was_standing_up: bool,
-    stand_up_count: u32,
-) -> u32 {
-    if !was_standing_up && is_standing_up {
-        return stand_up_count + 1;
-    }
-
-    if current_motion.is_stable() {
-        return 0;
-    }
-
-    stand_up_count
-}

crates/hulk_manifest/src/lib.rs

@@ -61,7 +61,7 @@ pub fn collect_hulk_cyclers(root: impl AsRef<Path>) -> Result<Cyclers, Error> {
                     // "control::motion::keeper_jump_right",
                     // "control::motion::look_around",
                     // "control::motion::look_at",
-                    // "control::motion::motion_selector",
+                    "control::motion::motion_selector",
                     // "control::motion::motor_commands_collector",
                     // "control::motion::obstacle_avoiding_arms",
                     // "control::motion::sit_down",
@@ -107,10 +107,12 @@ pub fn collect_hulk_cyclers(root: impl AsRef<Path>) -> Result<Cyclers, Error> {
                 instances: vec![""],
                 setup_nodes: vec!["world_state::trigger"],
                 nodes: vec![
-                    "world_state::game_controller_filter",
-                    "world_state::game_controller_state_filter",
+                    "world_state::ball_filter",
                     "world_state::ball_projector",
+                    "world_state::behavior::walk_to_ball",
                     "world_state::camera_matrix_calculator",
+                    "world_state::game_controller_filter",
+                    "world_state::game_controller_state_filter",
                     "world_state::ground_provider",
                     "world_state::kinematics_provider",
                 ],

crates/types/src/parameters.rs

@@ -51,6 +51,7 @@ pub struct BehaviorParameters {
     pub maximum_lookaround_duration: Duration,
     pub time_to_reach_delay_when_fallen: Duration,
     pub maximum_standup_attempts: u32,
+    pub walk_with_velocity: WalkWithVelocityParameters,
 }
 
 #[derive(
@@ -373,17 +374,6 @@ pub struct BallFilterNoise {
 )]
 pub struct BallFilterParameters {
     pub hypothesis_timeout: Duration,
-    pub maximum_number_of_hypotheses: usize,
-    pub log_likelihood_of_zero_velocity_threshold: f32,
-    pub hypothesis_merge_distance: f32,
-    pub visible_validity_exponential_decay_factor: f32,
-    pub hidden_validity_exponential_decay_factor: f32,
-    pub validity_output_threshold: f32,
-    pub validity_discard_threshold: f32,
-    pub velocity_decay_factor: f32,
-    pub noise: BallFilterNoise,
-    pub maximum_matching_cost: f32,
-    pub maximum_matching_cost_validity_penalty_factor: f32,
 }
 
 #[derive(
@@ -503,6 +493,15 @@ pub struct RLWalkingParameters {
     pub joint_position_smoothing_factor: f32,
 }
 
+#[derive(
+    Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
+)]
+pub struct WalkWithVelocityParameters {
+    pub max_velocity: f32,
+    pub max_angular_velocity: f32,
+    pub angular_velocity_scaling_factor: f32,
+}
+
 #[derive(
     Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
 )]

crates/world_state/src/ball_filter.rs

@@ -0,0 +1,78 @@
+use color_eyre::{eyre::OptionExt, Result};
+use linear_algebra::Vector2;
+use serde::{Deserialize, Serialize};
+
+use context_attribute::context;
+use coordinate_systems::Ground;
+use framework::MainOutput;
+use types::{
+    ball_detection::BallPercept,
+    ball_position::{BallPosition, HypotheticalBallPosition},
+    cycle_time::CycleTime,
+    parameters::BallFilterParameters,
+};
+
+#[derive(Deserialize, Serialize)]
+pub struct BallFilter {
+    last_ball_position: Option<BallPosition<Ground>>,
+}
+
+#[context]
+pub struct CreationContext {}
+
+#[context]
+pub struct CycleContext {
+    cycle_time: Input<CycleTime, "cycle_time">,
+    ball_percepts: Input<Option<Vec<BallPercept>>, "balls?">,
+    ball_filter_parameter: Parameter<BallFilterParameters, "ball_filter">,
+}
+
+#[context]
+#[derive(Default)]
+pub struct MainOutputs {
+    pub ball_position: MainOutput<Option<BallPosition<Ground>>>,
+    pub hypothetical_ball_positions: MainOutput<Vec<HypotheticalBallPosition<Ground>>>,
+}
+
+impl BallFilter {
+    pub fn new(_context: CreationContext) -> Result<Self> {
+        Ok(Self {
+            last_ball_position: None,
+        })
+    }
+
+    pub fn cycle(&mut self, context: CycleContext) -> Result<MainOutputs> {
+        let filtered_ball_percept = context.ball_percepts.ok_or_eyre("no ball percept")?.last();
+
+        let ball_position = filtered_ball_percept
+            .and_then(|ball_percept| {
+                self.last_ball_position = Some(BallPosition {
+                    position: ball_percept.percept_in_ground.mean.into(),
+                    velocity: Vector2::zeros(),
+                    last_seen: context.cycle_time.start_time,
+                });
+                self.last_ball_position
+            })
+            .or_else(|| {
+                if let Some(last_ball_position) = self.last_ball_position {
+                    if context
+                        .cycle_time
+                        .start_time
+                        .duration_since(last_ball_position.last_seen)
+                        .expect("time ran backwards")
+                        < context.ball_filter_parameter.hypothesis_timeout
+                    {
+                        return Some(last_ball_position);
+                    } else {
+                        self.last_ball_position = None;
+                    }
+                }
+                None
+            });
+
+        Ok(MainOutputs {
+            ball_position: ball_position.into(),
+            hypothetical_ball_positions: Vec::new().into(),
+        })
+    }
+}

crates/world_state/src/behavior/mod.rs

@@ -0,0 +1 @@
+pub mod walk_to_ball;