Fix robot legs when picked up + more contacts (#431)

Co-authored-by: Gijs de Jong <berichtaangijs@gmail.com>

Author: VerkuijlKim

deploy/config/yggdrasil.toml

@@ -6,7 +6,9 @@ held_duration_threshold = 500
 
 [filter.fsr]
 # Threshold for ground contact detection using average FSR sensor values from both feet.
-ground_contact_threshold = 0.01
+ground_contact_threshold = 0.12
+# Timeout for changing the value of the ground contact state, in milliseconds.
+ground_contact_timeout = 20
 # Maximum amount of pressure measured by a single sensor.
 max_pressure = 5.0
 # Initial value for minimum pressure measured by a single sensor.

yggdrasil/src/behavior/behaviors/sitting.rs

@@ -1,6 +1,9 @@
 use bevy::prelude::*;
 
-use nidhogg::types::{color, FillExt, RightEye};
+use crate::motion::keyframe::KeyframeExecutor;
+use crate::sensor::fsr::Contacts;
+use nidhogg::types::{color, FillExt, LegJoints, RightEye};
+use nidhogg::NaoState;
 
 use crate::{
     behavior::engine::{in_behavior, Behavior, BehaviorState},
@@ -21,23 +24,55 @@ impl Plugin for SittingBehaviorPlugin {
 
 /// This is often the starting behavior of the robot.
 /// In this state the robot sits down, after which it unstiffens its legs, arms and head.
-#[derive(Resource)]
-pub struct Sitting;
+#[derive(Resource, Default)]
+pub struct Sitting {
+    /// Stores the initial leg position when ground contact is lost.
+    locked_leg_position: Option<LegJoints<f32>>,
+}
 
 impl Behavior for Sitting {
     const STATE: BehaviorState = BehaviorState::Sitting;
 }
 
-pub fn sitting(mut walking_engine: ResMut<WalkingEngine>, mut nao_manager: ResMut<NaoManager>) {
+pub fn sitting(
+    mut sitting: ResMut<Sitting>,
+    mut walking_engine: ResMut<WalkingEngine>,
+    mut nao_manager: ResMut<NaoManager>,
+    nao_state: Res<NaoState>,
+    contacts: Res<Contacts>,
+    keyframe_executor: Res<KeyframeExecutor>,
+) {
     // Makes right eye blue.
     nao_manager.set_right_eye_led(RightEye::fill(color::f32::BLUE), Priority::default());
 
-    if walking_engine.is_sitting() {
-        // Makes robot floppy except for hip joints, locked in sitting position.
-        nao_manager.unstiff_sit(UNSTIFF_PRIORITY);
-    } else {
+    if !walking_engine.is_sitting() {
         walking_engine.request_sit();
+        nao_manager.unstiff_arms(UNSTIFF_PRIORITY);
+        return;
+    }
+
+    // When ground contact is lost, set the current position in air.
+    if !contacts.ground && !keyframe_executor.is_motion_active() {
+        if sitting.locked_leg_position.is_none() {
+            sitting.locked_leg_position = Some(capture_leg_position(&nao_state));
+        }
+        if let Some(leg_positions) = sitting.locked_leg_position.as_ref() {
+            nao_manager.stiff_sit(leg_positions.clone(), Priority::High);
+        }
+
+    // Resets locked position and makes robot floppy except for hip joints in sitting position.
+    } else {
+        sitting.locked_leg_position = None;
+        nao_manager.unstiff_sit(UNSTIFF_PRIORITY);
+        nao_manager.unstiff_arms(UNSTIFF_PRIORITY);
     }
+}
+
+fn capture_leg_position(nao_state: &NaoState) -> LegJoints<f32> {
+    let position = nao_state.position.clone();
 
-    nao_manager.unstiff_arms(UNSTIFF_PRIORITY);
+    LegJoints::builder()
+        .left_leg(position.left_leg_joints())
+        .right_leg(position.right_leg_joints())
+        .build()
 }

yggdrasil/src/behavior/engine.rs

@@ -7,7 +7,7 @@ use super::{
         WalkBehaviorPlugin, WalkToBehaviorPlugin, WalkToSetBehaviorPlugin,
     },
     roles::{
-        Defender, DefenderRolePlugin, Goalkeeper, GoalkeeperRolePlugin, InstinctRolePlugin,
+        DefenderRolePlugin, Goalkeeper, GoalkeeperRolePlugin, Instinct, InstinctRolePlugin,
         Striker, StrikerRolePlugin,
     },
 };
@@ -100,7 +100,7 @@ impl Role {
         match player_number {
             1 => commands.set_role(Goalkeeper),
             5 => commands.set_role(Striker::WalkToBall),
-            _ => commands.set_role(Defender),
+            _ => commands.set_role(Instinct),
         }
     }
 

yggdrasil/src/behavior/mod.rs

@@ -5,7 +5,6 @@ pub mod primary_state;
 pub mod roles;
 
 use bevy::{app::PluginGroupBuilder, prelude::*};
-use engine::BehaviorEnginePlugin;
 
 pub use behavior_config::BehaviorConfig;
 
@@ -15,7 +14,7 @@ pub struct BehaviorPlugins;
 impl PluginGroup for BehaviorPlugins {
     fn build(self) -> PluginGroupBuilder {
         PluginGroupBuilder::start::<Self>()
-            .add(BehaviorEnginePlugin)
+            .add(engine::BehaviorEnginePlugin)
             .add(primary_state::PrimaryStatePlugin)
     }
 }

yggdrasil/src/behavior/roles/instinct.rs

@@ -6,7 +6,7 @@ use nalgebra::Point2;
 use crate::{
     behavior::{
         behaviors::{CatchFall, Sitting, Stand, StandLookAt, Standup, WalkToSet},
-        engine::CommandsBehaviorExt,
+        engine::{in_role, CommandsBehaviorExt},
         primary_state::{update_primary_state, PrimaryState},
         roles::Striker,
     },
@@ -25,7 +25,9 @@ impl Plugin for InstinctRolePlugin {
     fn build(&self, app: &mut App) {
         app.add_systems(
             Update,
-            behavior.after(update_primary_state), // .run_if(resource_exists::<Instinct>),
+            behavior
+                .after(update_primary_state)
+                .run_if(in_role::<Instinct>),
         );
     }
 }
@@ -55,7 +57,7 @@ pub fn behavior(
 
     if behavior == &BehaviorState::StartUp {
         if walking_engine.is_sitting() || head_buttons.all_pressed() {
-            commands.set_behavior(Sitting);
+            commands.set_behavior(Sitting::default());
         }
         if *primary_state == PrimaryState::Initial {
             commands.set_behavior(Stand);
@@ -65,7 +67,9 @@ pub fn behavior(
 
     // unstiff has the number 1 precedence
     if *primary_state == PrimaryState::Sitting {
-        commands.set_behavior(Sitting);
+        if !matches!(behavior, BehaviorState::Sitting) {
+            commands.set_behavior(Sitting::default());
+        }
         return;
     }
 
@@ -109,7 +113,7 @@ pub fn behavior(
     let ball_or_origin = most_confident_ball.unwrap_or(Point2::origin());
 
     match *primary_state {
-        PrimaryState::Sitting => commands.set_behavior(Sitting),
+        PrimaryState::Sitting => commands.set_behavior(Sitting::default()),
         PrimaryState::Standby
         | PrimaryState::Penalized
         | PrimaryState::Finished

yggdrasil/src/nao/manager.rs

@@ -480,6 +480,11 @@ impl NaoManager {
         self.set_legs(leg_positions, leg_stiffness, priority)
     }
 
+    pub fn stiff_sit(&mut self, leg_positions: LegJoints<f32>, priority: Priority) -> &mut Self {
+        let leg_stiffness = LegJoints::fill(0.3);
+        self.set_legs(leg_positions, leg_stiffness, priority)
+    }
+
     /// Disable all motors in the arms.
     pub fn unstiff_arms(&mut self, priority: Priority) -> &mut Self {
         self.set_arms(

yggdrasil/src/sensor/fsr.rs

@@ -1,15 +1,22 @@
 use std::time::{Duration, Instant};
 
 use super::SensorConfig;
-use crate::{motion::walk::SwingFootSwitchedEvent, prelude::*};
+use crate::prelude::*;
+use crate::sensor::low_pass_filter::LowPassFilter;
 use bevy::prelude::*;
+use nalgebra::SVector;
+
+use crate::motion::walk::SwingFootSwitchedEvent;
 use nidhogg::{
     types::{FillExt, Fsr, FsrFoot},
     NaoState,
 };
 use serde::{Deserialize, Serialize};
 use serde_with::{serde_as, DurationMilliSeconds};
 
+// Omega for the low-pass filter.
+const OMEGA: f32 = 0.7;
+
 /// Plugin offering the Force Sensitive Resistor (FSR) sensor data of the Nao,
 /// derived from the raw [`NaoState`].
 pub struct FSRSensorPlugin;
@@ -43,13 +50,21 @@ impl Plugin for FSRSensorPlugin {
 pub struct FsrConfig {
     /// Threshold for ground contact detection using average FSR sensor values from both feet.
     pub ground_contact_threshold: f32,
+
+    /// Timeout for change of value of the ground contact state in milliseconds.
+    #[serde_as(as = "DurationMilliSeconds")]
+    pub ground_contact_timeout: Duration,
+
     /// Maximum amount of pressure measured by a single sensor.
     pub max_pressure: f32,
+
     /// Initial value for minimum pressure measured by a single sensor.
     pub min_pressure: f32,
+
     /// The time to sample pressure values before updating the maximum value for each sensor, in milliseconds.
     #[serde_as(as = "DurationMilliSeconds")]
     pub highest_pressure_update_rate: Duration,
+
     /// The number of foot switches required before updating the minimum value for each sensor.
     pub num_foot_switches: u32,
 }
@@ -67,30 +82,67 @@ impl FsrConfig {
 }
 
 /// Struct containing the various contact points of the Nao.
-#[derive(Resource, Default)]
+#[derive(Resource)]
 pub struct Contacts {
     /// Whether the robot has ground contact.
     pub ground: bool,
+
     /// Whether the robot's left foot has ground contact.
     pub left_foot: bool,
+
     /// Whether the robot's right foot has ground contact.
     pub right_foot: bool,
+
+    /// Timestamp of detected change in pressure state.
+    pub last_switched: Instant,
+
+    /// The first-order Butterworth low-pass filter to apply to the FSR sensor data.
+    pub lpf: LowPassFilter<1>,
 }
 
-pub fn force_sensitive_resistor_sensor(
-    nao_state: Res<NaoState>,
-    mut force_sensitive_resistors: ResMut<Fsr>,
-) {
-    force_sensitive_resistors.left_foot = nao_state.fsr.left_foot.clone();
-    force_sensitive_resistors.right_foot = nao_state.fsr.right_foot.clone();
+impl Default for Contacts {
+    fn default() -> Self {
+        Contacts {
+            ground: true,
+            left_foot: false,
+            right_foot: false,
+            last_switched: Instant::now(),
+            lpf: LowPassFilter::new(OMEGA),
+        }
+    }
+}
+
+pub fn force_sensitive_resistor_sensor(nao_state: Res<NaoState>, mut fsr: ResMut<Fsr>) {
+    fsr.left_foot = nao_state.fsr.left_foot.clone();
+    fsr.right_foot = nao_state.fsr.right_foot.clone();
 }
 
-fn update_contacts(config: Res<SensorConfig>, fsr: Res<Fsr>, mut contacts: ResMut<Contacts>) {
+fn update_contacts(
+    config: Res<SensorConfig>,
+    fsr: Res<Fsr>,
+    mut contacts: ResMut<Contacts>,
+    mut last_pressure: Local<bool>,
+) {
     let config = &config.fsr;
 
-    contacts.ground = fsr.avg() >= config.ground_contact_threshold;
     contacts.left_foot = fsr.left_foot.sum() >= config.min_pressure;
     contacts.right_foot = fsr.right_foot.sum() >= config.min_pressure;
+
+    // Retrieve FSR values and apply low-pass filter.
+    let fsr_vector = SVector::<f32, 1>::from([fsr.avg()]);
+    let filtered_fsr = contacts.lpf.update(fsr_vector);
+    let current_pressure = filtered_fsr.x > config.ground_contact_threshold;
+
+    if current_pressure != *last_pressure {
+        contacts.last_switched = Instant::now();
+    }
+
+    // Only update the ground state if timeout duration has elapsed.
+    if contacts.last_switched.elapsed() >= config.ground_contact_timeout {
+        contacts.ground = current_pressure;
+    }
+
+    *last_pressure = current_pressure;
 }
 
 #[derive(Debug, Clone, Default)]