Adjust catching steps (HULKs#1206)

* add longitudinal offset to catching steps * cleanify * re-enable catching steps
PatrickGoettsch · Jul 16, 2024 · e0b90e8 · e0b90e8
1 parent caa2d74
commit e0b90e8
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 21 deletions.
diff --git a/crates/control/src/fall_state_estimation.rs b/crates/control/src/fall_state_estimation.rs
@@ -62,7 +62,7 @@ pub struct CycleContext {
         Parameter<Vector3<Robot>, "fall_state_estimation.gravitational_force_sitting">,
     gravity_acceleration: Parameter<f32, "physical_constants.gravity_acceleration">,
     sitting_pose: Parameter<Joints<f32>, "fall_state_estimation.sitting_pose">,
-    use_catching_steps: Parameter<bool, "walking_engine.catching_steps.use_catching_steps">,
+    catching_steps_enabled: Parameter<bool, "walking_engine.catching_steps.enabled">,
 
     robot_orientation: RequiredInput<Option<Orientation3<Field>>, "robot_orientation?">,
     sensor_data: Input<SensorData, "sensor_data">,
@@ -147,7 +147,7 @@ impl FallStateEstimation {
             None
         };
 
-        let falling_angle_threshold = if *context.use_catching_steps {
+        let falling_angle_threshold = if *context.catching_steps_enabled {
             context.falling_angle_threshold_forward_with_catching_steps
         } else {
             context.falling_angle_threshold_forward

diff --git a/crates/walking_engine/src/mode/catching.rs b/crates/walking_engine/src/mode/catching.rs
@@ -30,21 +30,12 @@ pub struct Catching {
 impl Catching {
     pub fn new(context: &Context, support_side: Side) -> Self {
         let parameters = &context.parameters;
-        let target_overestimation_factor = context
-            .parameters
-            .catching_steps
-            .target_overestimation_factor;
 
         let step_duration = parameters.base.step_duration;
         let start_feet =
             Feet::from_joints(context.robot_to_walk, &context.current_joints, support_side);
 
-        let end_feet = catching_end_feet(
-            parameters,
-            *context.zero_moment_point,
-            target_overestimation_factor,
-            support_side,
-        );
+        let end_feet = catching_end_feet(parameters, *context.zero_moment_point, support_side);
         let max_swing_foot_lift =
             parameters.base.foot_lift_apex + parameters.catching_steps.additional_foot_lift;
         let midpoint = parameters.catching_steps.midpoint;
@@ -91,14 +82,17 @@ impl Catching {
 fn catching_end_feet(
     parameters: &Parameters,
     zero_moment_point: Point2<Ground>,
-    target_overestimation_factor: f32,
     support_side: Side,
 ) -> Feet {
+    let target_overestimation_factor = parameters.catching_steps.target_overestimation_factor;
+    let longitudinal_zero_moment_point_offset = parameters.catching_steps.longitudinal_offset;
     let max_adjustment = parameters.catching_steps.max_adjustment;
+
     Feet::end_from_request(
         parameters,
         Step {
-            forward: (zero_moment_point.x() * target_overestimation_factor)
+            forward: ((zero_moment_point.x() + longitudinal_zero_moment_point_offset)
+                * target_overestimation_factor)
                 .clamp(-max_adjustment, max_adjustment),
             left: (zero_moment_point.y() * target_overestimation_factor)
                 .clamp(-max_adjustment, max_adjustment),
@@ -150,10 +144,6 @@ impl Catching {
         self.step.plan.end_feet = catching_end_feet(
             context.parameters,
             *context.zero_moment_point,
-            context
-                .parameters
-                .catching_steps
-                .target_overestimation_factor,
             self.step.plan.support_side,
         );
         self.step.tick(context);

diff --git a/crates/walking_engine/src/mode/walking.rs b/crates/walking_engine/src/mode/walking.rs
@@ -60,7 +60,7 @@ impl WalkTransition for Walking {
     fn walk(self, context: &Context, requested_step: Step) -> Mode {
         let current_step = self.step;
 
-        if context.parameters.catching_steps.use_catching_steps {
+        if context.parameters.catching_steps.enabled {
             if context.robot_to_ground.is_none() {
                 return Mode::Stopping(Stopping::new(context, current_step.plan.support_side));
             }

diff --git a/crates/walking_engine/src/parameters.rs b/crates/walking_engine/src/parameters.rs
@@ -84,11 +84,12 @@ pub struct GyroBalancingParameters {
     PathIntrospect,
 )]
 pub struct CatchingStepsParameters {
+    pub enabled: bool,
     pub catching_step_zero_moment_point_frame_count_threshold: i32,
-    pub use_catching_steps: bool,
     pub max_adjustment: f32,
     pub midpoint: f32,
     pub target_overestimation_factor: f32,
+    pub longitudinal_offset: f32,
     pub additional_foot_lift: f32,
 }
 

diff --git a/etc/parameters/default.json b/etc/parameters/default.json
@@ -425,11 +425,12 @@
       "walk_height": 0.22
     },
     "catching_steps": {
+      "enabled": true,
       "catching_step_zero_moment_point_frame_count_threshold": 5,
-      "use_catching_steps": true,
       "max_adjustment": 0.07,
       "midpoint": 0.3,
       "target_overestimation_factor": 1.05,
+      "longitudinal_offset": -0.05,
       "additional_foot_lift": 0.02
     },
     "gyro_balancing": {