diff --git a/bitbots_dynamic_kick/CMakeLists.txt b/bitbots_dynamic_kick/CMakeLists.txt index c667de7e..86c1d885 100644 --- a/bitbots_dynamic_kick/CMakeLists.txt +++ b/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() diff --git a/bitbots_dynamic_kick/config/kick_config.yaml b/bitbots_dynamic_kick/config/kick_config.yaml index 5fffa806..a0b5fed3 100644 --- a/bitbots_dynamic_kick/config/kick_config.yaml +++ b/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 diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h index d74033d9..7eda407d 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h +++ b/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 diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_pywrapper.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_pywrapper.h index 51b38c70..7e0ad753 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_pywrapper.h +++ b/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); diff --git a/bitbots_dynamic_kick/scripts/dummy_client.py b/bitbots_dynamic_kick/scripts/dummy_client.py index ffe52684..f774cc2c 100755 --- a/bitbots_dynamic_kick/scripts/dummy_client.py +++ b/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 diff --git a/bitbots_dynamic_kick/src/bitbots_dynamic_kick/py_kick_wrapper.py b/bitbots_dynamic_kick/src/bitbots_dynamic_kick/py_kick_wrapper.py index 9d05734d..dc0178cf 100644 --- a/bitbots_dynamic_kick/src/bitbots_dynamic_kick/py_kick_wrapper.py +++ b/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) diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index f10d8319..405720a4 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/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); diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index 5ab5c294..6371d9ef 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/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("base_link_frame", base_link_frame_, "base_link"); pnh.param("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; diff --git a/bitbots_dynamic_kick/src/kick_pywrapper.cpp b/bitbots_dynamic_kick/src/kick_pywrapper.cpp index 6d37cb8c..132778ed 100644 --- a/bitbots_dynamic_kick/src/kick_pywrapper.cpp +++ b/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(joint_state_str); + kick_node_->jointStateCallback(joint_state); auto goal = from_python(goal_str); - auto trunk_to_base_footprint_msg = from_python(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() {