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 defense(self): # Prepare to block the ball if self._is_ball_safe_to_kick() and self.game_state.ball.is_immobile(): self.next_state = self.clear if self._ball_going_toward_goal(): self.next_state = self.intercept return self.intercept() # no time to loose circle_radius = self.game_state.field.goal_width / 2 circle_center = self.game_state.field.our_goal - self.OFFSET_FROM_GOAL_LINE solutions = intersection_line_and_circle(circle_center, circle_radius, self.game_state.ball.position, self._best_target_into_goal()) # Their is one or two intersection on the circle, take the one on the field for solution in solutions: if solution.x < self.game_state.field.field_length / 2\ and self.game_state.ball.position.x < self.game_state.field.field_length / 2: orientation_to_ball = (self.game_state.ball.position - self.player.position).angle return MoveTo(Pose(solution, orientation_to_ball), cruise_speed=3, end_speed=0) return MoveTo(Pose(self.game_state.field.our_goal, np.pi), cruise_speed=3, end_speed=0)
def __init__(self, game_state: GameState, player: Player, target: Pose = Pose(), args: List[str] = None, p_y_top: [int, float] = 3000, p_y_bottom: [int, float] = -3000, p_x_left: [int, float] = -4500, p_x_right: [int, float] = 4500, p_is_yellow: bool = False): super().__init__(game_state, player, target, args) assert isinstance(p_y_top, (int, float)) assert isinstance(p_y_bottom, (int, float)) assert isinstance(p_x_left, (int, float)) assert isinstance(p_x_right, (int, float)) assert isinstance(p_is_yellow, bool) self.y_top = p_y_top self.y_bottom = p_y_bottom self.x_left = p_x_left self.x_right = p_x_right self.is_yellow = p_is_yellow self.current_state = self.cover_zone self.next_state = self.cover_zone self.status_flag = Flags.WIP self.target = Pose( Position(int((p_x_right - p_x_left) / 2), int((p_y_top - p_y_bottom) / 2)))
def __init__( self, game_state: GameState, player: Player, target: Pose = Pose(), penalty_kick=False, args: List[str] = None, ): forbidden_area = [ Area.pad(game_state.field.their_goal_area, KEEPOUT_DISTANCE_FROM_GOAL) ] super().__init__(game_state, player, target, args, forbidden_areas=forbidden_area) self.current_state = self.defense self.next_state = self.defense self.target = Pose( self.game_state.field.our_goal, np.pi) # Ignore target argument, always go for our goal self.go_kick_tactic = None # Used by clear self.last_intersection = None # For debug self.OFFSET_FROM_GOAL_LINE = Position(ROBOT_RADIUS + 10, 0) self.GOAL_LINE = self.game_state.field.our_goal_line
def _put_in_robots_referential(robot: Robot, cmd: Pose) -> Pose: if config['GAME']['on_negative_side']: cmd.x *= -1 cmd.orientation *= -1 cmd.position = rotate(cmd.position, np.pi + robot.orientation) else: cmd.position = rotate(cmd.position, -robot.orientation) return cmd
def _put_in_world_referential(orientation: float, cmd: Pose) -> Pose: if config['GAME']['on_negative_side']: cmd.position = rotate(cmd.position, -np.pi - orientation) cmd.x *= -1 cmd.orientation *= -1 else: cmd.position = rotate(cmd.position, orientation) return cmd
def _find_best_passing_option(self): assignation_delay = (time.time() - self.target_assignation_last_time) if assignation_delay > TARGET_ASSIGNATION_DELAY: tentative_target_id = best_passing_option(self.player) if tentative_target_id is None: self.target = Pose.from_values(GameState().field.their_goal_x, 0, 0) else: self.target = Pose(GameState().get_player_position(tentative_target_id)) self.target_assignation_last_time = time.time()
def _change_tactic(self, cmd: STAChangeCommand): try: this_player = GameState().our_team.available_players[cmd.data['id']] except KeyError: self.logger.info('A tactic was assign to a player which is not on the field (id={}).'.format(cmd.data['id'])) this_player = GameState().our_team.players[cmd.data['id']] player_id = this_player.id tactic_name = cmd.data['tactic'] target = Position.from_list(cmd.data['target']) if config['GAME']['on_negative_side']: target = target.flip_x() target = Pose(target, this_player.orientation) args = cmd.data.get('args', '') try: tactic = self.play_state.get_new_tactic(tactic_name)(self.game_state, this_player, target, args) except: self.logger.exception('An error occurred.') self.logger.debug('The tactic was call with wrong arguments') raise if not isinstance(self.play_state.current_strategy, HumanControl): self.play_state.current_strategy = 'HumanControl' self.play_state.current_strategy.assign_tactic(tactic, player_id)
def go_behind_ball(self): if self.auto_update_target: self._find_best_passing_option() self.status_flag = Flags.WIP required_orientation = (self.target.position - self.game_state.ball_position).angle ball_speed = self.game_state.ball.velocity.norm ball_speed_modifier = (ball_speed/1000 + 1) angle_behind = self.get_alignment_with_ball_and_target() if angle_behind > 35: effective_ball_spacing = GO_BEHIND_SPACING * min(3, abs(angle_behind/45)) * ball_speed_modifier collision_ball = True else: effective_ball_spacing = GO_BEHIND_SPACING collision_ball = False position_behind_ball = self.get_destination_behind_ball(effective_ball_spacing) dist_from_ball = (self.player.position - self.game_state.ball_position).norm if self.get_alignment_with_ball_and_target() < 25 \ and compare_angle(self.player.pose.orientation, required_orientation, abs_tol=max(0.05, 0.05 * dist_from_ball/1000)): self.next_state = self.grab_ball else: self.next_state = self.go_behind_ball return CmdBuilder().addMoveTo(Pose(position_behind_ball, required_orientation), cruise_speed=3, end_speed=0, ball_collision=collision_ball)\ .addChargeKicker().addKick(self.kick_force).build()
def intercept(self): # Find the point where the ball will go if not self._ball_going_toward_goal( ) and not self.game_state.field.is_ball_in_our_goal_area(): self.next_state = self.defense elif self.game_state.field.is_ball_in_our_goal_area( ) and self.game_state.ball.is_immobile(): self.next_state = self.clear ball = self.game_state.ball where_ball_enter_goal = intersection_between_lines( self.GOAL_LINE.p1, self.GOAL_LINE.p2, ball.position, ball.position + ball.velocity) # This is where the ball is going to enter the goal where_ball_enter_goal = closest_point_on_segment( where_ball_enter_goal, self.GOAL_LINE.p1, self.GOAL_LINE.p2) enter_goal_to_ball = Line(where_ball_enter_goal, ball.position) # The goalkeeper can not enter goal since there a line blocking vision end_segment = enter_goal_to_ball.direction * ROBOT_RADIUS + where_ball_enter_goal intersect_pts = closest_point_on_segment(self.player.position, ball.position, end_segment) self.last_intersection = intersect_pts return MoveTo( Pose(intersect_pts, self.player.pose.orientation ), # It's a bit faster, to keep our orientation cruise_speed=3, end_speed=0, ball_collision=False)
def _format_entities(entities: Iterable[Union[RobotFilter, BallFilter]]) -> List[Dict[str, Any]]: formatted_list = [] for entity in entities: fields = dict() if type(entity) is RobotFilter: fields['pose'] = Pose.from_values(*entity.pose) fields['velocity'] = Pose.from_values(*entity.velocity) elif type(entity) is BallFilter: fields['position'] = Position(*entity.position) fields['velocity'] = Position(*entity.velocity) else: raise TypeError('Invalid type provided: {}'.format(type(entity))) fields['id'] = entity.id formatted_list.append(fields) return formatted_list
def _go_to_final_position(self): position = self.game_state.ball_position - Position.from_angle( self.target_orientation) * DISTANCE_FROM_BALL return CmdBuilder().addMoveTo( Pose(position, self.target_orientation), cruise_speed=self.speed, ball_collision=self.ball_collision).build()
def __init__(self, game_state: GameState, player: Player, p_target: Pose = Pose(), args: List[str] = None): super().__init__(game_state, player, p_target, args) self.current_state = self.move_to_ball self.next_state = self.move_to_ball
def _generate_move_to(self): player_pose = self.player.pose ball_position = self.game_state.ball_position dest_position = self.get_behind_ball_position(ball_position) destination_pose = Pose(dest_position, player_pose.orientation) return MoveTo(destination_pose)
def __init__(self, game_state: GameState, player: Player, target: Pose = Pose(), args: Optional[List[str]] = None): super().__init__(game_state, player, target, args) self.go_kick_tactic = None self.current_state = self.go_kick_low self.next_state = self.go_kick_low
def __init__(self, game_state: GameState, player: Player, target: Pose = Pose(), args: List[str] = None): super().__init__(game_state, player, target, args) self.current_state = self.kick self.next_state = self.kick self.kick_force = 10
def main_state(self): self.compute_wall_arc() if self.game_state.field.is_ball_in_our_goal_area(): return Idle # We must not block the goalkeeper angle = self.angle_on_wall_arc() dest = Position(cos(angle), sin( angle)) * self.dist_from_ball + self.game_state.ball_position dest_orientation = (self.game_state.ball_position - dest).angle return MoveTo(Pose(dest, dest_orientation), cruise_speed=0.8)
def main_state(self): if self.check_success(): self.current_position_index_to_go = random.randint(0, len(self.grid_of_positions) - 1) self.current_position_to_go = self.grid_of_positions[self.current_position_index_to_go] #self.current_angle_to_go = random.randint(-1, 1) * np.pi / 100. self.next_pose = Pose(self.current_position_to_go, self.current_angle_to_go) return MoveTo(self.next_pose, cruise_speed=2)
def move_to_pass_position(self): destination_orientation = (self.game_state.ball_position - self.player.pose.position).angle if (time.time() - self.last_evaluation) > EVALUATION_INTERVAL: self.best_position = self._find_best_player_position() if self.auto_position else self.target self.last_evaluation = time.time() return MoveTo(Pose(self.best_position, destination_orientation), ball_collision=False, cruise_speed=2)
def __init__(self, game_state: GameState, player: Player, p_target: Pose=Pose(), args: List[str]=None): super().__init__(game_state, player, p_target, args) # Since there no target provided, let's follow oneself if args is None: self.robot_to_follow_id = player.id else: self.robot_to_follow_id = int(args[0]) self.current_state = self.halt self.next_state = self.halt
def grab_ball(self): if self._get_distance_from_ball() < 120: self.next_state = self.kick self.last_time = time.time() elif self._is_player_towards_ball_and_target(-0.9): self.next_state = self.grab_ball else: self.next_state = self.get_behind_ball return MoveTo( Pose(self.game_state.ball.position, self.player.pose.orientation))
def next_corner(self): orientation = (self.points[0].position - self.player.position).angle if (self.player.position - self.points[0].position).norm < ROTATE_DISTANCE: self.points.rotate() return CmdBuilder().addMoveTo(Pose(self.points[0].position, orientation), cruise_speed=self.cruise_speed).build()
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 _update_field_const(self): self.our_goal = Position(self.our_goal_x, 0) self.our_goal_pose = Pose(self.our_goal, 0) self.their_goal = Position(self.their_goal_x, 0) self.their_goal_pose = Pose(self.their_goal, 0) p1 = self.field_lines["RightPenaltyStretch"].p1 p2 = self.field_lines["RightPenaltyStretch"].p2 p3 = self.field_lines["RightFieldLeftPenaltyStretch"].p1 p4 = self.field_lines["RightFieldLeftPenaltyStretch"].p2 self.our_goal_area = Area.from_4_point(p1, p2, p3, p4) self.their_goal_area = Area.flip_x(self.our_goal_area) self.goal_line = Line(p1=Position(self.our_goal_x, +self.goal_width / 2), p2=Position(self.our_goal_x, -self.goal_width / 2)) self.our_goal_line = Line(p1=Position(self.our_goal_x, +self.goal_width / 2), p2=Position(self.our_goal_x, -self.goal_width / 2)) self.their_goal_line = Line(p1=Position(self.their_goal_x, +self.goal_width / 2), p2=Position(self.their_goal_x, -self.goal_width / 2)) self.behind_our_goal_line = Area.from_limits( self.our_goal_area.top, self.our_goal_area.bottom, self.our_goal_area.right + 50 * self.goal_depth, self.our_goal_area.left) self.behind_their_goal_line = Area.flip_x(self.behind_our_goal_line) self.free_kick_avoid_area = Area.pad( self.their_goal_area, INDIRECT_KICK_OFFSET + KEEPOUT_DISTANCE_FROM_GOAL) self.our_goal_forbidden_area = Area.pad(self.our_goal_area, KEEPOUT_DISTANCE_FROM_GOAL) self.their_goal_forbidden_area = Area.pad(self.their_goal_area, KEEPOUT_DISTANCE_FROM_GOAL) self.center = Position(0, 0)
def target_pose(self) -> Pose: if self.target_orientation is not None: orientation = self.target_orientation else: orientation = self.orientation if self.target_position is not None: position = self.target_position else: position = self.position return Pose(position, orientation)
def _find_best_passing_option(self): assignation_delay = (time.time() - self.target_assignation_last_time) if assignation_delay > TARGET_ASSIGNATION_DELAY: scoring_target = player_covered_from_goal(self.player) tentative_target = best_passing_option(self.player, passer_can_kick_in_goal=self.can_kick_in_goal) # Kick in the goal where it's the easiest if self.can_kick_in_goal and scoring_target is not None: self.target = Pose(scoring_target, 0) self.kick_force = KickForce.HIGH # Kick in the goal center elif tentative_target is None: if not self.can_kick_in_goal: self.logger.warning("The kicker {} can not find an ally to pass to and can_kick_in_goal is False" ". So it kicks directly in the goal, sorry".format(self.player)) self.target = Pose(self.game_state.field.their_goal, 0) self.kick_force = KickForce.HIGH else: # Pass the ball to another player self.target = Pose(tentative_target.position) self.kick_force = KickForce.for_dist((self.target.position - self.game_state.ball.position).norm) self.target_assignation_last_time = time.time()
def main_state(self): self.compute_wall_segment() if self.game_state.field.is_ball_in_our_goal_area(): return Idle # We must not block the goalkeeper elif self._should_ball_be_kick_by_wall() \ and self._is_closest_not_goaler(self.player) \ and self._no_enemy_around_ball(): self.next_state = self.go_kick dest = self.position_on_wall_segment() dest_orientation = (self.object_to_block.position - dest).angle return MoveTo(Pose(dest, dest_orientation), cruise_speed=self.cruise_speed)
def __init__(self, game_state: GameState, player: Player, target: Pose = Pose(), args: Optional[List[str]] = None): super().__init__(game_state, player, target, args) self.current_state = self.kick_charge self.next_state = self.kick_charge self.last_time = time.time() if args: self.target_id = int(args[0]) else: self.target_id = 1
def execute(self, robot: Robot, dt: float): pos_error = robot.position_error orientation_error = robot.orientation_error command = Pose.from_values(self.controllers['x'].execute(pos_error.x), self.controllers['y'].execute(pos_error.y), self.controllers['orientation'].execute(orientation_error)) # Limit max linear speed command.position /= max(1.0, command.norm / MAX_LINEAR_SPEED) command.orientation /= max(1.0, abs(command.orientation) / MAX_ANGULAR_SPEED) return command
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 wait_for_ball_stop_spinning(self): if self.wait_timer is None: self.wait_timer = time.time() if time.time() - self.wait_timer > TIME_TO_WAIT_FOR_BALL_STOP_MOVING: self.next_state = self.get_away_from_ball self.wait_timer = None if self._has_ball_quit_dribbler(): self._fetch_ball() return CmdBuilder().addMoveTo( Pose(self.player_target_position, self.steady_orientation), ball_collision=False, enable_pathfinder=False).addStopDribbler().build()
def flip_pose(pose: Pose): assert isinstance(pose, Pose) if not config["GAME"]["on_negative_side"]: return pose return pose.mirror_x()
import unittest from time import sleep from Util import Pose, Position from ai.STA.Tactic.go_kick import GoKick, COMMAND_DELAY from tests.STA.perfect_sim import PerfectSim A_ROBOT_ID = 1 START_POSE = Pose.from_values(300, 0, 0) START_BALL_POSITION = START_POSE.position + Position(100, 0) GOAL_POSE = Pose.from_values(700, 0, 0) MAX_TICK_UNTIL_KICK = 7 class TestGoKick(unittest.TestCase): def setUp(self): self.sim = PerfectSim(GoKick) def test_givenARobotAndABall_thenKickTheBall(self): self.sim.add_robot(A_ROBOT_ID, START_POSE) self.sim.move_ball(START_BALL_POSITION) self.sim.start(A_ROBOT_ID, target=GOAL_POSE) self.sim.tick() # initialize for _ in range(0, MAX_TICK_UNTIL_KICK): self.sim.tick() if self.sim.has_kick() and self.sim.has_hit_ball: return
def test_givenWrongDict_whenFromDict_thenThrowsKeyError(self): with self.assertRaises(KeyError): Pose.from_dict(A_WRONG_DICT)
def test_givenXYOrientation_whenFromValues_thenReturnNewPose(self): pose = Pose.from_values(A_X, A_Y, A_ORIENTATION) self.assertEqual(pose.x, A_X) self.assertEqual(pose.y, A_Y) self.assertEqual(pose.orientation, A_ORIENTATION)
def test_givenDict_whenFromDict_thenReturnNewPose(self): pose = Pose.from_dict(A_DICT) self.assertEqual(pose.x, A_DICT['x']) self.assertEqual(pose.y, A_DICT['y']) self.assertEqual(pose.orientation, A_DICT['orientation'])