diff --git a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg index eaab2e9b..8a5026d8 100755 --- a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg +++ b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg @@ -37,6 +37,7 @@ group_engine_distances.add("trunk_yaw", double_t, 4, "Yaw of the trunk, positive means turning toward the kicking foot [rad]", min=-1, max=1) + group_engine_timings = group_engine.add_group("Timings") group_engine_timings.add("move_trunk_time", double_t, 3, "How long it should take to move the trunk above the support foot before raising the flying foot [s]", diff --git a/bitbots_dynamic_kick/config/kick_config_unstable.yaml b/bitbots_dynamic_kick/config/kick_config_unstable.yaml index 90f5000e..76b3f5fc 100644 --- a/bitbots_dynamic_kick/config/kick_config_unstable.yaml +++ b/bitbots_dynamic_kick/config/kick_config_unstable.yaml @@ -16,4 +16,8 @@ unstable: trunk_pitch: -0.488692190558412 trunk_roll: 0.102974425867665 trunk_yaw: 0.296705972839036 + + foot_extra_forward: 0.00 + use_center_of_pressure: false + foot_x_extra: 0.00 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..16701dc6 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 @@ -25,6 +26,7 @@ struct KickParams { double foot_distance; double kick_windup_distance; double trunk_height; + double foot_extra_forward; double trunk_roll; double trunk_pitch; @@ -42,6 +44,8 @@ struct KickParams { double stabilizing_point_y; double choose_foot_corridor_width; + + }; /** @@ -78,8 +82,8 @@ class KickEngine : public bitbots_splines::AbstractEngine normal_config_; + double ball_radius_; - std::string base_link_frame_, base_footprint_frame_, l_sole_frame_, r_sole_frame_; + std::string base_link_frame_, base_footprint_frame_, l_sole_frame_, r_sole_frame_, l_toe_frame_, r_toe_frame_; /** * Do main loop in which KickEngine::update() gets called repeatedly. 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..8739a900 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h @@ -4,6 +4,8 @@ #include #include +#define M_TAU M_PI * 2 + namespace bitbots_dynamic_kick { struct KickPositions { @@ -19,6 +21,7 @@ struct KickGoals { Eigen::Quaterniond kick_direction; double kick_speed = 0; Eigen::Isometry3d trunk_to_base_footprint; + double ball_radius; }; enum KickPhase { diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h index b990ead9..b544c35d 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h @@ -47,6 +47,11 @@ class Visualizer : bitbots_splines::AbstractVisualizer { void displayWindupPoint(const Eigen::Vector3d &kick_windup_point, const std::string &support_foot_frame); + void displayKickPoint(const Eigen::Vector3d &kick_windup_point, const std::string &support_foot_frame); + + void displayBallPoint(const Eigen::Vector3d &ball_point, const std::string &support_foot_frame); + + void publishGoals(const KickPositions &positions, const KickPositions &stabilized_positions, const robot_state::RobotStatePtr &robot_state, @@ -58,6 +63,8 @@ class Visualizer : bitbots_splines::AbstractVisualizer { ros::Publisher foot_spline_publisher_; ros::Publisher trunk_spline_publisher_; ros::Publisher windup_publisher_; + ros::Publisher kick_point_publisher_; + ros::Publisher ball_point_publisher_; ros::Publisher debug_publisher_; std::string base_topic_; const std::string marker_ns_ = "bitbots_dynamic_kick"; 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/scripts/dummy_client.py b/bitbots_dynamic_kick/scripts/dummy_client.py index 271dca71..578a347a 100755 --- a/bitbots_dynamic_kick/scripts/dummy_client.py +++ b/bitbots_dynamic_kick/scripts/dummy_client.py @@ -83,14 +83,14 @@ def feedback_cb(feedback): goal.header.stamp = rospy.Time.now() frame_prefix = "" if os.environ.get("ROS_NAMESPACE") is None else os.environ.get("ROS_NAMESPACE") + "/" goal.header.frame_id = frame_prefix + "base_footprint" - goal.ball_position.x = 0.2 + goal.ball_position.x = 0.1 goal.ball_position.y = args.ball_y goal.ball_position.z = 0 goal.unstable = args.unstable goal.kick_direction = Quaternion(*quaternion_from_euler(0, 0, math.radians(args.kick_direction))) - goal.kick_speed = 6.7 if args.unstable else 1 + goal.kick_speed = 6.7 if args.unstable else 1.0 """marker = Marker() marker.header.stamp = goal.ball_position.header.stamp diff --git a/bitbots_dynamic_kick/scripts/interactive_test.py b/bitbots_dynamic_kick/scripts/interactive_test.py index d36a91d0..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 @@ -61,14 +116,14 @@ } moveBallBindings = { - 't': (1, 0, 0), - 'g': (-1, 0, 0), - 'f': (0, 1, 0), - 'h': (0, -1, 0), - 'T': (10, 0, 0), - 'G': (-10, 0, 0), - 'F': (0, 10, 0), - 'H': (0, -10, 0), + 't': (1, 0), + 'g': (-1, 0), + 'f': (0, 1), + 'h': (0, -1), + 'T': (10, 0), + 'G': (-10, 0), + 'F': (0, 10), + 'H': (0, -10), } settings = termios.tcgetattr(sys.stdin) @@ -86,45 +141,17 @@ def getKey(): rospy.init_node('dynamic_kick_interactive_test', anonymous=True) print("Waiting for kick server and simulation") + def done_cb(state, result): return - print('Action completed: ', end='') - if state == GoalStatus.PENDING: - print('Pending') - elif state == GoalStatus.ACTIVE: - print('Active') - elif state == GoalStatus.PREEMPTED: - print('Preempted') - elif state == GoalStatus.SUCCEEDED: - print('Succeeded') - elif state == GoalStatus.ABORTED: - print('Aborted') - elif state == GoalStatus.REJECTED: - print('Rejected') - elif state == GoalStatus.PREEMPTING: - print('Preempting') - elif state == GoalStatus.RECALLING: - print('Recalling') - elif state == GoalStatus.RECALLED: - print('Recalled') - elif state == GoalStatus.LOST: - print('Lost') - else: - print('Unknown state', state) - print(str(result)) def active_cb(): return - print("Server accepted action") def feedback_cb(feedback): return - if len(sys.argv) > 1 and sys.argv[1] == '--feedback': - print('Feedback') - print(feedback) - print() sys.stdout.flush() @@ -148,6 +175,8 @@ def feedback_cb(feedback): frame_prefix = "" if os.environ.get("ROS_NAMESPACE") is None else os.environ.get("ROS_NAMESPACE") + "/" + joint_pub = rospy.Publisher("DynamixelController/command", JointCommand, queue_size=1) + def generate_kick_goal(x, y, direction, speed, unstable=False): kick_goal = KickGoal() @@ -194,6 +223,7 @@ def set_ball_position(): request.position.z = 0.04 response = set_ball_pos_service(request) + sys.stdout.write("\x1b[A") sys.stdout.write("\x1b[A") print(msg) @@ -235,17 +265,14 @@ def set_ball_position(): elif key == "r": set_robot_pose() set_ball_position() + elif key =="R": + # play walkready animation + walkready.header.stamp = rospy.Time.now() + joint_pub.publish(walkready) elif (key == '\x03'): break - - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") + sys.stdout.write("\x1b[A" * 7) print( f"robot_x: {round(robot_x, 2)} ball_x: {round(ball_x, 2)} \n" f"robot_y: {round(robot_y, 2)} ball_x: {round(ball_y, 2)} \n" diff --git a/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_dynamic_kick/src/kick_engine.cpp index 0c55d004..043d8a11 100644 --- a/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_dynamic_kick/src/kick_engine.cpp @@ -18,8 +18,9 @@ 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_radius_ = goals.ball_radius; ball_position_ = transformed_goal.first; kick_direction_ = transformed_goal.second; kick_direction_.normalize(); @@ -27,21 +28,22 @@ 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 */ calcSplines(trunk_to_support_foot.inverse() * trunk_to_flying_foot, trunk_frame); } + KickPositions KickEngine::update(double dt) { /* Only do an actual update when splines are present */ KickPositions positions; @@ -98,11 +100,13 @@ 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(); /* Flying foot position */ flying_foot_spline_.x()->addPoint(0, flying_foot_pose.translation().x()); @@ -155,6 +159,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); + /* Stabilizing point */ trunk_spline_.x()->addPoint(0, 0); trunk_spline_.x()->addPoint(phase_timings_.move_trunk, params_.stabilizing_point_x); @@ -232,9 +237,22 @@ 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; return vec; @@ -330,6 +348,14 @@ Eigen::Vector3d KickEngine::getWindupPoint() { return windup_point_; } +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 3cc0cd96..ba3100bc 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -12,6 +12,9 @@ KickNode::KickNode(const std::string &ns) : 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("ball_radius", ball_radius_, 0.07); unstable_config_ = getUnstableConfig(); @@ -23,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)); @@ -31,6 +33,8 @@ KickNode::KickNode(const std::string &ns) : current_state_.reset(new robot_state::RobotState(kinematic_model)); engine_.setRobotState(current_state_); + ik_.init(kinematic_model); + joint_goal_publisher_ = node_handle_.advertise("kick_motor_goals", 1); support_foot_publisher_ = node_handle_.advertise("dynamic_kick_support_state", 1, /* latch = */ true); @@ -42,14 +46,14 @@ 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."); + 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_sole_frame_) { - ROS_ERROR_STREAM("cop_r not in " << r_sole_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; } @@ -117,6 +121,7 @@ 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; + engine_.setParams(params); stabilizer_.useCop(config.use_center_of_pressure); @@ -149,11 +154,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; @@ -161,20 +166,23 @@ 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_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_.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_.displayTrunkSplines(engine_.getTrunkSplines(), (engine_.isLeftKick() ? r_toe_frame_ : l_toe_frame_)); return true; } 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 = diff --git a/bitbots_dynamic_kick/src/visualizer.cpp b/bitbots_dynamic_kick/src/visualizer.cpp index 3948c738..006c9703 100644 --- a/bitbots_dynamic_kick/src/visualizer.cpp +++ b/bitbots_dynamic_kick/src/visualizer.cpp @@ -20,6 +20,10 @@ 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); + 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); } @@ -33,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); @@ -83,6 +87,36 @@ 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.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, @@ -94,11 +128,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 */ 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);