def main_state(self): self.status_flag = Flags.WIP self.next_state = self.main_state if self.auto_update_target: self._find_best_passing_option() orientation = (self.target.position - self.game_state.ball_position).angle player_to_target = (self.target.position - self.player.pose.position) dist_from_ball = (self.player.position - self.game_state.ball_position).norm ball_speed = self.game_state.ball.velocity.norm ball_speed_modifier = (ball_speed / 1000 + 1) effective_ball_spacing = GO_BEHIND_SPACING * ball_speed_modifier position_behind_ball_for_approach = self.get_destination_behind_ball(effective_ball_spacing) position_behind_ball_for_grab = self.game_state.ball_position - normalize(player_to_target) * GRAB_BALL_SPACING position_behind_ball_for_kick = self.game_state.ball_position + normalize(player_to_target) * KICK_DISTANCE if self.is_able_to_grab_ball_directly(0.8): self.points_sequence = [] if compare_angle(self.player.pose.orientation, orientation, abs_tol=0.1) and \ (dist_from_ball < GRAB_BALL_SPACING * 1.25): self.next_state = self.validate_kick return CmdBuilder().addMoveTo(Pose(position_behind_ball_for_kick, orientation), ball_collision=False, cruise_speed=3).addKick(self.kick_force).build() return CmdBuilder().addMoveTo(Pose(position_behind_ball_for_grab, orientation), ball_collision=False, cruise_speed=3).build() else: self.points_sequence = [WayPoint(position_behind_ball_for_approach, ball_collision=True)] return CmdBuilder().addMoveTo(Pose(position_behind_ball_for_kick, orientation), ball_collision=False, way_points=self.points_sequence, cruise_speed=3).build()
def is_ball_going_to_collide(self, threshold=18): # threshold in degrees ball_approach_angle = np.arccos( np.dot( normalize(player.position - self.game_state.ball.position).array, normalize(self.game_state.ball.velocity).array)) * 180 / np.pi return ball_approach_angle > threshold
def is_able_to_grab_ball_directly(self, threshold): # plus que le threshold est gors (1 max), plus qu'on veut que le robot soit direct deriere la balle. vec_target_to_ball = normalize(self.game_state.ball.position - self.target.position) alignement_behind = np.dot(vec_target_to_ball.array, (normalize(self.player.position - self.game_state.ball_position)).array) return threshold < alignement_behind
def compute_wall_segment(self): """ We compute the position where the wall's robot can block the field of view of opposing robot with the ball. The field of view is defined as the triangle created by the ball and the goal_line extremities. """ nb_robots = len(self.robots_in_formation) wall_segment_length = nb_robots * ROBOT_DIAMETER + GAP_IN_WALL * (nb_robots - 1) goal_line = self.game_state.field.our_goal_line bisection_angle = angle_between_three_points(goal_line.p2, self.object_to_block.position, goal_line.p1) # We calculate the farthest distance from the object which completely block its FOV of the goal object_to_center_formation_dist = wall_segment_length / tan(bisection_angle) self.bisect_inter = find_bisector_of_triangle(self.object_to_block.position, goal_line.p1, goal_line.p2) vec_object_to_goal_line_bisect = self.bisect_inter - self.object_to_block.position # The penalty zone used to be a circle and thus really easy to handle, but now it's a rectangle... # It easier to first create the smallest circle that fit the rectangle. min_radius_over_penality_zone = ROBOT_RADIUS + (self.game_state.field.our_goal_area.upper_left - self.game_state.field.our_goal).norm object_to_block_to_center_formation_dist = min(vec_object_to_goal_line_bisect.norm - min_radius_over_penality_zone, object_to_center_formation_dist) self.center_formation = object_to_block_to_center_formation_dist * normalize(vec_object_to_goal_line_bisect) + self.object_to_block.position if self.stay_away_from_ball: if (self.game_state.ball_position - self.center_formation).norm < KEEPOUT_DISTANCE_FROM_BALL: self.center_formation = self._closest_point_away_from_ball() half_wall_segment = 0.5 * wall_segment_length * perpendicular(normalize(vec_object_to_goal_line_bisect)) self.wall_segment = Line(self.center_formation + half_wall_segment, self.center_formation - half_wall_segment)
def ball_going_toward_player(self, player): role = GameState().get_role_by_player_id(player.id) if self.roles_graph[role].current_tactic_name == 'PositionForPass' or self.roles_graph[role].current_tactic_name == 'ReceivePass': if self.game_state.ball.is_mobile(50): # to avoid division by zero and unstable ball_directions ball_approach_angle = np.arccos(np.dot(normalize(player.position - self.game_state.ball.position).array, normalize(self.game_state.ball.velocity).array)) * 180 / np.pi return ball_approach_angle > 25 return False
def get_destination_behind_ball(self, ball_spacing, velocity=True, velocity_offset=25) -> Position: """ Compute the point which is at ball_spacing mm behind the ball from the target. """ dir_ball_to_target = normalize(self.game_state.ball.velocity) position_behind = self.game_state.ball.position - dir_ball_to_target * ball_spacing if velocity: position_behind += (self.game_state.ball.velocity - (normalize(self.game_state.ball.velocity) * np.dot(dir_ball_to_target.array, self.game_state.ball.velocity.array))) / velocity_offset return position_behind
def ball_going_toward_player(self, player): role = GameState().get_role_by_player_id(player.id) if self.roles_graph[role].current_tactic_name == 'PositionForPass' or \ self.roles_graph[role].current_tactic_name == 'ReceivePass': if self.game_state.ball.is_mobile( 50 ): # to avoid division by zero and unstable ball_directions ball_approach_angle = np.arccos( np.dot( normalize(player.position - self.game_state.ball.position).array, normalize(self.game_state.ball.velocity).array) ) * 180 / np.pi return ball_approach_angle > 25 return False
def compute_wall_arc(self): nb_robots = len(self.robots_in_formation) # Angle d'un robot sur le cercle de rayon self.dist_from_ball self.robot_angle = 2 * atan(ROBOT_RADIUS / self.dist_from_ball) # Angle totale couvert par le wall self.wall_angle = nb_robots * self.robot_angle + GAP_ANGLE * (nb_robots - 1) goal_line = self.game_state.field.goal_line self.center_of_goal = find_bisector_of_triangle(self.game_state.ball_position, goal_line.p1, goal_line.p2) vec_ball_to_center_of_goal = self.center_of_goal - self.game_state.ball_position # Centre de la formation sur le cercle de rayon self.dist_from_ball self.center_formation = self.dist_from_ball * normalize( vec_ball_to_center_of_goal) + self.game_state.ball_position # J'ajoute robot_angle / 2, sinon, avec 1 seul robot, il ne se positionne pas dans le centre de la formation self.first_robot_angle = vec_ball_to_center_of_goal.angle - self.wall_angle / 2 + self.robot_angle / 2 self.first_robot_position = Position(cos(self.first_robot_angle), sin(self.first_robot_angle)) * self.dist_from_ball + \ self.game_state.ball_position self.last_robot_angle = self.first_robot_angle + self.wall_angle
def get_destination_behind_ball(self, ball_spacing) -> Position: """ Compute the point which is at ball_spacing mm behind the ball from the target. """ dir_ball_to_target = normalize(self.target.position - self.game_state.ball.position) return self.game_state.ball.position - dir_ball_to_target * ball_spacing
def GoBetween(position1: Position, position2: Position, target: Position, minimum_distance: float=0): delta = minimum_distance * normalize(position2 - position1) position1 = position1 + delta position2 = position2 - delta destination = closest_point_on_segment(target, position1, position2) dest_to_target = target - destination return CmdBuilder().addMoveTo(Pose(destination, dest_to_target.angle)).build()
def get_destination_behind_ball(self, ball_spacing, velocity=True, velocity_offset=4) -> Position: """ Compute the point which is at ball_spacing mm behind the ball from the target. """ dir_ball_to_target = normalize(self.target.position - self.game_state.ball.position) position_behind = self.game_state.ball.position - dir_ball_to_target * ball_spacing if velocity and self.game_state.ball.velocity.norm > 20: position_behind += (self.game_state.ball.velocity - (normalize(self.game_state.ball.velocity) * np.dot(dir_ball_to_target.array, self.game_state.ball.velocity.array))) / velocity_offset return position_behind
def following_path_vector(self, robot): direction_error = self.last_commanded_velocity - robot.velocity.position if direction_error.norm > 0: return normalize(direction_error) else: return direction_error
def debug_cmd(self): if not self.is_debug: return angle = None additional_dbg = [] if self.current_state == self.go_behind_ball: angle = 18 elif self.current_state == self.grab_ball: angle = 25 elif self.current_state == self.kick: angle = 45 additional_dbg = [DebugCommandFactory.circle(self.game_state.ball_position, KICK_DISTANCE, color=RED)] if angle is not None: angle *= np.pi/180.0 base_angle = (self.game_state.ball.position - self.target.position).angle magnitude = 3000 ori = self.game_state.ball.position upper = ori + Position.from_angle(base_angle + angle, magnitude) lower = ori + Position.from_angle(base_angle - angle, magnitude) ball_to_player = self.player.position - self.game_state.ball_position behind_player = (ball_to_player.norm + 1000) * normalize(ball_to_player) + self.game_state.ball_position return [DebugCommandFactory.line(ori, upper), DebugCommandFactory.line(ori, lower), DebugCommandFactory.line(self.game_state.ball_position, behind_player, color=CYAN)] + additional_dbg return []
def compute_wall_arc(self): nb_robots = len(self.robots_in_formation) # Angle d'un robot sur le cercle de rayon self.dist_from_ball self.robot_angle = 2 * atan(ROBOT_RADIUS / self.dist_from_ball) # Angle totale couvert par le wall self.wall_angle = nb_robots * self.robot_angle + GAP_ANGLE * ( nb_robots - 1) goal_line = self.game_state.field.goal_line self.center_of_goal = find_bisector_of_triangle( self.game_state.ball_position, goal_line.p1, goal_line.p2) vec_ball_to_center_of_goal = self.center_of_goal - self.game_state.ball_position # Centre de la formation sur le cercle de rayon self.dist_from_ball self.center_formation = self.dist_from_ball * normalize( vec_ball_to_center_of_goal) + self.game_state.ball_position # J'ajoute robot_angle / 2, sinon, avec 1 seul robot, il ne se positionne pas dans le centre de la formation self.first_robot_angle = vec_ball_to_center_of_goal.angle - self.wall_angle / 2 + self.robot_angle / 2 self.first_robot_position = Position(cos(self.first_robot_angle), sin(self.first_robot_angle)) * self.dist_from_ball + \ self.game_state.ball_position self.last_robot_angle = self.first_robot_angle + self.wall_angle
def kick(self): self.next_state = self.validate_kick self.tries_flag += 1 player_to_target = (self.target.position - self.player.pose.position) behind_ball = self.game_state.ball_position - normalize(player_to_target) * (BALL_RADIUS + ROBOT_CENTER_TO_KICKER) orientation = (self.target.position - self.game_state.ball_position).angle return CmdBuilder().addMoveTo(Pose(behind_ball, orientation), ball_collision=False).addKick(self.kick_force).build()
def get_away_from_ball(self): if self._check_success() and self._get_distance_from_ball() > KEEPOUT_DISTANCE_FROM_BALL: self.next_state = self.halt self.status_flag = Flags.SUCCESS target_to_player = normalize(self.player.position - self.target.position) pos_away_from_ball = 1.2 * KEEPOUT_DISTANCE_FROM_BALL * target_to_player + self.target.position # Move to a position away from ball return CmdBuilder().addMoveTo(Pose(pos_away_from_ball, self.player.orientation)).addStopDribbler().build()
def GoBetween(position1: Position, position2: Position, target: Position, minimum_distance: float = 0): delta = minimum_distance * normalize(position2 - position1) position1 = position1 + delta position2 = position2 - delta destination = closest_point_on_segment(target, position1, position2) dest_to_target = target - destination return CmdBuilder().addMoveTo(Pose(destination, dest_to_target.angle)).build()
def get_away_from_ball(self): if self._check_success( ) and self._get_distance_from_ball() > KEEPOUT_DISTANCE_FROM_BALL: self.next_state = self.halt self.status_flag = Flags.SUCCESS target_to_player = normalize(self.player.position - self.target.position) pos_away_from_ball = 1.2 * KEEPOUT_DISTANCE_FROM_BALL * target_to_player + self.target.position # Move to a position away from ball return CmdBuilder().addMoveTo( Pose(pos_away_from_ball, self.player.orientation)).addStopDribbler().build()
def move_ball(self): if self._check_success(): self.next_state = self.wait_for_ball_stop_spinning elif self._has_ball_quit_dribbler(): self._fetch_ball() ball_position = self.game_state.ball_position ball_to_target_dir = normalize(self.target.position - ball_position) self.player_target_position = ROBOT_CENTER_TO_KICKER * ball_to_target_dir + self.target.position return CmdBuilder().addMoveTo(Pose(self.player_target_position, self.steady_orientation), cruise_speed=self.move_ball_cruise_speed, ball_collision=False, enable_pathfinder=False ).addForceDribbler().build()
def move_ball(self): if self._check_success(): self.next_state = self.wait_for_ball_stop_spinning elif self._has_ball_quit_dribbler(): self._fetch_ball() ball_position = self.game_state.ball_position ball_to_target_dir = normalize(self.target.position - ball_position) self.player_target_position = ROBOT_CENTER_TO_KICKER * ball_to_target_dir + self.target.position return CmdBuilder().addMoveTo( Pose(self.player_target_position, self.steady_orientation), cruise_speed=self.move_ball_cruise_speed, ball_collision=False, enable_pathfinder=False).addForceDribbler().build()
def kick(self): if self.auto_update_target: self._find_best_passing_option() if self.get_alignment_with_ball_and_target() > 45: self.next_state = self.go_behind_ball return self.go_behind_ball() self.next_state = self.validate_kick player_to_target = (self.target.position - self.player.pose.position) position_behind_ball = self.game_state.ball_position + normalize(player_to_target) * ROBOT_CENTER_TO_KICKER required_orientation = (self.target.position - self.game_state.ball_position).angle return CmdBuilder().addMoveTo(Pose(position_behind_ball, required_orientation), ball_collision=False)\ .addKick(self.kick_force)\ .addForceDribbler().build()
def wait_for_ball(self): perp_vec = perpendicular(self.player.position - self.game_state.ball.position) component_lateral = perp_vec * np.dot(perp_vec.array, normalize(self.game_state.ball.velocity).array) small_segment_len = np.sqrt(1 - component_lateral.norm**2) latteral_move = component_lateral / small_segment_len * (self.player.position - self.game_state.ball.position).norm if self._get_distance_from_ball() < HAS_BALL_DISTANCE: self.next_state = self.halt return self.halt() if not self.is_ball_going_to_collide(threshold=18): self.next_state = self.wait_for_ball return CmdBuilder().build() orientation = (self.game_state.ball_position - self.player.position).angle return CmdBuilder().addMoveTo(Pose(self.player.position + latteral_move.view(Position), orientation), cruise_speed=3, end_speed=0, ball_collision=False).addChargeKicker().build()
def _find_best_player_position(self): if not self.auto_position: return self.target.position if self.is_offense: ball_offset = clamp(self.game_state.ball.position.x, 0, 1000) left = self.game_state.field.their_goal_area.right + ball_offset right = ball_offset else: ball_offset = clamp(self.game_state.ball.position.x, -1000, 0) left = self.game_state.field.our_goal_area.left + ball_offset right = ball_offset idx = self.idx_in_formation len_formation = len(self.robots_in_formation) PADDING = 300 # Add buffer zone between area and the field limit area_height = self.game_state.field.field_width - 2 * PADDING individual_area_size = area_height / len_formation top = idx * individual_area_size - area_height / 2 bot = top + individual_area_size self.area = Area.from_limits(top, bot, right, left) center = self.area.center # Act as if each enemy robot was creating a repulsive force v = Position(0, 0) for enemy in self.game_state.enemy_team.available_players.values(): d = enemy.position - center # Clamp distance norm d = MIN_DIST_FROM_CENTER * normalize(d) if d.norm < MIN_DIST_FROM_CENTER else d # Square of the inverse of the distance, a bit like Newton's law of universal gravitation v -= ATTENUATION * d / (d.norm ** 3) if self.area.point_inside(center + v): return center + v offset_from_center = Line(center, center + v) # The position must stay inside the area limits, so let's find the intersection between our vector and the area area_inter = self.area.intersect(offset_from_center) if area_inter: return area_inter[0] # First intersection return center + v
def wait_for_ball(self): perp_vec = perpendicular(self.player.position - self.game_state.ball.position) component_lateral = perp_vec * np.dot( perp_vec.array, normalize(self.game_state.ball.velocity).array) small_segment_len = np.sqrt(1 - component_lateral.norm**2) latteral_move = component_lateral / small_segment_len * ( self.player.position - self.game_state.ball.position).norm if self._get_distance_from_ball() < HAS_BALL_DISTANCE: self.next_state = self.halt return self.halt() if not self.is_ball_going_to_collide(threshold=18): self.next_state = self.wait_for_ball return CmdBuilder().build() orientation = (self.game_state.ball_position - self.player.position).angle return CmdBuilder().addMoveTo( Pose(self.player.position + latteral_move.view(Position), orientation), cruise_speed=3, end_speed=0, ball_collision=False).addChargeKicker().build()
def _get_destination_behind_ball(self, ball_spacing) -> Position: direction = self.target.position - self.game_state.ball.position return ball_spacing * normalize( direction) + self.game_state.ball.position
def get_alignment_with_ball_and_target(self): vec_target_to_ball = normalize(self.game_state.ball.position - self.target.position) alignement_behind = np.dot(vec_target_to_ball.array, (normalize(self.player.position - self.game_state.ball_position)).array) return np.arccos(alignement_behind) * 180 / np.pi
def test_normalize_identity(): assert normalize(A_POS) is not A_POS
def test_normalize_return_type(): assert isinstance(normalize(A_POS), Position)
def test_normalize_with_zero_posisiton(): with pytest.raises(ZeroDivisionError): normalize(A_ZERO_POS)
def test_normalize_base_case(): assert normalize(A_POS) == A_POS_NORMALIZED
def _get_destination_behind_ball(self, ball_spacing) -> Position: direction = self.target.position - self.game_state.ball.position return ball_spacing * normalize(direction) + self.game_state.ball.position
def is_ball_going_to_collide(self, threshold=18): # threshold in degrees ball_approach_angle = np.arccos(np.dot(normalize(player.position - self.game_state.ball.position).array, normalize(self.game_state.ball.velocity).array)) * 180 / np.pi return ball_approach_angle > threshold