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'])