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 test_eq(self): # les tests sont fait avec la tolérance par défaut défini dans position.py self.assertFalse(Position(900, 900) == Position()) # sanity check pos1 = Position() pos2 = Position() self.assertEqual(pos1, pos2) self.assertEqual(pos2, pos1) pos3 = Position(500, 400) pos4 = Position(400, -400) pos5 = Position(-500, 400) pos6 = Position(0.5, 0) self.assertFalse(pos3 == pos4) self.assertFalse(pos3 == pos5) self.assertFalse(pos3 == pos6) pos7 = Position(0.5, 0) pos8 = Position(0, 0) pos9 = Position(1, 0) self.assertTrue(pos7 == pos8) self.assertTrue(pos7 == pos9) pos10 = Position(0.5, 0) pos11 = Position(-0.5, 0) pos12 = Position(1, 0) self.assertTrue(pos10 == pos11) self.assertTrue(pos10 == pos12) # test pour voir si la tolérance est respectée pos11.abs_tol = 1e-1 self.assertFalse(pos11 == pos10) self.assertFalse(pos10 == pos11)
def setUp(self): self.start_point = Position(0, 0) self.goal = Position(0, 0) self.obstacle = Position(0, 0)
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 test_update_player(self): init_pose = self.first_player.pose self.assertEqual(init_pose, self.team.players[0].pose) self.team.update_player(0, [Pose(Position(500, 500))]) self.assertNotEqual(init_pose, self.team.players[0].pose) self.assertEqual(self.team.players[0].pose, self.first_player.pose)
def test_new(self): self.assertFalse(Position(900, 900) == Position()) # sanity check self.assertTrue(hasattr(Position(), 'z')) self.assertTrue(hasattr(Position(), 'abs_tol')) # Init from nothing self.assertEqual(Position(), Position(0, 0)) self.assertTrue(Position().z == 0) self.assertTrue(Position(0, 0).z == 0) # Init from positional argument pos1 = Position(0, 0) self.assertEqual(pos1.x, 0) self.assertEqual(pos1.y, 0) self.assertEqual(pos1.z, 0) pos2 = Position(-100, 0.001) self.assertEqual(pos2.x, -100) self.assertEqual(pos2.y, 0.001) self.assertEqual(pos2.z, 0) # Init from ndarray my_array = np.array([100, -50.23]) pos3 = Position(np.array(my_array)) self.assertEqual(pos3.x, 100) self.assertEqual(pos3.y, -50.23) self.assertEqual(pos3.z, 0) my_array[0] = 10 self.assertFalse(pos3.x == 10) # Init from list my_list = [9, 10] pos4 = Position(my_list) self.assertEqual(pos4.x, 9) self.assertEqual(pos4.y, 10) self.assertEqual(pos4.z, 0) my_list[0] = 10 self.assertFalse(pos4[0] == 10) # Init from tuple my_tuple = (9, 10) pos5 = Position(my_tuple) self.assertEqual(pos5.x, 9) self.assertEqual(pos5.y, 10) self.assertEqual(pos5.z, 0) pos5[0] = 10 self.assertTrue(pos5[0] == 10) # Init from another Position pos6 = Position(1, 1) pos7 = Position(pos6) self.assertEqual(pos6, pos7) self.assertFalse(pos6 is pos7) pos6[1] = 9 self.assertNotEqual(pos6, pos7) with self.assertRaises(ValueError): Position(0) with self.assertRaises(ValueError): Position({}) with self.assertRaises(ValueError): Position(Pose()) with self.assertRaises(ValueError): Position(0, 0, 0) with self.assertRaises(ValueError): Position([]) with self.assertRaises(ValueError): Position([0]) with self.assertRaises(ValueError): Position([0, 0, 0]) with self.assertRaises(ValueError): Position(()) with self.assertRaises(ValueError): Position((0, 0, 0)) with self.assertRaises(ValueError): Position(np.zeros(1)) with self.assertRaises(ValueError): Position(np.zeros(3))
def test_rotate(self): self.assertEqual(Position(1, 0).rotate(0), Position(1, 0)) self.assertEqual(Position(1, 0).rotate(m.pi / 2), Position(0, 1)) self.assertEqual(Position(0, 1).rotate(m.pi / 2), Position(-1, 0)) self.assertEqual(Position(-1, 0).rotate(m.pi / 2), Position(0, -1)) self.assertEqual(Position(0, -1).rotate(m.pi / 2), Position(1, 0)) self.assertEqual( Position(526, 878).rotate(2.14675), Position(-1022.833, -37.052)) self.assertTrue(type(Position(526, 878).rotate(2.14675)) is Position)
def test_init(self): # Default case pose = Pose() self.assertTrue(hasattr(pose, 'position')) self.assertTrue(hasattr(pose, 'orientation')) self.assertTrue(type(pose.position) is Position) self.assertTrue(type(pose.orientation) is float) # From another Pose pose1 = Pose(Position(1, 1), 1) pose2 = Pose(pose1) self.assertEqual(pose1.position, pose2.position) self.assertEqual(pose1.orientation, pose2.orientation) self.assertTrue(pose1 is not pose2) self.assertTrue(pose1.position is not pose2.position) self.assertTrue(pose1.orientation is not pose2.orientation) pose1 = Pose(Position(1, 1), 1 + 2 * m.pi) self.assertAlmostEqual(pose1.orientation, 1) pose2 = Pose(pose1) self.assertAlmostEqual(pose2.orientation, 1) # From a size 3 numpy array my_array = np.array([1, 1, np.pi / 4]) pose = Pose(my_array) self.assertEqual(pose.position, Position(1, 1)) self.assertEqual(pose.orientation, np.pi / 4) my_array[0] = 2 self.assertNotEqual(pose.position, Position(2, 1)) self.assertTrue(pose.orientation is not my_array[2]) my_array = np.array([1, 1, np.pi / 4 + 2 * np.pi]) pose = Pose(my_array) self.assertEqual(pose.position, Position(1, 1)) self.assertAlmostEqual(pose.orientation, np.pi / 4) # From positional arguments (Position(), orientation) my_position = Position(1, 2) pose = Pose(my_position, 3) self.assertEqual(pose.position, Position(1, 2)) self.assertEqual(pose.orientation, 3) self.assertTrue(pose.position is not my_position) # From positional arguments (x, y, orientation) pose = Pose(1, 2, 3) self.assertEqual(pose.position, Position(1, 2)) self.assertEqual(pose.orientation, 3) pose = Pose(1, 2, 3 + 2 * m.pi) self.assertAlmostEqual(pose.orientation, 3) # From a size 2 numpy array and orientation (np.array, orientation) my_array = np.array([1, 2]) pose = Pose(my_array) self.assertEqual(pose.position, Position(1, 2)) self.assertEqual(pose.orientation, 0) my_array[0] = 2 self.assertNotEqual(pose.position, Position(2, 2)) my_array = np.array([1, 2]) pose = Pose(my_array, 3) self.assertEqual(pose.position, Position(1, 2)) self.assertEqual(pose.orientation, 3) my_array[0] = 2 self.assertNotEqual(pose.position, Position(2, 2)) my_array = np.array([1, 2]) pose = Pose(my_array, 3 + 2 * m.pi) self.assertAlmostEqual(pose.orientation, 3) # Error cases with self.assertRaises(ValueError): Pose(0) with self.assertRaises(ValueError): Pose(0, 0, 0, 0) with self.assertRaises(ValueError): Pose({}) with self.assertRaises(ValueError): Pose([]) with self.assertRaises(ValueError): Pose([0]) with self.assertRaises(ValueError): Pose([0], 0) with self.assertRaises(ValueError): Pose([0, 0, 0]) with self.assertRaises(ValueError): Pose([0, 0, 0], 0) with self.assertRaises(ValueError): Pose(()) with self.assertRaises(ValueError): Pose((), 0) with self.assertRaises(ValueError): Pose((0, 0, 0)) with self.assertRaises(ValueError): Pose((0, 0, 0), 0) with self.assertRaises(ValueError): Pose(np.zeros(1)) with self.assertRaises(ValueError): Pose(np.zeros(1), 0) with self.assertRaises(ValueError): Pose(np.zeros(4)) with self.assertRaises(ValueError): Pose(np.zeros(4), 0)
def test_GoBetween(self): # test avec une droite verticale POS_TOP = Position(100, 100) POS_BOTTOM = Position(100, -100) POS_TARGET = Position(200, 0) POS_INBETWEEN = Position(100, 0) self.go_between = GoBetween(self.game_state, self.a_player, POS_TOP, POS_BOTTOM, POS_TARGET) ai_cmd = self.go_between.exec().pose_goal ai_cmd_expected = Pose(POS_INBETWEEN, 0) self.assertEqual(ai_cmd, ai_cmd_expected) # test avec une droite horizontale self.go_between = GoBetween(self.game_state, self.a_player, Position(100, 100), Position(-100, 100), Position(0, 200)) ai_cmd = self.go_between.exec().pose_goal ai_cmd_expected = Pose(Position(0, 100), pi / 2) self.assertEqual(ai_cmd, ai_cmd_expected) # test avec une droite quelconque self.go_between = GoBetween(self.game_state, self.a_player, Position(0, 500), Position(500, 0), Position(-300, -300)) ai_cmd = self.go_between.exec().pose_goal ai_cmd_expected = Pose(Position(250, 250), -3 * pi / 4) self.assertEqual(ai_cmd, ai_cmd_expected) # test destination calculée derrière position1 self.go_between = GoBetween(self.game_state, self.a_player, Position(1000, 75), Position(1500, -250), Position(0, 0), 0) ai_cmd = self.go_between.exec().pose_goal ai_cmd_expected = Pose(Position(1000, 75), -3.067) self.assertEqual(ai_cmd, ai_cmd_expected) # test destination calculée derrière position2 self.go_between = GoBetween(self.game_state, self.a_player, Position(-100, 50), Position(-50, 50), Position(-60.0 + sqrt(3), 51.0), 10) ai_cmd = self.go_between.exec().pose_goal ai_cmd_expected = Pose(Position(-60, 50), 0.5235) self.assertEqual(ai_cmd, ai_cmd_expected) # test correction pour respecter la distance minimale self.go_between = GoBetween(self.game_state, self.a_player, Position(-500, 25), Position(1, 25), Position(-179, 0), 180) ai_cmd = self.go_between.exec().pose_goal ai_cmd_expected = Pose(Position(-179, 25), -pi / 2) self.assertEqual(ai_cmd, ai_cmd_expected) # test distance entre les positions insuffisantes self.assertRaises(AssertionError, GoBetween, self.game_state, self.a_player, Position(1, 1), Position(-1, -1), 50)
def _friend_kalman_update(self, poses, delta): ret = self.kf.filter(poses, self.cmd, delta) self.pose = Pose(Position(ret[0], ret[1]), ret[4]) self.velocity = SpeedPose(Position(ret[2], ret[3]), ret[5])
def test_eq(self): self.assertEqual(Pose(), Pose()) from RULEngine.Util.Pose import ORIENTATION_ABSOLUTE_TOLERANCE tol = 0.9999 * ORIENTATION_ABSOLUTE_TOLERANCE self.assertEqual(Pose(Position(), 1), Pose(Position(), 1 + tol)) self.assertEqual(Pose(Position(), 1), Pose(Position(), 1 - tol)) self.assertNotEqual(Pose(Position(), 1), Pose(Position(), 1 + 1.1 * tol)) self.assertNotEqual(Pose(Position(), 1), Pose(Position(), 1 - 1.1 * tol)) self.assertEqual(Pose(Position(), 0), Pose(Position(), +tol)) self.assertEqual(Pose(Position(), 0), Pose(Position(), -tol)) self.assertEqual(Pose(Position(), +tol), Pose(Position(), 0)) self.assertEqual(Pose(Position(), -tol), Pose(Position(), 0)) self.assertNotEqual(Pose(Position(), 0), Pose(Position(), +1.1 * tol)) self.assertNotEqual(Pose(Position(), 0), Pose(Position(), -1.1 * tol)) self.assertNotEqual(Pose(Position(), +1.1 * tol), Pose(Position(), 0)) self.assertNotEqual(Pose(Position(), -1.1 * tol), Pose(Position(), 0)) self.assertEqual(Pose(Position(), 0), Pose(Position(), 2 * m.pi + tol)) self.assertEqual(Pose(Position(), 0), Pose(Position(), 2 * m.pi - tol)) self.assertNotEqual(Pose(Position(), 0), Pose(Position(), 2 * m.pi + 1.1 * tol)) self.assertNotEqual(Pose(Position(), 0), Pose(Position(), 2 * m.pi - 1.1 * tol)) self.assertEqual(Pose(Position(), m.pi), Pose(Position(), -m.pi)) self.assertEqual(Pose(Position(), m.pi + tol), Pose(Position(), -m.pi)) self.assertEqual(Pose(Position(), m.pi), Pose(Position(), -m.pi + tol)) self.assertEqual(Pose(Position(), m.pi - tol), Pose(Position(), -m.pi)) self.assertEqual(Pose(Position(), m.pi), Pose(Position(), -m.pi - tol)) self.assertNotEqual(Pose(Position(), m.pi + 1.1 * tol), Pose(Position(), -m.pi)) self.assertNotEqual(Pose(Position(), m.pi), Pose(Position(), -m.pi + 1.1 * tol)) self.assertNotEqual(Pose(Position(), m.pi - 1.1 * tol), Pose(Position(), -m.pi)) self.assertNotEqual(Pose(Position(), m.pi), Pose(Position(), -m.pi - 1.1 * tol))
def _update_players_of_team(players, team, delta): for player in players: player_position = Position(player.x, player.y, player.height) player_pose = Pose(player_position, player.orientation) team.update_player(player.robot_id, player_pose, delta)
def test_limit_speed(self): self.assertEqual(self.rm.limit_speed(Position(0, 0)), Position(0, 0)) self.assertEqual(self.rm.limit_speed(Position(0.5, 0.5)), Position(0.5, 0.5)) self.assertEqual(self.rm.limit_speed(Position(1.0, 1.0)), Position(1.0, 1.0)) self.assertEqual(self.rm.limit_speed(Position(2.0, 2.0)), Position(1.414, 1.414)) self.assertEqual(self.rm.limit_speed(Position(-0.5, -0.5)), Position(-0.5, -0.5)) self.assertEqual(self.rm.limit_speed(Position(-1.0, -1.0)), Position(-1.0, -1.0)) self.assertEqual(self.rm.limit_speed(Position(-2.0, -2.0)), Position(-1.414, -1.414)) self.assertEqual(self.rm.limit_speed(Position(0.5, -0.5)), Position(0.5, -0.5)) self.assertEqual(self.rm.limit_speed(Position(1.0, -1.0)), Position(1.0, -1.0)) self.assertEqual(self.rm.limit_speed(Position(2.0, -2.0)), Position(1.414, -1.414)) self.assertEqual(self.rm.limit_speed(Position(-0.5, 0)), Position(-0.5, 0.0)) self.assertEqual(self.rm.limit_speed(Position(-1.0, 0)), Position(-1.0, 0.0)) self.assertEqual(self.rm.limit_speed(Position(-2.0, 0)), Position(-2.0, 0.0)) self.assertEqual(self.rm.limit_speed(Position(-3.0, 0)), Position(-2.0, 0.0))
def step_response(player): test_vel = Position(1.0, 0) cmd = command.MoveTo(player, test_vel) print("Envoi commande x pure") gr_sim_cmd_sender.send_command(cmd)
def center_player(player): center_pos = Position(0, 0) center_cmd = command.MoveTo(player, center_pos) gr_sim_debug_sender.send_command(center_cmd) gr_sim_debug_sender.place_ball(Position(1, 2))
def _smoothed_path_to_pose_list(self, smoothed_path, target_orientation): smoothed_poses = [] for point in smoothed_path: smoothed_poses.append(Pose(Position(point[0], point[1]), target_orientation)) return smoothed_poses
def test_init(): l = [Position(0, 0), Position(100, 200)] c = Collision(l) assert c.field_objects == l
def get_destination(self): """ Calcule le point situé à x pixels derrière la position 1 par rapport à la position 2 :return: Un tuple (Pose, kick) où Pose est la destination du joueur et kick est nul (on ne botte pas) """ delta_x = self.position2.x - self.position1.x delta_y = self.position2.y - self.position1.y theta = math.atan2(delta_y, delta_x) x = self.position1.x - self.distance_behind * math.cos(theta) y = self.position1.y - self.distance_behind * math.sin(theta) player_x = self.player.pose.position.x player_y = self.player.pose.position.y norm_player_2_position2 = math.sqrt((player_x - self.position2.x)**2 + (player_y - self.position2.y)**2) norm_position1_2_position2 = math.sqrt( (self.position1.x - self.position2.x)**2 + (self.position1.y - self.position2.y)**2) if norm_player_2_position2 < norm_position1_2_position2: # on doit contourner l'objectif vecteur_position1_2_position2 = np.array([ self.position1.x - self.position2.x, self.position1.y - self.position2.y, 0 ]) vecteur_vertical = np.array([0, 0, 1]) vecteur_player_2_position1 = np.array( [self.position1.x - player_x, self.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] * self.rayon_avoid position_intermediaire_y = y + vecteur_perp[1] * self.rayon_avoid if math.sqrt((player_x - position_intermediaire_x)**2 + (player_y - position_intermediaire_y)**2) < 50: position_intermediaire_x += vecteur_perp[ 0] * self.rayon_avoid * 2 position_intermediaire_y += vecteur_perp[ 1] * self.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 # TODO why?!? MGL 2017/05/22 destination_orientation = 0 if self.orientation == 'front': destination_orientation = get_angle(destination_position, self.position1) elif self.orientation == 'back': destination_orientation = get_angle(destination_position, self.position1) + np.pi destination_pose = Pose(destination_position, destination_orientation) return destination_pose
def test_norm(self): self.assertEqual(Position().norm(), 0) self.assertEqual(Position(1, 1).norm(), m.sqrt(2)) self.assertEqual(Position(1, -1).norm(), m.sqrt(2)) self.assertEqual(Position(-1, 1).norm(), m.sqrt(2)) self.assertEqual(Position(3, 4).norm(), 5)
def passer2(self, coach, terrain, etats, equipe_bleu, equipe_jaune): coach.bouger(1, Position(-3000, -1000)) self.prochain_etat(self.termine)
def test_normalized(self): self.assertEqual(Position(1, 0).normalized(), Position(1, 0)) self.assertEqual(Position(0, -1).normalized(), Position(0, -1)) self.assertEqual( Position(10, 10).normalized(), Position(m.sqrt(2) / 2, m.sqrt(2) / 2)) normalized_vector = Position( np.array([12.45, -23.23]) / np.sqrt(np.square([12.45, -23.23]).sum())) self.assertEqual( Position(12.45, -23.23).normalized(), normalized_vector) self.assertTrue(type(Position(12.45, -23.23).normalized()) is Position) # Change this to fix some of motion executer behavior self.assertEquals(Position(0, 0), Position(0, 0).normalized())
def _is_ball_too_far(self): our_goal = Position(self.game_state.const["FIELD_OUR_GOAL_X_EXTERNAL"], 0) return (our_goal - self.game_state.get_ball_position() ).norm() > self.game_state.const["FIELD_GOAL_WIDTH"]
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)
def get_destination(self) -> Pose: """ 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) """ player = self.player.pose.position.conv_2_np() pt1 = self.position1.conv_2_np() pt2 = self.position2.conv_2_np() delta = self.minimum_distance * (pt2 - pt1) / np.linalg.norm(pt2 - pt1) pt1 = pt1 + delta pt2 = pt2 - delta pt1_to_player = player - pt1 pt2_to_player = player - pt2 pt1_to_pt2 = pt2 - pt1 destination = np.cross( pt1_to_player, pt1_to_pt2) / np.linalg.norm(pt1_to_pt2) + player outside_x = (destination[0] > pt1[0] and destination[0] > pt2[0]) or \ (destination[0] < pt1[0] and destination[0] < pt2[0]) outside_y = (destination[1] > pt1[1] and destination[1] > pt2[1]) or \ (destination[1] < pt1[1] and destination[1] < pt2[1]) if outside_x or outside_y: if np.linalg.norm(pt1_to_player) > np.linalg.norm(pt2_to_player): destination = pt1 else: destination = pt2 target = self.target.conv_2_np() player_to_target = target - player destination_orientation = np.arctan2(player_to_target[1], player_to_target[0]) # TODO remove MGL 2017/05/23 ''' 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_goal": Pose(destination_position, destination_orientation)} kick_strength = 0 ''' return Pose(Position.from_np(destination), destination_orientation)
def __init__(self, start=Position(), end=Position()): self.start = start self.goal = end self.points = [start, end] self.speeds = [0, 0]
def reshape_path(self, path: Path, player: OurPlayer, vel_cruise: [int, float] = 1000): self.path = path self.player = player cmd = self.player.ai_command if cmd.cruise_speed: vel_cruise = cmd.cruise_speed * 1000 # print(vel_cruise) self.vel_max = vel_cruise p1 = self.path.points[0].conv_2_np() point_list = [p1] speed_list = [0] for idx, point in enumerate(self.path.points[1:-1]): self.dist_from_path = 50 # mm i = idx + 1 p2 = point.conv_2_np() p3 = self.path.points[i + 1].conv_2_np() # if np.linalg.norm(p1, p2) / 2 < OurPlayer.max_acc * 2 / vel_cruise ** 2: # # on ne calcul pas le radius a partir de vel cruise. profil triangulaire # vel_pointe = np.sqrt(2 * OurPlayer.max_acc / (np.linalg.norm(p1,p2) / 2)) # radius_at_const_speed = vel_pointe ** 2 / (OurPlayer.max_acc * 1000) # else: radius_at_const_speed = vel_cruise**2 / (OurPlayer.max_acc * 1000) theta = np.math.atan2(p3[1] - p2[1], p3[0] - p2[0]) - np.math.atan2( p1[1] - p2[1], p1[0] - p2[0]) dist_deviation = (radius_at_const_speed / (np.math.sin(theta / 2))) - radius_at_const_speed speed = vel_cruise radius = radius_at_const_speed while dist_deviation > self.dist_from_path: speed *= 0.8 radius = speed**2 / (OurPlayer.max_acc * 1000) dist_deviation = (radius / (np.math.sin(theta / 2))) - radius # print(radius, radius_at_const_speed) if np.linalg.norm(p1 - p2) < 0.001 or np.linalg.norm( p2 - p3) < 0.001 or np.linalg.norm(p1 - p3) < 0.001: # on traite tout le cas ou le problème dégènere point_list += [p2] speed_list += [vel_cruise / 1000] else: p4 = p2 + np.sqrt(np.square(dist_deviation + radius) - radius ** 2) *\ (p1 - p2) / np.linalg.norm(p1 - p2) p5 = p2 + np.sqrt(np.square(dist_deviation + radius) - radius ** 2) *\ (p3 - p2) / np.linalg.norm(p3 - p2) if np.linalg.norm(p4 - p5) > np.linalg.norm(p3 - p1): point_list += [p2] speed_list += [vel_cruise / 1000] elif np.linalg.norm(p1 - p2) < np.linalg.norm(p4 - p2): radius *= np.linalg.norm(p1 - p2) / np.linalg.norm(p4 - p2) dist_deviation = (radius / (np.math.sin(theta / 2))) - radius p4 = p2 + np.sqrt( np.square(dist_deviation + radius) - radius**2) * (p1 - p2) / np.linalg.norm(p1 - p2) p5 = p2 + np.sqrt( np.square(dist_deviation + radius) - radius**2) * (p3 - p2) / np.linalg.norm(p3 - p2) point_list += [p4, p5] speed_list += [speed, speed] elif np.linalg.norm(p3 - p2) < np.linalg.norm(p5 - p2): radius *= np.linalg.norm(p3 - p2) / np.linalg.norm(p5 - p2) dist_deviation = (radius / (np.math.sin(theta / 2))) - radius p4 = p2 + np.sqrt( np.square(dist_deviation + radius) - radius**2) * (p1 - p2) / np.linalg.norm(p1 - p2) p5 = p2 + np.sqrt( np.square(dist_deviation + radius) - radius**2) * (p3 - p2) / np.linalg.norm(p3 - p2) point_list += [p4, p5] speed_list += [speed, speed] else: point_list += [p4, p5] speed_list += [speed, speed] # radius = abs(self.dist_from_path*np.sin(theta/2)/(1-np.sin(theta/2))) # print(radius, radius_at_const_speed) # if radius > radius_at_const_speed: # radius = radius_at_const_speed # self.dist_from_path = -radius + radius / abs(np.math.sin(theta / 2)) # if np.linalg.norm(p1-p2) < 0.001 or np.linalg.norm(p2-p3) < 0.001 or np.linalg.norm(p1-p3) < 0.001: # # on traite tout le cas ou le problème dégènere # point_list += [point] # speed_list += [vel_cruise/1000] # else: # p4 = p2 + np.sqrt(np.square(self.dist_from_path + radius) - radius ** 2) * \ # (p1 - p2)/np.linalg.norm(p1-p2) # p5 = p2 + np.sqrt(np.square(self.dist_from_path + radius) - radius ** 2) * \ # (p3 - p2) / np.linalg.norm(p3 - p2) # if np.linalg.norm(p4-p5) > np.linalg.norm(p3-p1): # point_list += [point] # speed_list += [vel_cruise/1000] # else: # point_list += [Position.from_np(p4), Position.from_np(p5)] # speed_list += [np.sqrt(radius / (OurPlayer.max_acc * 1000)), # np.sqrt(radius / (OurPlayer.max_acc * 1000))] p1 = point_list[-1] speed_list += [0] point_list += [self.path.goal.conv_2_np()] # on s'assure que le path est bel et bien réalisable par un robot et on # merge les points qui sont trop proches les un des autres. position_list = [Position().from_np(point_list[0])] new_speed_list = [speed_list[0]] for idx, point in enumerate(point_list[1:-1]): i = idx + 1 if np.linalg.norm(point_list[i] - point_list[i + 1]) < 10: continue min_dist = 0.5 * ( np.square(speed_list[i] - np.square(speed_list[i + 1])) / (OurPlayer.max_acc * 1000)) if min_dist > np.linalg.norm(point_list[i] - point_list[i + 1]): if speed_list[i] > speed_list[i + 1]: speed_list[i] *= np.linalg.norm( point_list[i] - point_list[i + 1]) / min_dist position_list += [Position().from_np(point_list[i])] new_speed_list += [speed_list[i]] position_list += [Position().from_np(point_list[-1])] new_speed_list += [speed_list[-1]] return Path().generate_path_from_points(position_list, new_speed_list)
def _find_best_player_position(self): if self.auto_position: pad = 200 if self.game_state.const["FIELD_OUR_GOAL_X_EXTERNAL"] > 0: our_goal_field_limit = self.game_state.const[ "FIELD_OUR_GOAL_X_EXTERNAL"] - pad our_side_center_field_limit = pad their_goal_field_limit = GameState( ).const["FIELD_THEIR_GOAL_X_EXTERNAL"] + pad their_side_center_field_limit = -pad else: our_goal_field_limit = self.game_state.const[ "FIELD_OUR_GOAL_X_EXTERNAL"] + pad our_side_center_field_limit = -pad their_goal_field_limit = self.game_state.const[ "FIELD_THEIR_GOAL_X_EXTERNAL"] - pad their_side_center_field_limit = pad field_width = self.game_state.const[ "FIELD_Y_TOP"] - self.game_state.const["FIELD_Y_BOTTOM"] self.role = self.game_state.get_role_by_player_id(self.player.id) offense_offset = 0 defense_offset = self.compute_defense_offset() if self.is_player_defense(self.player): # role is in defense: if self.role is Role.FIRST_DEFENCE: A = Position(our_goal_field_limit, self.game_state.const["FIELD_Y_TOP"] + pad) + defense_offset B = Position(our_side_center_field_limit, (self.game_state.const["FIELD_Y_TOP"] - field_width / self.number_of_defence_players) + pad) + defense_offset elif self.role is Role.MIDDLE: # center A = Position(our_goal_field_limit + 1000, (self.game_state.const["FIELD_Y_BOTTOM"] / self.number_of_defence_players) + pad) + defense_offset B = Position( our_side_center_field_limit, self.game_state.const["FIELD_Y_TOP"] / self.number_of_defence_players - pad) + defense_offset else: # bottom_defense A = Position(our_goal_field_limit, pad) + defense_offset B = Position(our_side_center_field_limit, (self.game_state.const["FIELD_Y_BOTTOM"]) + field_width / self.number_of_defence_players ) + defense_offset else: if self.role is Role.FIRST_ATTACK: # player.role is 'top_offence': A = Position(their_goal_field_limit, self.game_state.const["FIELD_Y_TOP"] + pad) + offense_offset B = Position(their_goal_field_limit, (self.game_state.const["FIELD_Y_TOP"] - field_width / self.number_of_offense_players) + pad) + offense_offset else: A = Position(their_goal_field_limit, pad) + offense_offset B = Position(their_goal_field_limit, (self.game_state.const["FIELD_Y_BOTTOM"] + field_width / self.number_of_offense_players) - pad) + offense_offset return best_position_in_region(self.player, A, B) else: return self.target_position
class RobotMotion(object): def __init__(self, world_state: WorldState, robot_id, is_sim=True): self.ws = world_state self.id = robot_id self.is_sim = is_sim self.setting = get_control_setting(is_sim) self.setting.translation.max_acc = None self.setting.translation.max_speed = None self.setting.rotation.max_angular_acc = None self.setting.rotation.max_speed = None self.current_pose = Pose() self.current_orientation = 0.0 self.current_velocity = Pose() self.current_angular_speed = 0.0 self.current_speed = 0.0 self.current_acceleration = Position() self.pose_error = Pose() self.position_error = Position() self.target_pose = Pose() self.target_speed = 0.0 self.target_direction = Position() self.target_angular_speed = 0.0 self.target_angle = 0.0 self.angle_error = 0.0 self.last_translation_cmd = Position() self.cruise_speed = 0.0 self.cruise_angular_speed = 0.0 self.next_speed = 0.0 self.next_angular_speed = 0.0 self.x_controller = PID(self.setting.translation.kp, self.setting.translation.ki, self.setting.translation.kd, self.setting.translation.antiwindup) self.y_controller = PID(self.setting.translation.kp, self.setting.translation.ki, self.setting.translation.kd, self.setting.translation.antiwindup) self.angle_controller = PID(self.setting.rotation.kp, self.setting.rotation.ki, self.setting.rotation.kd, self.setting.rotation.antiwindup, wrap_err=True) self.position_flag = False self.rotation_flag = False self.last_position = Position() self.target_turn = self.target_pose.position def update(self, cmd: AICommand) -> Pose(): #print(cmd.path_speeds) self.update_states(cmd) # Rotation control rotation_cmd = self.angle_controller.update( self.pose_error.orientation, dt=self.dt) rotation_cmd = self.apply_rotation_constraints(rotation_cmd) if abs(self.pose_error.orientation) < 0.2: self.rotation_flag = True # Translation control self.position_flag = False if self.position_error.norm( ) < MIN_DISTANCE_TO_REACH_TARGET_SPEED * max(1.0, self.cruise_speed): if self.target_speed < 0.01: self.position_flag = True if self.position_flag: translation_cmd = Position( self.x_controller.update(self.pose_error.position.x, dt=self.dt), self.y_controller.update(self.pose_error.position.y, dt=self.dt)) else: translation_cmd = self.get_next_velocity() # Adjust command to robot's orientation # self.ws.debug_interface.add_line(start_point=(self.current_pose.position[0] * 1000, self.current_pose.position[1] * 1000), # end_point=(self.current_pose.position[0] * 1000 + translation_cmd[0] * 600, self.current_pose.position[1] * 1000 + translation_cmd[1] * 600), # timeout=0.01, color=debug_interface.CYAN.repr()) compasation_ref_world = translation_cmd.rotate(self.dt * rotation_cmd) translation_cmd = translation_cmd.rotate( -(self.current_pose.orientation)) if not self.rotation_flag and cmd.path[-1] is not cmd.path[0]: translation_cmd *= translation_cmd * 0.0 self.next_speed = 0.0 self.x_controller.reset() self.y_controller.reset() if self.position_error.norm() > 0.1 and self.rotation_flag: self.angle_controller.reset() rotation_cmd = 0 # self.ws.debug_interface.add_line( # start_point=(self.current_pose.position[0] * 1000, self.current_pose.position[1] * 1000), # end_point=(self.current_pose.position[0] * 1000 + compasation_ref_world[0] * 600, # self.current_pose.position[1] * 1000 + compasation_ref_world[1] * 600), # timeout=0.01, color=debug_interface.ORANGE.repr()) translation_cmd = self.apply_translation_constraints(translation_cmd) #if not translation_cmd.norm() < 0.01: # print(translation_cmd, "self.target_reached()", self.target_reached(), "self.next_speed", self.next_speed,"self.target_speed", self.target_speed ) # self.debug(translation_cmd, rotation_cmd) return SpeedPose(translation_cmd, rotation_cmd) def get_next_velocity(self) -> Position: """Return the next velocity according to a constant acceleration model of a point mass. It try to produce a trapezoidal velocity path with the required cruising and target speed. The target speed is the speed that the robot need to reach at the target point.""" if self.current_speed < self.target_speed: # accelerate self.next_speed += self.setting.translation.max_acc * self.dt else: if self.distance_accelerate(): self.next_speed += self.setting.translation.max_acc * self.dt elif self.distance_break(): self.next_speed -= self.setting.translation.max_acc * self.dt else: self.next_speed = self.current_speed # if self.target_reached(): # We need to go to target speed # if self.next_speed < self.target_speed: # Target speed is faster than current speed # self.next_speed += self.setting.translation.max_acc * self.dt # if self.next_speed > self.target_speed: # Next_speed is too fast # self.next_speed = self.target_speed # else: # Target speed is slower than current speed # self.next_speed -= self.setting.translation.max_acc * self.dt *2 # else: # We need to go to the cruising speed # if self.next_speed < self.cruise_speed: # Going faster # self.next_speed += self.setting.translation.max_acc * self.dt # # self.next_speed = min(self.cruise_speed, self.next_speed) # else: # self.next_speed -= self.setting.translation.max_acc * self.dt * 2 self.next_speed = np.clip(self.next_speed, 0.0, self.cruise_speed) self.next_speed = np.clip(self.next_speed, 0.0, self.setting.translation.max_speed) next_velocity = Position(self.target_direction * self.next_speed) return next_velocity def apply_rotation_constraints(self, r_cmd: float) -> float: if self.current_speed < 0.1: deadzone = self.setting.rotation.deadzone else: deadzone = 0.0 sensibility = self.setting.rotation.sensibility max_speed = self.setting.rotation.max_speed r_cmd = self.limit_angular_speed(r_cmd) r_cmd = RobotMotion.apply_deadzone(r_cmd, deadzone, sensibility) r_cmd = clamp(r_cmd, -max_speed, max_speed) return r_cmd def apply_translation_constraints(self, t_cmd: Position) -> Position: deadzone = self.setting.translation.deadzone sensibility = self.setting.translation.sensibility t_cmd = self.limit_speed(t_cmd) t_cmd[0] = RobotMotion.apply_deadzone(t_cmd[0], deadzone, sensibility) t_cmd[1] = RobotMotion.apply_deadzone(t_cmd[1], deadzone, sensibility) return t_cmd @staticmethod def apply_deadzone(value, deadzone, sensibility): if m.fabs(value) < sensibility: value = 0.0 elif m.fabs(value) <= deadzone: value = m.copysign(deadzone, value) return value def limit_speed(self, translation_cmd: Position) -> Position: if translation_cmd.norm() != 0.0: translation_speed = translation_cmd.norm() translation_speed = clamp(translation_speed, 0, self.setting.translation.max_speed) new_speed = translation_cmd.normalized() * translation_speed else: new_speed = Position() return new_speed def limit_angular_speed(self, angular_speed: float) -> float: if m.fabs(angular_speed) != 0.0: rotation_sign = m.copysign(1, angular_speed) angular_speed = clamp(m.fabs(angular_speed), 0.0, self.setting.translation.max_speed) new_speed = m.copysign(angular_speed, rotation_sign) * angular_speed else: new_speed = 0.0 return new_speed def target_reached(self, boost_factor=1 ) -> bool: # distance_to_reach_target_speed distance = 0.5 * (self.target_speed**2 - self.current_speed** 2) / self.setting.translation.max_acc distance = boost_factor * m.fabs(distance) distance = max(distance, MIN_DISTANCE_TO_REACH_TARGET_SPEED) return self.position_error.norm() <= distance def distance_accelerate(self, boost_factor=1 ) -> bool: # distance_to_reach_target_speed distance = 0.5 * (self.target_speed**2 - self.current_speed** 2) / self.setting.translation.max_acc distance = boost_factor * m.fabs(distance) distance = max(distance, MIN_DISTANCE_TO_REACH_TARGET_SPEED) return self.position_error.norm() >= distance * 2 def distance_break(self, boost_factor=1 ) -> bool: # distance_to_reach_target_speed distance = 0.5 * (self.target_speed**2 - self.current_speed** 2) / self.setting.translation.max_acc distance = boost_factor * m.fabs(distance) distance = max(distance, MIN_DISTANCE_TO_REACH_TARGET_SPEED) return self.position_error.norm() <= distance def update_states(self, cmd: AICommand): self.dt = self.ws.game_state.game.delta_t # Dynamics constraints self.setting.translation.max_acc = self.ws.game_state.get_player( self.id).max_acc self.setting.translation.max_speed = self.ws.game_state.get_player( self.id).max_speed self.setting.translation.max_angular_acc = self.ws.game_state.get_player( self.id).max_angular_acc self.setting.rotation.max_speed = self.ws.game_state.get_player( self.id).max_angular_speed # Current state of the robot self.current_pose = self.ws.game_state.game.friends.players[ self.id].pose.scale(1 / 1000) self.current_velocity = self.ws.game_state.game.friends.players[ self.id].velocity.scale(1 / 1000) self.current_speed = self.current_velocity.position.norm() self.current_angular_speed = self.current_velocity.orientation self.current_orientation = self.current_pose.orientation # Desired parameters if cmd.path != []: current_path_position = Position(cmd.path[0] / 1000) if not self.last_position.is_close( current_path_position, 0.1) and self.target_speed < 0.2: self.reset() self.last_position = current_path_position self.target_pose = Pose(cmd.path[0], cmd.pose_goal.orientation).scale(1 / 1000) self.target_turn = cmd.path_turn[1] / 1000 self.target_speed = cmd.path_speeds[1] / 1000 else: # No pathfinder case self.target_pose = cmd.pose_goal.scale(1 / 1000) self.target_turn = self.target_pose.position self.target_speed = 0.0 self.target_angle = self.target_pose.orientation self.pose_error = self.target_pose - self.current_pose # Pose are always wrap to pi self.position_error = self.pose_error.position self.angle_error = self.pose_error.orientation if self.position_error.norm() != 0.0: self.target_direction = (self.target_turn - self.current_pose.position).normalized() self.cruise_speed = cmd.cruise_speed def reset(self): self.angle_controller.reset() self.x_controller.reset() self.y_controller.reset() self.position_flag = False self.rotation_flag = False self.last_translation_cmd = Position() self.next_speed = 0.0 self.next_angular_speed = 0.0 self.last_position = Position() def debug(self, translation_cmd, rotation_cmd): print( 'Speed: {:5.3f}, Command: {}, {:5.3f}, next speed: {:5.3f}, target_speed: {:5.3f}, ' '{:5.3f}, reached:{}, error: {}'.format( self.current_speed, translation_cmd, rotation_cmd, self.next_speed, self.target_speed, self.target_direction.angle() / m.pi * 180, self.target_reached(), self.pose_error))
def _enemy_kalman_update(self, poses, delta): ret = self.kf.filter(poses, delta) self.pose = Pose(Position(ret[0], ret[1]), ret[4]) self.velocity = [ret[2], ret[3], ret[5]]
def get_mark(self, angle, pond): x = pond * self.rayon * cos(angle) y = pond * self.rayon * sin(angle) return Position(self.centre.x + x, self.centre.y + y)
def stop_player(player): stop_cmd = command.MoveTo(player, Position()) print("Arret du robot") gr_sim_cmd_sender.send_command(stop_cmd)