diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py index 728a56811..713b8d106 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py @@ -1,3 +1,5 @@ +import math + from bitbots_blackboard.body_blackboard import BodyBlackboard from bitbots_blackboard.capsules.pathfinding_capsule import BallGoalType from dynamic_stack_decider.abstract_action_element import AbstractActionElement @@ -22,7 +24,9 @@ def __init__(self, blackboard, dsd, parameters): self.blocking = parameters.get("blocking", True) self.distance = parameters.get("distance", self.blackboard.config["ball_approach_dist"]) # Offset so we kick the ball with one foot instead of the center between the feet - self.side_offset = parameters.get("side_offset", 0.08) + self.side_offset = parameters.get("side_offset", 0.00) + ball_position_y_relative = self.blackboard.world_model.get_ball_position_uv()[1] + self.side_offset = math.copysign(self.side_offset, ball_position_y_relative) def perform(self, reevaluate=False): pose_msg = self.blackboard.pathfinding.get_ball_goal(self.target, self.distance, self.side_offset) 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 e5a6d1036..8a8cca2bd 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 @@ -44,7 +44,7 @@ $AvoidBall LEFT --> #PerformKickLeft RIGHT --> #PerformKickRight 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 + NO --> @ChangeAction + action:going_to_ball + r:false, @LookAtFieldFeatures + r:false, @AvoidBallActive + r:false, @GoToBall + target:map + blocking:false + distance:%ball_far_approach_dist + side_offset:0.08 YES --> $ReachedPathPlanningGoalPosition + threshold:%ball_far_approach_position_thresh YES --> @AvoidBallInactive NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:map + distance:%ball_far_approach_dist