Merge pull request #211 from bit-bots/fix/kick_parameter_optimization

Fixes for kick parameter optimization

bitbots_dynamic_kick/CMakeLists.txt

@@ -5,7 +5,6 @@ set(CMAKE_CXX_STANDARD 17)
 ## Find catkin macros and libraries
 find_package(catkin REQUIRED COMPONENTS
         roscpp
-        bio_ik
         bitbots_msgs
         bitbots_splines
         dynamic_reconfigure
@@ -21,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
         message_generation
         message_runtime
         rotconv
+        moveit_ros_planning
 )
 
 catkin_python_setup()

bitbots_dynamic_kick/config/kick_config.yaml

@@ -1,31 +1,32 @@
 ### Engine ###
 
+
 # Distances and positions
 engine_rate: 500
 foot_rise: 0.08
-foot_distance: 0.18
-kick_windup_distance: 0.2
-trunk_height: 0.4
-trunk_roll: 0
-trunk_pitch: 0
-trunk_yaw: 0
+foot_distance: 0.2
+kick_windup_distance: 0.29
+trunk_height: 0.41
+trunk_roll: deg(13.6)
+trunk_pitch: deg(-8.1)
+trunk_yaw: deg(3.5)
 
 # Timings
-move_trunk_time: 0.5
-raise_foot_time: 0.3
-move_to_ball_time: 0.1
-kick_time: 0.1
-move_back_time: 0.2
-lower_foot_time: 0.1
-move_trunk_back_time: 0.2
+move_trunk_time: 0.22
+raise_foot_time: 0.16
+move_to_ball_time: 0.33
+kick_time: 0.16
+move_back_time: 0.21
+lower_foot_time: 0.06
+move_trunk_back_time: 0.08
 
 # Decisions
 choose_foot_corridor_width: 0.4
 
 ### Stabilizer ###
 use_center_of_pressure: false
-stabilizing_point_x: 0.0
-stabilizing_point_y: 0.015
+stabilizing_point_x: -0.02
+stabilizing_point_y: -0.02
 
 # these values are used only if use_center_of_pressure is true
 pid_x/p: 0.2

bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h

