Current state

Author: Zhaoyu Ji

bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml

@@ -14,6 +14,13 @@ walking:
       kick_put_down_phase: 0.8
       kick_rise_factor: 1.5
 
+      fast_kick_knee_offset: 0.05
+      fast_kick_hip_offset: 0.288
+      fast_kick_phase_start: 0.19
+      fast_kick_phase_end: 0.5
+
+
+
       double_support_ratio: 0.0264282002140171
       first_step_swing_factor: 2.9
       foot_distance: 0.179900277671633

bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp

@@ -16,6 +16,7 @@ The original files can be found at:
 #include "bitbots_splines/abstract_engine.hpp"
 #include "bitbots_splines/pose_spline.hpp"
 #include "bitbots_splines/smooth_spline.hpp"
+#include "bitbots_splines/abstract_ik.hpp"
 
 namespace bitbots_quintic_walk {
 
@@ -61,6 +62,8 @@ class WalkEngine : public bitbots_splines::AbstractEngine<WalkRequest, WalkRespo
    */
   [[nodiscard]] bool isLeftSupport() const;
 
+  void fastKick(bitbots_splines::JointGoals &JointGoals);
+
   /**
    * Return true if both feet are currently on the ground
    */

bitbots_motion/bitbots_quintic_walk/src/parameters.yaml

@@ -151,6 +151,26 @@ 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]
+    fast_kick_knee_offset:
+      type: double
+      description: "Offset of knee for fast kick"  
+      validation:
+        bounds<>: [0, 1.2] 
+    fast_kick_hip_offset:
+      type: double
+      description: "Offset of hip for fast kick"  
+      validation:
+        bounds<>: [0, 1.2]  
+    fast_kick_phase_start:
+      type: double
+      description: "Offset of hip for fast kick"  
+      validation:
+        bounds<>: [0, 1]  
+    fast_kick_phase_end:
+      type: double
+      description: "Offset of hip for fast kick"  
+      validation:
+        bounds<>: [0, 1]      
   node:
     engine_freq:
       type: double

bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp

@@ -472,6 +472,7 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) {
                              foot_pos_acc_at_foot_change_.x());
   foot_spline_.x()->addPoint(double_support_length, foot_pos_at_foot_change_.x());
   if (kick_step) {
+    foot_spline_.x()->addPoint(t,x);
     foot_spline_.x()->addPoint(double_support_length + single_support_length * config_.kick_phase,
                                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,
@@ -804,6 +805,46 @@ double WalkEngine::getTrajsTime() const {
   return t;
 }
 
+void WalkEngine::fastKick(bitbots_splines::JointGoals &motor_goals_) {
+  // in this state we do a kick while doing a step
+  if (engine_state_ == WalkState::KICK and is_left_support_foot_ && (phase_ > config_.fast_kick_phase_start && phase_ < config_.fast_kick_phase_end)) {
+    RCLCPP_WARN(node_->get_logger(),
+          "fastKick right");
+    for (size_t i = 0; i < motor_goals_.first.size(); i++) {
+      if (motor_goals_.first[i] == "RHipPitch") {
+        motor_goals_.second[i] -= config_.fast_kick_hip_offset;
+      }
+    }  
+
+    for (size_t i = 0; i < motor_goals_.first.size(); i++) {
+      if (motor_goals_.first[i] == "RKnee") {
+        motor_goals_.second[i] += config_.fast_kick_knee_offset;
+        RCLCPP_WARN(node_->get_logger(),
+          "change goal of joint %ld at %s", i, motor_goals_.first[i].c_str());
+      }
+    }
+  }
+  if (engine_state_ == WalkState::KICK and !is_left_support_foot_ && (phase_ > (config_.fast_kick_phase_start + 0.5) && phase_ < (config_.fast_kick_phase_end + 0.5))) {
+    RCLCPP_WARN(node_->get_logger(),
+            "fastKick left");
+
+    for (size_t i = 0; i < motor_goals_.first.size(); i++) {
+      if (motor_goals_.first[i] == "LHipPitch") {
+        motor_goals_.second[i] += config_.fast_kick_hip_offset;
+      }
+    }
+
+    for (size_t i = 0; i < motor_goals_.first.size(); i++) {
+      if (motor_goals_.first[i] == "LKnee") {
+        motor_goals_.second[i] -= config_.fast_kick_knee_offset;
+      }
+    }
+    
+  }
+}
+
+
+
 bool WalkEngine::isLeftSupport() const { return is_left_support_foot_; }
 
 bool WalkEngine::isDoubleSupport() {

bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp

@@ -198,24 +198,7 @@ bitbots_msgs::msg::JointCommand WalkNode::step(double dt) {
     // compute motor goals from IK
     motor_goals_ = ik_.calculate(current_stabilized_response_);
 
-    if (engine_state_ == WalkState::KICK) {
-      // in this state we do a kick while doing a step
-      if (left_kick_requested_ && !is_left_support_foot_) {
-        for (int i = 0; i < motor_goal_.first.size() - 1; i++) {
-          if (motor_goal.first[i].compare("LKnee")) {
-            motor_goal.second[i] += 0.1;
-          }
-        }
-      }
-      if (right_kick_requested_ && is_left_support_foot_) {
-      }
-
-      if (half_step_finished) {
-        // kick step is finished, go on walking
-        buildTrajectories(TrajectoryType::NORMAL);
-        engine_state_ = WalkState::WALKING;
-      }
-    }
+    walk_engine_.fastKick(motor_goals_);
 
     command.header.stamp = node_->get_clock()->now();
     /*