add parameter to make foot move further

bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg

@@ -36,6 +36,39 @@ group_engine_distances.add("trunk_pitch", double_t, 4,
 group_engine_distances.add("trunk_yaw", double_t, 4,
                            "Yaw of the trunk, positive means turning toward the kicking foot [rad]",
                            min=-1, max=1)
+group_engine_distances.add("foot_extra_forward", double_t, 4,
+                           "How much the foot position during kick will be shifted forward to make sure we actually hit the ball [m]",
+                           min=0, max=1)
+group_engine_distances.add("foot_pitch", double_t, 4,
+                           "Pitch of the kicking foot [rad]",
+                           min=-1, max=1)
+group_engine_distances.add("foot_rise_lower", double_t, 4,
+                           "Pitch of the kicking foot [rad]",
+                           min=-1, max=1)
+group_engine_distances.add("foot_rise_kick", double_t, 4,
+                           "Pitch of the kicking foot [rad]",
+                           min=-1, max=1)
+group_engine_distances.add("earlier_time", double_t, 4,
+                           "Pitch of the kicking foot [rad]",
+                           min=-1, max=1)
+group_engine_distances.add("low_x", double_t, 4,
+                           "Pitch of the kicking foot [rad]",
+                           min=0, max=1)
+group_engine_distances.add("low_x_speed", double_t, 4,
+                           "Pitch of the kicking foot [rad]",
+                           min=0, max=100)
+
+group_engine_distances.add("rise_length", double_t, 4,
+                           "Pitch of the kicking foot [rad]",
+                           min=0, max=1)
+group_engine_distances.add("windup_length", double_t, 4,
+                           "Pitch of the kicking foot [rad]",
+                           min=0, max=1)
+group_engine_distances.add("windup_alpha", double_t, 4,
+                           "Pitch of the kicking foot [rad]",
+                           min=-1, max=1)
+
+
 
 group_engine_timings = group_engine.add_group("Timings")
 group_engine_timings.add("move_trunk_time", double_t, 3,
@@ -47,6 +80,9 @@ group_engine_timings.add("raise_foot_time", double_t, 3,
 group_engine_timings.add("move_to_ball_time", double_t, 3,
                          "How long it should take to move the raised foot to the windup point [s]",
                          min=0)
+group_engine_timings.add("low_time", double_t, 3,
+                         "How long it should take to move the raised foot to the windup point [s]",
+                         min=0)
 group_engine_timings.add("kick_time", double_t, 3,  # TODO replace this with dynamic calculation
                          "This will be removed in the future [s]",
                          min=0)

bitbots_dynamic_kick/config/kick_config.yaml

@@ -4,16 +4,27 @@
 engine_rate: 500
 foot_rise: 0.08
 foot_distance: 0.2
-kick_windup_distance: 0.29
+kick_windup_distance: 0.35
 trunk_height: 0.39
 trunk_roll: 0.101229096615671
 trunk_pitch: -0.136135681655558
 trunk_yaw: 0.0558505360638186
+foot_extra_forward: 0.00
+foot_pitch: 0.00
+foot_rise_lower: 0.04
+foot_rise_kick: 0.08
+low_x: 0
+low_x_speed: 10
+
+rise_length: 0.35
+windup_length: 0.35
+windup_alpha: 0.2
 
 # Timings
 move_trunk_time: 0.56
 raise_foot_time: 0.15
 move_to_ball_time: 0.19
+low_time: 0.2
 kick_time: 0.15
 move_back_time: 0.07
 lower_foot_time: 0.06

bitbots_dynamic_kick/config/kick_config_unstable.yaml

@@ -17,3 +17,4 @@ unstable:
   trunk_roll: 0.102974425867665
   trunk_yaw: 0.296705972839036
   use_center_of_pressure: false
+  foot_x_extra: 0.00

bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h

@@ -5,6 +5,7 @@
 #include <optional>
 #include <Eigen/Geometry>
 #include <rot_conv/rot_conv.h>
+#include <bitbots_splines/utils.h>
 #include <bitbots_splines/pose_spline.h>
 #include <bitbots_splines/position_spline.h>
 #include <bitbots_splines/abstract_engine.h>
@@ -25,6 +26,7 @@ struct KickParams {
   double foot_distance;
   double kick_windup_distance;
   double trunk_height;
+  double foot_extra_forward;
 
   double trunk_roll;
   double trunk_pitch;
@@ -42,6 +44,21 @@ struct KickParams {
   double stabilizing_point_y;
 
   double choose_foot_corridor_width;
+
+  double foot_pitch;
+  double foot_rise_lower;
+  double foot_rise_kick;
+
+  double earlier_time;
+  double low_time;
+  double low_x;
+  double low_x_speed;
+
+  double rise_length;
+  double windup_length;
+  double windup_alpha;
+
+
 };
 
 /**
@@ -52,6 +69,7 @@ class PhaseTimings {
   double move_trunk;
   double raise_foot;
   double windup;
+  double low;
   double kick;
   double move_back;
   double lower_foot;
@@ -78,8 +96,8 @@ class KickEngine : public bitbots_splines::AbstractEngine<KickGoals, KickPositio
    * @param ball_position Position of the ball
    * @param kick_direction Direction into which to kick the ball
    * @param kick_speed Speed with which to kick the ball
-   * @param r_foot_pose Current pose of right foot in l_sole frame
-   * @param l_foot_pose Current pose of left foot in r_sole frame
+   * @param r_foot_pose Current pose of right foot in l_toe frame
+   * @param l_foot_pose Current pose of left foot in r_toe frame
    *
    * @throws tf2::TransformException when goal cannot be converted into needed tf frames
    */
@@ -116,6 +134,7 @@ class KickEngine : public bitbots_splines::AbstractEngine<KickGoals, KickPositio
   bitbots_splines::PoseSpline getTrunkSplines() const;
 
   void setParams(KickParams params);
+  void setTrunkToHip(Eigen::Isometry3d trunk_to_hip_l, Eigen::Isometry3d trunk_to_hip_r);
 
   /**
    * Get the current phase of the engine
@@ -124,6 +143,10 @@ class KickEngine : public bitbots_splines::AbstractEngine<KickGoals, KickPositio
 
   Eigen::Vector3d getWindupPoint();
 
+  Eigen::Vector3d getKickPoint();
+  Eigen::Vector3d getBallPoint();
+
+
   /**
    * Set a pointer to the current state of the robot, updated from joint states
    */
@@ -139,8 +162,11 @@ class KickEngine : public bitbots_splines::AbstractEngine<KickGoals, KickPositio
   KickParams params_;
   PhaseTimings phase_timings_;
   Eigen::Vector3d windup_point_;
+  Eigen::Vector3d kick_point_;
   robot_state::RobotStatePtr current_state_;
-
+  double ball_radius_;
+  Eigen::Isometry3d trunk_to_hip_l_;
+  Eigen::Isometry3d trunk_to_hip_r_;
   /**
    *  Calculate splines for a complete kick whereby is_left_kick_ should already be set correctly
    *
@@ -154,6 +180,9 @@ class KickEngine : public bitbots_splines::AbstractEngine<KickGoals, KickPositio
    */
   Eigen::Vector3d calcKickWindupPoint();
 
+  Eigen::Vector3d calcKickPoint();
+
+
   /**
    * Choose with which foot the kick should be performed
    *
@@ -178,7 +207,7 @@ class KickEngine : public bitbots_splines::AbstractEngine<KickGoals, KickPositio
 
   /**
    * Transform then goal into our support_foots frame
-   * @param support_foot_frame Name of the support foots frame, meaning where to transform to
+   * @param hip_frame Name of the flying foots hip frame, meaning where to transform to
    * @param trunk_to_base_footprint Pose of the base_footprint relative to the trunk
    * @param ball_position Position of the ball
    * @param kick_direction Direction in which to kick the ball
@@ -187,7 +216,7 @@ class KickEngine : public bitbots_splines::AbstractEngine<KickGoals, KickPositio
    * @throws tf2::TransformException when goal cannot be transformed into support_foot_frame
    */
   std::pair<Eigen::Vector3d, Eigen::Quaterniond> transformGoal(
-      const std::string &support_foot_frame,
+      const std::string &hip_frame,
       const Eigen::Isometry3d &trunk_to_base_footprint,
       const Eigen::Vector3d &ball_position,
       const Eigen::Quaterniond &kick_direction);

bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h

@@ -107,8 +107,9 @@ class KickNode {
   robot_state::RobotStatePtr current_state_;
   DynamicKickConfig unstable_config_;
   std::optional<DynamicKickConfig> normal_config_;
+  double ball_radius_;
 
-  std::string base_link_frame_, base_footprint_frame_, l_sole_frame_, r_sole_frame_;
+  std::string base_link_frame_, base_footprint_frame_, l_toe_frame_, r_toe_frame_, r_hip_frame_, l_hip_frame_;
 
     /**
    * Do main loop in which KickEngine::update() gets called repeatedly.

bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h

@@ -4,12 +4,15 @@
 #include <Eigen/Geometry>
 #include <bitbots_dynamic_kick/KickDebug.h>
 
+#define M_TAU M_PI * 2
+
 namespace bitbots_dynamic_kick {
 
 struct KickPositions {
   bool is_left_kick = true;
   Eigen::Isometry3d trunk_pose;
   Eigen::Isometry3d flying_foot_pose;
+  Eigen::Isometry3d flying_foot_leg_space;
   bool cop_support_point = false;
   double engine_time;
 };
@@ -19,6 +22,7 @@ struct KickGoals {
   Eigen::Quaterniond kick_direction;
   double kick_speed = 0;
   Eigen::Isometry3d trunk_to_base_footprint;
+  double ball_radius;
 };
 
 enum KickPhase {

bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h

@@ -47,6 +47,11 @@ class Visualizer : bitbots_splines::AbstractVisualizer {
 
   void displayWindupPoint(const Eigen::Vector3d &kick_windup_point, const std::string &support_foot_frame);
 
+  void displayKickPoint(const Eigen::Vector3d &kick_windup_point, const std::string &support_foot_frame);
+
+  void displayBallPoint(const Eigen::Vector3d &ball_point, const std::string &support_foot_frame);
+
+
   void publishGoals(const KickPositions &positions,
                     const KickPositions &stabilized_positions,
                     const robot_state::RobotStatePtr &robot_state,
@@ -58,6 +63,8 @@ class Visualizer : bitbots_splines::AbstractVisualizer {
   ros::Publisher foot_spline_publisher_;
   ros::Publisher trunk_spline_publisher_;
   ros::Publisher windup_publisher_;
+  ros::Publisher kick_point_publisher_;
+  ros::Publisher ball_point_publisher_;
   ros::Publisher debug_publisher_;
   std::string base_topic_;
   const std::string marker_ns_ = "bitbots_dynamic_kick";

bitbots_dynamic_kick/launch/dynamic_kick.launch

@@ -8,7 +8,7 @@
     <rosparam command="load" file="$(find bitbots_dynamic_kick)/config/kick_config_unstable.yaml" />
     <param name="base_link_frame" value="$(arg tf_prefix)base_link"/>
     <param name="base_footprint_frame" value="$(arg tf_prefix)base_footprint"/>
-    <param name="r_sole_frame" value="$(arg tf_prefix)r_sole"/>
-    <param name="l_sole_frame" value="$(arg tf_prefix)l_sole"/>
+    <param name="r_toe_frame" value="$(arg tf_prefix)r_toe"/>
+    <param name="l_toe_frame" value="$(arg tf_prefix)l_toe"/>
   </node>
 </launch>

bitbots_dynamic_kick/msg/KickDebug.msg

@@ -30,3 +30,5 @@ geometry_msgs/Pose flying_foot_pose_goal
 geometry_msgs/Pose flying_foot_pose_stabilized_goal
 geometry_msgs/Pose flying_foot_pose_ik_result
 geometry_msgs/Point flying_foot_position_ik_offset
+
+geometry_msgs/Pose flying_foot_pose_goal_leg_space

bitbots_dynamic_kick/scripts/dummy_client.py

@@ -83,14 +83,14 @@ def feedback_cb(feedback):
     goal.header.stamp = rospy.Time.now()
     frame_prefix = "" if os.environ.get("ROS_NAMESPACE") is None else os.environ.get("ROS_NAMESPACE") + "/"
     goal.header.frame_id = frame_prefix + "base_footprint"
-    goal.ball_position.x = 0.2
+    goal.ball_position.x = 0.1
     goal.ball_position.y = args.ball_y
     goal.ball_position.z = 0
     goal.unstable = args.unstable
 
     goal.kick_direction = Quaternion(*quaternion_from_euler(0, 0, math.radians(args.kick_direction)))
 
-    goal.kick_speed = 6.7 if args.unstable else 1
+    goal.kick_speed = 6.7 if args.unstable else 0.0
 
     """marker = Marker()
     marker.header.stamp = goal.ball_position.header.stamp