Tune ground provider (HULKs#2010)

* fix head json

* low-pass-filter ground_provider

* tune catching-steps, mirror ground_to_robot, tune zmp in walk, reduce
support rectangle

Co-authored-by: ole.flb <[email protected]>

---------

Co-authored-by: ole.flb <[email protected]>

Author: schluis

crates/control/src/ground_provider.rs

@@ -1,23 +1,28 @@
 use color_eyre::Result;
+use filtering::low_pass_filter::LowPassFilter;
 use serde::{Deserialize, Serialize};
 
 use context_attribute::context;
-use coordinate_systems::{Field, Ground, Robot};
+use coordinate_systems::{Ground, Robot};
 use framework::MainOutput;
-use linear_algebra::{vector, Isometry3, Orientation3};
-use types::{robot_kinematics::RobotKinematics, support_foot::Side};
+use linear_algebra::{vector, Isometry3, Orientation3, Vector2};
+use types::{robot_kinematics::RobotKinematics, sensor_data::SensorData, support_foot::Side};
 
 #[derive(Deserialize, Serialize)]
-pub struct GroundProvider {}
+pub struct GroundProvider {
+    roll_pitch_filter: LowPassFilter<Vector2<Robot>>,
+}
 
 #[context]
-pub struct CreationContext {}
+pub struct CreationContext {
+    roll_pitch_smoothing_factor: Parameter<f32, "ground_provider.roll_pitch_smoothing_factor">,
+}
 
 #[context]
 pub struct CycleContext {
     robot_kinematics: Input<RobotKinematics, "robot_kinematics">,
-    robot_orientation: RequiredInput<Option<Orientation3<Field>>, "robot_orientation?">,
     support_side: RequiredInput<Option<Side>, "support_foot.support_side?">,
+    sensor_data: Input<SensorData, "sensor_data">,
 }
 
 #[context]
@@ -28,16 +33,28 @@ pub struct MainOutputs {
 }
 
 impl GroundProvider {
-    pub fn new(_context: CreationContext) -> Result<Self> {
-        Ok(Self {})
+    pub fn new(context: CreationContext) -> Result<Self> {
+        Ok(Self {
+            roll_pitch_filter: LowPassFilter::with_smoothing_factor(
+                vector![0.0, 0.0],
+                *context.roll_pitch_smoothing_factor,
+            ),
+        })
     }
 
     pub fn cycle(&mut self, context: CycleContext) -> Result<MainOutputs> {
         struct LeftSoleHorizontal;
         struct RightSoleHorizontal;
 
-        let (roll, pitch, _) = context.robot_orientation.inner.euler_angles();
-        let imu_orientation = Orientation3::from_euler_angles(roll, pitch, 0.0).mirror();
+        self.roll_pitch_filter
+            .update(context.sensor_data.inertial_measurement_unit.roll_pitch);
+
+        let imu_orientation = Orientation3::from_euler_angles(
+            self.roll_pitch_filter.state().x(),
+            self.roll_pitch_filter.state().y(),
+            0.0,
+        )
+        .mirror();
 
         let left_sole_horizontal_to_robot = Isometry3::from_parts(
             context

crates/walking_engine/src/mode/catching.rs

@@ -245,19 +245,18 @@ pub fn should_catch(context: &Context, end_feet: Feet, support_side: Side) -> bo
         Feet::from_joints(robot_to_walk, &context.last_actuated_joints, support_side);
 
     let zmp = context.zero_moment_point;
-    let zmp_scaling_x = if zmp.coords().x() < 0.0 {
-        catching_steps.zero_moment_point_x_scale_backward
+    let target_scaling_x = if zmp.coords().x() < 0.0 {
+        catching_steps.target_x_scale_backward
     } else {
-        catching_steps.zero_moment_point_x_scale_forward
+        catching_steps.target_x_scale_forward
     };
 
-    let tuned_zmp = zmp
+    let target = (robot_to_walk * ground_to_robot * zmp.extend(0.0))
+        .xy()
         .coords()
-        .component_mul(&vector![zmp_scaling_x, 1.0])
+        .component_mul(&vector![target_scaling_x, 1.0])
         .as_point();
 
-    let target = (robot_to_walk * ground_to_robot * tuned_zmp.extend(0.0)).xy();
-
     is_outside_support_polygon(end_feet, support_side, target, current_feet)
 }
 

crates/walking_engine/src/parameters.rs

@@ -126,8 +126,8 @@ pub struct FootLevelingParameters {
 )]
 pub struct CatchingStepsParameters {
     pub enabled: bool,
-    pub zero_moment_point_x_scale_backward: f32,
-    pub zero_moment_point_x_scale_forward: f32,
+    pub target_x_scale_backward: f32,
+    pub target_x_scale_forward: f32,
     pub max_target_distance: f32,
     pub over_estimation_factor: f32,
 }

etc/parameters/default.json

@@ -1,4 +1,7 @@
 {
+  "ground_provider": {
+    "roll_pitch_smoothing_factor": 0.25
+  },
   "pose_detection": {
     "enable": true,
     "maximum_intersection_over_union": 0.45,
@@ -361,10 +364,10 @@
     },
     "catching_steps": {
       "enabled": true,
-      "zero_moment_point_x_scale_backward": 0.5,
-      "zero_moment_point_x_scale_forward": 0.8,
-      "max_target_distance": 0.15,
-      "over_estimation_factor": 0.6
+      "target_x_scale_backward": 1.1,
+      "target_x_scale_forward": 1.0,
+      "max_target_distance": 0.1,
+      "over_estimation_factor": 0.4
     },
     "gyro_balancing": {
       "noise_scale": [0.5, 0.5],
@@ -387,7 +390,7 @@
       }
     },
     "dynamic_interpolation_speed": {
-      "active_range": [-0.1745, -0.0873],
+      "active_range": [-0.1445, -0.0573],
       "max_reduction": 0.7
     },
     "foot_leveling": {
@@ -404,7 +407,7 @@
     "forward_turn_threshold": 0.45,
     "foot_support": {
       "min": [-0.02, -0.02],
-      "max": [0.08, 0.02]
+      "max": [0.04, 0.02]
     },
     "max_rotation_speed": 50000.0,
     "max_forward_acceleration": 0.01,

etc/parameters/head.P0000074A09S31C00029.json

@@ -0,0 +1 @@
+{}