add parameter to make foot move further

bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg

@@ -37,6 +37,7 @@ 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_timings = group_engine.add_group("Timings")
 group_engine_timings.add("move_trunk_time", double_t, 3,
                          "How long it should take to move the trunk above the support foot before raising the flying foot [s]",

bitbots_dynamic_kick/config/kick_config_unstable.yaml

@@ -16,4 +16,8 @@ unstable:
   trunk_pitch: -0.488692190558412
   trunk_roll: 0.102974425867665
   trunk_yaw: 0.296705972839036
+
+  foot_extra_forward: 0.00
+
   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,8 @@ struct KickParams {
   double stabilizing_point_y;
 
   double choose_foot_corridor_width;
+
+
 };
 
 /**
@@ -78,8 +82,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
    */
@@ -124,6 +128,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 +147,9 @@ 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_;
   /**
    *  Calculate splines for a complete kick whereby is_left_kick_ should already be set correctly
    *
@@ -154,6 +163,9 @@ class KickEngine : public bitbots_splines::AbstractEngine<KickGoals, KickPositio
    */
   Eigen::Vector3d calcKickWindupPoint();
 
+  Eigen::Vector3d calcKickPoint();
+
+
   /**
    * Choose with which foot the kick should be performed
    *

bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h

@@ -108,8 +108,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_sole_frame_, r_sole_frame_, l_toe_frame_, r_toe_frame_;
 
     /**
    * Do main loop in which KickEngine::update() gets called repeatedly.

bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h

@@ -4,6 +4,8 @@
 #include <Eigen/Geometry>
 #include <bitbots_dynamic_kick/KickDebug.h>
 
+#define M_TAU M_PI * 2
+
 namespace bitbots_dynamic_kick {
 
 struct KickPositions {
@@ -19,6 +21,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/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 1.0
 
     """marker = Marker()
     marker.header.stamp = goal.ball_position.header.stamp

bitbots_dynamic_kick/scripts/interactive_test.py

@@ -12,14 +12,69 @@
 from actionlib_msgs.msg import GoalStatus
 from bitbots_msgs.srv import SetObjectPose, SetObjectPosition, SetObjectPoseRequest, SetObjectPositionRequest
 from geometry_msgs.msg import Vector3, Quaternion
-from bitbots_msgs.msg import KickGoal, KickAction, KickFeedback
+from bitbots_msgs.msg import KickGoal, KickAction, KickFeedback, JointCommand
 from visualization_msgs.msg import Marker
 
 from tf.transformations import quaternion_from_euler
 import sys, select, termios, tty
 
 showing_feedback = False
 
+
+__ids__ = [
+    "HeadPan",
+    "HeadTilt",
+    "LShoulderPitch",
+    "LShoulderRoll",
+    "LElbow",
+    "RShoulderPitch",
+    "RShoulderRoll",
+    "RElbow",
+    "LHipYaw",
+    "LHipRoll",
+    "LHipPitch",
+    "LKnee",
+    "LAnklePitch",
+    "LAnkleRoll",
+    "RHipYaw",
+    "RHipRoll",
+    "RHipPitch",
+    "RKnee",
+    "RAnklePitch",
+    "RAnkleRoll"
+]
+__velocity__ = 5.0
+__accelerations__ = -1.0
+__max_currents__ = -1.0
+
+walkready = JointCommand(
+    joint_names=__ids__,
+    velocities=[__velocity__] * len(__ids__),
+    accelerations=[__accelerations__] * len(__ids__),
+    max_currents=[__max_currents__] * len(__ids__),
+    positions=[
+        0.0,  # HeadPan
+        0.0,  # HeadTilt
+        math.radians(75.27),  # LShoulderPitch
+        0.0,  # LShoulderRoll
+        math.radians(35.86),  # LElbow
+        math.radians(-75.58),  # RShoulderPitch
+        0.0,  # RShoulderRoll
+        math.radians(-36.10),  # RElbow
+        -0.0112,  # LHipYaw
+        0.0615,  # LHipRoll
+        0.4732,  # LHipPitch
+        1.0058,  # LKnee
+        -0.4512,  # LAnklePitch
+        0.0625,  # LAnkleRoll
+        0.0112,  # RHipYaw
+        -0.0615,  # RHipRoll
+        -0.4732,  # RHipPitch
+        -1.0059,  # RKnee
+        0.4512,  # RAnklePitch
+        -0.0625,  # RAnkleRoll
+    ])
+
 msg = """
 BitBots Interactive Kick Test                              
 -----------------------------
@@ -36,7 +91,7 @@
 
 <: execute kick
 r: reset robot and ball
-
+R: set robot to walkready
 
 
 
@@ -61,14 +116,14 @@
 }
 
 moveBallBindings = {
-    't': (1, 0, 0),
-    'g': (-1, 0, 0),
-    'f': (0, 1, 0),
-    'h': (0, -1, 0),
-    'T': (10, 0, 0),
-    'G': (-10, 0, 0),
-    'F': (0, 10, 0),
-    'H': (0, -10, 0),
+    't': (1, 0),
+    'g': (-1, 0),
+    'f': (0, 1),
+    'h': (0, -1),
+    'T': (10, 0),
+    'G': (-10, 0),
+    'F': (0, 10),
+    'H': (0, -10),
 }
 
 settings = termios.tcgetattr(sys.stdin)
@@ -86,45 +141,17 @@ def getKey():
     rospy.init_node('dynamic_kick_interactive_test', anonymous=True)
     print("Waiting for kick server and simulation")
 
+
     def done_cb(state, result):
         return
-        print('Action completed: ', end='')
-        if state == GoalStatus.PENDING:
-            print('Pending')
-        elif state == GoalStatus.ACTIVE:
-            print('Active')
-        elif state == GoalStatus.PREEMPTED:
-            print('Preempted')
-        elif state == GoalStatus.SUCCEEDED:
-            print('Succeeded')
-        elif state == GoalStatus.ABORTED:
-            print('Aborted')
-        elif state == GoalStatus.REJECTED:
-            print('Rejected')
-        elif state == GoalStatus.PREEMPTING:
-            print('Preempting')
-        elif state == GoalStatus.RECALLING:
-            print('Recalling')
-        elif state == GoalStatus.RECALLED:
-            print('Recalled')
-        elif state == GoalStatus.LOST:
-            print('Lost')
-        else:
-            print('Unknown state', state)
-        print(str(result))
 
 
     def active_cb():
         return
-        print("Server accepted action")
 
 
     def feedback_cb(feedback):
         return
-        if len(sys.argv) > 1 and sys.argv[1] == '--feedback':
-            print('Feedback')
-            print(feedback)
-            print()
 
 
     sys.stdout.flush()
@@ -148,6 +175,8 @@ def feedback_cb(feedback):
 
     frame_prefix = "" if os.environ.get("ROS_NAMESPACE") is None else os.environ.get("ROS_NAMESPACE") + "/"
 
+    joint_pub = rospy.Publisher("DynamixelController/command", JointCommand, queue_size=1)
+
 
     def generate_kick_goal(x, y, direction, speed, unstable=False):
         kick_goal = KickGoal()
@@ -194,6 +223,7 @@ def set_ball_position():
         request.position.z = 0.04
         response = set_ball_pos_service(request)
 
+
     sys.stdout.write("\x1b[A")
     sys.stdout.write("\x1b[A")
     print(msg)
@@ -235,17 +265,14 @@ def set_ball_position():
         elif key == "r":
             set_robot_pose()
             set_ball_position()
+        elif key =="R":
+            # play walkready animation
+            walkready.header.stamp = rospy.Time.now()
+            joint_pub.publish(walkready)
         elif (key == '\x03'):
             break
 
-
-        sys.stdout.write("\x1b[A")
-        sys.stdout.write("\x1b[A")
-        sys.stdout.write("\x1b[A")
-        sys.stdout.write("\x1b[A")
-        sys.stdout.write("\x1b[A")
-        sys.stdout.write("\x1b[A")
-        sys.stdout.write("\x1b[A")
+        sys.stdout.write("\x1b[A" * 7)
         print(
             f"robot_x:    {round(robot_x, 2)}         ball_x:     {round(ball_x, 2)} \n"
             f"robot_y:    {round(robot_y, 2)}         ball_x:     {round(ball_y, 2)} \n"