From 9ded9681a50e6839316fcc14caeab571e4025110 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Thu, 23 Jan 2025 04:31:42 +0100 Subject: [PATCH] Kind of working state --- .../capsules/kick_capsule.py | 9 +++--- .../behavior_dsd/actions/kick_ball.py | 19 ++++++------ .../behavior_dsd/main.dsd | 11 ++----- .../config/body_behavior.yaml | 8 ++--- .../bitbots_quintic_walk/walk_engine.hpp | 2 +- .../bitbots_quintic_walk/walk_node.hpp | 2 +- .../bitbots_quintic_walk/src/walk_engine.cpp | 29 ++++++++++--------- .../bitbots_quintic_walk/src/walk_node.cpp | 14 +++++++-- 8 files changed, 50 insertions(+), 44 deletions(-) diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py index 6ed79c92b..6d3689ba6 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py @@ -7,6 +7,7 @@ from rclpy.publisher import Publisher from rclpy.time import Time from std_msgs.msg import Bool +from geometry_msgs.msg import PoseStamped from bitbots_blackboard.capsules import AbstractBlackboardCapsule from bitbots_msgs.action import Kick @@ -40,15 +41,15 @@ def __init__(self, node, blackboard): """ :param blackboard: Global blackboard instance """ - self.walk_kick_pub = self._node.create_publisher(Bool, "/kick", 1) + self.walk_kick_pub = self._node.create_publisher(PoseStamped, "/kick", 1) # self.connect_dynamic_kick() Do not connect if dynamic_kick is disabled - def walk_kick(self, target: WalkKickTargets) -> None: + def walk_kick(self, kick_point: PoseStamped) -> None: """ Kick the ball while walking - :param target: Target for the walk kick (e.g. left or right foot) + :param kick_point: Target point to kick at """ - self.walk_kick_pub.publish(Bool(data=target.value)) + self.walk_kick_pub.publish(kick_point) def connect_dynamic_kick(self) -> None: topic = self._blackboard.config["dynamic_kick"]["topic"] diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/kick_ball.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/kick_ball.py index c8743160a..dab5e69a5 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/kick_ball.py +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/kick_ball.py @@ -2,6 +2,7 @@ from bitbots_blackboard.capsules.kick_capsule import KickCapsule from bitbots_utils.transforms import quat_from_yaw from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from geometry_msgs.msg import PoseStamped from bitbots_msgs.action import Kick @@ -19,17 +20,17 @@ class WalkKick(AbstractKickAction): def __init__(self, blackboard, dsd, parameters): super().__init__(blackboard, dsd, parameters) - if "foot" not in parameters.keys(): - raise ValueError("No foot specified for walk kick") - elif "right" == parameters["foot"]: - self.target = KickCapsule.WalkKickTargets.RIGHT - elif "left" == parameters["foot"]: - self.target = KickCapsule.WalkKickTargets.LEFT - else: - raise ValueError(f"Invalid foot specified for walk kick: {parameters['foot']}") def perform(self, reevaluate=False): - self.blackboard.kick.walk_kick(self.target) + + ball_u, ball_v = self.blackboard.world_model.get_ball_position_uv() + + pose = PoseStamped() + pose.header.frame_id = self.blackboard.world_model.base_footprint_frame + pose.pose.position.x = ball_u - 0.13 / 2 - 0.02 + pose.pose.position.y = ball_v + + self.blackboard.kick.walk_kick(pose) self.pop() diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd index f120e145d..9a996d237 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd @@ -20,11 +20,8 @@ $DoOnce #GetWalkreadyAndLocalize @ChangeAction + action:waiting + r:false, @PlayAnimationInitInSim + r:false, @LookAtFieldFeatures + r:false, @GetWalkready + r:false, @WalkInPlace -#PerformKickLeft -@ChangeAction + action:kicking, @LookAtFront, @WalkKick + foot:left + r:false, @WalkInPlace + duration:1 + r:false, @ForgetBall + r:false, @WalkInPlace + duration:1 + r:false, @LookAtFieldFeatures + r:false - -#PerformKickRight -@ChangeAction + action:kicking, @LookAtFront, @WalkKick + foot:right + r:false, @WalkInPlace + duration:1 + r:false, @ForgetBall + r:false, @WalkInPlace + duration:1 + r:false, @LookAtFieldFeatures + r:false +#PerformKick +@ChangeAction + action:kicking, @LookAtFront, @WalkKick + r:false, @WalkInPlace + duration:1 + r:false, @ForgetBall + r:false, @WalkInPlace + duration:1 + r:false, @LookAtFieldFeatures + r:false #Dribble @ChangeAction + action:going_to_ball, @CancelPathplanning, @LookAtBall, @LookAtFront, @DribbleForward @@ -40,9 +37,7 @@ $DoOnce $AvoidBall NO --> $BallClose + distance:%ball_reapproach_dist + angle:%ball_reapproach_angle YES --> $BallKickArea - NEAR --> $FootSelection - LEFT --> #PerformKickLeft - RIGHT --> #PerformKickRight + NEAR --> #PerformKick FAR --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:map NO --> @ChangeAction + action:going_to_ball + r:false, @LookAtFieldFeatures + r:false, @AvoidBallActive + r:false, @GoToBall + target:map + blocking:false + distance:%ball_far_approach_dist YES --> $ReachedPathPlanningGoalPosition + threshold:%ball_far_approach_position_thresh diff --git a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml index cd65c1175..730f87b79 100644 --- a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml +++ b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml @@ -61,8 +61,8 @@ body_behavior: # An area in which the ball can be kicked # defined by min/max x/y values in meters which represent ball positions relative to base_footprint # http://www.ros.org/reps/rep-0103.html#axis-orientation - kick_x_enter: 0.2 - kick_x_leave: 0.25 + kick_x_enter: 0.32 + kick_x_leave: 0.35 kick_y_enter: 0.12 kick_y_leave: 0.14 @@ -113,14 +113,14 @@ body_behavior: ball_reapproach_dist: 1.0 # Distance at which the ball is normally approached - ball_approach_dist: 0.2 + ball_approach_dist: 0.2 # Angle at which the ball is normally approached again ball_reapproach_angle: 1.2 # The position where the supporter robot should place itself in order to accept a pass pass_position_x: 0.1 - pass_position_y: 1.0 + pass_position_y: 1.0 supporter_max_x: 4.0 # maximal angle of a ball kick diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp index 7806dbc71..bdf3e1b96 100644 --- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp +++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp @@ -66,7 +66,7 @@ class WalkEngine : public bitbots_splines::AbstractEngine #include #include -#include #include #include @@ -22,6 +21,7 @@ The original files can be found at: #include #include #include +#include #include #include #include diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp index c0d7cab4c..9c9198e4f 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp @@ -124,7 +124,7 @@ WalkResponse WalkEngine::update(double dt) { pause_requested_ = false; return createResponse(); } else if (half_step_finished && positioning_step_to_kick_point_ && - ((left_kick_requested_ && !is_left_support_foot_) || (right_kick_requested_ && is_left_support_foot_))) { + ((left_kick_requested_ && is_left_support_foot_) || (right_kick_requested_ && !is_left_support_foot_))) { // lets do a kick buildTrajectories(TrajectoryType::KICK); engine_state_ = WalkState::KICK; @@ -820,15 +820,16 @@ bool WalkEngine::isDoubleSupport() { return is_double_support_spline_.pos(getTrajsTime()) >= 0.5; } -void WalkEngine::requestKick(tf2::Transform kick_point) { +void WalkEngine::requestKick(tf2::Transform kick_point, bool left_kick) { // We assume that the kick point is given in the support foot frame if (is_left_support_foot_) { - kick_point = left_in_world_ * kick_point; + kick_point_ = left_in_world_ * kick_point; } else { - kick_point = right_in_world_ * kick_point; + kick_point_ = right_in_world_ * kick_point; } - left_kick_requested_ = true; // TODO better feet selection + left_kick_requested_ = left_kick; + right_kick_requested_ = !left_kick; } void WalkEngine::requestPause() { pause_requested_ = true; } @@ -881,7 +882,7 @@ void WalkEngine::stepToApproachKick(const tf2::Transform kick_point_and_directio tf2::Transform foot_goal; - if (is_left_support_foot_ == kick_with_left) { + if (is_left_support_foot_ != kick_with_left) { // We can not place the foot on the kick point, because it is the support foot // Therfore we try to place the other foot next to it in an attempt to get as close as possible // and place the kick foot in the step after @@ -908,17 +909,17 @@ void WalkEngine::stepToApproachKick(const tf2::Transform kick_point_and_directio // We need to subtract a small value from the foot distance, otherwise the claming is triggered which, // in turn, delays the kick, as we might not be able to reach the kick point yet if ((is_left_support_foot_ && foot_goal.getOrigin().getY() < config_.foot_distance - 0.01) || - (!is_left_support_foot_ && foot_goal.getOrigin().getY() > -config_.foot_distance + 0.01)) { + (!is_left_support_foot_ && foot_goal.getOrigin().getY() > -config_.foot_distance + 0.01)) { RCLCPP_WARN(node_->get_logger(), "Side step not possible, placing the foot back to zero pose"); - if (is_left_support_foot_) { - foot_goal.getOrigin().setY(config_.foot_distance); - } else { - foot_goal.getOrigin().setY(-config_.foot_distance); - } + if (is_left_support_foot_) { + foot_goal.getOrigin().setY(config_.foot_distance); + } else { + foot_goal.getOrigin().setY(-config_.foot_distance); + } - // If we tried to position the foot with this step this is not possible anymore and we need to do a normal step - positioning_step_to_kick_point_ = false; + // If we tried to position the foot with this step this is not possible anymore and we need to do a normal step + positioning_step_to_kick_point_ = false; } // Now we can perform the step diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp index 952e0c988..51e332e20 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp @@ -528,9 +528,17 @@ void WalkNode::kickCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { std::string support_foot_frame = walk_engine_.isLeftSupport() ? config_.node.tf.l_sole_frame : config_.node.tf.r_sole_frame; try { - tf2::Transform transform; - tf2::fromMsg(tf_buffer_.transform(msg, support_foot_frame)->pose, transform); - walk_engine_.requestKick(transform); + // Select foot used for kick + auto pose_base_footprint = tf_buffer_.transform(*msg, "base_footprint", tf2::durationFromSec(0.1)).pose; + bool use_left_foot = pose_base_footprint.position.y > 0; + + // Transform the goal pose of the contact point to the support foot frame + auto pose = tf_buffer_.transform(*msg, support_foot_frame, tf2::durationFromSec(0.1)).pose; + tf2::Transform transform; + tf2::fromMsg(pose, transform); + + // Request the kick + walk_engine_.requestKick(transform, use_left_foot); } catch (tf2::TransformException& ex) { RCLCPP_ERROR(node_->get_logger(), "Skipping kick, Could not transform kick goal to support foot frame: %s", ex.what());