@@ -67,7 +67,7 @@ class KickNode {
    * @param trunk_to_base_footprint transform from trunk to base_footprint
    * @return whether the setup was successful
    */
-  bool init(const bitbots_msgs::KickGoal &goal_msg, std::string &error_string, Eigen::Isometry3d &trunk_to_base_footprint);
+  bool init(const bitbots_msgs::KickGoal &goal_msg, std::string &error_string);
 
   /**
    * Set the current joint state of the robot

bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_pywrapper.h

@@ -15,7 +15,7 @@ class PyKickWrapper {
  public:
   explicit PyKickWrapper(std::string ns);
   moveit::py_bindings_tools::ByteString step(double dt, const std::string &joint_state_str);
-  bool set_goal(const std::string &goal_str, const std::string &trunk_to_base_footprint_str);
+  bool set_goal(const std::string &goal_str, const std::string &joint_state_str);
   double get_progress();
   void set_params(boost::python::object params);
 

bitbots_dynamic_kick/scripts/dummy_client.py

@@ -1,13 +1,13 @@
 #!/usr/bin/env python3
 
-from __future__ import print_function
-import sys
-
-from time import sleep
-import random
-import rospy
 import actionlib
+import argparse
+import math
 import os
+import random
+import rospy
+import sys
+from time import sleep
 
 from actionlib_msgs.msg import GoalStatus
 from geometry_msgs.msg import Vector3, Quaternion
@@ -19,6 +19,11 @@
 showing_feedback = False
 
 if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument('ball_y', type=float, help='y position of the ball [m]', default=0)
+    parser.add_argument('kick_direction', type=float, help='kick direction [°]', default=0)
+    args = parser.parse_args()
+
     print("Beware: this script may only work when calling it directly on the robot "
           "and will maybe result in tf errors otherwise")
     print("[..] Initializing node", end='')
@@ -78,10 +83,10 @@ def feedback_cb(feedback):
     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.y = float(sys.argv[1])
+    goal.ball_position.y = args.ball_y
     goal.ball_position.z = 0
 
-    goal.kick_direction = Quaternion(*quaternion_from_euler(0, 0, float(sys.argv[2])))
+    goal.kick_direction = Quaternion(*quaternion_from_euler(0, 0, math.radians(args.kick_direction)))
 
     goal.kick_speed = 1
 

bitbots_dynamic_kick/src/bitbots_dynamic_kick/py_kick_wrapper.py

@@ -56,15 +56,15 @@ def __init__(self, namespace=""):
     def __del__(self):
         roscpp_shutdown()
 
-    def set_goal(self, msg: KickGoal, trunk_to_base_footprint: Transform):
+    def set_goal(self, msg: KickGoal, joint_state: JointState):
         """
         Set a goal for the kick.
 
         :param msg: The goal, instance of bitbots_msgs/KickGoal
-        :param trunk_to_base_footprint: Transform from trunk to base_footprint, needed to convert the ball position
+        :param joint_state: The current motor positions of the robot
         :return: whether the goal was set successfully
         """
-        return self.py_kick_wrapper.set_goal(to_cpp(msg))
+        return self.py_kick_wrapper.set_goal(to_cpp(msg), to_cpp(joint_state))
 
     def step(self, dt: float, joint_state: JointState):
         """
@@ -77,6 +77,8 @@ def step(self, dt: float, joint_state: JointState):
         if dt == 0.0:
             # preventing weird spline interpolation errors on edge case
             dt = 0.001
+        elif dt > 1:
+            print('dt is very large, maybe forgot to reset?')
         step = self.py_kick_wrapper.step(dt, to_cpp(joint_state))
         return from_cpp(step, JointCommand)
 

bitbots_dynamic_kick/src/kick_engine.cpp

@@ -118,14 +118,14 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei
   flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk_back, 0);
 
   flying_foot_spline_.y()->addPoint(0, flying_foot_pose.translation().y());
-  flying_foot_spline_.y()->addPoint(phase_timings_.move_trunk, kick_foot_sign * params_.foot_distance);
+  flying_foot_spline_.y()->addPoint(phase_timings_.move_trunk, flying_foot_pose.translation().y());
   flying_foot_spline_.y()->addPoint(phase_timings_.raise_foot, kick_foot_sign * params_.foot_distance);
   flying_foot_spline_.y()->addPoint(phase_timings_.windup, windup_point_.y(), 0, 0);
   flying_foot_spline_.y()
       ->addPoint(phase_timings_.kick, ball_position_.y(), speed_vector.y() * kick_speed_, 0);
   flying_foot_spline_.y()->addPoint(phase_timings_.move_back, kick_foot_sign * params_.foot_distance);
   flying_foot_spline_.y()->addPoint(phase_timings_.lower_foot, kick_foot_sign * params_.foot_distance);
-  flying_foot_spline_.y()->addPoint(phase_timings_.move_trunk_back, kick_foot_sign * params_.foot_distance);
+  flying_foot_spline_.y()->addPoint(phase_timings_.move_trunk_back, flying_foot_pose.translation().y());
 
   flying_foot_spline_.z()->addPoint(0, flying_foot_pose.translation().z());
   flying_foot_spline_.z()->addPoint(phase_timings_.move_trunk, 0);

bitbots_dynamic_kick/src/kick_node.cpp

@@ -5,8 +5,8 @@ namespace bitbots_dynamic_kick {
 KickNode::KickNode(const std::string &ns) :
     server_(node_handle_, "dynamic_kick", boost::bind(&KickNode::executeCb, this, _1), false),
     listener_(tf_buffer_),
-    visualizer_("debug/dynamic_kick"),
-    robot_model_loader_("robot_description", false) {
+    visualizer_(ns + "debug/dynamic_kick"),
+    robot_model_loader_(ns + "robot_description", false) {
   ros::NodeHandle pnh("~");
   pnh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
   pnh.param<std::string>("base_footprint_frame", base_footprint_frame_, "base_footprint");
@@ -88,8 +88,7 @@ void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &conf
 }
 
 bool KickNode::init(const bitbots_msgs::KickGoal &goal_msg,
-                    std::string &error_string,
-                    Eigen::Isometry3d &trunk_to_base_footprint) {
+                    std::string &error_string) {
   /* currently, the ball must always be in the base_footprint frame */
   if (goal_msg.header.frame_id != base_footprint_frame_) {
     ROS_ERROR_STREAM("Goal should be in base_footprint frame");
@@ -104,6 +103,13 @@ bool KickNode::init(const bitbots_msgs::KickGoal &goal_msg,
   stabilizer_.reset();
   ik_.reset();
 
+  /* get trunk to base footprint transform, assume left and right foot are next to each other (same x and z) */
+  Eigen::Isometry3d trunk_to_l_sole = current_state_->getGlobalLinkTransform("l_sole");
+  Eigen::Isometry3d trunk_to_r_sole = current_state_->getGlobalLinkTransform("r_sole");
+  Eigen::Isometry3d trunk_to_base_footprint = trunk_to_l_sole;
+  trunk_to_base_footprint.translation().y() =
+      (trunk_to_r_sole.translation().y() + trunk_to_l_sole.translation().y()) / 2.0;
+
   /* Set engines goal_msg and start calculating */
   KickGoals goals;
   tf2::convert(goal_msg.ball_position, goals.ball_position);
@@ -126,13 +132,17 @@ void KickNode::executeCb(const bitbots_msgs::KickGoalConstPtr &goal) {
   ROS_INFO("Accepted new goal");
 
   /* get transform to base_footprint */
-  geometry_msgs::TransformStamped
-      tf_trunk_to_base_footprint = tf_buffer_.lookupTransform(base_link_frame_, base_footprint_frame_, ros::Time(0));
-  Eigen::Isometry3d trunk_to_base_footprint = tf2::transformToEigen(tf_trunk_to_base_footprint);
+  geometry_msgs::TransformStamped goal_frame_to_base_footprint =
+      tf_buffer_.lookupTransform(base_footprint_frame_, goal->header.frame_id, ros::Time(0));
+  geometry_msgs::Point base_footprint_ball_position;
+  tf2::doTransform(goal->ball_position, base_footprint_ball_position, goal_frame_to_base_footprint);
+  bitbots_msgs::KickGoal base_footprint_kick_goal = *goal;
+  base_footprint_kick_goal.ball_position = base_footprint_ball_position;
+  base_footprint_kick_goal.header.frame_id = base_footprint_frame_;
 
   /* pass everything to the init function */
   std::string error_string;
-  bool success = init(*goal, error_string, trunk_to_base_footprint);
+  bool success = init(base_footprint_kick_goal, error_string);
   /* there was an error, abort the kick */
   if (!success) {
     bitbots_msgs::KickResult result;

bitbots_dynamic_kick/src/kick_pywrapper.cpp

@@ -88,12 +88,12 @@ void PyKickWrapper::set_params(const boost::python::object params) {
   kick_node_->reconfigureCallback(conf, 0xff);
 }
 
-bool PyKickWrapper::set_goal(const std::string &goal_str, const std::string &trunk_to_base_footprint_str) {
+bool PyKickWrapper::set_goal(const std::string &goal_str, const std::string &joint_state_str) {
+  auto joint_state = from_python<sensor_msgs::JointState>(joint_state_str);
+  kick_node_->jointStateCallback(joint_state);
   auto goal = from_python<bitbots_msgs::KickGoal>(goal_str);
-  auto trunk_to_base_footprint_msg = from_python<geometry_msgs::Transform>(trunk_to_base_footprint_str);
-  Eigen::Isometry3d trunk_to_base_footprint = tf2::transformToEigen(trunk_to_base_footprint_msg);
   std::string error_string;
-  return kick_node_->init(goal, error_string, trunk_to_base_footprint);
+  return kick_node_->init(goal, error_string);
 }
 
 double PyKickWrapper::get_progress() {