added pitch to walking kick

bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_robot.yaml

@@ -19,6 +19,11 @@ walking:
       kick_put_down_phase: 0.8
       kick_rise_factor: 1.5
       kick_vel: 0.2
+      kick_early_pitch_prevention_phase: 0.4
+      kick_pitch_downwards_phase: 0.9
+      kick_pitch_downwards: 0.8
+      kick_pitch_upwards_phase: 1.1
+      kick_pitch_upwards: -0.9
       trunk_height: 0.38
       trunk_pause: 0.0
       trunk_phase: -0.1

bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml

@@ -13,6 +13,11 @@ walking:
       kick_phase: 0.28
       kick_put_down_phase: 0.8
       kick_rise_factor: 1.5
+      kick_early_pitch_prevention_phase: 0.4
+      kick_pitch_downwards_phase: 0.9
+      kick_pitch_downwards: 0.8
+      kick_pitch_upwards_phase: 1.1
+      kick_pitch_upwards: -0.9
 
       double_support_ratio: 0.0264282002140171
       first_step_swing_factor: 2.9

bitbots_motion/bitbots_quintic_walk/src/parameters.yaml

@@ -151,6 +151,36 @@ walking:
       description: "Scales the step rise parameter, as the kick is a special case, where the foot is moved higher of the ground than in normal walking (ratio)"
       validation:
         bounds<>: [0.0, 5.0]
+    kick_early_pitch_prevention_phase:
+      type: double
+      description: "Time at which the foot is not supposed to pitch yet, in order to prevent falling to the side (time relative within the kick phase)"
+      default_value: 0.4
+      validation:
+        bounds<>: [0.2, 0.5]
+    kick_pitch_downwards_phase:
+      type: double
+      description: "Time at which the foot pitches downwards during the kick phase"
+      default_value: 0.9
+      validation:
+        bounds<>: [0.6, 0.9]
+    kick_pitch_downwards:
+      type: double
+      description: "Angle at which the foot pitches down (in rad)"
+      default_value: 0.8
+      validation:
+        bounds<>: [0.6, 0.9]
+    kick_pitch_upwards_phase:
+      type: double
+      description: "Time at which the foot pitches upwards during the kick phase"
+      default_value: 1.1
+      validation:
+        bounds<>: [0.95, 1.5]
+    kick_pitch_upwards:
+      type: double
+      description: "Angle at which the foot pitches up (in rad)"
+      default_value: -0.9
+      validation:
+        bounds<>: [-1.5, -0.5]
   node:
     engine_freq:
       type: double

bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp

@@ -476,6 +476,19 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) {
                                support_to_next_.getOrigin().x() + config_.kick_length, config_.kick_vel);
     foot_spline_.x()->addPoint(double_support_length + single_support_length * config_.kick_put_down_phase,
                                support_to_next_.getOrigin().x());
+    // playing around with pitch
+    foot_spline_.pitch()->addPoint(
+        double_support_length + single_support_length * config_.kick_phase * config_.kick_early_pitch_prevention_phase,
+        0.0,
+        config_.kick_vel);  // prevent fall pls
+    foot_spline_.pitch()->addPoint(
+        double_support_length + single_support_length * config_.kick_phase * config_.kick_pitch_downwards_phase,
+        config_.kick_pitch_downwards, config_.kick_vel);
+    foot_spline_.pitch()->addPoint(
+        double_support_length + single_support_length * config_.kick_phase * config_.kick_pitch_upwards_phase,
+        config_.kick_pitch_upwards, config_.kick_vel);
+    // foot_spline_.pitch()->addPoint(double_support_length + single_support_length *
+    // config_.kick_put_down_phase,0,config_.kick_vel);
   } else {
     foot_spline_.x()->addPoint(
         double_support_length + single_support_length * config_.foot_put_down_phase * config_.foot_overshoot_phase,