def find_closest_obstacle(self, point, path):
dist_point_obs = np.inf
closest_obs = None
closest_player = self.players_obstacles[0].pose.position.conv_2_np()
if get_distance(path.start, path.goal) < 0.001:
return [closest_obs, dist_point_obs, closest_player]
if point == path.start:
return [closest_obs, dist_point_obs, closest_player]
pose_start = path.start.conv_2_np()
direction = (point.conv_2_np() - pose_start
) / np.linalg.norm(point.conv_2_np() - pose_start)
for idx, pose_obs in enumerate(self.pose_obstacle):
vec_robot_2_obs_temp = pose_obs - pose_start
dist_from_path_temp = np.linalg.norm(
np.cross(direction, vec_robot_2_obs_temp))
if self.gap_proxy > dist_from_path_temp and self.is_path_collide(
path, [pose_obs]):
obstacle_pos = Position.from_np(pose_obs)
dist = get_distance(path.start, obstacle_pos)
if dist < dist_point_obs:
dist_point_obs = dist
closest_obs = obstacle_pos
closest_player = self.players_obstacles[idx]
return [closest_obs, dist_point_obs, closest_player]
def find_closest_obstacle(self, point, path):
dist_point_obs = np.inf
closest_obs = None
closest_player = self.players_obstacles[0].pose.position.conv_2_np()
if get_distance(path.start, path.goal) < 0.001:
return closest_obs, dist_point_obs, closest_player
if point == path.start:
return closest_obs, dist_point_obs, closest_player
pose_start = path.start.conv_2_np()
direction = (point.conv_2_np() - pose_start
) / np.linalg.norm(point.conv_2_np() - pose_start)
for idx, pose_obs in enumerate(self.pose_obstacle):
vec_robot_2_obs_temp = pose_obs - pose_start
len_along_path_temp = np.dot(vec_robot_2_obs_temp, direction)
dist_from_path_temp = np.sqrt(
np.linalg.norm(vec_robot_2_obs_temp)**2 -
len_along_path_temp**2)
if self.gap_proxy > dist_from_path_temp and len_along_path_temp > 0:
obstacle_pos = Position.from_np(pose_obs)
dist = get_distance(path.start, obstacle_pos)
if dist < dist_point_obs:
dist_point_obs = dist
closest_obs = obstacle_pos
# print(len(self.players_obstacles), " -> ", idx)
closest_player = self.players_obstacles[idx]
return closest_obs, dist_point_obs, closest_player
def switch_target(self):
player_position = self.game_state.get_player_position(self.player_id)
distance_end = get_distance(player_position, self.end.position)
distance_start = get_distance(player_position, self.start.position)
if distance_end < distance_start:
self.target = self.start
else:
self.target = self.end
def passBallAction(self, i):
bot_pst = self.team.players[i].pose.position
tar_pst = self.field.ball.position
goal_pst = self.team.players[not i].pose.position + Position(-50, 0)
next_pst = self.comeBehind(bot_pst, tar_pst, goal_pst)
agl = get_angle(bot_pst, goal_pst)
agl2 = get_angle(bot_pst, tar_pst)
if get_distance(tar_pst, bot_pst) < 130 and -0.045 < agl - agl2 < 0.045:
kick_f = get_distance(goal_pst, bot_pst) // 500
self.kickAction(i, force=kick_f)
else:
self._send_command(Command.MoveToAndRotate(self.team.players[i], self.team, Pose(next_pst, agl)))
def _simple_advance_path(self):
current_ai_cmd = self.ws.play_state.current_ai_commands
for ai_cmd in current_ai_cmd.values():
if ai_cmd.path:
current_pose = self.ws.game_state.get_player_pose(ai_cmd.robot_id)
next_position = ai_cmd.path[0]
distance = get_distance(current_pose.position, next_position)
while distance < PATHFINDER_DEADZONE and len(ai_cmd.path) > 1:
self.ws.debug_interface.add_log(1, "Gestion path; retrait point trop rapproche.")
ai_cmd.path = ai_cmd.path[1:]
next_position = ai_cmd.path[0]
distance = get_distance(current_pose.position, next_position)
ai_cmd.pose_goal = Pose(next_position, ai_cmd.pose_goal.orientation)
def _bougerPlusAim(self, joueur):
#TODO : ajuster la deadzone en fonction du type du goal
destination = self._convertirPosition(self.robot_goals[joueur])
cible = self._convertirPosition(self.robot_aim[joueur])
deadzone = 225 #225 est la deadzone nécessaire pour que le robot ne fonce pas dans la balle
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, destination)
angle = m.fabs(
geometry.get_angle(player.pose.position, cible) -
player.pose.orientation) #angle between the robot and the ball
if (0 < dist <= deadzone and 0 < angle <= 0.09
): #0.087 rad = 5 deg : marge d'erreur de l'orientation
self._succeed(joueur)
elif (dist > deadzone and 0 < angle <= 0.09):
command = Command.MoveTo(player, self.team, destination)
self._send_command(command)
elif (0 < dist <= deadzone and angle > 0.09):
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.Rotate(player, self.team, orientation)
self._send_command(command)
else:
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.MoveToAndRotate(
player, self.team, Pose(destination, orientation))
self._send_command(command)
"""
def _find_intermediate_target(self, robot_id, path):
default_target = path[0]
player_pst = self.ws.game_state.get_player_pose(robot_id).position
for target in path:
if get_distance(player_pst, target) > INTERMEDIATE_DISTANCE_THRESHOLD:
return target
return default_target
def check_success(self):
player_position = self.game_state.get_player_position(self.player_id)
distance = get_distance(player_position, self.target.position)
if distance < self.game_state.const["POSITION_DEADZONE"]:
return True
return False
def check_success(self):
player_pose = self.player.pose
distance = get_distance(player_pose.position, self.target.position)
if distance < POSITION_DEADZONE and \
m.fabs(player_pose.orientation - self.target.orientation) <= 2*ANGLE_TO_HALT:
return True
return False
def calculate_path(self):
self.initial_position = self.player.pose.position
self.initial_orientation = self.player.pose.orientation
self.initial_distance = get_distance(self.origin, self.initial_position)
self.initial_angle = get_angle(self.origin, self.initial_position)
self.target_angle = get_angle(self.origin, self.target.position)
self.delta_angle = (self.target_angle + m.pi - self.initial_angle) % (2 * m.pi)
if self.delta_angle > m.pi:
self.delta_angle -= 2 * m.pi
if m.fabs(self.delta_angle) >= self.ANGLE_INCREMENT:
self.num_points = int(
m.fabs(self.delta_angle) // self.ANGLE_INCREMENT) # incrément d'environ pi/8 radians (22.5 degrés)
self.angle_increment = self.delta_angle / self.num_points
else:
self.num_points = 1
self.angle_increment = self.delta_angle
for i in range(self.num_points):
pos = rotate_point_around_origin(self.initial_position, self.origin, (i + 1) * self.angle_increment)
theta = self.initial_orientation + (i + 1) * self.angle_increment
self.pose_list.append(Pose(pos, theta))
self.next_state = self.rotate_around
return MoveToPosition(self.game_state, self.player, self.pose_list[self.index])
def calculate_path(self):
self.initial_position = self.game_state.get_player_position(self.player_id)
self.initial_orientation = self.game_state.get_player_pose(self.player_id).orientation
self.initial_distance = get_distance(self.origin, self.initial_position)
self.initial_angle = get_angle(self.origin, self.initial_position)
self.target_angle = get_angle(self.origin, self.target.position)
self.delta_angle = (self.target_angle + m.pi - self.initial_angle) % (2 * m.pi)
if self.delta_angle > m.pi:
self.delta_angle -= 2 * m.pi
if m.fabs(self.delta_angle) >= self.ANGLE_INCREMENT:
self.num_points = int(
m.fabs(self.delta_angle) // self.ANGLE_INCREMENT) # incrément d'environ pi/8 radians (22.5 degrés)
self.angle_increment = self.delta_angle / self.num_points
else:
self.num_points = 1
self.angle_increment = self.delta_angle
for i in range(self.num_points):
pos = rotate_point_around_origin(self.initial_position, self.origin, (i + 1) * self.angle_increment)
theta = self.initial_orientation + (i + 1) * self.angle_increment
self.pose_list.append(Pose(pos, theta))
self.next_state = self.rotate_around
return MoveToPosition(self.game_state, self.player_id, self.pose_list[self.index])
def check_success(self):
player_pose = self.game_state.get_player_pose(player_id=self.player_id)
distance = get_distance(player_pose.position, self.target.position)
if distance < POSITION_DEADZONE and \
m.fabs(player_pose.orientation - self.target.orientation) <= 2*ANGLE_TO_HALT:
return True
return False
def __init__(self,
p_game_state,
p_player_id,
p_position1,
p_position2,
p_target,
p_minimum_distance=0):
"""
:param p_game_state: L'état courant du jeu.
:param p_player_id: Identifiant du joueur qui doit se déplacer
:param p_position1: La première position formant la droite
:param p_position2: La deuxième position formant la droite
:param p_target: La position vers laquelle le robot devrait s'orienter
:param p_minimum_distance: La distance minimale qu'il doit y avoir entre le robot et chacun des points
"""
Action.__init__(self, p_game_state)
assert (isinstance(p_player_id, int))
assert PLAYER_PER_TEAM >= p_player_id >= 0
assert (isinstance(p_position1, Position))
assert (isinstance(p_position2, Position))
assert (isinstance(p_target, Position))
assert (isinstance(p_minimum_distance, (int, float)))
assert (get_distance(p_position1, p_position2) >
2 * p_minimum_distance)
self.player_id = p_player_id
self.position1 = p_position1
self.position2 = p_position2
self.target = p_target
self.minimum_distance = p_minimum_distance
def verify_sub_target(self, sub_target):
for pose_obs in self.pose_obstacle:
dist_sub_2_obs = get_distance(Position.from_np(pose_obs),
sub_target)
if dist_sub_2_obs < self.gap_proxy:
return True
return False
def check_success(self):
player_position = self.game_state.get_player_position(
player_id=self.player_id)
distance = get_distance(player_position, self.target.position)
if distance < POSITION_DEADZONE:
return True
return False
def raycast2(game_state, initial_position, final_position, width,
blue_players_ignored, yellow_players_ignored,
is_ball_ignored=True):
"""
Retourne si le rayon tracé d'une position initiale à une position
finale provoque une collision.
Args:
`game_state`: L'état du jeu
`initial_position`: Position de départ du rayon
`final_position`: Position finale du rayon
`width`: largeur du rayon
`blue_players_ignored`: vrai si on ignore les joueurs bleu
`yellow_players_ignored`: vrai si on ignore les joueurs jaune
`is_ball_ignored`: (default=True) vrai si on ignore la balle
"""
assert isinstance(game_state, GameState)
assert isinstance(initial_position, Position)
assert isinstance(final_position, Position)
assert isinstance(width, (int, float))
assert isinstance(blue_players_ignored, list)
assert isinstance(yellow_players_ignored, list)
assert isinstance(is_ball_ignored, bool)
for i in range(0, PLAYER_PER_TEAM):
if i not in blue_players_ignored:
player_position = game_state.get_player_pose(i).position
pos = get_closest_point_on_line(player_position, initial_position,
final_position)
if get_distance(player_position, pos) <= width + ROBOT_RADIUS:
return True
if i not in yellow_players_ignored:
enemy_position = game_state.get_player_pose(i, False).position
pos = get_closest_point_on_line(enemy_position, initial_position,
final_position)
if get_distance(enemy_position, pos) <= width + ROBOT_RADIUS:
return True
if not is_ball_ignored:
ball_position = game_state.get_ball_position()
pos = get_closest_point_on_line(ball_position, initial_position,
final_position)
if get_distance(ball_position, pos) <= width + BALL_RADIUS:
return True
return False
def _collision(self, pos, pos2=None, max_distance=ROBOT_RADIUS):
""" Logique pour la collision. """
logging.info('Number of object to check: %d', len(self.field_objects))
if pos2 is None:
for i in self.field_objects:
obj_pos = i
distance = get_distance(pos, obj_pos)
logging.info('Distance is %d', distance)
if distance < 2 * max_distance:
return True
return False
else:
distance = get_distance(pos, pos2)
if distance < 2 * max_distance:
return True
else:
return False
def _collision(self, pos, pos2=None, max_distance=ROBOT_RADIUS):
""" Logique pour la collision. """
logging.info('Number of object to check: %d', len(self.field_objects))
if pos2 is None:
for i in self.field_objects:
obj_pos = i
distance = get_distance(pos, obj_pos)
logging.info('Distance is %d', distance)
if distance < 2*max_distance:
return True
return False
else:
distance = get_distance(pos, pos2)
if distance < 2*max_distance:
return True
else:
return False
def comeBehind(bot, tar, obj):
result_pst = Position()
aglTar2Obj = get_angle(tar, obj)
aglBot2Tar = get_angle(tar, bot)
diffAglBot2Obj = aglBot2Tar - aglTar2Obj
if get_distance(bot, tar) > 130:
if 0 <= diffAglBot2Obj < 1.57:
result_pst = stayOutsideCircle(tar, obj, get_distance(obj, tar) + 500)
result_pst.x = result_pst.x + 500 * math.cos(aglTar2Obj + 1.57)
result_pst.y = result_pst.y + 500 * math.sin(aglTar2Obj + 1.57)
elif -1.57 <= diffAglBot2Obj < 0:
result_pst = stayOutsideCircle(tar, obj, get_distance(obj, tar) + 500)
result_pst.x = result_pst.x + 500 * math.cos(aglTar2Obj - 1.57)
result_pst.y = result_pst.y + 500 * math.sin(aglTar2Obj - 1.57)
elif 1.57 <= diffAglBot2Obj < 2.9:
result_pst = stayOutsideCircle(tar, obj, get_distance(obj, tar) + 500)
elif -2.9 <= diffAglBot2Obj < -1.57:
result_pst = stayOutsideCircle(tar, obj, get_distance(obj, tar) + 500)
else:
result_pst = stayOutsideCircle(tar, obj, get_distance(tar, obj) + 90)
else:
result_pst = stayOutsideCircle(tar, obj, get_distance(tar, obj))
return result_pst
def forwarder(self, i):
bot_pst = self.team.players[i].pose.position
bot_dir = self.team.players[i].pose.orientation
ball_pst = self.field.ball.position
goal_pst = self.opponent_team.players[0].pose.position
agl = get_angle(bot_pst, ball_pst)
dst = get_distance(bot_pst, ball_pst)
enemy_dst_top = get_distance(goal_pst, Position(-4500, 800))
enemy_dst_bot = get_distance(goal_pst, Position(-4500, -800))
if time.time() > self.beginTime + 12:
self.kickBallAction(i)
elif enemy_dst_bot > enemy_dst_top:
self.kickBallAction(i) if i else self.passBallAction(i)
else:
self.passBallAction(i) if i else self.kickBallAction(i)
def move_to_ball(self):
self.status_flag = Flags.WIP
self.target = Pose(self.game_state.get_ball_position())
if get_distance(self.player.pose.position, self.target.position) < POSITION_DEADZONE + ROBOT_RADIUS:
self.next_state = self.halt
else:
self.next_state = self.move_to_ball
return PathfindToPosition(self.game_state, self.player, self.target)
def halt(self):
self.status_flag = Flags.SUCCESS
if get_distance(self.player.pose.position, self.game_state.get_ball_position()) < \
POSITION_DEADZONE + ROBOT_RADIUS:
self.next_state = self.halt
else:
self.next_state = self.move_to_ball
return Idle(self.game_state, self.player)
def kickBallAction(self, i):
bot_pst = self.team.players[i].pose.position
tar_pst = self.field.ball.position
goal_pst = Position(-4500, -150) if i else Position(-4500, 150)
next_pst = self.comeBehind(bot_pst, tar_pst, goal_pst)
agl = get_angle(bot_pst, goal_pst)
agl2 = get_angle(bot_pst, tar_pst)
if get_distance(tar_pst, bot_pst) < 130 and -0.045 < agl - agl2 < 0.045 and tar_pst.x < -1000:
self.kickAction(i, force=8)
else:
self._send_command(Command.MoveToAndRotate(self.team.players[i], self.team, Pose(next_pst, agl)))
def halt(self, reset=False):
self.status_flag = Flags.SUCCESS
stop = Idle(self.game_state, self.player_id)
if get_distance(self.game_state.get_player_pose(self.player_id).position, self.game_state.get_ball_position()) \
< POSITION_DEADZONE + BALL_RADIUS:
self.next_state = self.halt
else:
self.next_state = self.move_to_ball
return stop
def halt(self, reset=False):
stop = Idle(self.game_state, self.player_id)
if get_distance(
self.game_state.get_player_pose(self.player_id).position,
self.game_state.get_ball_position()
) < POSITION_DEADZONE + BALL_RADIUS:
self.next_state = self.halt
else:
self.next_state = self.move_to_ball
return stop
def move_to_ball(self):
self.target = Pose(self.game_state.get_ball_position())
move = MoveTo(self.game_state, self.player_id, self.target)
if get_distance(
self.game_state.get_player_pose(self.player_id).position,
self.target.position) < POSITION_DEADZONE + BALL_RADIUS:
self.next_state = self.halt
else:
self.next_state = self.move_to_ball
return move
def move_to_ball(self):
self.status_flag = Flags.WIP
self.target = Pose(self.game_state.get_ball_position())
move = MoveToPosition(self.game_state, self.player_id, self.target, FOLLOW_SPEED)
if get_distance(self.game_state.get_player_pose(self.player_id).position, self.target.position) <\
POSITION_DEADZONE + BALL_RADIUS:
self.next_state = self.halt
else:
self.next_state = self.move_to_ball
return move
def _simple_advance_path(self):
current_ai_cmd = self.ws.play_state.current_ai_commands
for ai_cmd in current_ai_cmd.values():
if ai_cmd.path:
current_pose = self.ws.game_state.get_player_pose(
ai_cmd.robot_id)
next_position = ai_cmd.path[0]
distance = get_distance(current_pose.position, next_position)
# if ai_cmd.path[-1] == next_position:
# pathfinder_dead_zone = 10
# else:
# pathfinder_dead_zone = 150
while distance < 10 and len(ai_cmd.path) > 1:
self.ws.debug_interface.add_log(
1, "Gestion path; retrait point trop rapproche.")
ai_cmd.path = ai_cmd.path
next_position = ai_cmd.path[0]
distance = get_distance(current_pose.position,
next_position)
ai_cmd.pose_goal = Pose(next_position,
ai_cmd.pose_goal.orientation)
def _bouger(self, joueur):
#TODO: ajuster la deadzone en fonction du type du goal
position = self._convertirPosition(self.robot_goals[joueur])
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, position)
deadzone = self._getDeadZone(self.robot_goals[joueur])
if dist < deadzone: # si la distance est exactement 0, la position n'est pas bonne
self._succeed(joueur)
else:
orientation = player.pose.orientation
command = Command.MoveToAndRotate(player, self.team,
Pose(position, orientation))
self._send_command(command)
def _bouger(self, joueur):
#TODO: ajuster la deadzone en fonction du type du goal
position = self._convertirPosition(self.robot_goals[joueur])
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, position)
deadzone = self._getDeadZone(self.robot_goals[joueur])
if dist < deadzone: # si la distance est exactement 0, la position n'est pas bonne
self._succeed(joueur)
else:
orientation = player.pose.orientation
command = Command.MoveToAndRotate(player, self.team,
Pose(position, orientation))
self._send_command(command)
def _bouger(self, joueur):
#TODO: ajuster la deadzone en fonction du type du goal
position = self._convertirPosition(self.robot_goals[joueur])
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, position)
print(dist)
deadzone = 225 #225 est la deadzone nécessaire pour que le robot ne fonce pas dans la balle
if 0 < dist < deadzone: # si la distance est exactement 0, la position n'est pas bonne
self._succeed(joueur)
else:
orientation = player.pose.orientation
command = Command.MoveToAndRotate(player, self.team,
Pose(position, orientation))
self._send_command(command)
def _bouger(self, joueur):
#TODO: ajuster la deadzone en fonction du type du goal
position = self._convertirPosition(self.robot_goals[joueur])
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, position)
print(dist)
deadzone = 225 #225 est la deadzone nécessaire pour que le robot ne fonce pas dans la balle
if 0 < dist < deadzone: # si la distance est exactement 0, la position n'est pas bonne
self._succeed(joueur)
else:
orientation = player.pose.orientation
command = Command.MoveToAndRotate(player, self.team,
Pose(position, orientation))
self._send_command(command)
def move_to_ball(self):
self.status_flag = Flags.WIP
self.target = self.game_state.get_player_pose(self.robot_to_follow_id)
if get_distance(
self.player.pose.position,
self.target.position) < POSITION_DEADZONE + ROBOT_RADIUS:
self.next_state = self.halt
else:
self.next_state = self.move_to_ball
return PathfindToPosition(self.game_state,
self.player,
self.target,
cruise_speed=2)
def push_ball(self):
# self.debug.add_log(1, "Grab ball called")
# self.debug.add_log(1, "vector player 2 ball : {} mm".format(self.vector_norm))
if get_distance(self.last_ball_position, self.player.pose.position) < 40:
self.next_state = self.halt
self.last_time = time.time()
elif self._is_player_opposing_ball_and_target(-0.9):
self.next_state = self.push_ball
else:
self.next_state = self.get_behind_ball
# self.debug.add_log(1, "orientation go get ball {}".format(self.last_angle))
target = self.target.position.conv_2_np()
player = self.player.pose.position.conv_2_np()
player_to_target = target - player
player_to_target = 0.5 * player_to_target / np.linalg.norm(player_to_target)
speed_pose = Pose(Position.from_np(player_to_target))
return Move(self.game_state, self.player, speed_pose)
def _lance_position(self, joueur):
player = self.team.players[joueur]
robot = self._convertirPosition(player)
balle = self._convertirPosition(self.field.ball)
cible = self._convertirPosition(self.robot_aim[joueur])
dist = geometry.get_distance(robot, balle)
lim_dist = dist*0.5
deadzone = lim_dist if lim_dist > 125 else 60
angle = m.fabs(geometry.get_angle(robot, cible) - player.pose.orientation)
if angle > 0.3:
deadzone = max(deadzone, 200)
#print(deadzone)
angle = m.atan2(robot.y-cible.y,
robot.x-cible.x)
x = balle.x + deadzone*m.cos(angle)
y = balle.y + deadzone*m.sin(angle)
return Position(x, y)
def getRequiredKickForce(position1: Position, position2: Position):
"""
Calcul la force nécessaire pour que le botteur du joueur puisse envoyer
la balle à destination. On assume que les joueur est capable de botter
la balle
Args:
position1: La position du joueur
position2: La destination
Returns:
La force nécessaire, en mm/s.
"""
assert isinstance(position1, Position)
assert isinstance(position2, Position)
distance = get_distance(position1, position2)
max_field_distance_possible = m.sqrt((FIELD_X_RIGHT - FIELD_X_LEFT)**2 +
(FIELD_Y_TOP - FIELD_Y_BOTTOM)**2)
kick_force = distance * KICK_MAX_SPD / max_field_distance_possible
return kick_force
def getRequiredKickForce(position1: Position, position2: Position):
"""
Calcul la force nécessaire pour que le botteur du joueur puisse envoyer
la balle à destination. On assume que les joueur est capable de botter
la balle
Args:
position1: La position du joueur
position2: La destination
Returns:
La force nécessaire, en mm/s.
"""
assert isinstance(position1, Position)
assert isinstance(position2, Position)
distance = get_distance(position1, position2)
max_field_distance_possible = m.sqrt((FIELD_X_RIGHT - FIELD_X_LEFT)**2 +
(FIELD_Y_TOP - FIELD_Y_BOTTOM)**2)
kick_force = distance * KICK_MAX_SPD / max_field_distance_possible
return kick_force
def _lance_position(self, joueur):
player = self.team.players[joueur]
robot = self._convertirPosition(player)
balle = self._convertirPosition(self.field.ball)
cible = self._convertirPosition(self.robot_aim[joueur])
dist = geometry.get_distance(robot, balle)
lim_dist = dist*0.5
deadzone = lim_dist if lim_dist > 125 else 60
angle = m.fabs(geometry.get_angle(robot, cible) - player.pose.orientation)
if angle > 0.3:
deadzone = max(deadzone, 200)
#print(deadzone)
angle = m.atan2(robot.y-cible.y,
robot.x-cible.x)
x = balle.x + deadzone*m.cos(angle)
y = balle.y + deadzone*m.sin(angle)
return Position(x, y)
def is_path_collide(self, path: Path):
for i in range(len(path.points) - 1):
dist = get_distance(path.points[i],
path.points[i + 1]) # int, float
if dist > 0.001:
pose_start = path.points[i].conv_2_np() # np.ndarray(2)
direction = (path.points[i + 1].conv_2_np() -
pose_start) / np.linalg.norm(
path.points[i + 1].conv_2_np() - pose_start)
for pose_obs in self.pose_obstacle:
vec_robot_2_obs_temp = pose_obs - pose_start # np.ndarray(2)
len_along_path_temp = np.dot(vec_robot_2_obs_temp,
direction)
dist_from_path_temp = np.sqrt(
np.linalg.norm(vec_robot_2_obs_temp)**2 -
len_along_path_temp**2)
if self.gap_proxy > dist_from_path_temp:
if len_along_path_temp > 0:
return True
return False
def _bougerPlusAim(self, joueur, deadzone=None):
destination = self._convertirPosition(self.robot_goals[joueur])
cible = self._convertirPosition(self.robot_aim[joueur])
if not deadzone:
deadzone = self._getDeadZone(self.robot_goals[joueur])
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, destination)
angle = m.fabs(geometry.get_angle(player.pose.position, cible) - player.pose.orientation) #angle between the robot and the ball
if(dist <= deadzone and angle <= 0.01): #0.087 rad = 5 deg : marge d'erreur de l'orientation
self._succeed(joueur)
elif(dist > deadzone and angle <= 0.01):
command = Command.MoveTo(player, self.team, destination)
self._send_command(command)
elif(dist <= deadzone and angle > 0.01):
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.Rotate(player, self.team, orientation)
self._send_command(command)
else:
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.MoveToAndRotate(player, self.team, Pose(destination, orientation))
self._send_command(command)
def _bougerPlusAim(self, joueur, deadzone=None):
destination = self._convertirPosition(self.robot_goals[joueur])
cible = self._convertirPosition(self.robot_aim[joueur])
if not deadzone:
deadzone = self._getDeadZone(self.robot_goals[joueur])
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, destination)
angle = m.fabs(geometry.get_angle(player.pose.position, cible) - player.pose.orientation) #angle between the robot and the ball
if(dist <= deadzone and angle <= 0.01): #0.087 rad = 5 deg : marge d'erreur de l'orientation
self._succeed(joueur)
elif(dist > deadzone and angle <= 0.01):
command = Command.MoveTo(player, self.team, destination)
self._send_command(command)
elif(dist <= deadzone and angle > 0.01):
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.Rotate(player, self.team, orientation)
self._send_command(command)
else:
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.MoveToAndRotate(player, self.team, Pose(destination, orientation))
self._send_command(command)
def _bougerPlusAim(self, joueur):
#TODO : ajuster la deadzone en fonction du type du goal
destination = self._convertirPosition(self.robot_goals[joueur])
cible = self._convertirPosition(self.robot_aim[joueur])
deadzone = 225 #225 est la deadzone nécessaire pour que le robot ne fonce pas dans la balle
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, destination)
angle = m.fabs(geometry.get_angle(player.pose.position, cible) - player.pose.orientation) #angle between the robot and the ball
if(0 < dist <= deadzone and 0 < angle <= 0.09): #0.087 rad = 5 deg : marge d'erreur de l'orientation
self._succeed(joueur)
elif(dist > deadzone and 0 < angle <= 0.09):
command = Command.MoveTo(player, self.team, destination)
self._send_command(command)
elif(0 < dist <= deadzone and angle > 0.09):
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.Rotate(player, self.team, orientation)
self._send_command(command)
else:
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.MoveToAndRotate(player, self.team, Pose(destination, orientation))
self._send_command(command)
"""
def move_bottom(self, i):
if get_distance(self.team.players[i].pose.position, self.target[i]) < 250:
self.target[i] = self.init_pst[i] + Position(-500, 0)
self.state[i] = self.move_left
def get_score(self):
return get_angle(self.centre, self.marque), get_distance(self.centre, self.marque) / self.rayon
def is_inside(self, pt):
dst = get_distance(self.centre, pt)
return dst < self.rayon
def is_cover_by(self, circle):
dst = get_distance(self.centre, circle.centre)
return circle.rayon > dst + self.rayon
def is_inside(self, pt):
dst = get_distance(self.centre, pt)
return dst < self.rayon
def get_score(self):
return get_angle(
self.centre,
self.marque), get_distance(self.centre, self.marque) / self.rayon
def _get_distance_from_ball(self):
return get_distance(self.game_state.get_player_pose(self.player_id).position,
self.game_state.get_ball_position())
def is_cover_by(self, circle):
dst = get_distance(self.centre, circle.centre)
return circle.rayon > dst + self.rayon
def search_point(self, path, avoid_dir=None):
pose_robot = path.start
pose_target = path.goal
pose_obstacle_closest, dist_point_obs, closest_player = self.find_closest_obstacle(
pose_target, path)
if pose_obstacle_closest is None:
sub_target = pose_target
return sub_target
direction = (pose_target.conv_2_np() - pose_robot.conv_2_np()
) / np.linalg.norm(pose_target.conv_2_np() -
pose_robot.conv_2_np())
vec_robot_2_obs = np.array(
conv_position_2_list(pose_obstacle_closest - pose_robot))
len_along_path = np.dot(vec_robot_2_obs, direction)
dist_from_path = np.linalg.norm(np.cross(direction, vec_robot_2_obs))
projection_obs_on_direction = np.dot(
direction, vec_robot_2_obs / np.linalg.norm(vec_robot_2_obs))
if 0 < len_along_path < get_distance(pose_target, pose_robot):
vec_perp = np.cross(np.append(direction, [0]), np.array([0, 0, 1]))
vec_perp = vec_perp[0:2] / np.linalg.norm(vec_perp)
# print(self.player.velocity)
cruise_speed = np.array(self.player.velocity[0:2])
self.closest_obs_speed = np.array(closest_player.velocity[0:2])
avoid_dir = -vec_perp
if avoid_dir is None:
avoid_dir = -vec_perp
sub_target_1 = np.array(conv_position_2_list(pose_robot)) + \
direction * len_along_path + vec_perp * self.res
sub_target_2 = np.array(conv_position_2_list(pose_robot)) + \
direction * len_along_path - vec_perp * self.res
bool_sub_target_1 = self.verify_sub_target(
Position(sub_target_1[0], sub_target_1[1]))
bool_sub_target_2 = self.verify_sub_target(
Position(sub_target_2[0], sub_target_2[1]))
while bool_sub_target_1:
sub_target_1 += vec_perp * self.res
bool_sub_target_1 = self.verify_sub_target(
Position(sub_target_1[0], sub_target_1[1]))
sub_target_1 += vec_perp * 0.01 * self.res
while bool_sub_target_2:
sub_target_2 -= vec_perp * self.res
bool_sub_target_2 = self.verify_sub_target(
Position(sub_target_2[0], sub_target_2[1]))
sub_target_2 -= vec_perp * 0.01 * self.res
if np.linalg.norm(cruise_speed) < 0.1:
sub_target = sub_target_1
elif abs(np.dot(direction, (sub_target_1 - path.start.conv_2_np()) /
np.linalg.norm(sub_target_1 - path.start.conv_2_np()))) > \
abs(np.dot(direction, (sub_target_2 - path.start.conv_2_np()) /
np.linalg.norm(sub_target_2 - path.start.conv_2_np()))):
sub_target = sub_target_1
else:
sub_target = sub_target_2
else:
# if np.dot(avoid_dir, np.transpose(vec_perp)) < 0:
# vec_perp = -vec_perp
if np.linalg.norm(avoid_dir) > 0.001:
avoid_dir /= np.linalg.norm(avoid_dir)
elif np.dot(avoid_dir, np.transpose(vec_perp)) < 0:
avoid_dir = -vec_perp
else:
avoid_dir = vec_perp
sub_target = np.array(conv_position_2_list(pose_robot)) +\
direction * len_along_path + vec_perp * self.res
bool_sub_target = self.verify_sub_target(
Position(sub_target[0], sub_target[1]))
while bool_sub_target:
sub_target -= avoid_dir * self.res
bool_sub_target = self.verify_sub_target(
Position(sub_target[0], sub_target[1]))
sub_target -= avoid_dir * 0.01 * self.res
avoid_dir = vec_perp
sub_target = Position(sub_target[0], sub_target[1])
else:
sub_target = pose_target
return [sub_target, avoid_dir]
def _reset_ttl(self):
super()._reset_ttl()
if get_distance(self.last_ball_position, self.game_state.get_ball_position()) > POSITION_DEADZONE:
self.last_ball_position = self.game_state.get_ball_position()
self.move_action = self._generate_move_to()
def _potential_field(self):
current_ai_c = self.ws.play_state.current_ai_commands
for ai_c in current_ai_c.values():
if ai_c.command == AICommandType.MOVE: # and len(ai_c.path) > 0:
force = [0, 0]
current_robot_pos = self.ws.game_state.get_player_position(ai_c.robot_id)
current_robot_velocity = self.ws.game_state.game.friends.players[ai_c.robot_id].velocity
current_robot_velocity_norm = math.sqrt(current_robot_velocity[0] ** 2 + current_robot_velocity[1] ** 2)
current_robot_velocity_angle = math.atan2(current_robot_velocity[1], current_robot_velocity[0])
for robot in self.ws.game_state.game.friends.players.values():
if robot.id != ai_c.robot_id:
dist = get_distance(current_robot_pos, robot.pose.position)
angle = math.atan2(current_robot_pos.y - robot.pose.position.y,
current_robot_pos.x - robot.pose.position.x)
projection = current_robot_velocity_norm * math.cos(current_robot_velocity_angle - angle)
rmax = projection * 1000
if dist < rmax:
try:
force[0] += 1 / dist * math.cos(angle) * projection
except ZeroDivisionError:
print("div 0 - 1")
pass
try:
force[1] += 1 / dist * math.sin(angle) * projection
except ZeroDivisionError:
print("div 0 - 2")
pass
for robot in self.ws.game_state.game.enemies.players.values():
dist = get_distance(current_robot_pos, robot.pose.position)
angle = math.atan2(current_robot_pos.y - robot.pose.position.y,
current_robot_pos.x - robot.pose.position.x)
projection = -current_robot_velocity_norm * math.cos(current_robot_velocity_angle - angle)
projection = max(0, projection)
rmax = projection * 1000
if dist < rmax:
try:
force[0] += 1 / dist * math.cos(angle) * projection
except ZeroDivisionError:
print("div 0 - 3")
pass
try:
force[1] += 1 / dist * math.sin(angle) * projection
except ZeroDivisionError:
print("div 0 - 4")
pass
# dist_goal = get_distance(current_robot_pos, ai_c.pose_goal.position)
# angle_goal = math.atan2(current_robot_pos.y - ai_c.pose_goal.position.y,
# current_robot_pos.x - ai_c.pose_goal.position.x)
c = force[0] * ROBOT_NEAR_FORCE # + (acc_goal * math.cos(angle_acc_goal))
d = force[1] * ROBOT_NEAR_FORCE # + (acc_goal * math.cos(angle_acc_goal))
angle_acc = math.atan2(d, c)
norm_acc = math.sqrt(c ** 2 + d ** 2)
acc_robot_x = norm_acc * math.cos(angle_acc)
acc_robot_y = norm_acc * math.sin(angle_acc)
ai_c.pose_goal.position.x += acc_robot_x * 1
ai_c.pose_goal.position.y += acc_robot_y * 1
def _has_reached_pose(self, pose):
same_position = get_distance(self.game_state.get_player_pose(self.player_id).position, pose.position)\
<= POSITION_DEADZONE
same_orientation = m.fabs(self.game_state.get_player_pose(self.player_id).orientation - pose.orientation)\
<= ANGLE_TO_HALT
return same_position and same_orientation
def _reset_ttl(self):
super()._reset_ttl()
if get_distance(self.last_ball_position, self.game_state.get_ball_position()) > POSITION_DEADZONE:
self.last_ball_position = self.game_state.get_ball_position()
self.move_action = self._generate_move_to()
def move_top(self, i):
if get_distance(self.team.players[i].pose.position, self.target[i]) < 250:
self.target[i] = self.init_pst[i] + Position(500, 0)
self.state[i] = self.move_right
def _get_distance_from_ball(self):
return get_distance(self.player.pose.position,
self.game_state.get_ball_position())
def exec(self):
"""
Calcul le point le plus proche du robot sur la droite entre les deux positions
:return: Un tuple (Pose, kick) où Pose est la destination du joueur et kick est nul (on ne botte pas)
"""
robot_position = self.game_state.get_player_pose(
self.player_id).position
delta_x = self.position2.x - self.position1.x
delta_y = self.position2.y - self.position1.y
if delta_x != 0 and delta_y != 0: # droite quelconque
# Équation de la droite reliant les deux positions
a1 = delta_y / delta_x # pente
b1 = self.position1.y - a1 * self.position1.x # ordonnée à l'origine
# Équation de la droite perpendiculaire
a2 = -1 / a1 # pente perpendiculaire à a1
b2 = robot_position.y - a2 * robot_position.x # ordonnée à l'origine
# Calcul des coordonnées de la destination
x = (b2 - b1) / (a1 - a2) # a1*x + b1 = a2*x + b2
y = a1 * x + b1
elif delta_x == 0: # droite verticale
x = self.position1.x
y = robot_position.y
elif delta_y == 0: # droite horizontale
x = robot_position.x
y = self.position1.y
destination_position = Position(x, y)
# Vérification que destination_position se trouve entre position1 et position2
distance_positions = math.sqrt(delta_x**2 + delta_y**2)
distance_dest_pos1 = get_distance(self.position1, destination_position)
distance_dest_pos2 = get_distance(self.position2, destination_position)
if distance_dest_pos1 >= distance_positions and distance_dest_pos1 > distance_dest_pos2:
# Si position2 est entre position1 et destination_position
new_x = self.position2.x - self.minimum_distance * delta_x / distance_positions
new_y = self.position2.y - self.minimum_distance * delta_y / distance_positions
destination_position = Position(new_x, new_y)
elif distance_dest_pos2 >= distance_positions and distance_dest_pos2 > distance_dest_pos1:
# Si position1 est entre position2 et destination_position
new_x = self.position1.x + self.minimum_distance * delta_x / distance_positions
new_y = self.position1.y + self.minimum_distance * delta_y / distance_positions
destination_position = Position(new_x, new_y)
# Vérification que destination_position respecte la distance minimale
if distance_dest_pos1 <= distance_dest_pos2:
destination_position = stayOutsideCircle(destination_position,
self.position1,
self.minimum_distance)
else:
destination_position = stayOutsideCircle(destination_position,
self.position2,
self.minimum_distance)
# Calcul de l'orientation de la pose de destination
destination_orientation = get_angle(destination_position, self.target)
destination_pose = Pose(destination_position, destination_orientation)
kick_strength = 0
return AICommand(destination_pose, kick_strength)
