def player_covered_from_goal(player: Player):
shooting_angle = angle_between_three_points(GameState().field.their_goal_line.p1,
player.position, GameState().field.their_goal_line.p2)
vec_player_to_goal = GameState().field.their_goal - player.position
our_team = [other_player for other_player in GameState().our_team.available_players.values() if other_player is not player]
enemy_team = [other_player for other_player in GameState().enemy_team.available_players.values()]
pertinent_collisions = []
for other_player in our_team + enemy_team:
if object_pointing_toward_point(player.position,
vec_player_to_goal.angle,
other_player.position,
wrap_to_pi(shooting_angle + 5 * np.pi / 180)):
pertinent_collisions.append(Obstacle(other_player.position.array, avoid_distance=90))
if not any(pertinent_collisions):
return GameState().field.their_goal
pertinent_collisions_positions = np.array([obs.position for obs in pertinent_collisions])
pertinent_collisions_avoid_radius = np.array([obs.avoid_distance for obs in pertinent_collisions])
results = []
for i in range(0, 15 + 1): # discretisation de la ligne de but
goal_point = GameState().field.their_goal_line.p1 + GameState().field.their_goal_line.direction * \
(GameState().field.their_goal_line.length * i / 15)
is_colliding = is_path_colliding(pertinent_collisions, pertinent_collisions_positions,
pertinent_collisions_avoid_radius, player.position.array, goal_point.array)
results.append((is_colliding, goal_point))
max_len_seg, indexend = find_max_consecutive_bool(results)
if max_len_seg == 0 and indexend == 0:
return None
return results[int(indexend-1 - np.math.ceil(max_len_seg / 2))][1]
def compute_turn_radius(
p1,
p2,
p3,
speed: float,
max_deviation: float = 50,
acc: float = MAX_LINEAR_ACCELERATION) -> Tuple[float, float]:
"""Assume the raw path is p1->p2->p3.
Deviation is compute from p2 to the circle with a line passing by the center of the circle."""
radius_at_const_speed = speed**2 / acc
path_angle = wrap_to_pi((p3 - p2).angle - (p1 - p2).angle)
const_speed_deviation = deviation(radius_at_const_speed, path_angle)
if const_speed_deviation < max_deviation:
speed_deviation = const_speed_deviation
turn_radius = radius_at_const_speed
else:
speed *= 0.95
radius = speed**2 / acc
speed_deviation = deviation(radius, path_angle)
turn_radius = speed_deviation / (1 / sin(path_angle / 2) - 1)
while speed_deviation > max_deviation:
speed *= 0.9
radius = speed**2 / acc
speed_deviation = deviation(radius, path_angle)
turn_radius = speed_deviation / (1 / sin(path_angle / 2) - 1)
if abs(speed) < 0.01:
speed_deviation = 0
turn_radius = 0
break
return abs(turn_radius), speed_deviation
def execute(self, err: float) -> float:
current_time = time()
if not self.last_time:
self.last_time = current_time
return err * self.kp
dt, self.last_time = current_time - self.last_time, current_time
if self.signed_error and (abs(err) > np.pi):
d_err = err - self.last_err
d_err = copysign(d_err, wrap_to_pi(d_err))
else:
d_err = err - self.last_err
self.last_err = err
self.err_sum += err
if self.anti_windup:
self.error_deque.append((err, dt))
self.anti_windup_time += dt
while self.anti_windup_time > self.anti_windup_max_time:
old_err, old_dt = self.error_deque.popleft()
self.anti_windup_time -= old_dt
self.err_sum -= old_err
kd = 0 if abs(err) < self.deadzone else self.kd
return (err * self.kp) + (self.err_sum * self.ki * dt) + (d_err * kd /
dt)
def compute_turn_radius(p1, p2, p3, speed: float, max_deviation: float=50, acc: float=MAX_LINEAR_ACCELERATION) -> Tuple[float, float]:
"""Assume the raw path is p1->p2->p3.
Deviation is compute from p2 to the circle with a line passing by the center of the circle."""
radius_at_const_speed = speed ** 2 / acc
path_angle = wrap_to_pi((p3 - p2).angle - (p1 - p2).angle)
const_speed_deviation = deviation(radius_at_const_speed, path_angle)
if const_speed_deviation < max_deviation:
speed_deviation = const_speed_deviation
turn_radius = radius_at_const_speed
else:
speed *= 0.95
radius = speed ** 2 / acc
speed_deviation = deviation(radius, path_angle)
turn_radius = speed_deviation / (1 / sin(path_angle / 2) - 1)
while speed_deviation > max_deviation:
speed *= 0.9
radius = speed ** 2 / acc
speed_deviation = deviation(radius, path_angle)
turn_radius = speed_deviation / (1 / sin(path_angle / 2) - 1)
if abs(speed) < 0.01:
speed_deviation = 0
turn_radius = 0
break
return abs(turn_radius), speed_deviation
def _change_frame_side(detection_frame: Dict[str, List[Dict[str, Any]]]) -> Dict[str, List[Dict[str, Any]]]:
for robot_obs in detection_frame.get('robots_blue', []) + detection_frame.get('robots_yellow', []):
robot_obs['x'] *= -1
robot_obs['orientation'] = wrap_to_pi(np.pi - robot_obs['orientation'])
for ball_obs in detection_frame.get('balls', ()):
ball_obs['x'] *= -1
return detection_frame
def mirror_x(self):
return Pose.from_values(-self.x, self.y, geometry.wrap_to_pi(np.pi - self.orientation))
def orientation_error(self) -> Optional[float]:
if self.target_orientation is not None and self.orientation is not None:
return wrap_to_pi(self.target_orientation - self.orientation)
def test_wrap_to_pi_with_angle_greater_than_pi():
assert wrap_to_pi(
AN_ANGLE_GREATER_THAN_PI) == AN_ANGLE_GREATER_THAN_PI - 2 * m.pi
def test_wrap_to_pi_with_angle_less_than_pi():
assert wrap_to_pi(AN_ANGLE_LESS_THAN_PI) == AN_ANGLE_LESS_THAN_PI
def orientation_error(self) -> Optional[float]:
if self.target_orientation is not None and self.orientation is not None:
return wrap_to_pi(self.target_orientation - self.orientation)
def GoBehind(player: Player, position1: Position, position2: Optional[Position]=None,
distance_behind: float=250,
orientation: str= 'front'):
"""
Action GoBehind: Déplace le robot au point le plus proche sur la droite, derrière un objet dont la position
est passée en paramètre, de sorte que cet objet se retrouve entre le robot et la seconde position passée
en paramètre
Méthodes :
exec(self): Retourne la pose où se rendre
Attributs (en plus de ceux de Action):
player_id : L'identifiant du joueur
position1 : La position de l'objet derrière lequel le robot doit se placer (exemple: le ballon)
position2 : La position par rapport à laquelle le robot doit être "derrière" l'objet de la position 1
(exemple: le but)
"""
delta_x = position2.x - position1.x
delta_y = position2.y - position1.y
theta = math.atan2(delta_y, delta_x)
x = position1.x - distance_behind * math.cos(theta)
y = position1.y - distance_behind * math.sin(theta)
player_x = player.pose.position.x
player_y = player.pose.position.y
norm_player_2_position2 = math.sqrt((player_x - position2.x) ** 2+(player_y - position2.y) ** 2)
norm_position1_2_position2 = math.sqrt((position1.x - position2.x) ** 2 +
(position1.y - position2.y) ** 2)
if norm_player_2_position2 < norm_position1_2_position2:
# on doit contourner l'objectif
vecteur_position1_2_position2 = np.array([position1.x - position2.x,
position1.y - position2.y, 0])
vecteur_vertical = np.array([0, 0, 1])
vecteur_player_2_position1 = np.array([position1.x - player_x,
position1.y - player_y, 0])
vecteur_perp = np.cross(vecteur_position1_2_position2, vecteur_vertical)
vecteur_perp /= np.linalg.norm(vecteur_perp)
if np.dot(vecteur_perp, vecteur_player_2_position1) > 0:
vecteur_perp = -vecteur_perp
position_intermediaire_x = x + vecteur_perp[0] * RAYON_AVOID
position_intermediaire_y = y + vecteur_perp[1] * RAYON_AVOID
if math.sqrt((player_x-position_intermediaire_x)**2+(player_y-position_intermediaire_y)**2) < 50:
position_intermediaire_x += vecteur_perp[0] * RAYON_AVOID * 2
position_intermediaire_y += vecteur_perp[1] * RAYON_AVOID * 2
destination_position = Position(position_intermediaire_x, position_intermediaire_y)
else:
if math.sqrt((player_x-x)**2+(player_y-y)**2) < 50:
x -= math.cos(theta) * 2
y -= math.sin(theta) * 2
destination_position = Position(x, y)
# Calcul de l'orientation de la pose de destination
destination_orientation = 0
if orientation == 'front':
destination_orientation = wrap_to_pi((position1 - destination_position).angle)
elif orientation == 'back':
destination_orientation = wrap_to_pi((position1 - destination_position).angle + np.pi)
destination_pose = Pose(destination_position, destination_orientation)
return MoveTo(destination_pose)
def test_wrap_to_pi_with_angle_greater_than_pi():
assert wrap_to_pi(AN_ANGLE_GREATER_THAN_PI) == AN_ANGLE_GREATER_THAN_PI - 2*m.pi
def test_wrap_to_pi_with_angle_less_than_pi():
assert wrap_to_pi(AN_ANGLE_LESS_THAN_PI) == AN_ANGLE_LESS_THAN_PI
