def update(self, cmd: AICommand) -> Pose(): self.update_states(cmd) self.target_reached = self.target_is_reached() # Rotation control rotation_cmd = np.array( self.angle_controller.update(self.pos_error[Pos.THETA])) rotation_cmd = np.clip(rotation_cmd, -self.setting.rotation.max_speed, self.setting.rotation.max_speed) if self.setting.rotation.sensibility < np.abs( rotation_cmd) < self.setting.rotation.deadzone: rotation_cmd = 0 # Translation control translation_cmd = self.get_next_velocity() translation_cmd += np.array([ self.x_controller.update(self.pos_error[Pos.X]), self.y_controller.update(self.pos_error[Pos.Y]) ]) translation_cmd = self.limit_acceleration(translation_cmd) translation_cmd = np.clip(translation_cmd, -self.cruise_speed, self.cruise_speed) #translation_cmd = self.apply_deadzone(translation_cmd) # Send new command to robot translation_cmd = fixed2robot(translation_cmd, self.current_orientation) return Pose(Position(translation_cmd[Pos.X], translation_cmd[Pos.Y]), rotation_cmd)
def test_GoBehind(self): # TODO: faire davantage de cas de test distance_behind = 500 # test avec une droite quelconque self.go_behind = GoBehind(self.game_state, self.player_id, Position(1.5, 2.3), Position(18.3, 27.8), distance_behind) aicmd_obtenu = GoBehind.exec(self.go_behind) aicmd_cible = AICommand(Pose(Position(-273, -415), 0.9882), 0) self.assertEqual(aicmd_obtenu, aicmd_cible) # test avec une droite verticale self.go_behind = GoBehind(self.game_state, self.player_id, Position(1000, 250.3), Position(1000, 725.8), distance_behind) aicmd_obtenu = GoBehind.exec(self.go_behind) aicmd_cible = AICommand(Pose(Position(1000, -249), 1.5707), 0) self.assertEqual(aicmd_obtenu, aicmd_cible) # test avec une droite horizontale self.go_behind = GoBehind(self.game_state, self.player_id, Position(175.8, -200.34), Position(-276.8, -200.34), distance_behind) aicmd_obtenu = GoBehind.exec(self.go_behind) aicmd_cible = AICommand(Pose(Position(675, -200), -3.1415), 0) self.assertEqual(aicmd_obtenu, aicmd_cible)
def go_between_ball_and_target(self): self.status_flag = Flags.WIP ball = self.game_state.get_ball_position() ball_velocity = self.game_state.get_ball_velocity().conv_2_np() if np.linalg.norm(ball_velocity) > 50: self.target = Pose( Position.from_np(ball.conv_2_np() - ball_velocity), 0) dist_behind = np.linalg.norm(ball_velocity) + 1 / np.sqrt( np.linalg.norm(ball_velocity)) else: self.target = None dist_behind = 250 if self.target is None: if self.game_state.get_our_team_color() == 0: # yellow self.target = Pose( self.game_state.const["FIELD_GOAL_BLUE_MID_GOAL"], 0) else: self.target = Pose( self.game_state.const["FIELD_GOAL_YELLOW_MID_GOAL"], 0) if self._is_player_towards_ball_and_target(): self.next_state = self.grab_ball else: self.next_state = self.go_between_ball_and_target return GoBehind(self.game_state, self.player, ball, self.target.position, dist_behind)
def test_kick(self): # test avec la valeur 0 (nulle) target = Pose(Position(1, 1)) ball_position = Position(5, 0) self.game_state.set_ball_position(ball_position, A_DELTA_T) expected_cmd = AICommand( self.a_player, AICommandType.MOVE, **{ "pose_goal": Pose(ball_position, 0.785), "charge_kick": True, "kick": True, "pathfinder_on": True, "cruise_speed": 0.1, "end_speed": 0 }) return_cmd = Kick(self.game_state, self.a_player, force=0, target=target).exec() self.assertEqual(expected_cmd, return_cmd) # test avec la valeur 1 (force maximale) expected_cmd.kick_strength = 1 return_cmd = Kick(self.game_state, self.a_player, 1, target=target).exec() self.assertEqual(return_cmd, expected_cmd) # test avec la valeur 0.3 (force intermediaire) expected_cmd.kick_strength = 0.3 return_cmd = Kick(self.game_state, self.a_player, 0.3, target=target).exec() self.assertEqual(return_cmd, expected_cmd)
def test_move_to(self): A_CRUISE_SPEED = 0.1 self.pose = Pose(Position(0, 0), 0.0) self.move = MoveToPosition(self.game_state, self.a_player, self.pose, False, A_CRUISE_SPEED) return_cmd = self.move.exec() expected_cmd = AICommand( self.a_player, AICommandType.MOVE, **{ "pose_goal": self.pose, "pathfinder_on": False, "cruise_speed": A_CRUISE_SPEED }) self.assertEqual(return_cmd, expected_cmd) self.pose = Pose(Position(0.5, 0.3), 3.2) self.move = MoveToPosition(self.game_state, self.a_player, self.pose, False, A_CRUISE_SPEED) self.assertEqual( MoveToPosition.exec(self.move), AICommand( self.a_player, AICommandType.MOVE, **{ "pose_goal": self.pose, "pathfinder_on": False, "cruise_speed": A_CRUISE_SPEED }))
def test_GoBehind(self): # TODO: faire davantage de cas de test distance_behind = 500 # test avec une droite verticale self.go_behind = GoBehind(self.game_state, self.a_player, Position(1000, 250.3), Position(1000, 725.8), distance_behind) aicmd_obtenu = self.go_behind.exec() aicmd_expected = AICommand( self.a_player, AICommandType.MOVE, **{"pose_goal": Pose(Position(1000, -249.700), 1.5707)}) self.assertEqual(aicmd_obtenu, aicmd_expected) # test avec une droite quelconque self.go_behind = GoBehind(self.game_state, self.a_player, Position(1.5, 2.3), Position(18.3, 27.8), distance_behind) aicmd_obtenu = self.go_behind.exec() aicmd_expected = AICommand( self.a_player, AICommandType.MOVE, **{"pose_goal": Pose(Position(-273.579, -415.230), 0.9882)}) self.assertEqual(aicmd_obtenu, aicmd_expected) # test avec une droite horizontale self.go_behind = GoBehind(self.game_state, self.a_player, Position(175.8, -200.34), Position(-276.8, -200.34), distance_behind) aicmd_obtenu = GoBehind.exec(self.go_behind) aicmd_cible = AICommand( self.a_player, AICommandType.MOVE, **{"pose_goal": Pose(Position(675.800, 99.660), -2.601)}) self.assertEqual(aicmd_obtenu, aicmd_cible)
def __init__(self, game_state: GameState, player: OurPlayer, 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 _update_camera_kalman(self, packets): self.new_image_flag = False if not packets: return # change the packets of a camera if frame_number of camera is higher # than what we have for packet in packets: if packet.HasField("detection"): c_id = packet.detection.camera_id f_nb = packet.detection.frame_number if f_nb > self.last_camera_frame[c_id]["frame_number"]: new_camera = deepcopy(empty_camera) new_camera["camera_id"] = c_id new_camera["frame_number"] = f_nb new_camera["t_capture"] = packet.detection.t_capture new_camera["timestamp"] = time.time() for ball in packet.detection.balls: new_camera["ball"] = Position(ball.x, ball.y) for blue in packet.detection.robots_blue: new_camera["blues"][blue.robot_id] = Pose( Position(blue.x, blue.y), blue.orientation) for yellow in packet.detection.robots_yellow: new_camera["yellows"][yellow.robot_id] = Pose( Position(yellow.x, yellow.y), yellow.orientation) self.last_camera_frame[c_id] = new_camera self.new_image_flag = True
def __init__(self, player: OurPlayer, command_type=AICommandType.STOP, **other_args): """ Initialise. :param player: (OurPlayer) l'instance de notre player à qui appartient cette ai_commande :param p_command: (AICommandType) le type de AICommand :param other_args: (Dict) les flags et arguments à passer """ assert isinstance( player, OurPlayer ), "Création d'un ai_command sans passer une instance de OurPlayer." assert isinstance(command_type, AICommandType), "Besoin d'une AiCommandType!" self.player = player self.robot_id = player.id self.command = command_type self.dribbler_on = other_args.get("dribbler_on", 0) self.pathfinder_on = other_args.get("pathfinder_on", False) self.kick_strength = other_args.get("kick_strength", 0) self.charge_kick = other_args.get("charge_kick", False) self.kick = other_args.get("kick", False) self.pose_goal = other_args.get("pose_goal", Pose()) self.speed = Pose() self.wheel_speed = (0, 0, 0, 0) self.cruise_speed = other_args.get("cruise_speed", 0.0001337) self.control_loop_type = other_args.get("control_loop_type", AIControlLoopType.POSITION) # this is for the pathfinder only no direct assignation self.path = [] self.path_speeds = [0, 0]
def __init__(self, team, id): self.id = id self.team = team self.pose = Pose() self.velocity = Pose() self.kf = EnemyKalmanFilter() self.update = self._update if ConfigService().config_dict["IMAGE"]["kalman"] == "true": self.update = self._kalman_update
def test_to_tuple(self): uut = Pose() #sanity check self.assertNotEqual(type(uut.to_tuple()), type(Pose())) self.assertEqual(type(uut.to_tuple()), type(tuple())) self.assertEqual(uut.to_tuple(), tuple((0, 0))) uut = Pose(Position(557, -778.5), 0) self.assertEqual(uut.to_tuple(), tuple((557, -778.5))) self.assertNotEqual(uut.to_tuple(), tuple((-42, 3897)))
def test__kalman_update(self): self.assertEqual(self.player1.pose, Pose()) # one kalman update and the kalman filter will put the pose of the player to Pose(9999, 9999, 0) or something self.player1._kalman_update([None], 0.5) self.assertNotEqual(self.player1.pose, Pose()) self.assertNotEqual(self.player1.pose, Pose(200, 200, 1)) # send enough image where the robot has no position for the kalman to say the robot # isnt on the field with Pose() for i in range(21): self.player1._kalman_update([None], 0.5) self.assertEqual(self.player1.pose, Pose())
def __init__(self, world_state: WorldState, robot_id, is_sim=True): self.ws = world_state self.id = robot_id self.is_sim = is_sim self.setting = get_control_setting(is_sim) self.setting.translation.max_acc = None self.setting.translation.max_speed = None self.setting.rotation.max_angular_acc = None self.setting.rotation.max_speed = None self.current_pose = Pose() self.current_orientation = 0.0 self.current_velocity = Pose() self.current_angular_speed = 0.0 self.current_speed = 0.0 self.current_acceleration = Position() self.pose_error = Pose() self.position_error = Position() self.target_pose = Pose() self.target_speed = 0.0 self.target_direction = Position() self.target_angular_speed = 0.0 self.target_angle = 0.0 self.angle_error = 0.0 self.last_translation_cmd = Position() self.cruise_speed = 0.0 self.cruise_angular_speed = 0.0 self.next_speed = 0.0 self.next_angular_speed = 0.0 self.x_controller = PID(self.setting.translation.kp, self.setting.translation.ki, self.setting.translation.kd, self.setting.translation.antiwindup) self.y_controller = PID(self.setting.translation.kp, self.setting.translation.ki, self.setting.translation.kd, self.setting.translation.antiwindup) self.angle_controller = PID(self.setting.rotation.kp, self.setting.rotation.ki, self.setting.rotation.kd, self.setting.rotation.antiwindup, wrap_err=True) self.position_flag = False self.rotation_flag = False self.last_position = Position() self.target_turn = self.target_pose.position
def test_MoveWithBall(self): self.move_with_ball = MoveWithBall(self.game_state, self.player_id, Position(100, 0)) self.game_state._update_ball_position(Position(5, 0)) ai_cmd = self.move_with_ball.exec() ai_cmd_expected = AICommand(Pose(Position(100, 0), 0), 0) self.assertEqual(ai_cmd, ai_cmd_expected) self.game_state._update_ball_position(Position(5, 2)) ai_cmd = self.move_with_ball.exec() ai_cmd_expected = AICommand(Pose(Position(100, 0), atan(2 / 5)), 0) self.assertEqual(ai_cmd, ai_cmd_expected)
def test_ProtectGoal(self): # test de base self.game_state._update_player(0, Pose(Position(4450, 10), 0)) self.game_state._update_ball_position(Position(0, 0)) self.protectGoal = ProtectGoal(self.game_state, 0) aicmd_obtenu = self.protectGoal.exec() aicmd_cible = AICommand(Pose(Position(4000, 0), -pi), 0) self.assertEqual(aicmd_obtenu, aicmd_cible) # test distance max < distance min self.assertRaises(AssertionError, ProtectGoal, self.game_state, 0, True, 50, 40)
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( GameState().const["FIELD_THEIR_GOAL_X_EXTERNAL"], 0, 0) else: self.target = Pose( GameState().get_player_position(tentative_target_id)) self.target_assignation_last_time = time.time()
def _update_entities(self) -> None: # should there be a sanity check for multiple balls/players on the field viewed by the same camera? for ball in self.packet.detection.balls: self.ball = Position(ball.x, ball.y) # maybe a sanity check on the id of the robots. always 0 to 11 ? Possible key error on the set operations for blue in self.packet.detection.robots_blue: self.robots_blue[blue.robot_id] = Pose(blue.x, blue.y, blue.orientation) for yellow in self.packet.detection.robots_yellow: self.robots_yellow[yellow.robot_id] = Pose(yellow.x, yellow.y, yellow.orientation)
def test_move_to(self): self.pose = Pose(Position(0, 0, 0), orientation=0.0) self.move = MoveToPosition(self.game_state, self.player_id, self.pose) self.assertEqual( self.move.exec(), AICommand(1, AICommandType.MOVE, **{"pose_goal": self.pose})) self.pose = Pose(Position(0.5, 0.3, 0.2), orientation=3.2) self.move = MoveToPosition(self.game_state, self.player_id, self.pose) self.assertEqual( MoveToPosition.exec(self.move), AICommand(self.player_id, AICommandType.MOVE, **{"pose_goal": self.pose}))
def test_move_to(self): self.pose = Pose(Position(0, 0, 0), orientation=0.0) self.move = MoveTo(self.game_state, self.player_id, self.pose) self.assertEqual( str(MoveTo.exec(self.move)), "AICommand(move_destination=[(x=0.0, y=0.0, z=0.0), theta=0.0], kick_strength=0)" ) self.pose = Pose(Position(0.5, 0.3, 0.2), orientation=3.2) self.move = MoveTo(self.game_state, self.player_id, self.pose) self.assertEqual( str(MoveTo.exec(self.move)), "AICommand(move_destination=[(x=0.5, y=0.3, z=0.2), theta=" + "-3.083185307179586], kick_strength=0)")
def test_sub(self): pose1 = Pose(Position(1, 2), 3) pose2 = Pose(Position(5, 7), 9) pose3 = pose1 - pose2 self.assertEqual(pose3.position, Position(-4, -5)) self.assertEqual(pose3.orientation, -6 + 2 * m.pi) pos = Position(10, 20) pose4 = pose1 - pos self.assertEqual(pose4.position, Position(-9, -18)) self.assertEqual(pose4.orientation, 3) with self.assertRaises(TypeError): pos - pose1
def test_add(self): pose1 = Pose(Position(1, 2), 3) pose2 = Pose(Position(5, 7), 9) pose3 = pose1 + pose2 self.assertEqual(pose3.position, Position(6, 9)) self.assertEqual(pose3.orientation, 12 - 4 * m.pi) pos = Position(10, 20) pose4 = pose1 + pos self.assertEqual(pose4.position, Position(11, 22)) self.assertEqual(pose4.orientation, 3) with self.assertRaises(TypeError): pos + pose1
def test_init_normal(self): random_player_id = 0 p = Player(self.team, random_player_id) self.assertIsNotNone(p) self.assertIsNotNone(p.id) self.assertEqual(random_player_id, p.id) self.assertIsNotNone(p.team) self.assertIsNotNone(p.pose) self.assertEqual(p.pose, Pose()) self.assertIsNotNone(p.velocity) self.assertEqual(p.velocity, Pose()) self.assertIsNotNone(p.kf) self.assertTrue(isinstance(p.kf, EnemyKalmanFilter)) self.assertIsNotNone(p.update)
def test_GrabBall(self): self.grab_ball = GrabBall(self.game_state, self.player_id) self.game_state._update_ball_position(Position(5, 0)) ai_cmd = self.grab_ball.exec() ai_cmd_expected = AICommand(Pose(Position(5, 0)), 0) print(self.game_state.get_player_pose(self.player_id)) print(ai_cmd) print(ai_cmd_expected) self.assertEqual(ai_cmd, ai_cmd_expected) self.game_state._update_ball_position(Position(-5, 5)) ai_cmd = self.grab_ball.exec() ai_cmd_expected = AICommand(Pose(Position(-5, 5), 3 * pi / 4), 0) self.assertEqual(ai_cmd, ai_cmd_expected)
def __init__(self, p_game_state): super().__init__(p_game_state, keep_roles=False) # TODO: HARDCODED ID FOR QUALIFICATION, REMOVE LATER self.roles_graph = {r: Graph() for r in Role} role_mapping = { Role.GOALKEEPER: 2, Role.MIDDLE: 4, Role.FIRST_ATTACK: 6 } self.game_state.map_players_to_roles_by_player_id(role_mapping) ourgoal = Pose( Position(GameState().const["FIELD_OUR_GOAL_X_EXTERNAL"], 0), 0) self.theirgoal = Pose( Position(GameState().const["FIELD_THEIR_GOAL_X_EXTERNAL"], 0), 0) roles_to_consider = [Role.MIDDLE, Role.FIRST_ATTACK, Role.GOALKEEPER] role_by_robots = [(i, self.game_state.get_player_by_role(i)) for i in roles_to_consider] self.robots = [ player for _, player in role_by_robots if player is not None ] goalkeeper = self.game_state.get_player_by_role(Role.GOALKEEPER) self.add_tactic( Role.GOALKEEPER, GoalKeeper(self.game_state, goalkeeper, ourgoal, penalty_kick=True)) for index, player in role_by_robots: if player: self.add_tactic( index, PositionForPass(self.game_state, player, auto_position=True, robots_in_formation=self.robots)) self.add_tactic( index, GoKick(self.game_state, player, target=self.theirgoal)) self.add_condition(index, 0, 1, partial(self.is_closest, player)) self.add_condition(index, 1, 0, partial(self.is_not_closest, player)) self.add_condition(index, 1, 1, partial(self.has_kicked, player))
def test_ProtectGoal(self): # test de base self.game_state.game.friends.players[0].update(Pose(Position(4450, 10))) self.game_state.game.ball.set_position(Position(0, 0), 0) self.protectGoal = ProtectGoal(self.game_state, 0) aicmd_obtenu = self.protectGoal.exec() aicmd_cible = AICommand(self.player_id, AICommandType.MOVE, **{"pose_goal": Pose(Position(4000, 0), -pi)}) self.assertEqual(aicmd_obtenu, aicmd_cible) # test distance max < distance min self.assertRaises(AssertionError, ProtectGoal, self.game_state, 0, True, 50, 40)
def protect_goal(self): if not self.penalty_kick: if not self._is_ball_too_far and \ self.player == closest_player_to_point(self.game_state.get_ball_position()).player and\ self._get_distance_from_ball() < ROBOT_RADIUS *3: self.next_state = self.go_behind_ball else: self.next_state = self.protect_goal return ProtectGoal(self.game_state, self.player, self.is_yellow, minimum_distance=300, maximum_distance=self.game_state.game.field. constant["FIELD_GOAL_RADIUS"] / 2) else: our_goal = Position( self.game_state.const["FIELD_OUR_GOAL_X_EXTERNAL"], 0) opponent_kicker = player_with_ball(2 * ROBOT_RADIUS) ball_position = self.game_state.get_ball_position() if opponent_kicker is not None: ball_to_goal = our_goal.x - ball_position.x if self.game_state.field.our_side is FieldSide.POSITIVE: opponent_kicker_orientation = clamp( opponent_kicker.pose.orientation, -pi / 5, pi / 5) goalkeeper_orientation = wrap_to_pi( opponent_kicker_orientation - pi) else: opponent_kicker_orientation = clamp( wrap_to_pi(opponent_kicker.pose.orientation - pi), -pi / 5, pi / 5) goalkeeper_orientation = opponent_kicker_orientation y_position_on_line = ball_to_goal * tan( opponent_kicker_orientation) width = self.game_state.const["FIELD_GOAL_WIDTH"] y_position_on_line = clamp(y_position_on_line, -width, width) destination = Pose(our_goal.x, y_position_on_line, goalkeeper_orientation) else: destination = Pose(our_goal) return MoveToPosition(self.game_state, self.player, destination, pathfinder_on=True, cruise_speed=2)
def exec(self): """ Calcul la pose que doit prendre le gardien en fonction de la position de la balle. :return: Un tuple (Pose, kick) où Pose est la destination du gardien et kick est nul (on ne botte pas) """ goalkeeper_position = self.game_state.get_player_pose(self.player_id).position ball_position = self.game_state.get_ball_position() goal_x = FIELD_X_RIGHT if self.is_right_goal else FIELD_X_LEFT goal_position = Position(goal_x, 0) # Calcul de la position d'interception entre la balle et le centre du but destination_position = get_closest_point_on_line(goalkeeper_position, goal_position, ball_position) # Vérification que destination_position respecte la distance minimale destination_position = stayOutsideCircle(destination_position, goal_position, self.minimum_distance) # Vérification que destination_position respecte la distance maximale if self.maximum_distance is None: destination_position = stayInsideGoalArea(destination_position, self.is_right_goal) else: destination_position = stayInsideCircle(destination_position, goal_position, self.maximum_distance) # Calcul de l'orientation de la pose de destination destination_orientation = get_angle(destination_position, ball_position) destination_pose = Pose(destination_position, destination_orientation) kick_strength = 0 return AICommand(destination_pose, kick_strength)
def __init__(self, game_state: GameState, player: OurPlayer, target: Pose = Pose(), args: List = None): """ Initialise la tactic avec des valeurs :param game_state: L'état du monde pour le jeu en cours :param player: Le joueur executant la tactic :param target: Pose général pouvant être utilisé par les classes enfants comme elles veulent """ assert isinstance(game_state, GameState), "Le game_state doit être un GameState" assert isinstance( player, OurPlayer), "Le player doit être un OurPlayer {}".format(player) assert isinstance(target, Pose), "La target devrait être une Pose" self.game_state = game_state self.player = player self.player_id = player.id if args is None: self.args = [] else: self.args = args self.current_state = self.halt self.next_state = self.halt self.status_flag = Flags.INIT self.target = target self.initialized = True
def __init__(self, game_state: GameState, player: OurPlayer, **other_args: Dict): """ :param game_state: L'état courant du jeu. :param player: Identifiant du joueur qui s'arrête """ Action.__init__(self, game_state, player) self.other_args = { "dribbler_on": other_args.get("dribbler_on", False), "control_loop_type": other_args.get("control_loop_type", AIControlLoopType.POSITION), "pathfinder_on": other_args.get("pathfinder_on", False), "kick_strength": other_args.get("kick_strength", 0), "charge_kick": other_args.get("charge_kick", False), "kick": other_args.get("kick", False), "pose_goal": other_args.get("pose_goal", Pose()) } self.ai_command_type = other_args.get("ai_command_type", AICommandType.STOP) # this is for the pathfinder only no direct assignation # TODO put that correctly self.path = other_args.get("path", [])
def __init__(self, game_state: GameState, player: OurPlayer, target: Pose = Pose(), args=None): super().__init__(game_state, player, target, args) self.status_flag = Flags.SUCCESS