Limit turn_acceleration when previous forward step magnitude was large

to reduce chance of tipping due to too large forward momentum

Author: schluis

crates/control/src/motion/step_planner.rs

@@ -66,7 +66,7 @@ impl StepPlanner {
             None
         };
 
-        let (max_turn_left, max_turn_right) = if let Some(support_side) = support_side {
+        let (mut max_turn_left, mut max_turn_right) = if let Some(support_side) = support_side {
             if support_side == Side::Left {
                 (-*context.max_inside_turn, context.max_step_size.turn)
             } else {

crates/walking_engine/src/mode/walking.rs

@@ -43,12 +43,21 @@ impl Walking {
             )
         };
 
+        let turn_acceleration =
+            if last_requested_step.forward.abs() > context.parameters.forward_turn_threshold {
+                context.parameters.max_turn_acceleration * context.parameters.forward_turn_reduction
+            } else {
+                context.parameters.max_turn_acceleration
+            };
+
         let requested_step = Step {
             forward: last_requested_step.forward
                 + (requested_step.forward - last_requested_step.forward)
                     .clamp(backward_acceleration, forward_acceleration),
             left: requested_step.left,
-            turn: requested_step.turn,
+            turn: last_requested_step.turn
+                + (requested_step.turn - last_requested_step.turn)
+                    .clamp(-turn_acceleration, turn_acceleration),
         };
         let plan = StepPlan::new_from_request(context, requested_step, support_side);
         let step = StepState::new(plan);

crates/walking_engine/src/parameters.rs

@@ -17,17 +17,20 @@ pub struct Parameters {
     pub catching_steps: CatchingStepsParameters,
     pub gyro_balancing: GyroBalancingParameters,
     pub foot_leveling: FootLevelingParameters,
-    pub max_forward_acceleration: f32,
+    pub forward_turn_reduction: f32,
+    pub forward_turn_threshold: f32,
     pub max_base_inside_turn: f32,
+    pub max_forward_acceleration: f32,
     pub max_inside_turn_increase: f32,
     pub max_rotation_speed: f32,
     pub max_step_duration: Duration,
     pub max_support_foot_lift_speed: f32,
+    pub max_turn_acceleration: f32,
     pub min_step_duration: Duration,
     pub sole_pressure_threshold: f32,
     pub step_midpoint: Step,
-    pub stiffnesses: Stiffnesses,
     pub stiffness_loss_compensation: StiffnessLossCompensation,
+    pub stiffnesses: Stiffnesses,
     pub swinging_arms: SwingingArmsParameters,
 }
 

etc/parameters/default.json

@@ -384,7 +384,7 @@
     "rotation_exponent": 2.0,
     "request_scale": {
       "forward": 1.0,
-      "left": 1.4,
+      "left": 1.4, // todo
       "turn": 1.0
     }
   },
@@ -458,12 +458,15 @@
       "roll_scale": 0.0872665,
       "start_reduce_to_zero": 0.75
     },
-    "max_rotation_speed": 50000.0,
-    "max_forward_acceleration": 0.01,
+    "forward_turn_reduction": 0.7,
+    "forward_turn_threshold": 0.45,
     "max_base_inside_turn": 0.1,
+    "max_forward_acceleration": 0.01,
     "max_inside_turn_increase": 4,
+    "max_rotation_speed": 50000.0,
     "max_step_duration": { "nanos": 0, "secs": 2 },
     "max_support_foot_lift_speed": 0.025,
+    "max_turn_acceleration": 0.7,
     "min_step_duration": { "nanos": 100000000, "secs": 0 },
     "sole_pressure_threshold": 0.4,
     "step_midpoint": {