Fix step context not switching feet properly (#646)

Author: oxkitsune

yggdrasil/src/localization/mod.rs

@@ -86,7 +86,8 @@ fn initialize_pose(
     commands.insert_resource(pose);
 }
 
-fn motion_is_unsafe(
+#[must_use]
+pub fn motion_is_unsafe(
     keyframe_executor: Res<KeyframeExecutor>,
     motion_state: Res<State<Gait>>,
     contacts: Res<Contacts>,

yggdrasil/src/localization/odometry.rs

@@ -4,7 +4,7 @@ use nalgebra::{Isometry2, Translation2, UnitComplex, Vector2};
 use serde::{Deserialize, Serialize};
 
 use crate::{
-    behavior::{behaviors::Standup, engine::in_behavior},
+    behavior::engine::BehaviorState,
     kinematics::{
         Kinematics,
         spaces::{LeftSole, RightSole},
@@ -21,14 +21,15 @@ pub(super) struct OdometryPlugin;
 
 impl Plugin for OdometryPlugin {
     fn build(&self, app: &mut App) {
-        app.init_resource::<Odometry>().add_systems(
-            PreUpdate,
-            update_odometry
-                .run_if(not(in_behavior::<Standup>))
-                .after(crate::kinematics::update_kinematics)
-                .after(crate::sensor::orientation::update_orientation)
-                .after(WalkingEngineSet::Prepare),
-        );
+        app.init_resource::<Odometry>()
+            .add_systems(OnExit(BehaviorState::Standup), reset_odometry)
+            .add_systems(
+                PreUpdate,
+                update_odometry
+                    .after(crate::kinematics::update_kinematics)
+                    .after(crate::sensor::orientation::update_orientation)
+                    .after(WalkingEngineSet::Prepare),
+            );
     }
 }
 
@@ -47,8 +48,6 @@ pub fn update_odometry(
         return;
     }
 
-    // TODO: We should probably reset the odometry in some cases
-    // See: https://github.com/IntelligentRoboticsLab/yggdrasil/issues/400
     odometry.update(
         &localization_config.odometry,
         &foot_support,
@@ -57,6 +56,16 @@ pub fn update_odometry(
     );
 }
 
+fn reset_odometry(
+    mut odometry: ResMut<Odometry>,
+    orientation: Res<RobotOrientation>,
+    kinematics: Res<Kinematics>,
+) {
+    odometry.offset_to_last = Isometry2::default();
+    odometry.last_left_sole_to_right_sole = kinematics.vector::<LeftSole, RightSole>().inner.xy();
+    odometry.reset_orientation(&orientation);
+}
+
 /// Configuration for the odometry.
 #[derive(Resource, Debug, Clone, Serialize, Deserialize)]
 pub struct OdometryConfig {
@@ -103,7 +112,6 @@ impl Odometry {
             Side::Left => left_sole_to_right_sole - self.last_left_sole_to_right_sole,
             Side::Right => -left_sole_to_right_sole + self.last_left_sole_to_right_sole,
         } / 2.0;
-
         self.last_left_sole_to_right_sole = left_sole_to_right_sole;
         let scaled_offset = offset.component_mul(&config.scale_factor);
 

yggdrasil/src/motion/keyframe/manager.rs

@@ -86,7 +86,8 @@ impl ActiveMotion {
     pub fn execute_exit_routine(&self, step_context: &mut StepContext) {
         if let Some(ExitRoutine::Standing) = self.motion.settings.exit_routine {
             // Since the robot is now standing, we can reset the hip height to the default value.
-            step_context.request_reset();
+            step_context.finish_step();
+            step_context.request_stand();
         }
         // Add more exit routines here! (along with adding an appropriate enum value)
     }

yggdrasil/src/motion/walking_engine/foot_support.rs

@@ -24,6 +24,12 @@ impl Plugin for FootSupportPlugin {
                 .run_if(not(in_state(BehaviorState::Standup)))
                 .in_set(WalkingEngineSet::Prepare),
         );
+
+        // Reset foot support state when exiting standup to ensure correct foot selection
+        app.add_systems(
+            OnExit(BehaviorState::Standup),
+            reset_foot_support_after_standup,
+        );
     }
 }
 
@@ -89,6 +95,11 @@ impl FootSupportState {
         self.support = self.support.opposite();
     }
 
+    /// Reset the foot support state to default values and re-initialize support side detection.
+    pub(super) fn reset_after_standup(&mut self) {
+        *self = Self::default();
+    }
+
     /// Return `true` if a foot switch has occurred or is predicted in future cycles.
     #[must_use]
     pub fn predicted_or_switched(&self) -> bool {
@@ -164,3 +175,7 @@ fn update_foot_support(
         state.predicted_switch = predicted_switch;
     }
 }
+
+fn reset_foot_support_after_standup(mut foot_support: ResMut<FootSupportState>) {
+    foot_support.reset_after_standup();
+}

yggdrasil/src/motion/walking_engine/schedule.rs

@@ -1,7 +1,7 @@
 use bevy::{ecs::schedule::InternedSystemSet, prelude::*};
 
-use crate::kinematics::Kinematics;
 use crate::prelude::*;
+use crate::{behavior::engine::BehaviorState, kinematics::Kinematics};
 
 use super::{config::WalkingEngineConfig, step::PlannedStep, step_context::StepContext};
 
@@ -65,6 +65,12 @@ impl Plugin for WalkingEngineSchedulePlugin {
             PostStartup,
             setup_motion_state.in_set(WalkingEngineSet::Finalize),
         );
+
+        // Dirty hack: Reset the entire step context after standing up.
+        app.add_systems(
+            OnExit(BehaviorState::Standup),
+            setup_motion_state.in_set(WalkingEngineSet::Finalize),
+        );
     }
 }
 

yggdrasil/src/motion/walking_engine/step_context.rs

@@ -1,6 +1,7 @@
 use std::time::{Duration, Instant};
 
 use crate::{
+    behavior::{behaviors::Standup, engine::in_behavior},
     core::debug::{
         DebugContext,
         debug_system::{DebugAppExt, SystemToggle},
@@ -38,6 +39,7 @@ impl Plugin for StepContextPlugin {
             PreUpdate,
             plan_step
                 .run_if(on_event::<FootSwitchedEvent>)
+                .run_if(not(in_behavior::<Standup>))
                 .in_set(WalkingEngineSet::PlanStep)
                 .after(crate::kinematics::update_kinematics),
         );
@@ -80,10 +82,7 @@ impl StepContext {
 
     pub fn request_sit(&mut self) {
         self.requested_gait = Gait::Sitting;
-        self.last_step = PlannedStep {
-            swing_side: self.last_step.swing_side,
-            ..Default::default()
-        };
+        self.last_step = PlannedStep::default();
         self.requested_step = Step::default();
     }
 
@@ -157,12 +156,6 @@ impl StepContext {
         }
     }
 
-    pub fn request_reset(&mut self) {
-        self.requested_reset = true;
-        self.request_stand();
-        self.requested_gait = Gait::Standing;
-    }
-
     pub fn finish_step(&mut self) {
         self.last_step = self.planned_step;
     }
@@ -208,7 +201,7 @@ impl StepContext {
             target,
             swing_foot_height: config.base_foot_lift + foot_lift_modifier,
             swing_side: next_swing_foot,
-        }
+        };
     }
 }
 

yggdrasil/src/sensor/foot_bumpers.rs

@@ -1,5 +1,6 @@
 use super::SensorConfig;
 use super::button::{LeftFootButtons, RightFootButtons};
+use crate::localization::motion_is_unsafe;
 use crate::prelude::*;
 use crate::{
     motion::path_finding::Obstacle,
@@ -17,7 +18,7 @@ pub struct FootBumperPlugin;
 
 impl Plugin for FootBumperPlugin {
     fn build(&self, app: &mut App) {
-        app.add_systems(Sensor, obstacle_detection)
+        app.add_systems(Sensor, obstacle_detection.run_if(not(motion_is_unsafe)))
             .init_resource::<ObstacleStateFromBumpers>()
             .init_resource::<FootBumperValues>();
     }