Add foot step based kick approach

bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp

@@ -66,7 +66,7 @@ class WalkEngine : public bitbots_splines::AbstractEngine<WalkRequest, WalkRespo
    */
   bool isDoubleSupport();
 
-  void requestKick(bool left);
+  void requestKick(tf2::Transform kick_point);
 
   void requestPause();
 
@@ -120,6 +120,8 @@ class WalkEngine : public bitbots_splines::AbstractEngine<WalkRequest, WalkRespo
   // kick handling
   bool left_kick_requested_ = false;
   bool right_kick_requested_ = false;
+  bool positioning_step_to_kick_point_ = false;
+  tf2::Transform kick_point_ = tf2::Transform::getIdentity();
 
   // Current support foot (left or right).
   bool is_left_support_foot_ = false;
@@ -174,6 +176,8 @@ class WalkEngine : public bitbots_splines::AbstractEngine<WalkRequest, WalkRespo
    */
   void stepFromSupport(const tf2::Transform &diff);
 
+  void stepToApproachKick(const tf2::Transform kick_point_and_direction, bool kick_with_left);
+
   /**
    * Set target pose of current support foot using diff orders.
    * Zero vector means in place walking.

bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.hpp

@@ -13,7 +13,9 @@ The original files can be found at:
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2/LinearMath/Transform.h>
 #include <tf2/LinearMath/Vector3.h>
+#include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
 #include <unistd.h>
 
 #include <Eigen/Dense>
@@ -131,7 +133,7 @@ class WalkNode {
 
   void jointStateCb(sensor_msgs::msg::JointState::SharedPtr msg);
 
-  void kickCb(std_msgs::msg::Bool::SharedPtr msg);
+  void kickCb(geometry_msgs::msg::PoseStamped::SharedPtr msg);
 
   double getTimeDelta();
 
@@ -167,7 +169,7 @@ class WalkNode {
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
   rclcpp::Subscription<bitbots_msgs::msg::RobotControlState>::SharedPtr robot_state_sub_;
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
-  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr kick_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr kick_sub_;
   rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
   rclcpp::Subscription<bitbots_msgs::msg::FootPressure>::SharedPtr pressure_sub_left_;
   rclcpp::Subscription<bitbots_msgs::msg::FootPressure>::SharedPtr pressure_sub_right_;
@@ -177,6 +179,10 @@ class WalkNode {
   moveit::core::RobotModelPtr kinematic_model_;
   moveit::core::RobotStatePtr current_state_;
 
+  // TF listener and buffer
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
   WalkStabilizer stabilizer_;
   WalkIK ik_;
   WalkVisualizer visualizer_;

bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp

@@ -123,13 +123,14 @@ WalkResponse WalkEngine::update(double dt) {
       engine_state_ = WalkState::PAUSED;
       pause_requested_ = false;
       return createResponse();
-    } else if (half_step_finished &&
+    } else if (half_step_finished && positioning_step_to_kick_point_ &&
                ((left_kick_requested_ && !is_left_support_foot_) || (right_kick_requested_ && is_left_support_foot_))) {
       // lets do a kick
       buildTrajectories(TrajectoryType::KICK);
       engine_state_ = WalkState::KICK;
       left_kick_requested_ = false;
       right_kick_requested_ = false;
+      positioning_step_to_kick_point_ = false;
     } else if (half_step_finished) {
       // current step is finished, lets see if we have to change state
       if (request_.single_step) {
@@ -395,7 +396,14 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) {
   if (!start_movement) {
     saveCurrentRobotState();
     if (!stop_step) {
-      stepFromOrders(request_.linear_orders, request_.angular_z);
+      if (left_kick_requested_ or right_kick_requested_) {
+        // we are in a kick step, so
+        // we need to position ourselves to the kick point
+        stepToApproachKick(kick_point_, left_kick_requested_);
+      } else {
+        // we are in a normal step, the requested translation/rotation is the step
+        stepFromOrders(request_.linear_orders, request_.angular_z);
+      }
     } else {
       stepFromOrders({0, 0, 0}, 0);
     }
@@ -812,12 +820,15 @@ bool WalkEngine::isDoubleSupport() {
   return is_double_support_spline_.pos(getTrajsTime()) >= 0.5;
 }
 
-void WalkEngine::requestKick(bool left) {
-  if (left) {
-    left_kick_requested_ = true;
+void WalkEngine::requestKick(tf2::Transform kick_point) {
+  // We assume that the kick point is given in the support foot frame
+  if (is_left_support_foot_) {
+    kick_point = left_in_world_ * kick_point;
   } else {
-    right_kick_requested_ = true;
+    kick_point = right_in_world_ * kick_point;
   }
+
+  left_kick_requested_ = true;  // TODO better feet selection
 }
 
 void WalkEngine::requestPause() { pause_requested_ = true; }
@@ -851,10 +862,73 @@ void WalkEngine::stepFromSupport(const tf2::Transform& diff) {
   is_left_support_foot_ = !is_left_support_foot_;
 }
 
+void WalkEngine::stepToApproachKick(const tf2::Transform kick_point_and_direction, bool kick_with_left) {
+  // Compute where it is relative to the support foot, the kick point and direction are given in "world" / walk odom
+  // coordinates
+  tf2::Transform kick_point_from_support;
+  if (is_left_support_foot_) {
+    kick_point_from_support = right_in_world_.inverseTimes(kick_point_and_direction);
+  } else {
+    kick_point_from_support = left_in_world_.inverseTimes(kick_point_and_direction);
+  }
+
+  double distance_to_kick_point = 0.15;  // Offset to the kick point, because we don't want to accidentally touch it and
+                                         // our feet have a certain size
+  tf2::Transform kick_point_from_support_with_offset = tf2::Transform::getIdentity();
+  kick_point_from_support_with_offset.getOrigin().setX(-distance_to_kick_point);
+
+  tf2::Transform kick_foot_placement = kick_point_from_support * kick_point_from_support_with_offset;
+
+  tf2::Transform foot_goal;
+
+  if (is_left_support_foot_ == kick_with_left) {
+    // We can not place the foot on the kick point, because it is the support foot
+    // Therfore we try to place the other foot next to it in an attempt to get as close as possible
+    // and place the kick foot in the step after
+
+    tf2::Transform foot_distance = tf2::Transform::getIdentity();
+    if (is_left_support_foot_) {
+      foot_distance.getOrigin().setY(config_.foot_distance);
+    } else {
+      foot_distance.getOrigin().setY(-config_.foot_distance);
+    }
+    foot_goal = kick_foot_placement * foot_distance;
+    RCLCPP_WARN(node_->get_logger(), "Placing the kick foot not possible, placing the other foot next to it");
+  } else {
+    // Now we do the step to the kick point (the positioning step)
+    positioning_step_to_kick_point_ = true;
+
+    RCLCPP_WARN(node_->get_logger(), "Placing the kick foot");
+
+    foot_goal = kick_foot_placement;
+  }
+
+  // Allow lateral step only on external foot
+  //(internal foot will return to zero pose)
+  // We need to subtract a small value from the foot distance, otherwise the claming is triggered which,
+  // in turn, delays the kick, as we might not be able to reach the kick point yet
+  if ((is_left_support_foot_ && foot_goal.getOrigin().getY() < config_.foot_distance - 0.01) ||
+      (!is_left_support_foot_ && foot_goal.getOrigin().getY() > -config_.foot_distance + 0.01)) {
+    RCLCPP_WARN(node_->get_logger(), "Side step not possible, placing the foot back to zero pose");
+
+    if (is_left_support_foot_) {
+      foot_goal.getOrigin().setY(config_.foot_distance);
+    } else {
+      foot_goal.getOrigin().setY(-config_.foot_distance);
+    }
+
+    // If we tried to position the foot with this step this is not possible anymore and we need to do a normal step
+    positioning_step_to_kick_point_ = false;
+  }
+
+  // Now we can perform the step
+  stepFromSupport(foot_goal);
+}
+
 void WalkEngine::stepFromOrders(const std::vector<double>& linear_orders, double angular_z) {
   // Compute step diff in next support foot frame
-  tf2::Transform tmp_diff = tf2::Transform();
-  tmp_diff.setIdentity();
+  tf2::Transform tmp_diff = tf2::Transform::getIdentity();
+
   // No change in forward step and upward step
   tmp_diff.getOrigin()[0] = linear_orders[0];
   tmp_diff.getOrigin()[2] = linear_orders[2];

bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp

@@ -15,6 +15,8 @@ WalkNode::WalkNode(rclcpp::Node::SharedPtr node, const std::string& ns,
       param_listener_(node_),
       config_(param_listener_.get_params()),
       walk_engine_(node_, config_.engine),
+      tf_buffer_(node_->get_clock()),
+      tf_listener_(tf_buffer_, node),
       stabilizer_(node_),
       ik_(node_, config_.node.ik),
       visualizer_(node_, config_.node.tf) {
@@ -73,7 +75,8 @@ WalkNode::WalkNode(rclcpp::Node::SharedPtr node, const std::string& ns,
       "robot_state", 1, std::bind(&WalkNode::robotStateCb, this, _1));
   joint_state_sub_ = node_->create_subscription<sensor_msgs::msg::JointState>(
       "joint_states", 1, std::bind(&WalkNode::jointStateCb, this, _1));
-  kick_sub_ = node_->create_subscription<std_msgs::msg::Bool>("kick", 1, std::bind(&WalkNode::kickCb, this, _1));
+  kick_sub_ =
+      node_->create_subscription<geometry_msgs::msg::PoseStamped>("kick", 1, std::bind(&WalkNode::kickCb, this, _1));
   imu_sub_ = node_->create_subscription<sensor_msgs::msg::Imu>("imu/data", 1, std::bind(&WalkNode::imuCb, this, _1));
   pressure_sub_left_ = node_->create_subscription<bitbots_msgs::msg::FootPressure>(
       "foot_pressure_left/filtered", 1, std::bind(&WalkNode::pressureLeftCb, this, _1));
@@ -520,7 +523,20 @@ void WalkNode::jointStateCb(const sensor_msgs::msg::JointState::SharedPtr msg) {
   }
 }
 
-void WalkNode::kickCb(const std_msgs::msg::Bool::SharedPtr msg) { walk_engine_.requestKick(msg->data); }
+void WalkNode::kickCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
+  // Transform the goal pose of the contact point to the support foot frame
+  std::string support_foot_frame =
+      walk_engine_.isLeftSupport() ? config_.node.tf.l_sole_frame : config_.node.tf.r_sole_frame;
+  try {
+    tf2::Transform transform;
+    tf2::fromMsg(tf_buffer_.transform(msg, support_foot_frame)->pose, transform);
+    walk_engine_.requestKick(transform);
+  } catch (tf2::TransformException& ex) {
+    RCLCPP_ERROR(node_->get_logger(), "Skipping kick, Could not transform kick goal to support foot frame: %s",
+                 ex.what());
+    return;
+  }
+}
 
 nav_msgs::msg::Odometry WalkNode::getOdometry() {
   // odometry to trunk is transform to support foot * transform from support to trunk