rl-walking: add stabilization step(s) before standing still (HULKs#2180)

Co-authored-by: Johannes <149575214+pejotejo@users.noreply.github.com>

Author: alexschmander

Cargo.lock

@@ -492,6 +492,8 @@ pub enum StepPlannerMode {
 )]
 pub struct RLWalkingParameters {
     pub gait_frequency: f32,
+    pub stabilizing_interval_compression_factor: f32,
+    pub stabilizing_interval_completion_threshold: f32,
     pub number_of_actions: usize,
     pub number_of_observations: usize,
     pub torque_limits: Joints,

crates/types/src/parameters.rs

@@ -6,6 +6,7 @@ license.workspace = true
 homepage.workspace = true
 
 [dependencies]
+approx = { workspace = true }
 booster = { workspace = true }
 color-eyre = { workspace = true }
 context_attribute = { workspace = true }

crates/walking_inference/Cargo.toml

@@ -1,5 +1,6 @@
 use std::f32::consts::PI;
 
+use approx::AbsDiffEq;
 use booster::{JointsMotorState, MotorState};
 use color_eyre::{eyre::ContextCompat, Result};
 use coordinate_systems::{Ground, Robot};
@@ -75,12 +76,34 @@ impl WalkingInferenceInputs {
             _ => todo!(),
         };
 
-        let (gait_frequency, last_gait_progress) =
-            if linear_velocity_command.norm() < 1e-5 && angular_velocity_command.abs() < 1e-5 {
-                (0.0, 0.0)
-            } else {
-                (walking_parameters.gait_frequency, last_gait_progress)
-            };
+        let stabilizing_interval_progress = last_gait_progress
+            + walking_parameters.gait_frequency * cycle_time.last_cycle_duration.as_secs_f32();
+
+        let is_step_finished = (stabilizing_interval_progress
+            * walking_parameters.stabilizing_interval_compression_factor
+            * PI)
+            .sin()
+            .abs_diff_eq(
+                &0.0,
+                walking_parameters.stabilizing_interval_completion_threshold,
+            )
+            || (stabilizing_interval_progress
+                * walking_parameters.stabilizing_interval_compression_factor
+                * PI)
+                .cos()
+                .abs_diff_eq(
+                    &1.0,
+                    walking_parameters.stabilizing_interval_completion_threshold,
+                );
+
+        let (gait_frequency, last_gait_progress) = if linear_velocity_command.norm() < 1e-5
+            && angular_velocity_command.abs() < 1e-5
+            && is_step_finished
+        {
+            (0.0, 0.0)
+        } else {
+            (walking_parameters.gait_frequency, last_gait_progress)
+        };
         let gait_progress =
             last_gait_progress + gait_frequency * cycle_time.last_cycle_duration.as_secs_f32();
 

crates/walking_inference/src/inputs.rs

@@ -1,6 +1,8 @@
 {
   "rl_walking": {
     "gait_frequency": 1,
+    "stabilizing_interval_completion_threshold": 0.01,
+    "stabilizing_interval_compression_factor": 1.0,
     "number_of_actions": 12,
     "number_of_observations": 47,
     "torque_limits": {