From 9d7e4de3e4c99c7055fcaf58e8112af560599b3e Mon Sep 17 00:00:00 2001 From: Marc Bestmann Date: Mon, 21 Jun 2021 12:50:22 +0200 Subject: [PATCH 01/13] add parameter to make foot move further --- .../cfg/bitbots_dynamic_kick_params.cfg | 3 +++ bitbots_dynamic_kick/config/kick_config.yaml | 1 + .../config/kick_config_unstable.yaml | 1 + .../include/bitbots_dynamic_kick/kick_engine.h | 1 + bitbots_dynamic_kick/src/kick_engine.cpp | 14 ++++++++------ 5 files changed, 14 insertions(+), 6 deletions(-) diff --git a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg index eaab2e9b..dfd7ba6c 100755 --- a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg +++ b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg @@ -36,6 +36,9 @@ 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_timings = group_engine.add_group("Timings") group_engine_timings.add("move_trunk_time", double_t, 3, diff --git a/bitbots_dynamic_kick/config/kick_config.yaml b/bitbots_dynamic_kick/config/kick_config.yaml index a14e00ce..e1882c00 100644 --- a/bitbots_dynamic_kick/config/kick_config.yaml +++ b/bitbots_dynamic_kick/config/kick_config.yaml @@ -9,6 +9,7 @@ trunk_height: 0.39 trunk_roll: 0.101229096615671 trunk_pitch: -0.136135681655558 trunk_yaw: 0.0558505360638186 +foot_x_extra: 0.05 # Timings move_trunk_time: 0.56 diff --git a/bitbots_dynamic_kick/config/kick_config_unstable.yaml b/bitbots_dynamic_kick/config/kick_config_unstable.yaml index 90f5000e..4b1b399f 100644 --- a/bitbots_dynamic_kick/config/kick_config_unstable.yaml +++ b/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 diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h index c35c901b..460c978e 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h @@ -25,6 +25,7 @@ struct KickParams { double foot_distance; double kick_windup_distance; double trunk_height; + double foot_extra_forward; double trunk_roll; double trunk_pitch; diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index 0c55d004..a70d72bb 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_dynamic_kick/src/kick_engine.cpp @@ -103,13 +103,15 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei /* build vector of speeds in each direction */ double speed_yaw = rot_conv::EYawOfQuat(kick_direction_); Eigen::Vector3d speed_vector(cos(speed_yaw), sin(speed_yaw), 0); + double target_yaw = calcKickFootYaw(); /* Flying foot position */ flying_foot_spline_.x()->addPoint(0, flying_foot_pose.translation().x()); flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk, 0); flying_foot_spline_.x()->addPoint(phase_timings_.raise_foot, 0); flying_foot_spline_.x()->addPoint(phase_timings_.windup, windup_point_.x(), 0, 0); - flying_foot_spline_.x()->addPoint(phase_timings_.kick, ball_position_.x(), + flying_foot_spline_.x()->addPoint(phase_timings_.kick, + ball_position_.x() + params_.foot_extra_forward * cos(target_yaw), speed_vector.x() * kick_speed_, 0); flying_foot_spline_.x()->addPoint(phase_timings_.move_back, 0); flying_foot_spline_.x()->addPoint(phase_timings_.lower_foot, 0); @@ -119,8 +121,9 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei 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_.kick, + ball_position_.y() + params_.foot_extra_forward * sin(target_yaw), + 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, flying_foot_pose.translation().y()); @@ -139,7 +142,6 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei double start_r, start_p, start_y; Eigen::Quaterniond flying_foot_rotation(flying_foot_pose.rotation()); rot_conv::EulerFromQuat(flying_foot_rotation, start_y, start_p, start_r); - double target_y = calcKickFootYaw(); /* Add these quaternions in the same fashion as before to our splines (current, target, current) */ flying_foot_spline_.roll()->addPoint(0, start_r); @@ -150,8 +152,8 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.pitch()->addPoint(phase_timings_.move_trunk_back, start_p); flying_foot_spline_.yaw()->addPoint(0, start_y); flying_foot_spline_.yaw()->addPoint(phase_timings_.raise_foot, start_y); - flying_foot_spline_.yaw()->addPoint(phase_timings_.windup, target_y); - flying_foot_spline_.yaw()->addPoint(phase_timings_.kick, target_y); + flying_foot_spline_.yaw()->addPoint(phase_timings_.windup, target_yaw); + flying_foot_spline_.yaw()->addPoint(phase_timings_.kick, target_yaw); flying_foot_spline_.yaw()->addPoint(phase_timings_.move_back, start_y); flying_foot_spline_.yaw()->addPoint(phase_timings_.move_trunk_back, start_y); From aed9531622cac189544c1c53c5335bc6de96c3ea Mon Sep 17 00:00:00 2001 From: Marc Bestmann Date: Tue, 22 Jun 2021 21:49:51 +0200 Subject: [PATCH 02/13] start implementing kick improvements --- .../cfg/bitbots_dynamic_kick_params.cfg | 6 ++++++ bitbots_dynamic_kick/config/kick_config.yaml | 2 ++ .../include/bitbots_dynamic_kick/kick_engine.h | 3 +++ bitbots_dynamic_kick/src/kick_engine.cpp | 10 +++++++++- bitbots_dynamic_kick/src/kick_node.cpp | 6 +++++- 5 files changed, 25 insertions(+), 2 deletions(-) diff --git a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg index dfd7ba6c..870fed17 100755 --- a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg +++ b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg @@ -39,6 +39,12 @@ group_engine_distances.add("trunk_yaw", double_t, 4, 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_timings = group_engine.add_group("Timings") group_engine_timings.add("move_trunk_time", double_t, 3, diff --git a/bitbots_dynamic_kick/config/kick_config.yaml b/bitbots_dynamic_kick/config/kick_config.yaml index e1882c00..eb87229e 100644 --- a/bitbots_dynamic_kick/config/kick_config.yaml +++ b/bitbots_dynamic_kick/config/kick_config.yaml @@ -10,6 +10,8 @@ trunk_roll: 0.101229096615671 trunk_pitch: -0.136135681655558 trunk_yaw: 0.0558505360638186 foot_x_extra: 0.05 +foot_pitch: 0.4 +foot_rise_lower: 0.04 # Timings move_trunk_time: 0.56 diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h index 460c978e..662d87df 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h @@ -43,6 +43,9 @@ struct KickParams { double stabilizing_point_y; double choose_foot_corridor_width; + + double foot_pitch; + double foot_rise_lower; }; /** diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index a70d72bb..58d70bcf 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_dynamic_kick/src/kick_engine.cpp @@ -14,6 +14,7 @@ void KickEngine::reset() { } void KickEngine::setGoals(const KickGoals &goals) { + ROS_WARN("setgoal"); is_left_kick_ = calcIsLeftFootKicking(goals.ball_position, goals.kick_direction); // TODO Internal state is dirty when goal transformation fails @@ -83,6 +84,7 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei */ /* calculate timings for this kick */ + ROS_ERROR_STREAM(params_.move_to_ball_time); phase_timings_.move_trunk = 0 + params_.move_trunk_time; phase_timings_.raise_foot = phase_timings_.move_trunk + params_.raise_foot_time; phase_timings_.windup = phase_timings_.raise_foot + params_.move_to_ball_time; @@ -132,6 +134,8 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.z()->addPoint(phase_timings_.move_trunk, 0); flying_foot_spline_.z()->addPoint(phase_timings_.raise_foot, params_.foot_rise); flying_foot_spline_.z()->addPoint(phase_timings_.windup, params_.foot_rise); + flying_foot_spline_.z()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, + params_.foot_rise_lower); flying_foot_spline_.z()->addPoint(phase_timings_.kick, params_.foot_rise); flying_foot_spline_.z()->addPoint(phase_timings_.move_back, params_.foot_rise); flying_foot_spline_.z()->addPoint(phase_timings_.lower_foot, 0.4 * params_.foot_rise); @@ -147,9 +151,13 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.roll()->addPoint(0, start_r); flying_foot_spline_.roll()->addPoint(phase_timings_.windup, start_r); flying_foot_spline_.roll()->addPoint(phase_timings_.move_trunk_back, start_r); + flying_foot_spline_.pitch()->addPoint(0, start_p); - flying_foot_spline_.pitch()->addPoint(phase_timings_.windup, start_p); + flying_foot_spline_.pitch()->addPoint(phase_timings_.raise_foot, start_p); + flying_foot_spline_.pitch()->addPoint(phase_timings_.windup, params_.foot_pitch); + flying_foot_spline_.pitch()->addPoint(phase_timings_.kick, 0); flying_foot_spline_.pitch()->addPoint(phase_timings_.move_trunk_back, start_p); + flying_foot_spline_.yaw()->addPoint(0, start_y); flying_foot_spline_.yaw()->addPoint(phase_timings_.raise_foot, start_y); flying_foot_spline_.yaw()->addPoint(phase_timings_.windup, target_yaw); diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index e11634de..50bd4e34 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -112,11 +112,15 @@ void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &conf params.move_to_ball_time = config.move_to_ball_time; params.kick_time = config.kick_time; params.move_back_time = config.move_back_time; + ROS_WARN_STREAM(params.move_to_ball_time); params.lower_foot_time = config.lower_foot_time; params.move_trunk_back_time = config.move_trunk_back_time; params.stabilizing_point_x = config.stabilizing_point_x; params.stabilizing_point_y = config.stabilizing_point_y; params.choose_foot_corridor_width = config.choose_foot_corridor_width; + params.foot_extra_forward = config.foot_extra_forward; + params.foot_pitch = config.foot_pitch; + params.foot_rise_lower = config.foot_rise_lower; engine_.setParams(params); stabilizer_.useCop(config.use_center_of_pressure); @@ -174,7 +178,7 @@ bool KickNode::init(const bitbots_msgs::KickGoal &goal_msg, void KickNode::executeCb(const bitbots_msgs::KickGoalConstPtr &goal) { // TODO: maybe switch to goal callback to be able to reject goals properly - ROS_INFO("Accepted new goal"); + ROS_INFO("Kick accepted new goal"); /* get transform to base_footprint */ geometry_msgs::TransformStamped goal_frame_to_base_footprint = From 59050fe4ec041d0d583fb2d855545246567aaf2d Mon Sep 17 00:00:00 2001 From: Timon Engelke Date: Tue, 22 Jun 2021 22:18:11 +0200 Subject: [PATCH 03/13] fix for dynamic reconfigure --- bitbots_dynamic_kick/src/kick_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index e11634de..fcc5043a 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -92,9 +92,9 @@ DynamicKickConfig KickNode::getUnstableConfig() { return config; } -void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &config, uint32_t /* level */) { - if (!normal_config_) { - // This is called at the beginning, so we can set the normal config here +void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &config, uint32_t level) { + if (level != 9999) { + // The level is set to 9999 for the unstable kick, so we don't update the normal config in that case normal_config_ = config; } engine_rate_ = config.engine_rate; @@ -136,7 +136,7 @@ bool KickNode::init(const bitbots_msgs::KickGoal &goal_msg, } if (goal_msg.unstable) { - reconfigureCallback(unstable_config_, 0); + reconfigureCallback(unstable_config_, 9999); } else { reconfigureCallback(normal_config_.value(), 0); } From e30775ef669f9c78c974b03775ebbb409c31ab69 Mon Sep 17 00:00:00 2001 From: Marc Bestmann Date: Tue, 22 Jun 2021 23:02:00 +0200 Subject: [PATCH 04/13] improving kick --- .../cfg/bitbots_dynamic_kick_params.cfg | 3 +++ bitbots_dynamic_kick/config/kick_config.yaml | 5 +++-- .../include/bitbots_dynamic_kick/kick_engine.h | 1 + bitbots_dynamic_kick/src/kick_engine.cpp | 16 +++++++++++----- bitbots_dynamic_kick/src/kick_node.cpp | 1 + 5 files changed, 19 insertions(+), 7 deletions(-) diff --git a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg index 870fed17..414ee903 100755 --- a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg +++ b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg @@ -45,6 +45,9 @@ group_engine_distances.add("foot_pitch", double_t, 4, 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_timings = group_engine.add_group("Timings") group_engine_timings.add("move_trunk_time", double_t, 3, diff --git a/bitbots_dynamic_kick/config/kick_config.yaml b/bitbots_dynamic_kick/config/kick_config.yaml index eb87229e..c31394fd 100644 --- a/bitbots_dynamic_kick/config/kick_config.yaml +++ b/bitbots_dynamic_kick/config/kick_config.yaml @@ -4,14 +4,15 @@ 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_x_extra: 0.05 +foot_x_extra: 0.02 foot_pitch: 0.4 foot_rise_lower: 0.04 +foot_rise_kick: 0.08 # Timings move_trunk_time: 0.56 diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h index 662d87df..40424221 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h @@ -46,6 +46,7 @@ struct KickParams { double foot_pitch; double foot_rise_lower; + double foot_rise_kick; }; /** diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index 58d70bcf..df8873cc 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_dynamic_kick/src/kick_engine.cpp @@ -136,9 +136,9 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.z()->addPoint(phase_timings_.windup, params_.foot_rise); flying_foot_spline_.z()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, params_.foot_rise_lower); - flying_foot_spline_.z()->addPoint(phase_timings_.kick, params_.foot_rise); - flying_foot_spline_.z()->addPoint(phase_timings_.move_back, params_.foot_rise); - flying_foot_spline_.z()->addPoint(phase_timings_.lower_foot, 0.4 * params_.foot_rise); + flying_foot_spline_.z()->addPoint(phase_timings_.kick, params_.foot_rise_kick); + flying_foot_spline_.z()->addPoint(phase_timings_.move_back, params_.foot_rise_kick); + flying_foot_spline_.z()->addPoint(phase_timings_.lower_foot, 0.4 * params_.foot_rise_kick); flying_foot_spline_.z()->addPoint(phase_timings_.move_trunk_back, 0); /* Flying foot orientation */ @@ -149,12 +149,18 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei /* Add these quaternions in the same fashion as before to our splines (current, target, current) */ flying_foot_spline_.roll()->addPoint(0, start_r); - flying_foot_spline_.roll()->addPoint(phase_timings_.windup, start_r); + flying_foot_spline_.roll()->addPoint(phase_timings_.raise_foot, start_r); + flying_foot_spline_.roll()->addPoint(phase_timings_.windup, -sin(target_yaw) * params_.foot_pitch); + flying_foot_spline_.roll()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, + - sin(target_yaw) * params_.foot_pitch); + flying_foot_spline_.roll()->addPoint(phase_timings_.kick, 0); flying_foot_spline_.roll()->addPoint(phase_timings_.move_trunk_back, start_r); flying_foot_spline_.pitch()->addPoint(0, start_p); flying_foot_spline_.pitch()->addPoint(phase_timings_.raise_foot, start_p); - flying_foot_spline_.pitch()->addPoint(phase_timings_.windup, params_.foot_pitch); + flying_foot_spline_.pitch()->addPoint(phase_timings_.windup, cos(target_yaw) * params_.foot_pitch); + flying_foot_spline_.pitch()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, + cos(target_yaw) * params_.foot_pitch); flying_foot_spline_.pitch()->addPoint(phase_timings_.kick, 0); flying_foot_spline_.pitch()->addPoint(phase_timings_.move_trunk_back, start_p); diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index 08352732..d582c882 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -121,6 +121,7 @@ void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &conf params.foot_extra_forward = config.foot_extra_forward; params.foot_pitch = config.foot_pitch; params.foot_rise_lower = config.foot_rise_lower; + params.foot_rise_kick = config.foot_rise_kick; engine_.setParams(params); stabilizer_.useCop(config.use_center_of_pressure); From 0ab83db0cfbc1020f888a962b792c75d40686cdc Mon Sep 17 00:00:00 2001 From: Marc Bestmann Date: Tue, 22 Jun 2021 23:57:26 +0200 Subject: [PATCH 05/13] more parameter --- bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg | 3 +++ bitbots_dynamic_kick/config/kick_config.yaml | 4 ++-- .../include/bitbots_dynamic_kick/kick_engine.h | 1 + bitbots_dynamic_kick/scripts/dummy_client.py | 2 +- bitbots_dynamic_kick/src/kick_engine.cpp | 3 +++ bitbots_dynamic_kick/src/kick_node.cpp | 1 + 6 files changed, 11 insertions(+), 3 deletions(-) diff --git a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg index 414ee903..f71289c6 100755 --- a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg +++ b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg @@ -48,6 +48,9 @@ group_engine_distances.add("foot_rise_lower", double_t, 4, 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_timings = group_engine.add_group("Timings") group_engine_timings.add("move_trunk_time", double_t, 3, diff --git a/bitbots_dynamic_kick/config/kick_config.yaml b/bitbots_dynamic_kick/config/kick_config.yaml index c31394fd..2b0d260c 100644 --- a/bitbots_dynamic_kick/config/kick_config.yaml +++ b/bitbots_dynamic_kick/config/kick_config.yaml @@ -9,8 +9,8 @@ trunk_height: 0.39 trunk_roll: 0.101229096615671 trunk_pitch: -0.136135681655558 trunk_yaw: 0.0558505360638186 -foot_x_extra: 0.02 -foot_pitch: 0.4 +foot_x_extra: 0.05 +foot_pitch: 0.6 foot_rise_lower: 0.04 foot_rise_kick: 0.08 diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h index 40424221..48825894 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h @@ -47,6 +47,7 @@ struct KickParams { double foot_pitch; double foot_rise_lower; double foot_rise_kick; + double earlier_time; }; /** diff --git a/bitbots_dynamic_kick/scripts/dummy_client.py b/bitbots_dynamic_kick/scripts/dummy_client.py index 271dca71..d9d14946 100755 --- a/bitbots_dynamic_kick/scripts/dummy_client.py +++ b/bitbots_dynamic_kick/scripts/dummy_client.py @@ -90,7 +90,7 @@ def feedback_cb(feedback): 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.1 """marker = Marker() marker.header.stamp = goal.ball_position.header.stamp diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index df8873cc..cecae7b0 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_dynamic_kick/src/kick_engine.cpp @@ -136,6 +136,7 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.z()->addPoint(phase_timings_.windup, params_.foot_rise); flying_foot_spline_.z()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, params_.foot_rise_lower); + flying_foot_spline_.z()->addPoint(phase_timings_.kick - params_.earlier_time, params_.foot_rise_kick); flying_foot_spline_.z()->addPoint(phase_timings_.kick, params_.foot_rise_kick); flying_foot_spline_.z()->addPoint(phase_timings_.move_back, params_.foot_rise_kick); flying_foot_spline_.z()->addPoint(phase_timings_.lower_foot, 0.4 * params_.foot_rise_kick); @@ -153,6 +154,7 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.roll()->addPoint(phase_timings_.windup, -sin(target_yaw) * params_.foot_pitch); flying_foot_spline_.roll()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, - sin(target_yaw) * params_.foot_pitch); + flying_foot_spline_.roll()->addPoint(phase_timings_.kick - params_.earlier_time, 0); flying_foot_spline_.roll()->addPoint(phase_timings_.kick, 0); flying_foot_spline_.roll()->addPoint(phase_timings_.move_trunk_back, start_r); @@ -161,6 +163,7 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.pitch()->addPoint(phase_timings_.windup, cos(target_yaw) * params_.foot_pitch); flying_foot_spline_.pitch()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, cos(target_yaw) * params_.foot_pitch); + flying_foot_spline_.pitch()->addPoint(phase_timings_.kick - params_.earlier_time, 0); flying_foot_spline_.pitch()->addPoint(phase_timings_.kick, 0); flying_foot_spline_.pitch()->addPoint(phase_timings_.move_trunk_back, start_p); diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index d582c882..a993f6a4 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -122,6 +122,7 @@ void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &conf params.foot_pitch = config.foot_pitch; params.foot_rise_lower = config.foot_rise_lower; params.foot_rise_kick = config.foot_rise_kick; + params.earlier_time = config.earlier_time; engine_.setParams(params); stabilizer_.useCop(config.use_center_of_pressure); From f27b53df06e49b03f5555c515626109d00cc2787 Mon Sep 17 00:00:00 2001 From: Timon Engelke Date: Wed, 23 Jun 2021 00:26:12 +0200 Subject: [PATCH 06/13] add new params to python interface --- bitbots_dynamic_kick/src/kick_pywrapper.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/bitbots_dynamic_kick/src/kick_pywrapper.cpp b/bitbots_dynamic_kick/src/kick_pywrapper.cpp index ce71716d..68794310 100644 --- a/bitbots_dynamic_kick/src/kick_pywrapper.cpp +++ b/bitbots_dynamic_kick/src/kick_pywrapper.cpp @@ -82,6 +82,11 @@ void PyKickWrapper::set_params(const boost::python::object params) { else if (keystr == "stabilizing_point_x") { conf.stabilizing_point_x = std::stof(valstr); } else if (keystr == "stabilizing_point_y") { conf.stabilizing_point_y = std::stof(valstr); } else if (keystr == "spline_smoothness") { conf.spline_smoothness = std::stoi(valstr); } + else if (keystr == "earlier_time") { conf.earlier_time = std::stof(valstr); } + else if (keystr == "foot_rise_kick") { conf.foot_rise_kick = std::stof(valstr); } + else if (keystr == "foot_rise_lower") { conf.foot_rise_lower = std::stof(valstr); } + else if (keystr == "foot_pitch") { conf.foot_pitch = std::stof(valstr); } + else if (keystr == "foot_extra_forward") { conf.foot_extra_forward = std::stof(valstr); } else { std::cerr << keystr << " not known. WILL BE IGNORED." << std::endl; } } From 83fdaa0d2e91f8075c8db912299b3bd73fec333a Mon Sep 17 00:00:00 2001 From: Marc Bestmann Date: Wed, 23 Jun 2021 23:25:25 +0200 Subject: [PATCH 07/13] change from sole to toe --- .../bitbots_dynamic_kick/kick_engine.h | 4 +-- .../include/bitbots_dynamic_kick/kick_node.h | 2 +- .../launch/dynamic_kick.launch | 4 +-- bitbots_dynamic_kick/src/kick_engine.cpp | 10 +++---- bitbots_dynamic_kick/src/kick_node.cpp | 26 +++++++++---------- bitbots_dynamic_kick/src/visualizer.cpp | 8 +++--- 6 files changed, 27 insertions(+), 27 deletions(-) diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h index 48825894..83b5086e 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h @@ -84,8 +84,8 @@ class KickEngine : public bitbots_splines::AbstractEngine normal_config_; - 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_; /** * Do main loop in which KickEngine::update() gets called repeatedly. diff --git a/bitbots_dynamic_kick/launch/dynamic_kick.launch b/bitbots_dynamic_kick/launch/dynamic_kick.launch index 3cd77ca0..7f23e0a6 100644 --- a/bitbots_dynamic_kick/launch/dynamic_kick.launch +++ b/bitbots_dynamic_kick/launch/dynamic_kick.launch @@ -8,7 +8,7 @@ - - + + diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index cecae7b0..3d7e52b5 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_dynamic_kick/src/kick_engine.cpp @@ -19,7 +19,7 @@ void KickEngine::setGoals(const KickGoals &goals) { // TODO Internal state is dirty when goal transformation fails /* Save given goals because we reuse them later */ - auto transformed_goal = transformGoal((is_left_kick_) ? "r_sole" : "l_sole", + auto transformed_goal = transformGoal((is_left_kick_) ? "r_toe" : "l_toe", goals.trunk_to_base_footprint, goals.ball_position, goals.kick_direction); ball_position_ = transformed_goal.first; kick_direction_ = transformed_goal.second; @@ -28,15 +28,15 @@ void KickEngine::setGoals(const KickGoals &goals) { time_ = 0; - Eigen::Isometry3d trunk_to_flying_foot = current_state_->getGlobalLinkTransform(is_left_kick_ ? "l_sole" : "r_sole"); - Eigen::Isometry3d trunk_to_support_foot = current_state_->getGlobalLinkTransform(is_left_kick_ ? "r_sole" : "l_sole"); + Eigen::Isometry3d trunk_to_flying_foot = current_state_->getGlobalLinkTransform(is_left_kick_ ? "l_toe" : "r_toe"); + Eigen::Isometry3d trunk_to_support_foot = current_state_->getGlobalLinkTransform(is_left_kick_ ? "r_toe" : "l_toe"); /* get the start position of the trunk relative to the support foot */ Eigen::Isometry3d trunk_frame; if (is_left_kick_) { - trunk_frame = current_state_->getGlobalLinkTransform("r_sole").inverse(); + trunk_frame = current_state_->getGlobalLinkTransform("r_toe").inverse(); } else { - trunk_frame = current_state_->getGlobalLinkTransform("l_sole").inverse(); + trunk_frame = current_state_->getGlobalLinkTransform("l_toe").inverse(); } /* Plan new splines according to new goal */ diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index a993f6a4..659cdb19 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -10,8 +10,8 @@ KickNode::KickNode(const std::string &ns) : robot_model_loader_(ns + "robot_description", false) { private_node_handle_.param("base_link_frame", base_link_frame_, "base_link"); private_node_handle_.param("base_footprint_frame", base_footprint_frame_, "base_footprint"); - private_node_handle_.param("r_sole_frame", r_sole_frame_, "r_sole"); - private_node_handle_.param("l_sole_frame", l_sole_frame_, "l_sole"); + private_node_handle_.param("r_toe_frame", r_toe_frame_, "r_toe"); + private_node_handle_.param("l_toe_frame", l_toe_frame_, "l_toe"); unstable_config_ = getUnstableConfig(); @@ -41,15 +41,15 @@ KickNode::KickNode(const std::string &ns) : } void KickNode::copLCallback(const geometry_msgs::PointStamped &cop) { - if (cop.header.frame_id != l_sole_frame_) { - ROS_ERROR_STREAM("cop_l not in " << l_sole_frame_ << " frame! Stabilizing will not work."); + if (cop.header.frame_id != l_toe_frame_) { + ROS_ERROR_STREAM("cop_l not in " << l_toe_frame_ << " frame! Stabilizing will not work."); } stabilizer_.cop_left = cop.point; } void KickNode::copRCallback(const geometry_msgs::PointStamped &cop) { - if (cop.header.frame_id != r_sole_frame_) { - ROS_ERROR_STREAM("cop_r not in " << r_sole_frame_ << " frame! Stabilizing will not work."); + if (cop.header.frame_id != r_toe_frame_) { + ROS_ERROR_STREAM("cop_r not in " << r_toe_frame_ << " frame! Stabilizing will not work."); } stabilizer_.cop_right = cop.point; } @@ -155,11 +155,11 @@ bool KickNode::init(const bitbots_msgs::KickGoal &goal_msg, 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; + Eigen::Isometry3d trunk_to_l_toe = current_state_->getGlobalLinkTransform("l_toe"); + Eigen::Isometry3d trunk_to_r_toe = current_state_->getGlobalLinkTransform("r_toe"); + Eigen::Isometry3d trunk_to_base_footprint = trunk_to_l_toe; trunk_to_base_footprint.translation().y() = - (trunk_to_r_sole.translation().y() + trunk_to_l_sole.translation().y()) / 2.0; + (trunk_to_r_toe.translation().y() + trunk_to_l_toe.translation().y()) / 2.0; /* Set engines goal_msg and start calculating */ KickGoals goals; @@ -171,9 +171,9 @@ bool KickNode::init(const bitbots_msgs::KickGoal &goal_msg, /* visualization */ visualizer_.displayReceivedGoal(goal_msg); - visualizer_.displayWindupPoint(engine_.getWindupPoint(), (engine_.isLeftKick()) ? r_sole_frame_ : l_sole_frame_); - visualizer_.displayFlyingSplines(engine_.getFlyingSplines(), (engine_.isLeftKick()) ? r_sole_frame_ : l_sole_frame_); - visualizer_.displayTrunkSplines(engine_.getTrunkSplines(), (engine_.isLeftKick() ? r_sole_frame_ : l_sole_frame_)); + visualizer_.displayWindupPoint(engine_.getWindupPoint(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); + visualizer_.displayFlyingSplines(engine_.getFlyingSplines(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); + visualizer_.displayTrunkSplines(engine_.getTrunkSplines(), (engine_.isLeftKick() ? r_toe_frame_ : l_toe_frame_)); return true; } diff --git a/bitbots_dynamic_kick/src/visualizer.cpp b/bitbots_dynamic_kick/src/visualizer.cpp index 3948c738..d8788cf3 100644 --- a/bitbots_dynamic_kick/src/visualizer.cpp +++ b/bitbots_dynamic_kick/src/visualizer.cpp @@ -94,11 +94,11 @@ void Visualizer::publishGoals(const KickPositions &positions, std::string support_foot_frame, flying_foot_frame; if (positions.is_left_kick) { - support_foot_frame = "r_sole"; - flying_foot_frame = "l_sole"; + support_foot_frame = "r_toe"; + flying_foot_frame = "l_toe"; } else { - support_foot_frame = "l_sole"; - flying_foot_frame = "r_sole"; + support_foot_frame = "l_toe"; + flying_foot_frame = "r_toe"; } /* Derive positions from robot state */ From cd3be29d5accfff83f8b6332bf26f66a2b964145 Mon Sep 17 00:00:00 2001 From: Marc Bestmann Date: Thu, 24 Jun 2021 12:34:15 +0200 Subject: [PATCH 08/13] change flying foot spline --- .../cfg/bitbots_dynamic_kick_params.cfg | 10 ++++ bitbots_dynamic_kick/config/kick_config.yaml | 7 ++- .../bitbots_dynamic_kick/kick_engine.h | 14 +++++ .../include/bitbots_dynamic_kick/kick_node.h | 1 + .../include/bitbots_dynamic_kick/kick_utils.h | 1 + .../include/bitbots_dynamic_kick/visualizer.h | 3 ++ bitbots_dynamic_kick/scripts/dummy_client.py | 2 +- bitbots_dynamic_kick/src/kick_engine.cpp | 53 ++++++++++++++----- bitbots_dynamic_kick/src/kick_node.cpp | 11 ++-- bitbots_dynamic_kick/src/visualizer.cpp | 17 ++++++ 10 files changed, 100 insertions(+), 19 deletions(-) diff --git a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg index f71289c6..d91b1a5c 100755 --- a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg +++ b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg @@ -51,6 +51,13 @@ group_engine_distances.add("foot_rise_kick", double_t, 4, 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_timings = group_engine.add_group("Timings") group_engine_timings.add("move_trunk_time", double_t, 3, @@ -62,6 +69,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) diff --git a/bitbots_dynamic_kick/config/kick_config.yaml b/bitbots_dynamic_kick/config/kick_config.yaml index 2b0d260c..e4843676 100644 --- a/bitbots_dynamic_kick/config/kick_config.yaml +++ b/bitbots_dynamic_kick/config/kick_config.yaml @@ -9,15 +9,18 @@ trunk_height: 0.39 trunk_roll: 0.101229096615671 trunk_pitch: -0.136135681655558 trunk_yaw: 0.0558505360638186 -foot_x_extra: 0.05 -foot_pitch: 0.6 +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 # 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 diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h index 83b5086e..3b9fb40b 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h @@ -47,7 +47,12 @@ struct KickParams { double foot_pitch; double foot_rise_lower; double foot_rise_kick; + double earlier_time; + double low_time; + double low_x; + double low_x_speed; + }; /** @@ -58,6 +63,7 @@ class PhaseTimings { double move_trunk; double raise_foot; double windup; + double low; double kick; double move_back; double lower_foot; @@ -130,6 +136,9 @@ class KickEngine : public bitbots_splines::AbstractEngine normal_config_; + double ball_radius_; std::string base_link_frame_, base_footprint_frame_, l_toe_frame_, r_toe_frame_; diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h index f7e39a38..72feb9f9 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h @@ -19,6 +19,7 @@ struct KickGoals { Eigen::Quaterniond kick_direction; double kick_speed = 0; Eigen::Isometry3d trunk_to_base_footprint; + double ball_radius; }; enum KickPhase { diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h index b990ead9..5ee00323 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h @@ -47,6 +47,8 @@ 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 publishGoals(const KickPositions &positions, const KickPositions &stabilized_positions, const robot_state::RobotStatePtr &robot_state, @@ -58,6 +60,7 @@ 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 debug_publisher_; std::string base_topic_; const std::string marker_ns_ = "bitbots_dynamic_kick"; diff --git a/bitbots_dynamic_kick/scripts/dummy_client.py b/bitbots_dynamic_kick/scripts/dummy_client.py index d9d14946..a1a7c8b4 100755 --- a/bitbots_dynamic_kick/scripts/dummy_client.py +++ b/bitbots_dynamic_kick/scripts/dummy_client.py @@ -83,7 +83,7 @@ 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 diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index 3d7e52b5..be5c7353 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_dynamic_kick/src/kick_engine.cpp @@ -14,13 +14,13 @@ void KickEngine::reset() { } void KickEngine::setGoals(const KickGoals &goals) { - ROS_WARN("setgoal"); is_left_kick_ = calcIsLeftFootKicking(goals.ball_position, goals.kick_direction); // TODO Internal state is dirty when goal transformation fails /* Save given goals because we reuse them later */ auto transformed_goal = transformGoal((is_left_kick_) ? "r_toe" : "l_toe", goals.trunk_to_base_footprint, goals.ball_position, goals.kick_direction); + ball_radius_ = goals.ball_radius; ball_position_ = transformed_goal.first; kick_direction_ = transformed_goal.second; kick_direction_.normalize(); @@ -84,11 +84,11 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei */ /* calculate timings for this kick */ - ROS_ERROR_STREAM(params_.move_to_ball_time); phase_timings_.move_trunk = 0 + params_.move_trunk_time; phase_timings_.raise_foot = phase_timings_.move_trunk + params_.raise_foot_time; phase_timings_.windup = phase_timings_.raise_foot + params_.move_to_ball_time; - phase_timings_.kick = phase_timings_.windup + params_.kick_time; + phase_timings_.low = phase_timings_.windup + params_.low_time; + phase_timings_.kick = phase_timings_.low + params_.kick_time; phase_timings_.move_back = phase_timings_.kick + params_.move_back_time; phase_timings_.lower_foot = phase_timings_.move_back + params_.lower_foot_time; phase_timings_.move_trunk_back = phase_timings_.lower_foot + params_.move_trunk_back_time; @@ -100,21 +100,26 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei kick_foot_sign = -1; } + kick_point_ = calcKickPoint(); windup_point_ = calcKickWindupPoint(); /* build vector of speeds in each direction */ double speed_yaw = rot_conv::EYawOfQuat(kick_direction_); Eigen::Vector3d speed_vector(cos(speed_yaw), sin(speed_yaw), 0); double target_yaw = calcKickFootYaw(); + ROS_WARN_STREAM(target_yaw); /* Flying foot position */ flying_foot_spline_.x()->addPoint(0, flying_foot_pose.translation().x()); flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk, 0); flying_foot_spline_.x()->addPoint(phase_timings_.raise_foot, 0); - flying_foot_spline_.x()->addPoint(phase_timings_.windup, windup_point_.x(), 0, 0); + flying_foot_spline_.x()->addPoint(phase_timings_.windup, windup_point_.x(), 0); + flying_foot_spline_.x()->addPoint(phase_timings_.low, + params_.low_x * cos(target_yaw), + params_.low_x_speed * cos(target_yaw)); flying_foot_spline_.x()->addPoint(phase_timings_.kick, - ball_position_.x() + params_.foot_extra_forward * cos(target_yaw), - speed_vector.x() * kick_speed_, 0); + kick_point_.x() + params_.foot_extra_forward * cos(target_yaw), + speed_vector.x() * kick_speed_); flying_foot_spline_.x()->addPoint(phase_timings_.move_back, 0); flying_foot_spline_.x()->addPoint(phase_timings_.lower_foot, 0); flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk_back, 0); @@ -122,20 +127,23 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.y()->addPoint(0, flying_foot_pose.translation().y()); 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_.windup, windup_point_.y(), 0); + //flying_foot_spline_.y()->addPoint(phase_timings_.low, + // kick_foot_sign * params_.foot_distance + params_.low_x * sin(target_yaw), + // params_.low_x_speed * sin(target_yaw)); flying_foot_spline_.y()->addPoint(phase_timings_.kick, - ball_position_.y() + params_.foot_extra_forward * sin(target_yaw), - speed_vector.y() * kick_speed_, 0); + kick_point_.y() + params_.foot_extra_forward * sin(target_yaw), + speed_vector.y() * kick_speed_); 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, 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); flying_foot_spline_.z()->addPoint(phase_timings_.raise_foot, params_.foot_rise); flying_foot_spline_.z()->addPoint(phase_timings_.windup, params_.foot_rise); - flying_foot_spline_.z()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, - params_.foot_rise_lower); + flying_foot_spline_.z()->addPoint(phase_timings_.low, params_.foot_rise_lower); flying_foot_spline_.z()->addPoint(phase_timings_.kick - params_.earlier_time, params_.foot_rise_kick); flying_foot_spline_.z()->addPoint(phase_timings_.kick, params_.foot_rise_kick); flying_foot_spline_.z()->addPoint(phase_timings_.move_back, params_.foot_rise_kick); @@ -174,6 +182,8 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.yaw()->addPoint(phase_timings_.move_back, start_y); flying_foot_spline_.yaw()->addPoint(phase_timings_.move_trunk_back, start_y); + ROS_WARN_STREAM(flying_foot_spline_.getDebugString()); + /* Stabilizing point */ trunk_spline_.x()->addPoint(0, 0); trunk_spline_.x()->addPoint(phase_timings_.move_trunk, params_.stabilizing_point_x); @@ -251,14 +261,27 @@ Eigen::Vector3d KickEngine::calcKickWindupPoint() { /* take windup distance into account */ vec *= -params_.kick_windup_distance; - /* add the ball position because the windup point is in support_foot_frame and not ball_frame */ - vec += ball_position_; + /* add the kick position because the windup point is in support_foot_frame and not ball_frame */ + vec += kick_point_; vec.z() = params_.foot_rise; return vec; } +Eigen::Vector3d KickEngine::calcKickPoint() { + // calculate the point where we will hit the ball with the front of the foot + double yaw = rot_conv::EYawOfQuat(kick_direction_); + /* create a vector which points in the negative direction of kick_direction_ */ + Eigen::Vector3d vec(cos(yaw), sin(yaw), 0); + vec.normalize(); + + vec = ball_position_ + vec * ball_radius_; + vec.z() = params_.foot_rise_kick; + + return vec; +} + bool KickEngine::calcIsLeftFootKicking(const Eigen::Vector3d &ball_position, const Eigen::Quaterniond &kick_direction) { /* it is important that everything is in base_footprint frame! */ @@ -349,6 +372,10 @@ Eigen::Vector3d KickEngine::getWindupPoint() { return windup_point_; } +Eigen::Vector3d KickEngine::getKickPoint() { + return kick_point_; +} + void KickEngine::setRobotState(robot_state::RobotStatePtr current_state) { current_state_ = current_state; } diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index 659cdb19..f979c746 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -12,6 +12,7 @@ KickNode::KickNode(const std::string &ns) : private_node_handle_.param("base_footprint_frame", base_footprint_frame_, "base_footprint"); private_node_handle_.param("r_toe_frame", r_toe_frame_, "r_toe"); private_node_handle_.param("l_toe_frame", l_toe_frame_, "l_toe"); + private_node_handle_.param("ball_radius", ball_radius_, 0.07); unstable_config_ = getUnstableConfig(); @@ -42,14 +43,14 @@ KickNode::KickNode(const std::string &ns) : void KickNode::copLCallback(const geometry_msgs::PointStamped &cop) { if (cop.header.frame_id != l_toe_frame_) { - ROS_ERROR_STREAM("cop_l not in " << l_toe_frame_ << " frame! Stabilizing will not work."); + ROS_ERROR_THROTTLE(10, "cop_l not in correct frame! Stabilizing will not work."); } stabilizer_.cop_left = cop.point; } void KickNode::copRCallback(const geometry_msgs::PointStamped &cop) { if (cop.header.frame_id != r_toe_frame_) { - ROS_ERROR_STREAM("cop_r not in " << r_toe_frame_ << " frame! Stabilizing will not work."); + ROS_ERROR_THROTTLE(10, "cop_r not in correct frame! Stabilizing will not work."); } stabilizer_.cop_right = cop.point; } @@ -110,9 +111,9 @@ void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &conf params.move_trunk_time = config.move_trunk_time; params.raise_foot_time = config.raise_foot_time; params.move_to_ball_time = config.move_to_ball_time; + params.low_time = config.low_time; params.kick_time = config.kick_time; params.move_back_time = config.move_back_time; - ROS_WARN_STREAM(params.move_to_ball_time); params.lower_foot_time = config.lower_foot_time; params.move_trunk_back_time = config.move_trunk_back_time; params.stabilizing_point_x = config.stabilizing_point_x; @@ -123,6 +124,8 @@ void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &conf params.foot_rise_lower = config.foot_rise_lower; params.foot_rise_kick = config.foot_rise_kick; params.earlier_time = config.earlier_time; + params.low_x = config.low_x; + params.low_x_speed = config.low_x_speed; engine_.setParams(params); stabilizer_.useCop(config.use_center_of_pressure); @@ -167,11 +170,13 @@ bool KickNode::init(const bitbots_msgs::KickGoal &goal_msg, tf2::convert(goal_msg.kick_direction, goals.kick_direction); goals.kick_speed = goal_msg.kick_speed; goals.trunk_to_base_footprint = trunk_to_base_footprint; + goals.ball_radius = ball_radius_; engine_.setGoals(goals); /* visualization */ visualizer_.displayReceivedGoal(goal_msg); visualizer_.displayWindupPoint(engine_.getWindupPoint(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); + visualizer_.displayKickPoint(engine_.getKickPoint(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); visualizer_.displayFlyingSplines(engine_.getFlyingSplines(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); visualizer_.displayTrunkSplines(engine_.getTrunkSplines(), (engine_.isLeftKick() ? r_toe_frame_ : l_toe_frame_)); diff --git a/bitbots_dynamic_kick/src/visualizer.cpp b/bitbots_dynamic_kick/src/visualizer.cpp index d8788cf3..36d17754 100644 --- a/bitbots_dynamic_kick/src/visualizer.cpp +++ b/bitbots_dynamic_kick/src/visualizer.cpp @@ -20,6 +20,8 @@ Visualizer::Visualizer(const std::string &base_topic) : /* queue_size */ 5, /* latch */ true); windup_publisher_ = node_handle_.advertise(base_topic_ + "kick_windup_point", /* queue_size */ 5, /* latch */ true); + kick_point_publisher_ = node_handle_.advertise(base_topic_ + "kick_point", + /* queue_size */ 5, /* latch */ true); debug_publisher_ = node_handle_.advertise(base_topic_ + "debug", /* queue_size */ 5, /* latch */ false); } @@ -83,6 +85,21 @@ void Visualizer::displayWindupPoint(const Eigen::Vector3d &kick_windup_point, co windup_publisher_.publish(marker); } +void Visualizer::displayKickPoint(const Eigen::Vector3d &kick_point, const std::string &support_foot_frame) { + if (kick_point_publisher_.getNumSubscribers() == 0) + return; + + tf2::Vector3 tf_kick_point; + tf2::convert(kick_point, tf_kick_point); + visualization_msgs::Marker marker = getMarker(tf_kick_point, support_foot_frame); + + marker.ns = marker_ns_; + marker.id = MarkerIDs::RECEIVED_GOAL; + marker.color.g = 1; + + kick_point_publisher_.publish(marker); +} + void Visualizer::publishGoals(const KickPositions &positions, const KickPositions &stabilized_positions, const robot_state::RobotStatePtr &robot_state, From 1da84de2c737a6001b015f8f2820db3c6662143b Mon Sep 17 00:00:00 2001 From: Marc Bestmann Date: Thu, 24 Jun 2021 21:07:24 +0200 Subject: [PATCH 09/13] leg space kick --- .../cfg/bitbots_dynamic_kick_params.cfg | 11 + bitbots_dynamic_kick/config/kick_config.yaml | 4 + .../bitbots_dynamic_kick/kick_engine.h | 15 +- .../include/bitbots_dynamic_kick/kick_node.h | 2 +- .../include/bitbots_dynamic_kick/kick_utils.h | 3 + .../include/bitbots_dynamic_kick/visualizer.h | 4 + bitbots_dynamic_kick/msg/KickDebug.msg | 2 + bitbots_dynamic_kick/scripts/dummy_client.py | 2 +- .../scripts/interactive_test.py | 256 ++++++++++++++++++ bitbots_dynamic_kick/src/kick_engine.cpp | 124 +++++---- bitbots_dynamic_kick/src/kick_node.cpp | 16 +- bitbots_dynamic_kick/src/visualizer.cpp | 23 +- .../bitbots_splines/abstract_visualizer.h | 20 +- .../include/bitbots_splines/pose_spline.h | 4 + .../include/bitbots_splines/utils.h | 38 +++ bitbots_splines/src/Spline/pose_spline.cpp | 19 ++ 16 files changed, 482 insertions(+), 61 deletions(-) create mode 100644 bitbots_dynamic_kick/scripts/interactive_test.py create mode 100644 bitbots_splines/include/bitbots_splines/utils.h diff --git a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg index d91b1a5c..b32d70d9 100755 --- a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg +++ b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg @@ -58,6 +58,17 @@ 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, diff --git a/bitbots_dynamic_kick/config/kick_config.yaml b/bitbots_dynamic_kick/config/kick_config.yaml index e4843676..210e6c51 100644 --- a/bitbots_dynamic_kick/config/kick_config.yaml +++ b/bitbots_dynamic_kick/config/kick_config.yaml @@ -16,6 +16,10 @@ 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 diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h index 3b9fb40b..ad0304e8 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -53,6 +54,11 @@ struct KickParams { double low_x; double low_x_speed; + double rise_length; + double windup_length; + double windup_alpha; + + }; /** @@ -128,6 +134,7 @@ class KickEngine : public bitbots_splines::AbstractEngine 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); 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 e92e9f46..043fcf5d 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h @@ -109,7 +109,7 @@ class KickNode { std::optional normal_config_; double ball_radius_; - std::string base_link_frame_, base_footprint_frame_, l_toe_frame_, r_toe_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. diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h index 72feb9f9..f4f9b810 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h @@ -4,12 +4,15 @@ #include #include +#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; }; diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h index 5ee00323..b544c35d 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h @@ -49,6 +49,9 @@ class Visualizer : bitbots_splines::AbstractVisualizer { 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, @@ -61,6 +64,7 @@ class Visualizer : bitbots_splines::AbstractVisualizer { 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"; diff --git a/bitbots_dynamic_kick/msg/KickDebug.msg b/bitbots_dynamic_kick/msg/KickDebug.msg index eaae6471..165277de 100644 --- a/bitbots_dynamic_kick/msg/KickDebug.msg +++ b/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 \ No newline at end of file diff --git a/bitbots_dynamic_kick/scripts/dummy_client.py b/bitbots_dynamic_kick/scripts/dummy_client.py index a1a7c8b4..088d6f9d 100755 --- a/bitbots_dynamic_kick/scripts/dummy_client.py +++ b/bitbots_dynamic_kick/scripts/dummy_client.py @@ -90,7 +90,7 @@ def feedback_cb(feedback): goal.kick_direction = Quaternion(*quaternion_from_euler(0, 0, math.radians(args.kick_direction))) - goal.kick_speed = 6.7 if args.unstable else 0.1 + goal.kick_speed = 6.7 if args.unstable else 0.0 """marker = Marker() marker.header.stamp = goal.ball_position.header.stamp diff --git a/bitbots_dynamic_kick/scripts/interactive_test.py b/bitbots_dynamic_kick/scripts/interactive_test.py new file mode 100644 index 00000000..d36a91d0 --- /dev/null +++ b/bitbots_dynamic_kick/scripts/interactive_test.py @@ -0,0 +1,256 @@ +#!/usr/bin/env python3 + +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 bitbots_msgs.srv import SetObjectPose, SetObjectPosition, SetObjectPoseRequest, SetObjectPositionRequest +from geometry_msgs.msg import Vector3, Quaternion +from bitbots_msgs.msg import KickGoal, KickAction, KickFeedback +from visualization_msgs.msg import Marker + +from tf.transformations import quaternion_from_euler +import sys, select, termios, tty + +showing_feedback = False + +msg = """ +BitBots Interactive Kick Test +----------------------------- +Move Robot: Move Ball: + q w e t + a s d f g h + +SHIFT increases with factor 10 + +y/Y: set kick y command +x/X: set kick x command +c/C: set direction command +v/V: set speed command + +<: execute kick +r: reset robot and ball + + + + + + + + +""" +moveRobotBindings = { + 'w': (1, 0, 0), + 's': (-1, 0, 0), + 'a': (0, 1, 0), + 'd': (0, -1, 0), + 'q': (0, 0, 1), + 'e': (0, 0, -1), + 'W': (10, 0, 0), + 'S': (-10, 0, 0), + 'A': (0, 10, 0), + 'D': (0, -10, 0), + 'Q': (0, 0, 10), + 'E': (0, 0, -10), +} + +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), +} + +settings = termios.tcgetattr(sys.stdin) + + +def getKey(): + tty.setraw(sys.stdin.fileno()) + select.select([sys.stdin], [], [], 0) + key = sys.stdin.read(1) + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key + + +if __name__ == "__main__": + 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() + client = actionlib.SimpleActionClient('dynamic_kick', KickAction) + if not client.wait_for_server(): + exit(1) + + robot_x = 2.8 + robot_y = 0 + robot_yaw = 0 + ball_x = 3 + ball_y = 0 + kick_x = 0.1 + kick_y = 0 + kick_direction = 0 + kick_speed = 1 + + x_speed_step = 0.01 + y_speed_step = 0.01 + turn_speed_step = 0.01 + + frame_prefix = "" if os.environ.get("ROS_NAMESPACE") is None else os.environ.get("ROS_NAMESPACE") + "/" + + + def generate_kick_goal(x, y, direction, speed, unstable=False): + kick_goal = KickGoal() + kick_goal.header.stamp = rospy.Time.now() + kick_goal.header.frame_id = frame_prefix + "base_footprint" + kick_goal.ball_position.x = x + kick_goal.ball_position.y = y + kick_goal.ball_position.z = 0 + kick_goal.kick_direction = Quaternion(*quaternion_from_euler(0, 0, direction)) + kick_goal.kick_speed = speed + kick_goal.unstable = unstable + return kick_goal + + + def execute_kick(): + goal = generate_kick_goal(kick_x, kick_y, kick_direction, kick_speed) + client.send_goal(goal) + client.done_cb = done_cb + client.feedback_cb = feedback_cb + client.active_cb = active_cb + client.wait_for_result() + + + rospy.wait_for_service("set_robot_pose") + set_robot_pose_service = rospy.ServiceProxy("set_robot_pose", SetObjectPose) + rospy.wait_for_service("set_ball_position") + set_ball_pos_service = rospy.ServiceProxy("set_ball_position", SetObjectPosition) + + + def set_robot_pose(): + request = SetObjectPoseRequest() + request.object_name = "amy" + request.pose.position.x = robot_x + request.pose.position.y = robot_y + request.pose.position.z = 0.40 + request.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, robot_yaw)) + response = set_robot_pose_service(request) + + + def set_ball_position(): + request = SetObjectPositionRequest() + request.position.x = ball_x + request.position.y = ball_y + request.position.z = 0.04 + response = set_ball_pos_service(request) + + sys.stdout.write("\x1b[A") + sys.stdout.write("\x1b[A") + print(msg) + + while not rospy.is_shutdown(): + key = getKey() + if key in moveRobotBindings.keys(): + robot_x += moveRobotBindings[key][0] * x_speed_step + robot_x = round(robot_x, 4) + robot_y += moveRobotBindings[key][1] * y_speed_step + robot_y = round(robot_y, 4) + robot_yaw += moveRobotBindings[key][2] * turn_speed_step + robot_yaw = round(robot_yaw, 4) + set_robot_pose() + elif key in moveBallBindings.keys(): + ball_x += moveBallBindings[key][0] * x_speed_step + ball_x = round(ball_x, 4) + ball_y += moveBallBindings[key][1] * y_speed_step + ball_y = round(ball_y, 4) + set_ball_position() + elif key == "<": + execute_kick() + elif key == "y": + kick_y -= 0.01 + elif key == "Y": + kick_y += 0.01 + elif key == "x": + kick_x -= 0.01 + elif key == "X": + kick_x += 0.01 + elif key == "c": + kick_direction -= 0.01 + elif key == "C": + kick_direction += 0.01 + elif key == "v": + kick_speed -= 0.1 + elif key == "V": + kick_speed += 0.1 + elif key == "r": + set_robot_pose() + set_ball_position() + 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") + 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" + f"robot_yaw: {round(robot_yaw, 2)} \n" + f"kick x: {round(kick_x, 2)} \n" + f"kick y: {round(kick_y, 2)} \n" + f"kick dir: {round(kick_direction, 2)} \n" + f"kick speed: {round(kick_speed, 2)} ") diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index be5c7353..0a322b71 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_dynamic_kick/src/kick_engine.cpp @@ -30,6 +30,10 @@ void KickEngine::setGoals(const KickGoals &goals) { Eigen::Isometry3d trunk_to_flying_foot = current_state_->getGlobalLinkTransform(is_left_kick_ ? "l_toe" : "r_toe"); Eigen::Isometry3d trunk_to_support_foot = current_state_->getGlobalLinkTransform(is_left_kick_ ? "r_toe" : "l_toe"); + Eigen::Isometry3d + trunk_to_flying_hip = current_state_->getGlobalLinkTransform(is_left_kick_ ? "l_upper_leg" : "r_upper_leg"); + // no rotation + trunk_to_flying_hip.matrix().block<3,3>(0,0) = Eigen::Matrix3d::Identity(); /* get the start position of the trunk relative to the support foot */ Eigen::Isometry3d trunk_frame; @@ -40,7 +44,16 @@ void KickEngine::setGoals(const KickGoals &goals) { } /* Plan new splines according to new goal */ - calcSplines(trunk_to_support_foot.inverse() * trunk_to_flying_foot, trunk_frame); + ROS_ERROR_STREAM("trunk flying foot"); + ROS_ERROR_STREAM(trunk_to_flying_foot.translation()); + ROS_ERROR_STREAM("trunk hip inv"); + ROS_ERROR_STREAM(trunk_to_flying_hip.inverse().translation()); + calcSplines(trunk_to_flying_hip.inverse() * trunk_to_flying_foot, trunk_frame); +} + +void KickEngine::setTrunkToHip(Eigen::Isometry3d trunk_to_hip_l, Eigen::Isometry3d trunk_to_hip_r) { + trunk_to_hip_l_ = trunk_to_hip_l; + trunk_to_hip_r_ = trunk_to_hip_r; } KickPositions KickEngine::update(double dt) { @@ -48,7 +61,20 @@ KickPositions KickEngine::update(double dt) { KickPositions positions; /* Get should-be pose from planned splines (every axis) at current time */ positions.trunk_pose = tf2::transformToEigen(tf2::toMsg(trunk_spline_.getTfTransform(time_))); - positions.flying_foot_pose = tf2::transformToEigen(tf2::toMsg(flying_foot_spline_.getTfTransform(time_))); + // for debug + positions.flying_foot_leg_space = tf2::transformToEigen(tf2::toMsg(flying_foot_spline_.getTfTransform(time_))); + // this is in leg space and in hip frame. transform + Eigen::Isometry3d trunk_to_hip; + if (is_left_kick_) { + trunk_to_hip = trunk_to_hip_l_; + } else { + trunk_to_hip = trunk_to_hip_r_; + } + Eigen::Isometry3d + hip_to_flying_goal = tf2::transformToEigen(tf2::toMsg(flying_foot_spline_.getTfTransformLegSpace(time_))); + Eigen::Isometry3d trunk_to_flying_foot_goal = trunk_to_hip * hip_to_flying_goal; + + positions.flying_foot_pose = trunk_to_flying_foot_goal; positions.is_left_kick = is_left_kick_; positions.engine_time = time_; @@ -87,8 +113,7 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei phase_timings_.move_trunk = 0 + params_.move_trunk_time; phase_timings_.raise_foot = phase_timings_.move_trunk + params_.raise_foot_time; phase_timings_.windup = phase_timings_.raise_foot + params_.move_to_ball_time; - phase_timings_.low = phase_timings_.windup + params_.low_time; - phase_timings_.kick = phase_timings_.low + params_.kick_time; + phase_timings_.kick = phase_timings_.windup + params_.kick_time; phase_timings_.move_back = phase_timings_.kick + params_.move_back_time; phase_timings_.lower_foot = phase_timings_.move_back + params_.lower_foot_time; phase_timings_.move_trunk_back = phase_timings_.lower_foot + params_.move_trunk_back_time; @@ -110,45 +135,45 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei ROS_WARN_STREAM(target_yaw); /* Flying foot position */ - flying_foot_spline_.x()->addPoint(0, flying_foot_pose.translation().x()); - flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk, 0); - flying_foot_spline_.x()->addPoint(phase_timings_.raise_foot, 0); - flying_foot_spline_.x()->addPoint(phase_timings_.windup, windup_point_.x(), 0); - flying_foot_spline_.x()->addPoint(phase_timings_.low, - params_.low_x * cos(target_yaw), - params_.low_x_speed * cos(target_yaw)); + Eigen::Vector3d start_leg_space = bitbots_splines::cartesian2leg(flying_foot_pose.translation()); + Eigen::Vector3d windup_leg_space = bitbots_splines::cartesian2leg(windup_point_); + Eigen::Vector3d kick_leg_space = bitbots_splines::cartesian2leg(kick_point_); + ROS_WARN_STREAM(flying_foot_pose.translation()); + ROS_WARN_STREAM(bitbots_splines::leg2cartesian(start_leg_space)); + + // length + flying_foot_spline_.x()->addPoint(0, start_leg_space.x()); + /*flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk, start_leg_space.x()); + flying_foot_spline_.x()->addPoint(phase_timings_.raise_foot, params_.rise_length); + flying_foot_spline_.x()->addPoint(phase_timings_.windup, params_.windup_length); flying_foot_spline_.x()->addPoint(phase_timings_.kick, - kick_point_.x() + params_.foot_extra_forward * cos(target_yaw), + kick_leg_space.x(), speed_vector.x() * kick_speed_); - flying_foot_spline_.x()->addPoint(phase_timings_.move_back, 0); - flying_foot_spline_.x()->addPoint(phase_timings_.lower_foot, 0); - 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, 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); - //flying_foot_spline_.y()->addPoint(phase_timings_.low, - // kick_foot_sign * params_.foot_distance + params_.low_x * sin(target_yaw), - // params_.low_x_speed * sin(target_yaw)); + flying_foot_spline_.x()->addPoint(phase_timings_.move_back, params_.rise_length); + flying_foot_spline_.x()->addPoint(phase_timings_.lower_foot, start_leg_space.x());*/ + flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk_back, start_leg_space.x()); + + // alpha + flying_foot_spline_.y()->addPoint(0, start_leg_space.y()); + /*flying_foot_spline_.y()->addPoint(phase_timings_.move_trunk, start_leg_space.y()); + flying_foot_spline_.y()->addPoint(phase_timings_.raise_foot, start_leg_space.y()); + flying_foot_spline_.y()->addPoint(phase_timings_.windup, params_.windup_alpha); flying_foot_spline_.y()->addPoint(phase_timings_.kick, - kick_point_.y() + params_.foot_extra_forward * sin(target_yaw), + kick_leg_space.y(), speed_vector.y() * kick_speed_); - 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, 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); - flying_foot_spline_.z()->addPoint(phase_timings_.raise_foot, params_.foot_rise); - flying_foot_spline_.z()->addPoint(phase_timings_.windup, params_.foot_rise); - flying_foot_spline_.z()->addPoint(phase_timings_.low, params_.foot_rise_lower); - flying_foot_spline_.z()->addPoint(phase_timings_.kick - params_.earlier_time, params_.foot_rise_kick); - flying_foot_spline_.z()->addPoint(phase_timings_.kick, params_.foot_rise_kick); - flying_foot_spline_.z()->addPoint(phase_timings_.move_back, params_.foot_rise_kick); - flying_foot_spline_.z()->addPoint(phase_timings_.lower_foot, 0.4 * params_.foot_rise_kick); - flying_foot_spline_.z()->addPoint(phase_timings_.move_trunk_back, 0); + flying_foot_spline_.y()->addPoint(phase_timings_.move_back, start_leg_space.y()); + flying_foot_spline_.y()->addPoint(phase_timings_.lower_foot, start_leg_space.y());*/ + flying_foot_spline_.y()->addPoint(phase_timings_.move_trunk_back, start_leg_space.y()); + + // beta + flying_foot_spline_.z()->addPoint(0, start_leg_space.z()); //todo set this based on direction + /*flying_foot_spline_.z()->addPoint(phase_timings_.move_trunk, start_leg_space.z()); + flying_foot_spline_.z()->addPoint(phase_timings_.raise_foot, start_leg_space.z()); + flying_foot_spline_.z()->addPoint(phase_timings_.windup, start_leg_space.z()); + flying_foot_spline_.z()->addPoint(phase_timings_.kick, start_leg_space.z()); + flying_foot_spline_.z()->addPoint(phase_timings_.move_back, start_leg_space.z()); + flying_foot_spline_.z()->addPoint(phase_timings_.lower_foot, start_leg_space.z());*/ + flying_foot_spline_.z()->addPoint(phase_timings_.move_trunk_back, start_leg_space.z()); /* Flying foot orientation */ /* Get euler angles for foot rotation */ @@ -160,18 +185,16 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.roll()->addPoint(0, start_r); flying_foot_spline_.roll()->addPoint(phase_timings_.raise_foot, start_r); flying_foot_spline_.roll()->addPoint(phase_timings_.windup, -sin(target_yaw) * params_.foot_pitch); - flying_foot_spline_.roll()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, - - sin(target_yaw) * params_.foot_pitch); - flying_foot_spline_.roll()->addPoint(phase_timings_.kick - params_.earlier_time, 0); + flying_foot_spline_.roll()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) / 2, + -sin(target_yaw) * params_.foot_pitch); flying_foot_spline_.roll()->addPoint(phase_timings_.kick, 0); flying_foot_spline_.roll()->addPoint(phase_timings_.move_trunk_back, start_r); flying_foot_spline_.pitch()->addPoint(0, start_p); flying_foot_spline_.pitch()->addPoint(phase_timings_.raise_foot, start_p); flying_foot_spline_.pitch()->addPoint(phase_timings_.windup, cos(target_yaw) * params_.foot_pitch); - flying_foot_spline_.pitch()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, - cos(target_yaw) * params_.foot_pitch); - flying_foot_spline_.pitch()->addPoint(phase_timings_.kick - params_.earlier_time, 0); + flying_foot_spline_.pitch()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) / 2, + cos(target_yaw) * params_.foot_pitch); flying_foot_spline_.pitch()->addPoint(phase_timings_.kick, 0); flying_foot_spline_.pitch()->addPoint(phase_timings_.move_trunk_back, start_p); @@ -182,7 +205,7 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.yaw()->addPoint(phase_timings_.move_back, start_y); flying_foot_spline_.yaw()->addPoint(phase_timings_.move_trunk_back, start_y); - ROS_WARN_STREAM(flying_foot_spline_.getDebugString()); + //ROS_WARN_STREAM(flying_foot_spline_.getDebugString()); /* Stabilizing point */ trunk_spline_.x()->addPoint(0, 0); @@ -245,11 +268,14 @@ std::pair KickEngine::transformGoal( Eigen::Isometry3d base_footprint_to_support_foot = trunk_to_base_footprint.inverse() * trunk_to_support_foot; /* now, apply the transforms. Because of eigen, the transform has to be on the left hand side, therefore it must be inversed */ Eigen::Vector3d ball_transformed = base_footprint_to_support_foot.inverse() * ball_position; - Eigen::Matrix3d kick_direction_transformed_matrix = (base_footprint_to_support_foot.inverse() * kick_direction).rotation(); + Eigen::Matrix3d + kick_direction_transformed_matrix = (base_footprint_to_support_foot.inverse() * kick_direction).rotation(); Eigen::Quaterniond kick_direction_transformed(kick_direction_transformed_matrix); return std::make_pair(ball_transformed, kick_direction_transformed); } + + Eigen::Vector3d KickEngine::calcKickWindupPoint() { /* retrieve yaw from kick_direction_ */ double yaw = rot_conv::EYawOfQuat(kick_direction_); @@ -376,6 +402,10 @@ Eigen::Vector3d KickEngine::getKickPoint() { return kick_point_; } +Eigen::Vector3d KickEngine::getBallPoint() { + return ball_position_; +} + void KickEngine::setRobotState(robot_state::RobotStatePtr current_state) { current_state_ = current_state; } diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index f979c746..d98d794b 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -12,6 +12,8 @@ KickNode::KickNode(const std::string &ns) : private_node_handle_.param("base_footprint_frame", base_footprint_frame_, "base_footprint"); private_node_handle_.param("r_toe_frame", r_toe_frame_, "r_toe"); private_node_handle_.param("l_toe_frame", l_toe_frame_, "l_toe"); + private_node_handle_.param("r_hip_frame", r_hip_frame_, "r_upper_leg"); + private_node_handle_.param("l_hip_frame", l_hip_frame_, "l_upper_leg"); private_node_handle_.param("ball_radius", ball_radius_, 0.07); unstable_config_ = getUnstableConfig(); @@ -24,7 +26,6 @@ KickNode::KickNode(const std::string &ns) : exit(1); } stabilizer_.setRobotModel(kinematic_model); - ik_.init(kinematic_model); /* this debug variable represents the goal state of the robot, i.e. what the ik goals are */ goal_state_.reset(new robot_state::RobotState(kinematic_model)); @@ -32,6 +33,11 @@ KickNode::KickNode(const std::string &ns) : current_state_.reset(new robot_state::RobotState(kinematic_model)); engine_.setRobotState(current_state_); + Eigen::Isometry3d trunk_to_hip_l = current_state_->getGlobalLinkTransform(l_hip_frame_); + Eigen::Isometry3d trunk_to_hip_r = current_state_->getGlobalLinkTransform(r_hip_frame_); + ik_.init(kinematic_model); + engine_.setTrunkToHip(trunk_to_hip_l, trunk_to_hip_r); + joint_goal_publisher_ = node_handle_.advertise("kick_motor_goals", 1); support_foot_publisher_ = node_handle_.advertise("dynamic_kick_support_state", 1, /* latch = */ true); @@ -126,6 +132,11 @@ void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &conf params.earlier_time = config.earlier_time; params.low_x = config.low_x; params.low_x_speed = config.low_x_speed; + + params.rise_length = config.rise_length; + params.windup_length = config.windup_length; + params.windup_alpha = config.windup_alpha; + engine_.setParams(params); stabilizer_.useCop(config.use_center_of_pressure); @@ -175,9 +186,10 @@ bool KickNode::init(const bitbots_msgs::KickGoal &goal_msg, /* visualization */ visualizer_.displayReceivedGoal(goal_msg); + visualizer_.displayBallPoint(engine_.getBallPoint(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); visualizer_.displayWindupPoint(engine_.getWindupPoint(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); visualizer_.displayKickPoint(engine_.getKickPoint(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); - visualizer_.displayFlyingSplines(engine_.getFlyingSplines(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); + visualizer_.displayFlyingSplines(engine_.getFlyingSplines(), (engine_.isLeftKick()) ? r_hip_frame_ : l_hip_frame_); visualizer_.displayTrunkSplines(engine_.getTrunkSplines(), (engine_.isLeftKick() ? r_toe_frame_ : l_toe_frame_)); return true; diff --git a/bitbots_dynamic_kick/src/visualizer.cpp b/bitbots_dynamic_kick/src/visualizer.cpp index 36d17754..a53083ed 100644 --- a/bitbots_dynamic_kick/src/visualizer.cpp +++ b/bitbots_dynamic_kick/src/visualizer.cpp @@ -22,6 +22,8 @@ Visualizer::Visualizer(const std::string &base_topic) : /* queue_size */ 5, /* latch */ true); kick_point_publisher_ = node_handle_.advertise(base_topic_ + "kick_point", /* queue_size */ 5, /* latch */ true); + ball_point_publisher_ = node_handle_.advertise(base_topic_ + "ball_point", + /* queue_size */ 5, /* latch */ true); debug_publisher_ = node_handle_.advertise(base_topic_ + "debug", /* queue_size */ 5, /* latch */ false); } @@ -35,7 +37,7 @@ void Visualizer::displayFlyingSplines(bitbots_splines::PoseSpline splines, if (foot_spline_publisher_.getNumSubscribers() == 0) return; - visualization_msgs::MarkerArray path = getPath(splines, support_foot_frame, params_.spline_smoothness); + visualization_msgs::MarkerArray path = getPath(splines, support_foot_frame, params_.spline_smoothness, true); path.markers[0].color.g = 1; foot_spline_publisher_.publish(path); @@ -95,11 +97,26 @@ void Visualizer::displayKickPoint(const Eigen::Vector3d &kick_point, const std:: marker.ns = marker_ns_; marker.id = MarkerIDs::RECEIVED_GOAL; - marker.color.g = 1; + marker.color.b = 1; kick_point_publisher_.publish(marker); } +void Visualizer::displayBallPoint(const Eigen::Vector3d &ball_point, const std::string &support_foot_frame) { + if (kick_point_publisher_.getNumSubscribers() == 0) + return; + + tf2::Vector3 tf_ball_point; + tf2::convert(ball_point, tf_ball_point); + visualization_msgs::Marker marker = getMarker(tf_ball_point, support_foot_frame); + + marker.ns = marker_ns_; + marker.id = MarkerIDs::RECEIVED_GOAL; + marker.color.r = 1; + + ball_point_publisher_.publish(marker); +} + void Visualizer::publishGoals(const KickPositions &positions, const KickPositions &stabilized_positions, const robot_state::RobotStatePtr &robot_state, @@ -143,6 +160,8 @@ void Visualizer::publishGoals(const KickPositions &positions, msg.flying_foot_pose_ik_result = tf2::toMsg(flying_foot_pose_ik_result); msg.flying_foot_position_ik_offset = tf2::toMsg(flying_foot_position_ik_offset); + msg.flying_foot_pose_goal_leg_space = tf2::toMsg(positions.flying_foot_leg_space); + debug_publisher_.publish(msg); } diff --git a/bitbots_splines/include/bitbots_splines/abstract_visualizer.h b/bitbots_splines/include/bitbots_splines/abstract_visualizer.h index 2077e74c..028bea0c 100644 --- a/bitbots_splines/include/bitbots_splines/abstract_visualizer.h +++ b/bitbots_splines/include/bitbots_splines/abstract_visualizer.h @@ -5,6 +5,7 @@ #include #include #include +#include namespace bitbots_splines { @@ -47,7 +48,8 @@ class AbstractVisualizer { */ visualization_msgs::MarkerArray getPath(bitbots_splines::PoseSpline &spline, const std::string &frame, - const double smoothness) { + const double smoothness, + bool use_leg_space=false) { visualization_msgs::MarkerArray marker_array; visualization_msgs::Marker base_marker; @@ -73,10 +75,18 @@ class AbstractVisualizer { // Taking the manually set points is not enough because velocities and accelerations influence the curve for (double i = first_time; i <= last_time; i += (last_time - first_time) / smoothness) { geometry_msgs::Point point; - point.x = spline.x()->pos(i); - point.y = spline.y()->pos(i); - point.z = spline.z()->pos(i); - + if (use_leg_space){ + Eigen::Vector3d leg(spline.x()->pos(i), spline.y()->pos(i), spline.z()->pos(i)); + Eigen::Vector3d cartesian = leg2cartesian(leg); + tf2::Vector3 pos; + point.x = cartesian.x(); + point.y = cartesian.y(); + point.z = cartesian.z(); + }else{ + point.x = spline.x()->pos(i); + point.y = spline.y()->pos(i); + point.z = spline.z()->pos(i); + } path_marker.points.push_back(point); } marker_array.markers.push_back(path_marker); diff --git a/bitbots_splines/include/bitbots_splines/pose_spline.h b/bitbots_splines/include/bitbots_splines/pose_spline.h index 6a47bed4..86ba48c6 100644 --- a/bitbots_splines/include/bitbots_splines/pose_spline.h +++ b/bitbots_splines/include/bitbots_splines/pose_spline.h @@ -1,6 +1,7 @@ #ifndef BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_POSE_SPLINE_H_ #define BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_POSE_SPLINE_H_ +#include #include #include #include @@ -16,6 +17,7 @@ namespace bitbots_splines { class PoseSpline { public: tf2::Transform getTfTransform(double time); + tf2::Transform getTfTransformLegSpace(double time); geometry_msgs::Pose getGeometryMsgPose(double time); geometry_msgs::Point getGeometryMsgPosition(double time); @@ -24,6 +26,8 @@ class PoseSpline { tf2::Vector3 getPositionPos(double time); tf2::Vector3 getPositionVel(double time); tf2::Vector3 getPositionAcc(double time); + tf2::Vector3 getPositionPosLegSpace(double time); + tf2::Vector3 getEulerAngles(double time); tf2::Vector3 getEulerVel(double time); diff --git a/bitbots_splines/include/bitbots_splines/utils.h b/bitbots_splines/include/bitbots_splines/utils.h new file mode 100644 index 00000000..2e713c04 --- /dev/null +++ b/bitbots_splines/include/bitbots_splines/utils.h @@ -0,0 +1,38 @@ +#ifndef BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_UTILS_H_ +#define BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_UTILS_H_ + +#include /* atan */ +#include + +namespace bitbots_splines { + +inline Eigen::Vector3d cartesian2leg(Eigen::Vector3d cartesian){ + // 3d pythagoras + double length = cartesian.norm(); + // solving triangle + double alpha = atan(cartesian.x() / cartesian.z()); + double beta = atan(cartesian.y() / cartesian.z()); + Eigen::Vector3d leg(length, alpha, beta); + return leg; +} + +inline Eigen::Vector3d leg2cartesian(Eigen::Vector3d leg){ + // solution by solving: + // x = tan(alpha)* z + // y = tan(beta) * z + // l² = x²+y²+z² = tan(alpha)²*z²+tan(beta)²*z²+z² + // l²=z²*(tan(alpha)²+tan(beta)²+1) + // z=root(l²/(tan(alpha)²+tan(beta)²+1)) + + double l = leg.x(); + double alpha = leg.y(); + double beta = leg.z(); + double z= -sqrt((l*l)/(tan(alpha)*tan(alpha)+tan(beta)*tan(beta)+1)); + double x= tan(alpha) * z; + double y= tan(beta) * z; + Eigen::Vector3d cartesian(x, y, z); + return cartesian; +} +} + +#endif //BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_UTILS_H_ diff --git a/bitbots_splines/src/Spline/pose_spline.cpp b/bitbots_splines/src/Spline/pose_spline.cpp index c547677b..54087cb6 100644 --- a/bitbots_splines/src/Spline/pose_spline.cpp +++ b/bitbots_splines/src/Spline/pose_spline.cpp @@ -8,6 +8,14 @@ tf2::Transform PoseSpline::getTfTransform(double time) { trans.setRotation(getOrientation(time)); return trans; } + +tf2::Transform PoseSpline::getTfTransformLegSpace(double time) { + tf2::Transform trans; + trans.setOrigin(getPositionPosLegSpace(time)); + trans.setRotation(getOrientation(time)); + return trans; +} + geometry_msgs::Pose PoseSpline::getGeometryMsgPose(double time) { geometry_msgs::Pose msg; msg.position = getGeometryMsgPosition(time); @@ -30,6 +38,17 @@ geometry_msgs::Quaternion PoseSpline::getGeometryMsgOrientation(double time) { return msg; } +tf2::Vector3 PoseSpline::getPositionPosLegSpace(double time) { + Eigen::Vector3d leg(x_.pos(time), y_.pos(time), z_.pos(time)); + Eigen::Vector3d cartesian = leg2cartesian(leg); + tf2::Vector3 pos; + pos[0] = cartesian.x(); + pos[1] = cartesian.y(); + pos[2] = cartesian.z(); + return pos; +} + + tf2::Vector3 PoseSpline::getPositionPos(double time) { tf2::Vector3 pos; pos[0] = x_.pos(time); From f6b80f0db81f50659c6203c359b3d14b94e52470 Mon Sep 17 00:00:00 2001 From: Marc Bestmann Date: Thu, 9 Dec 2021 16:12:17 +0100 Subject: [PATCH 10/13] fixes for review --- .../include/bitbots_dynamic_kick/kick_node.h | 2 +- .../scripts/interactive_test.py | 56 ++++--------------- bitbots_dynamic_kick/src/kick_engine.cpp | 4 -- bitbots_dynamic_kick/src/kick_node.cpp | 6 +- 4 files changed, 16 insertions(+), 52 deletions(-) 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 15b54bfe..aa9fcdcd 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h @@ -110,7 +110,7 @@ class KickNode { std::optional normal_config_; double ball_radius_; - std::string base_link_frame_, base_footprint_frame_, l_toe_frame_, r_toe_frame_, r_hip_frame_, l_hip_frame_; + std::string base_link_frame_, base_footprint_frame_, l_sole_frame_, r_sole_frame_, l_toe_frame_, r_toe_frame_, r_hip_frame_, l_hip_frame_; /** * Do main loop in which KickEngine::update() gets called repeatedly. diff --git a/bitbots_dynamic_kick/scripts/interactive_test.py b/bitbots_dynamic_kick/scripts/interactive_test.py index d36a91d0..a559ff5d 100644 --- a/bitbots_dynamic_kick/scripts/interactive_test.py +++ b/bitbots_dynamic_kick/scripts/interactive_test.py @@ -61,14 +61,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 +86,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() @@ -194,6 +166,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) @@ -238,14 +211,7 @@ def set_ball_position(): 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" diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index 0a322b71..a2cdfe49 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_dynamic_kick/src/kick_engine.cpp @@ -44,10 +44,6 @@ void KickEngine::setGoals(const KickGoals &goals) { } /* Plan new splines according to new goal */ - ROS_ERROR_STREAM("trunk flying foot"); - ROS_ERROR_STREAM(trunk_to_flying_foot.translation()); - ROS_ERROR_STREAM("trunk hip inv"); - ROS_ERROR_STREAM(trunk_to_flying_hip.inverse().translation()); calcSplines(trunk_to_flying_hip.inverse() * trunk_to_flying_foot, trunk_frame); } diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index 24d04d02..199f7d36 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -10,6 +10,8 @@ KickNode::KickNode(const std::string &ns) : robot_model_loader_(ns + "robot_description", false) { private_node_handle_.param("base_link_frame", base_link_frame_, "base_link"); private_node_handle_.param("base_footprint_frame", base_footprint_frame_, "base_footprint"); + private_node_handle_.param("r_sole_frame", r_sole_frame_, "r_sole"); + private_node_handle_.param("l_sole_frame", l_sole_frame_, "l_sole"); private_node_handle_.param("r_toe_frame", r_toe_frame_, "r_toe"); private_node_handle_.param("l_toe_frame", l_toe_frame_, "l_toe"); private_node_handle_.param("r_hip_frame", r_hip_frame_, "r_upper_leg"); @@ -48,14 +50,14 @@ KickNode::KickNode(const std::string &ns) : } void KickNode::copLCallback(const geometry_msgs::PointStamped &cop) { - if (cop.header.frame_id != l_toe_frame_) { + if (cop.header.frame_id != l_sole_frame_) { ROS_ERROR_THROTTLE(10, "cop_l not in correct frame! Stabilizing will not work."); } stabilizer_.cop_left = cop.point; } void KickNode::copRCallback(const geometry_msgs::PointStamped &cop) { - if (cop.header.frame_id != r_toe_frame_) { + if (cop.header.frame_id != r_sole_frame_) { ROS_ERROR_THROTTLE(10, "cop_r not in correct frame! Stabilizing will not work."); } stabilizer_.cop_right = cop.point; From e923d204e7aaae694291f57b05afd2b8ca718487 Mon Sep 17 00:00:00 2001 From: Marc Bestmann Date: Thu, 9 Dec 2021 16:51:02 +0100 Subject: [PATCH 11/13] revert changes that made kick work in leg space --- .../cfg/bitbots_dynamic_kick_params.cfg | 11 -- bitbots_dynamic_kick/config/kick_config.yaml | 3 - .../config/kick_config_unstable.yaml | 9 ++ .../bitbots_dynamic_kick/kick_engine.h | 7 +- .../include/bitbots_dynamic_kick/kick_node.h | 2 +- .../include/bitbots_dynamic_kick/kick_utils.h | 1 - bitbots_dynamic_kick/msg/KickDebug.msg | 2 - bitbots_dynamic_kick/scripts/dummy_client.py | 2 +- bitbots_dynamic_kick/src/kick_engine.cpp | 114 +++++++----------- bitbots_dynamic_kick/src/kick_node.cpp | 11 +- bitbots_dynamic_kick/src/visualizer.cpp | 2 - 11 files changed, 60 insertions(+), 104 deletions(-) diff --git a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg index b32d70d9..d91b1a5c 100755 --- a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg +++ b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg @@ -58,17 +58,6 @@ 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, diff --git a/bitbots_dynamic_kick/config/kick_config.yaml b/bitbots_dynamic_kick/config/kick_config.yaml index 210e6c51..6a191e2f 100644 --- a/bitbots_dynamic_kick/config/kick_config.yaml +++ b/bitbots_dynamic_kick/config/kick_config.yaml @@ -16,9 +16,6 @@ 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 diff --git a/bitbots_dynamic_kick/config/kick_config_unstable.yaml b/bitbots_dynamic_kick/config/kick_config_unstable.yaml index 4b1b399f..868dd4e0 100644 --- a/bitbots_dynamic_kick/config/kick_config_unstable.yaml +++ b/bitbots_dynamic_kick/config/kick_config_unstable.yaml @@ -2,6 +2,7 @@ unstable: foot_distance: 0.21 foot_rise: 0.11 kick_speed: 6.7 + low_time: 0.2 kick_time: 0.11 kick_windup_distance: 0.33 lower_foot_time: 0.16 @@ -16,5 +17,13 @@ unstable: trunk_pitch: -0.488692190558412 trunk_roll: 0.102974425867665 trunk_yaw: 0.296705972839036 + + 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 + use_center_of_pressure: false foot_x_extra: 0.00 diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h index ad0304e8..84300e26 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h @@ -134,7 +134,6 @@ class KickEngine : public bitbots_splines::AbstractEngine transformGoal( - const std::string &hip_frame, + const std::string &support_foot_frame, const Eigen::Isometry3d &trunk_to_base_footprint, const Eigen::Vector3d &ball_position, const Eigen::Quaterniond &kick_direction); 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 aa9fcdcd..21a46aa4 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h @@ -110,7 +110,7 @@ class KickNode { std::optional normal_config_; double ball_radius_; - std::string base_link_frame_, base_footprint_frame_, l_sole_frame_, r_sole_frame_, l_toe_frame_, r_toe_frame_, r_hip_frame_, l_hip_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. diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h index f4f9b810..8739a900 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h @@ -12,7 +12,6 @@ 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; }; diff --git a/bitbots_dynamic_kick/msg/KickDebug.msg b/bitbots_dynamic_kick/msg/KickDebug.msg index 165277de..eaae6471 100644 --- a/bitbots_dynamic_kick/msg/KickDebug.msg +++ b/bitbots_dynamic_kick/msg/KickDebug.msg @@ -30,5 +30,3 @@ 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 \ No newline at end of file diff --git a/bitbots_dynamic_kick/scripts/dummy_client.py b/bitbots_dynamic_kick/scripts/dummy_client.py index 088d6f9d..578a347a 100755 --- a/bitbots_dynamic_kick/scripts/dummy_client.py +++ b/bitbots_dynamic_kick/scripts/dummy_client.py @@ -90,7 +90,7 @@ def feedback_cb(feedback): goal.kick_direction = Quaternion(*quaternion_from_euler(0, 0, math.radians(args.kick_direction))) - goal.kick_speed = 6.7 if args.unstable else 0.0 + goal.kick_speed = 6.7 if args.unstable else 1.0 """marker = Marker() marker.header.stamp = goal.ball_position.header.stamp diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index a2cdfe49..41becf31 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_dynamic_kick/src/kick_engine.cpp @@ -30,10 +30,6 @@ void KickEngine::setGoals(const KickGoals &goals) { Eigen::Isometry3d trunk_to_flying_foot = current_state_->getGlobalLinkTransform(is_left_kick_ ? "l_toe" : "r_toe"); Eigen::Isometry3d trunk_to_support_foot = current_state_->getGlobalLinkTransform(is_left_kick_ ? "r_toe" : "l_toe"); - Eigen::Isometry3d - trunk_to_flying_hip = current_state_->getGlobalLinkTransform(is_left_kick_ ? "l_upper_leg" : "r_upper_leg"); - // no rotation - trunk_to_flying_hip.matrix().block<3,3>(0,0) = Eigen::Matrix3d::Identity(); /* get the start position of the trunk relative to the support foot */ Eigen::Isometry3d trunk_frame; @@ -44,33 +40,16 @@ void KickEngine::setGoals(const KickGoals &goals) { } /* Plan new splines according to new goal */ - calcSplines(trunk_to_flying_hip.inverse() * trunk_to_flying_foot, trunk_frame); + calcSplines(trunk_to_support_foot.inverse() * trunk_to_flying_foot, trunk_frame); } -void KickEngine::setTrunkToHip(Eigen::Isometry3d trunk_to_hip_l, Eigen::Isometry3d trunk_to_hip_r) { - trunk_to_hip_l_ = trunk_to_hip_l; - trunk_to_hip_r_ = trunk_to_hip_r; -} KickPositions KickEngine::update(double dt) { /* Only do an actual update when splines are present */ KickPositions positions; /* Get should-be pose from planned splines (every axis) at current time */ positions.trunk_pose = tf2::transformToEigen(tf2::toMsg(trunk_spline_.getTfTransform(time_))); - // for debug - positions.flying_foot_leg_space = tf2::transformToEigen(tf2::toMsg(flying_foot_spline_.getTfTransform(time_))); - // this is in leg space and in hip frame. transform - Eigen::Isometry3d trunk_to_hip; - if (is_left_kick_) { - trunk_to_hip = trunk_to_hip_l_; - } else { - trunk_to_hip = trunk_to_hip_r_; - } - Eigen::Isometry3d - hip_to_flying_goal = tf2::transformToEigen(tf2::toMsg(flying_foot_spline_.getTfTransformLegSpace(time_))); - Eigen::Isometry3d trunk_to_flying_foot_goal = trunk_to_hip * hip_to_flying_goal; - - positions.flying_foot_pose = trunk_to_flying_foot_goal; + positions.flying_foot_pose = tf2::transformToEigen(tf2::toMsg(flying_foot_spline_.getTfTransform(time_))); positions.is_left_kick = is_left_kick_; positions.engine_time = time_; @@ -109,7 +88,8 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei phase_timings_.move_trunk = 0 + params_.move_trunk_time; phase_timings_.raise_foot = phase_timings_.move_trunk + params_.raise_foot_time; phase_timings_.windup = phase_timings_.raise_foot + params_.move_to_ball_time; - phase_timings_.kick = phase_timings_.windup + params_.kick_time; + phase_timings_.low = phase_timings_.windup + params_.low_time; + phase_timings_.kick = phase_timings_.low + params_.kick_time; phase_timings_.move_back = phase_timings_.kick + params_.move_back_time; phase_timings_.lower_foot = phase_timings_.move_back + params_.lower_foot_time; phase_timings_.move_trunk_back = phase_timings_.lower_foot + params_.move_trunk_back_time; @@ -131,45 +111,45 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei ROS_WARN_STREAM(target_yaw); /* Flying foot position */ - Eigen::Vector3d start_leg_space = bitbots_splines::cartesian2leg(flying_foot_pose.translation()); - Eigen::Vector3d windup_leg_space = bitbots_splines::cartesian2leg(windup_point_); - Eigen::Vector3d kick_leg_space = bitbots_splines::cartesian2leg(kick_point_); - ROS_WARN_STREAM(flying_foot_pose.translation()); - ROS_WARN_STREAM(bitbots_splines::leg2cartesian(start_leg_space)); - - // length - flying_foot_spline_.x()->addPoint(0, start_leg_space.x()); - /*flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk, start_leg_space.x()); - flying_foot_spline_.x()->addPoint(phase_timings_.raise_foot, params_.rise_length); - flying_foot_spline_.x()->addPoint(phase_timings_.windup, params_.windup_length); + flying_foot_spline_.x()->addPoint(0, flying_foot_pose.translation().x()); + flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk, 0); + flying_foot_spline_.x()->addPoint(phase_timings_.raise_foot, 0); + flying_foot_spline_.x()->addPoint(phase_timings_.windup, windup_point_.x(), 0); + flying_foot_spline_.x()->addPoint(phase_timings_.low, + params_.low_x * cos(target_yaw), + params_.low_x_speed * cos(target_yaw)); flying_foot_spline_.x()->addPoint(phase_timings_.kick, - kick_leg_space.x(), + kick_point_.x() + params_.foot_extra_forward * cos(target_yaw), speed_vector.x() * kick_speed_); - flying_foot_spline_.x()->addPoint(phase_timings_.move_back, params_.rise_length); - flying_foot_spline_.x()->addPoint(phase_timings_.lower_foot, start_leg_space.x());*/ - flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk_back, start_leg_space.x()); - - // alpha - flying_foot_spline_.y()->addPoint(0, start_leg_space.y()); - /*flying_foot_spline_.y()->addPoint(phase_timings_.move_trunk, start_leg_space.y()); - flying_foot_spline_.y()->addPoint(phase_timings_.raise_foot, start_leg_space.y()); - flying_foot_spline_.y()->addPoint(phase_timings_.windup, params_.windup_alpha); + flying_foot_spline_.x()->addPoint(phase_timings_.move_back, 0); + flying_foot_spline_.x()->addPoint(phase_timings_.lower_foot, 0); + 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, 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); + //flying_foot_spline_.y()->addPoint(phase_timings_.low, + // kick_foot_sign * params_.foot_distance + params_.low_x * sin(target_yaw), + // params_.low_x_speed * sin(target_yaw)); flying_foot_spline_.y()->addPoint(phase_timings_.kick, - kick_leg_space.y(), + kick_point_.y() + params_.foot_extra_forward * sin(target_yaw), speed_vector.y() * kick_speed_); - flying_foot_spline_.y()->addPoint(phase_timings_.move_back, start_leg_space.y()); - flying_foot_spline_.y()->addPoint(phase_timings_.lower_foot, start_leg_space.y());*/ - flying_foot_spline_.y()->addPoint(phase_timings_.move_trunk_back, start_leg_space.y()); - - // beta - flying_foot_spline_.z()->addPoint(0, start_leg_space.z()); //todo set this based on direction - /*flying_foot_spline_.z()->addPoint(phase_timings_.move_trunk, start_leg_space.z()); - flying_foot_spline_.z()->addPoint(phase_timings_.raise_foot, start_leg_space.z()); - flying_foot_spline_.z()->addPoint(phase_timings_.windup, start_leg_space.z()); - flying_foot_spline_.z()->addPoint(phase_timings_.kick, start_leg_space.z()); - flying_foot_spline_.z()->addPoint(phase_timings_.move_back, start_leg_space.z()); - flying_foot_spline_.z()->addPoint(phase_timings_.lower_foot, start_leg_space.z());*/ - flying_foot_spline_.z()->addPoint(phase_timings_.move_trunk_back, start_leg_space.z()); + 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, 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); + flying_foot_spline_.z()->addPoint(phase_timings_.raise_foot, params_.foot_rise); + flying_foot_spline_.z()->addPoint(phase_timings_.windup, params_.foot_rise); + flying_foot_spline_.z()->addPoint(phase_timings_.low, params_.foot_rise_lower); + flying_foot_spline_.z()->addPoint(phase_timings_.kick - params_.earlier_time, params_.foot_rise_kick); + flying_foot_spline_.z()->addPoint(phase_timings_.kick, params_.foot_rise_kick); + flying_foot_spline_.z()->addPoint(phase_timings_.move_back, params_.foot_rise_kick); + flying_foot_spline_.z()->addPoint(phase_timings_.lower_foot, 0.4 * params_.foot_rise_kick); + flying_foot_spline_.z()->addPoint(phase_timings_.move_trunk_back, 0); /* Flying foot orientation */ /* Get euler angles for foot rotation */ @@ -181,16 +161,18 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.roll()->addPoint(0, start_r); flying_foot_spline_.roll()->addPoint(phase_timings_.raise_foot, start_r); flying_foot_spline_.roll()->addPoint(phase_timings_.windup, -sin(target_yaw) * params_.foot_pitch); - flying_foot_spline_.roll()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) / 2, - -sin(target_yaw) * params_.foot_pitch); + flying_foot_spline_.roll()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, + - sin(target_yaw) * params_.foot_pitch); + flying_foot_spline_.roll()->addPoint(phase_timings_.kick - params_.earlier_time, 0); flying_foot_spline_.roll()->addPoint(phase_timings_.kick, 0); flying_foot_spline_.roll()->addPoint(phase_timings_.move_trunk_back, start_r); flying_foot_spline_.pitch()->addPoint(0, start_p); flying_foot_spline_.pitch()->addPoint(phase_timings_.raise_foot, start_p); flying_foot_spline_.pitch()->addPoint(phase_timings_.windup, cos(target_yaw) * params_.foot_pitch); - flying_foot_spline_.pitch()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) / 2, - cos(target_yaw) * params_.foot_pitch); + flying_foot_spline_.pitch()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, + cos(target_yaw) * params_.foot_pitch); + flying_foot_spline_.pitch()->addPoint(phase_timings_.kick - params_.earlier_time, 0); flying_foot_spline_.pitch()->addPoint(phase_timings_.kick, 0); flying_foot_spline_.pitch()->addPoint(phase_timings_.move_trunk_back, start_p); @@ -201,7 +183,6 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.yaw()->addPoint(phase_timings_.move_back, start_y); flying_foot_spline_.yaw()->addPoint(phase_timings_.move_trunk_back, start_y); - //ROS_WARN_STREAM(flying_foot_spline_.getDebugString()); /* Stabilizing point */ trunk_spline_.x()->addPoint(0, 0); @@ -264,14 +245,11 @@ std::pair KickEngine::transformGoal( Eigen::Isometry3d base_footprint_to_support_foot = trunk_to_base_footprint.inverse() * trunk_to_support_foot; /* now, apply the transforms. Because of eigen, the transform has to be on the left hand side, therefore it must be inversed */ Eigen::Vector3d ball_transformed = base_footprint_to_support_foot.inverse() * ball_position; - Eigen::Matrix3d - kick_direction_transformed_matrix = (base_footprint_to_support_foot.inverse() * kick_direction).rotation(); + Eigen::Matrix3d kick_direction_transformed_matrix = (base_footprint_to_support_foot.inverse() * kick_direction).rotation(); Eigen::Quaterniond kick_direction_transformed(kick_direction_transformed_matrix); return std::make_pair(ball_transformed, kick_direction_transformed); } - - Eigen::Vector3d KickEngine::calcKickWindupPoint() { /* retrieve yaw from kick_direction_ */ double yaw = rot_conv::EYawOfQuat(kick_direction_); diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index 199f7d36..4930711f 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -14,8 +14,6 @@ KickNode::KickNode(const std::string &ns) : private_node_handle_.param("l_sole_frame", l_sole_frame_, "l_sole"); private_node_handle_.param("r_toe_frame", r_toe_frame_, "r_toe"); private_node_handle_.param("l_toe_frame", l_toe_frame_, "l_toe"); - private_node_handle_.param("r_hip_frame", r_hip_frame_, "r_upper_leg"); - private_node_handle_.param("l_hip_frame", l_hip_frame_, "l_upper_leg"); private_node_handle_.param("ball_radius", ball_radius_, 0.07); unstable_config_ = getUnstableConfig(); @@ -35,10 +33,7 @@ KickNode::KickNode(const std::string &ns) : current_state_.reset(new robot_state::RobotState(kinematic_model)); engine_.setRobotState(current_state_); - Eigen::Isometry3d trunk_to_hip_l = current_state_->getGlobalLinkTransform(l_hip_frame_); - Eigen::Isometry3d trunk_to_hip_r = current_state_->getGlobalLinkTransform(r_hip_frame_); ik_.init(kinematic_model); - engine_.setTrunkToHip(trunk_to_hip_l, trunk_to_hip_r); joint_goal_publisher_ = node_handle_.advertise("kick_motor_goals", 1); support_foot_publisher_ = @@ -135,10 +130,6 @@ void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &conf params.low_x = config.low_x; params.low_x_speed = config.low_x_speed; - params.rise_length = config.rise_length; - params.windup_length = config.windup_length; - params.windup_alpha = config.windup_alpha; - engine_.setParams(params); stabilizer_.useCop(config.use_center_of_pressure); @@ -191,7 +182,7 @@ bool KickNode::init(const bitbots_msgs::KickGoal &goal_msg, visualizer_.displayBallPoint(engine_.getBallPoint(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); visualizer_.displayWindupPoint(engine_.getWindupPoint(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); visualizer_.displayKickPoint(engine_.getKickPoint(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); - visualizer_.displayFlyingSplines(engine_.getFlyingSplines(), (engine_.isLeftKick()) ? r_hip_frame_ : l_hip_frame_); + visualizer_.displayFlyingSplines(engine_.getFlyingSplines(), (engine_.isLeftKick()) ? r_toe_frame_ : l_toe_frame_); visualizer_.displayTrunkSplines(engine_.getTrunkSplines(), (engine_.isLeftKick() ? r_toe_frame_ : l_toe_frame_)); return true; diff --git a/bitbots_dynamic_kick/src/visualizer.cpp b/bitbots_dynamic_kick/src/visualizer.cpp index a53083ed..006c9703 100644 --- a/bitbots_dynamic_kick/src/visualizer.cpp +++ b/bitbots_dynamic_kick/src/visualizer.cpp @@ -160,8 +160,6 @@ void Visualizer::publishGoals(const KickPositions &positions, msg.flying_foot_pose_ik_result = tf2::toMsg(flying_foot_pose_ik_result); msg.flying_foot_position_ik_offset = tf2::toMsg(flying_foot_position_ik_offset); - msg.flying_foot_pose_goal_leg_space = tf2::toMsg(positions.flying_foot_leg_space); - debug_publisher_.publish(msg); } From c0a6891dea4d45aa0b6300e84da644d1183dc841 Mon Sep 17 00:00:00 2001 From: Marc Bestmann Date: Thu, 9 Dec 2021 17:07:36 +0100 Subject: [PATCH 12/13] add walkready to script --- .../scripts/interactive_test.py | 65 ++++++++++++++++++- 1 file changed, 63 insertions(+), 2 deletions(-) diff --git a/bitbots_dynamic_kick/scripts/interactive_test.py b/bitbots_dynamic_kick/scripts/interactive_test.py index a559ff5d..3e3c4d56 100644 --- a/bitbots_dynamic_kick/scripts/interactive_test.py +++ b/bitbots_dynamic_kick/scripts/interactive_test.py @@ -12,7 +12,7 @@ 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 @@ -20,6 +20,61 @@ 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 @@ -120,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() @@ -208,6 +265,10 @@ 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 From ddb62608a88814f56cd68050c185b0cc8ce3e524 Mon Sep 17 00:00:00 2001 From: Marc Bestmann Date: Thu, 9 Dec 2021 17:43:12 +0100 Subject: [PATCH 13/13] removed new parameters again --- .../cfg/bitbots_dynamic_kick_params.cfg | 24 -------- bitbots_dynamic_kick/config/kick_config.yaml | 10 +--- .../config/kick_config_unstable.yaml | 6 -- .../bitbots_dynamic_kick/kick_engine.h | 14 ----- bitbots_dynamic_kick/src/kick_engine.cpp | 56 ++++++------------- bitbots_dynamic_kick/src/kick_node.cpp | 8 --- bitbots_dynamic_kick/src/kick_pywrapper.cpp | 5 -- 7 files changed, 17 insertions(+), 106 deletions(-) diff --git a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg index d91b1a5c..8a5026d8 100755 --- a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg +++ b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg @@ -36,27 +36,6 @@ 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_timings = group_engine.add_group("Timings") @@ -69,9 +48,6 @@ 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) diff --git a/bitbots_dynamic_kick/config/kick_config.yaml b/bitbots_dynamic_kick/config/kick_config.yaml index 6a191e2f..a14e00ce 100644 --- a/bitbots_dynamic_kick/config/kick_config.yaml +++ b/bitbots_dynamic_kick/config/kick_config.yaml @@ -4,24 +4,16 @@ engine_rate: 500 foot_rise: 0.08 foot_distance: 0.2 -kick_windup_distance: 0.35 +kick_windup_distance: 0.29 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 - # 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 diff --git a/bitbots_dynamic_kick/config/kick_config_unstable.yaml b/bitbots_dynamic_kick/config/kick_config_unstable.yaml index 868dd4e0..76b3f5fc 100644 --- a/bitbots_dynamic_kick/config/kick_config_unstable.yaml +++ b/bitbots_dynamic_kick/config/kick_config_unstable.yaml @@ -2,7 +2,6 @@ unstable: foot_distance: 0.21 foot_rise: 0.11 kick_speed: 6.7 - low_time: 0.2 kick_time: 0.11 kick_windup_distance: 0.33 lower_foot_time: 0.16 @@ -19,11 +18,6 @@ unstable: trunk_yaw: 0.296705972839036 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 use_center_of_pressure: false foot_x_extra: 0.00 diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h index 84300e26..16701dc6 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h @@ -45,19 +45,6 @@ struct KickParams { 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; - }; @@ -69,7 +56,6 @@ class PhaseTimings { double move_trunk; double raise_foot; double windup; - double low; double kick; double move_back; double lower_foot; diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index 41becf31..043d8a11 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_dynamic_kick/src/kick_engine.cpp @@ -88,8 +88,7 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei phase_timings_.move_trunk = 0 + params_.move_trunk_time; phase_timings_.raise_foot = phase_timings_.move_trunk + params_.raise_foot_time; phase_timings_.windup = phase_timings_.raise_foot + params_.move_to_ball_time; - phase_timings_.low = phase_timings_.windup + params_.low_time; - phase_timings_.kick = phase_timings_.low + params_.kick_time; + phase_timings_.kick = phase_timings_.windup + params_.kick_time; phase_timings_.move_back = phase_timings_.kick + params_.move_back_time; phase_timings_.lower_foot = phase_timings_.move_back + params_.lower_foot_time; phase_timings_.move_trunk_back = phase_timings_.lower_foot + params_.move_trunk_back_time; @@ -108,19 +107,14 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei double speed_yaw = rot_conv::EYawOfQuat(kick_direction_); Eigen::Vector3d speed_vector(cos(speed_yaw), sin(speed_yaw), 0); double target_yaw = calcKickFootYaw(); - ROS_WARN_STREAM(target_yaw); /* Flying foot position */ flying_foot_spline_.x()->addPoint(0, flying_foot_pose.translation().x()); flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk, 0); flying_foot_spline_.x()->addPoint(phase_timings_.raise_foot, 0); - flying_foot_spline_.x()->addPoint(phase_timings_.windup, windup_point_.x(), 0); - flying_foot_spline_.x()->addPoint(phase_timings_.low, - params_.low_x * cos(target_yaw), - params_.low_x_speed * cos(target_yaw)); - flying_foot_spline_.x()->addPoint(phase_timings_.kick, - kick_point_.x() + params_.foot_extra_forward * cos(target_yaw), - speed_vector.x() * kick_speed_); + flying_foot_spline_.x()->addPoint(phase_timings_.windup, windup_point_.x(), 0, 0); + flying_foot_spline_.x()->addPoint(phase_timings_.kick, ball_position_.x(), + speed_vector.x() * kick_speed_, 0); flying_foot_spline_.x()->addPoint(phase_timings_.move_back, 0); flying_foot_spline_.x()->addPoint(phase_timings_.lower_foot, 0); flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk_back, 0); @@ -128,27 +122,20 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.y()->addPoint(0, flying_foot_pose.translation().y()); 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); - //flying_foot_spline_.y()->addPoint(phase_timings_.low, - // kick_foot_sign * params_.foot_distance + params_.low_x * sin(target_yaw), - // params_.low_x_speed * sin(target_yaw)); - flying_foot_spline_.y()->addPoint(phase_timings_.kick, - kick_point_.y() + params_.foot_extra_forward * sin(target_yaw), - speed_vector.y() * kick_speed_); + 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, 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); flying_foot_spline_.z()->addPoint(phase_timings_.raise_foot, params_.foot_rise); flying_foot_spline_.z()->addPoint(phase_timings_.windup, params_.foot_rise); - flying_foot_spline_.z()->addPoint(phase_timings_.low, params_.foot_rise_lower); - flying_foot_spline_.z()->addPoint(phase_timings_.kick - params_.earlier_time, params_.foot_rise_kick); - flying_foot_spline_.z()->addPoint(phase_timings_.kick, params_.foot_rise_kick); - flying_foot_spline_.z()->addPoint(phase_timings_.move_back, params_.foot_rise_kick); - flying_foot_spline_.z()->addPoint(phase_timings_.lower_foot, 0.4 * params_.foot_rise_kick); + flying_foot_spline_.z()->addPoint(phase_timings_.kick, params_.foot_rise); + flying_foot_spline_.z()->addPoint(phase_timings_.move_back, params_.foot_rise); + flying_foot_spline_.z()->addPoint(phase_timings_.lower_foot, 0.4 * params_.foot_rise); flying_foot_spline_.z()->addPoint(phase_timings_.move_trunk_back, 0); /* Flying foot orientation */ @@ -156,30 +143,19 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei double start_r, start_p, start_y; Eigen::Quaterniond flying_foot_rotation(flying_foot_pose.rotation()); rot_conv::EulerFromQuat(flying_foot_rotation, start_y, start_p, start_r); + double target_y = calcKickFootYaw(); /* Add these quaternions in the same fashion as before to our splines (current, target, current) */ flying_foot_spline_.roll()->addPoint(0, start_r); - flying_foot_spline_.roll()->addPoint(phase_timings_.raise_foot, start_r); - flying_foot_spline_.roll()->addPoint(phase_timings_.windup, -sin(target_yaw) * params_.foot_pitch); - flying_foot_spline_.roll()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, - - sin(target_yaw) * params_.foot_pitch); - flying_foot_spline_.roll()->addPoint(phase_timings_.kick - params_.earlier_time, 0); - flying_foot_spline_.roll()->addPoint(phase_timings_.kick, 0); + flying_foot_spline_.roll()->addPoint(phase_timings_.windup, start_r); flying_foot_spline_.roll()->addPoint(phase_timings_.move_trunk_back, start_r); - flying_foot_spline_.pitch()->addPoint(0, start_p); - flying_foot_spline_.pitch()->addPoint(phase_timings_.raise_foot, start_p); - flying_foot_spline_.pitch()->addPoint(phase_timings_.windup, cos(target_yaw) * params_.foot_pitch); - flying_foot_spline_.pitch()->addPoint(phase_timings_.windup + (phase_timings_.kick - phase_timings_.windup) /2, - cos(target_yaw) * params_.foot_pitch); - flying_foot_spline_.pitch()->addPoint(phase_timings_.kick - params_.earlier_time, 0); - flying_foot_spline_.pitch()->addPoint(phase_timings_.kick, 0); + flying_foot_spline_.pitch()->addPoint(phase_timings_.windup, start_p); flying_foot_spline_.pitch()->addPoint(phase_timings_.move_trunk_back, start_p); - flying_foot_spline_.yaw()->addPoint(0, start_y); flying_foot_spline_.yaw()->addPoint(phase_timings_.raise_foot, start_y); - flying_foot_spline_.yaw()->addPoint(phase_timings_.windup, target_yaw); - flying_foot_spline_.yaw()->addPoint(phase_timings_.kick, target_yaw); + flying_foot_spline_.yaw()->addPoint(phase_timings_.windup, target_y); + flying_foot_spline_.yaw()->addPoint(phase_timings_.kick, target_y); flying_foot_spline_.yaw()->addPoint(phase_timings_.move_back, start_y); flying_foot_spline_.yaw()->addPoint(phase_timings_.move_trunk_back, start_y); @@ -277,7 +253,7 @@ Eigen::Vector3d KickEngine::calcKickPoint() { vec.normalize(); vec = ball_position_ + vec * ball_radius_; - vec.z() = params_.foot_rise_kick; + vec.z() = params_.foot_rise; return vec; } diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index 4930711f..ba3100bc 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -114,7 +114,6 @@ void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &conf params.move_trunk_time = config.move_trunk_time; params.raise_foot_time = config.raise_foot_time; params.move_to_ball_time = config.move_to_ball_time; - params.low_time = config.low_time; params.kick_time = config.kick_time; params.move_back_time = config.move_back_time; params.lower_foot_time = config.lower_foot_time; @@ -122,13 +121,6 @@ void KickNode::reconfigureCallback(bitbots_dynamic_kick::DynamicKickConfig &conf params.stabilizing_point_x = config.stabilizing_point_x; params.stabilizing_point_y = config.stabilizing_point_y; params.choose_foot_corridor_width = config.choose_foot_corridor_width; - params.foot_extra_forward = config.foot_extra_forward; - params.foot_pitch = config.foot_pitch; - params.foot_rise_lower = config.foot_rise_lower; - params.foot_rise_kick = config.foot_rise_kick; - params.earlier_time = config.earlier_time; - params.low_x = config.low_x; - params.low_x_speed = config.low_x_speed; engine_.setParams(params); diff --git a/bitbots_dynamic_kick/src/kick_pywrapper.cpp b/bitbots_dynamic_kick/src/kick_pywrapper.cpp index 68794310..ce71716d 100644 --- a/bitbots_dynamic_kick/src/kick_pywrapper.cpp +++ b/bitbots_dynamic_kick/src/kick_pywrapper.cpp @@ -82,11 +82,6 @@ void PyKickWrapper::set_params(const boost::python::object params) { else if (keystr == "stabilizing_point_x") { conf.stabilizing_point_x = std::stof(valstr); } else if (keystr == "stabilizing_point_y") { conf.stabilizing_point_y = std::stof(valstr); } else if (keystr == "spline_smoothness") { conf.spline_smoothness = std::stoi(valstr); } - else if (keystr == "earlier_time") { conf.earlier_time = std::stof(valstr); } - else if (keystr == "foot_rise_kick") { conf.foot_rise_kick = std::stof(valstr); } - else if (keystr == "foot_rise_lower") { conf.foot_rise_lower = std::stof(valstr); } - else if (keystr == "foot_pitch") { conf.foot_pitch = std::stof(valstr); } - else if (keystr == "foot_extra_forward") { conf.foot_extra_forward = std::stof(valstr); } else { std::cerr << keystr << " not known. WILL BE IGNORED." << std::endl; } }