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) 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 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): #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 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 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 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)