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 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 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 test_normalize_with_zero_posisiton():
with pytest.raises(ZeroDivisionError):
normalize(A_ZERO_POS)
def test_normalize_return_type():
assert isinstance(normalize(A_POS), Position)
def test_normalize_identity():
assert normalize(A_POS) is not A_POS
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
