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