From a15719825cd617d1ab208024a82f301c6cc11787 Mon Sep 17 00:00:00 2001 From: JanNiklasFeld Date: Tue, 9 Jan 2024 17:22:53 +0100 Subject: [PATCH 1/2] New goalie code for testing without quaternions --- .../actions/go_to_block_position.py | 123 +++++++++++++++--- .../config/body_behavior.yaml | 4 +- 2 files changed, 109 insertions(+), 18 deletions(-) diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py index f2c623c9b..d523a468e 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py @@ -1,3 +1,6 @@ +import math + +import numpy as np from dynamic_stack_decider.abstract_action_element import AbstractActionElement from tf2_geometry_msgs import PoseStamped @@ -10,7 +13,15 @@ class GoToBlockPosition(AbstractActionElement): def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) self.block_position_goal_offset = self.blackboard.config["block_position_goal_offset"] - self.block_position_gradient_factor = self.blackboard.config["block_position_gradient_factor"] + self.block_radius = self.blackboard.config["block_radius_robot"] + self.left_goalpost_position = [ + -self.blackboard.world_model.field_length / 2, + self.blackboard.world_model.goal_width / 2, + ] # position of left goalpost + self.right_goalpost_position = [ + -self.blackboard.world_model.field_length / 2, + -self.blackboard.world_model.goal_width / 2, + ] # position of right goalpost def perform(self, reevaluate=False): # The block position should be a position between the ball and the center of the goal @@ -28,28 +39,108 @@ def perform(self, reevaluate=False): # | # +------------------+--------------> x # 0 - - goal_position = (-self.blackboard.world_model.field_length / 2, 0) # position of the own goal ball_position = self.blackboard.world_model.get_ball_position_xy() - x_delta = ball_position[0] - goal_position[0] - y_delta = ball_position[1] # goal Y-position is always 0 - gradient = float(y_delta) / float(x_delta) * self.block_position_gradient_factor - goalie_y = self.block_position_goal_offset * gradient + ball_to_line_distance = ball_position[0] - self.left_goalpost_position[0] + + opening_angle = self._calc_opening_angle( + ball_to_line_distance, ball_position + ) # angle of ball to both goalposts + angle_bisector = opening_angle / 2 # of the angle between the goalpost + + goalie_pos = self._get_robot_pose( + angle_bisector, ball_to_line_distance, self.left_goalpost_position, self.right_goalpost_position + ) + + goalie_pos = self._cut_goalie_pos(goalie_pos, ball_position) pose_msg = PoseStamped() pose_msg.header.stamp = self.blackboard.node.get_clock().now().to_msg() pose_msg.header.frame_id = self.blackboard.map_frame - pose_msg.pose.position.x = float( - -(self.blackboard.world_model.field_length / 2) + self.block_position_goal_offset - ) - pose_msg.pose.position.y = float(self._stay_in_front_of_goal(goalie_y)) - pose_msg.pose.orientation.w = 1.0 + pose_msg.pose.position.x = float(goalie_pos[0]) + pose_msg.pose.position.y = float(goalie_pos[1]) + pose_msg.pose.orientation.w = 1.0 # TODO: Change orientation self.blackboard.pathfinding.publish(pose_msg) - def _stay_in_front_of_goal(self, y): - # keeps the y-values of the position in between of the goalposts. - # this ensures, that y is in [-self.blackboard.world_model.goal_width / 2, self.blackboard.world_model.goal_width / 2]. - return max(-self.blackboard.world_model.goal_width / 2, min(self.blackboard.world_model.goal_width / 2, y)) + def _calc_opening_angle(self, ball_to_line, ball_pos): + """ + Calculates the opening angle of the ball to both goalposts. + With it we can get the angle bisector, in which we place the robot. + Args: + ball_to_line (float): distance of the ball to our goal line + + Returns: + float: opening angle + """ + ball_angle_left = np.arctan2( + ball_pos[1] - self.left_goalpost_position[1], ball_to_line + ) # angle of ball to left goalpost + ball_angle_right = np.arctan2( + ball_pos[1] - self.right_goalpost_position[1], ball_to_line + ) # angle of ball to right goalpost + opening_angle_ball = ball_angle_right - ball_angle_left # Subtract to get the opening angle + return opening_angle_ball + + def _get_robot_pose(self, angle_bisector, ball_to_line_distance, ball_pos): + """ + Calculates the position where the robot should be to block the ball. + The position is the place where the circle of the robot blocking radius is touching both lines + from the ball to the goalposts. So the goal is completely guarded. + + Args: + angle_bisector (float): bisector angle of the ball with goalposts + ball_to_line_distance (float): distance of the ball to the goal line + + Returns: + list: goalie_pos + """ + left_goalpost_to_ball = math.dist(self.left_goalpost_position, ball_pos) + right_goalpost_to_ball = math.dist(self.right_goalpost_position, ball_pos) + + wanted_distance_robot_to_ball = self.block_radius / np.sin(angle_bisector) # ball obstruction distance of robot + angle_of_left_goalpost_to_ball = np.arccos(ball_to_line_distance / left_goalpost_to_ball) + angle_of_right_goalpost_to_ball = np.arccos(ball_to_line_distance / right_goalpost_to_ball) + + if ball_pos[1] > self.blackboard.world_model.goal_width / 2: + angle_to_bisector = angle_of_left_goalpost_to_ball + angle_bisector # here left goalpost angle used + goalie_x = ball_pos[0] - wanted_distance_robot_to_ball * np.cos(angle_to_bisector) + goalie_y = ball_pos[1] - wanted_distance_robot_to_ball * np.sin(angle_to_bisector) + elif ball_pos[1] < -self.blackboard.world_model.goal_width / 2: + angle_to_bisector = angle_of_right_goalpost_to_ball + angle_bisector # here right goalpost angle used + goalie_x = ball_pos[0] - wanted_distance_robot_to_ball * np.cos(angle_to_bisector) + goalie_y = ball_pos[1] + wanted_distance_robot_to_ball * np.sin(angle_to_bisector) + else: + angle_to_bisector = angle_of_left_goalpost_to_ball - angle_bisector # here right goalpost angle used + goalie_x = ball_pos[0] - wanted_distance_robot_to_ball * np.cos(angle_to_bisector) + goalie_y = ball_pos[1] + wanted_distance_robot_to_ball * np.sin(angle_to_bisector) + return [goalie_x, goalie_y] + + def _cut_goalie_pos(self, goalie_pos, ball_pos): + """ + Cut the goalie position if he is behind the goal or wants to leave the designated + goal area. + + Args: + goalie_pos (list): a list which contains [goalie_y, goalie_x] position + + Returns: + list: goalie_pos + """ + penalty_area_length = 2.0 # TODO: Look for world_model parameter + # cut if goalie is behind the goal happens when the ball is too close to the corner + if goalie_pos[0] < -self.blackboard.world_model.field_length / 2: + goalie_pos[0] = -self.blackboard.world_model.field_length / 2 + self.block_position_goal_offset + # instead put goalie to goalpost position + if ball_pos[1] > 0: + goalie_pos[1] = self.blackboard.world_model.goal_width / 2 + else: + goalie_pos[1] = -self.blackboard.world_model.goal_width / 2 + # cut so goalie does not leave goalkeeper area + goalie_pos[0] = np.clip( + goalie_pos[0], + -self.blackboard.world_model.field_length / 2, + -self.blackboard.world_model.field_length / 2 + penalty_area_length, + ) + return goalie_pos diff --git a/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_body_behavior/config/body_behavior.yaml index 874497607..5cc76c2b3 100644 --- a/bitbots_body_behavior/config/body_behavior.yaml +++ b/bitbots_body_behavior/config/body_behavior.yaml @@ -112,8 +112,8 @@ # The value describes the offset in meters from the goal line. block_position_goal_offset: 0.15 - # this factor defines how extreme the goalie reacts to a ball offset - block_position_gradient_factor: 4.0 + # This is the radius in which the goalie is able to block the ball + block_radius_robot: 0.2 # configurations for the use of bitbots_dynamic_kick package dynamic_kick: From 97ce5d6facda6a63ec533d0317f3845c3847edb8 Mon Sep 17 00:00:00 2001 From: Jan-Niklas Feldhusen Date: Tue, 16 Jan 2024 21:22:47 +0100 Subject: [PATCH 2/2] Updated after testing in sim --- .../actions/go_to_block_position.py | 40 ++++++++++++------- 1 file changed, 25 insertions(+), 15 deletions(-) diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py index d523a468e..1976b066f 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py @@ -1,6 +1,7 @@ import math import numpy as np +from bitbots_utils.transforms import quat_from_yaw from dynamic_stack_decider.abstract_action_element import AbstractActionElement from tf2_geometry_msgs import PoseStamped @@ -49,7 +50,7 @@ def perform(self, reevaluate=False): angle_bisector = opening_angle / 2 # of the angle between the goalpost goalie_pos = self._get_robot_pose( - angle_bisector, ball_to_line_distance, self.left_goalpost_position, self.right_goalpost_position + angle_bisector, ball_to_line_distance, ball_position ) goalie_pos = self._cut_goalie_pos(goalie_pos, ball_position) @@ -60,7 +61,9 @@ def perform(self, reevaluate=False): pose_msg.pose.position.x = float(goalie_pos[0]) pose_msg.pose.position.y = float(goalie_pos[1]) - pose_msg.pose.orientation.w = 1.0 # TODO: Change orientation + + yaw = self._calc_turn_to_ball_angle(ball_position, goalie_pos) + pose_msg.pose.orientation = quat_from_yaw(yaw) self.blackboard.pathfinding.publish(pose_msg) @@ -83,18 +86,17 @@ def _calc_opening_angle(self, ball_to_line, ball_pos): opening_angle_ball = ball_angle_right - ball_angle_left # Subtract to get the opening angle return opening_angle_ball - def _get_robot_pose(self, angle_bisector, ball_to_line_distance, ball_pos): + def _get_robot_pose(self, angle_bisector: float, ball_to_line_distance: float, ball_pos: tuple) -> tuple[float, float]: """ Calculates the position where the robot should be to block the ball. The position is the place where the circle of the robot blocking radius is touching both lines from the ball to the goalposts. So the goal is completely guarded. Args: - angle_bisector (float): bisector angle of the ball with goalposts - ball_to_line_distance (float): distance of the ball to the goal line + angle_bisector: bisector angle of the ball with goalposts + ball_to_line_distance: distance of the ball to the goal line + ball_pos: ball position in world koordinate system - Returns: - list: goalie_pos """ left_goalpost_to_ball = math.dist(self.left_goalpost_position, ball_pos) right_goalpost_to_ball = math.dist(self.right_goalpost_position, ball_pos) @@ -115,7 +117,8 @@ def _get_robot_pose(self, angle_bisector, ball_to_line_distance, ball_pos): angle_to_bisector = angle_of_left_goalpost_to_ball - angle_bisector # here right goalpost angle used goalie_x = ball_pos[0] - wanted_distance_robot_to_ball * np.cos(angle_to_bisector) goalie_y = ball_pos[1] + wanted_distance_robot_to_ball * np.sin(angle_to_bisector) - return [goalie_x, goalie_y] + # calculate the angle, the robot needs to turn to look at the ball + return (goalie_x, goalie_y) def _cut_goalie_pos(self, goalie_pos, ball_pos): """ @@ -128,19 +131,26 @@ def _cut_goalie_pos(self, goalie_pos, ball_pos): Returns: list: goalie_pos """ + goalie_pos_x, goalie_pos_y = goalie_pos penalty_area_length = 2.0 # TODO: Look for world_model parameter # cut if goalie is behind the goal happens when the ball is too close to the corner - if goalie_pos[0] < -self.blackboard.world_model.field_length / 2: - goalie_pos[0] = -self.blackboard.world_model.field_length / 2 + self.block_position_goal_offset + if goalie_pos_x < -self.blackboard.world_model.field_length / 2: + goalie_pos_x = -self.blackboard.world_model.field_length / 2 + self.block_position_goal_offset # instead put goalie to goalpost position if ball_pos[1] > 0: - goalie_pos[1] = self.blackboard.world_model.goal_width / 2 + goalie_pos_y = self.blackboard.world_model.goal_width / 2 else: - goalie_pos[1] = -self.blackboard.world_model.goal_width / 2 + goalie_pos_y = -self.blackboard.world_model.goal_width / 2 # cut so goalie does not leave goalkeeper area - goalie_pos[0] = np.clip( - goalie_pos[0], + goalie_pos_x = np.clip( + goalie_pos_x, -self.blackboard.world_model.field_length / 2, -self.blackboard.world_model.field_length / 2 + penalty_area_length, ) - return goalie_pos + return (goalie_pos_x, goalie_pos_y) + + def _calc_turn_to_ball_angle(self, ball_pos, goalie_pos): + robot_to_ball_x = ball_pos[0] - goalie_pos[0] + robot_to_ball_y = ball_pos[1] - goalie_pos[1] + angle = np.arctan2(robot_to_ball_y, robot_to_ball_x) + return angle