def main_state(self):
self.status_flag = Flags.WIP
self.next_state = self.main_state
if self.auto_update_target:
self._find_best_passing_option()
orientation = (self.target.position - self.game_state.ball_position).angle
player_to_target = (self.target.position - self.player.pose.position)
dist_from_ball = (self.player.position - self.game_state.ball_position).norm
ball_speed = self.game_state.ball.velocity.norm
ball_speed_modifier = (ball_speed / 1000 + 1)
effective_ball_spacing = GO_BEHIND_SPACING * ball_speed_modifier
position_behind_ball_for_approach = self.get_destination_behind_ball(effective_ball_spacing)
position_behind_ball_for_grab = self.game_state.ball_position - normalize(player_to_target) * GRAB_BALL_SPACING
position_behind_ball_for_kick = self.game_state.ball_position + normalize(player_to_target) * KICK_DISTANCE
if self.is_able_to_grab_ball_directly(0.8):
self.points_sequence = []
if compare_angle(self.player.pose.orientation, orientation, abs_tol=0.1) and \
(dist_from_ball < GRAB_BALL_SPACING * 1.25):
self.next_state = self.validate_kick
return CmdBuilder().addMoveTo(Pose(position_behind_ball_for_kick, orientation),
ball_collision=False, cruise_speed=3).addKick(self.kick_force).build()
return CmdBuilder().addMoveTo(Pose(position_behind_ball_for_grab, orientation),
ball_collision=False, cruise_speed=3).build()
else:
self.points_sequence = [WayPoint(position_behind_ball_for_approach, ball_collision=True)]
return CmdBuilder().addMoveTo(Pose(position_behind_ball_for_kick, orientation),
ball_collision=False,
way_points=self.points_sequence,
cruise_speed=3).build()
def go_behind_ball(self):
if self.auto_update_target:
self._find_best_passing_option()
self.status_flag = Flags.WIP
required_orientation = (self.target.position - self.game_state.ball_position).angle
ball_speed = self.game_state.ball.velocity.norm
ball_speed_modifier = (ball_speed/1000 + 1)
angle_behind = self.get_alignment_with_ball_and_target()
if angle_behind > 35:
effective_ball_spacing = GO_BEHIND_SPACING * min(3, abs(angle_behind/45)) * ball_speed_modifier
collision_ball = True
else:
effective_ball_spacing = GO_BEHIND_SPACING
collision_ball = False
position_behind_ball = self.get_destination_behind_ball(effective_ball_spacing)
dist_from_ball = (self.player.position - self.game_state.ball_position).norm
if self.get_alignment_with_ball_and_target() < 25 \
and compare_angle(self.player.pose.orientation,
required_orientation,
abs_tol=max(0.05, 0.05 * dist_from_ball/1000)):
self.next_state = self.grab_ball
else:
self.next_state = self.go_behind_ball
return CmdBuilder().addMoveTo(Pose(position_behind_ball, required_orientation),
cruise_speed=3,
end_speed=0,
ball_collision=collision_ball)\
.addChargeKicker().addKick(self.kick_force).build()
def next_position(self):
self.target_orientation = (self.target.position - self.game_state.ball_position).angle
self.position = (self.game_state.ball_position - Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL)
if self.start_time is not None:
if time.time() - self.start_time >= self.rotate_time:
self.rotation_sign = self._get_direction()
if compare_angle(self.target_orientation, (self.game_state.ball_position - self.player.position).angle,
VALID_DIFF_ANGLE):
self.next_state = self.halt
return self._go_to_final_position()
elif time.time() - self.iter_time >= self.switch_time:
self.iter_time = time.time()
self._switch_rotation()
if (self.player.pose.position - self.position).norm < VALID_DISTANCE:
if self.start_time is None:
self.start_time = time.time()
self.iter_time = time.time()
self.ball_collision = True
self.speed = 1
self.offset_orientation += DIFF_ANGLE * self.rotation_sign
self.position = (self.game_state.ball_position -
Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL)
if self.start_time is not None:
orientation = self.offset_orientation if time.time() - self.start_time < \
self.rotate_time else self.target_orientation
else:
orientation = self.target_orientation
return CmdBuilder().addMoveTo(Pose(self.position, orientation),
cruise_speed=self.speed,
ball_collision=self.ball_collision).build()
def _robot_can_hit_ball(self, robot):
KICK_DISTANCE_MIN = ROBOT_CENTER_TO_KICKER - BALL_RADIUS * 0.5
KICK_DISTANCE_MAX = ROBOT_CENTER_TO_KICKER + BALL_RADIUS * 1.5
MAX_ANGLE_FOR_KICK = 15
ball_position = self.game_state.ball.position
robot_to_ball = robot.pose.position - ball_position
return KICK_DISTANCE_MIN < robot_to_ball.norm < KICK_DISTANCE_MAX \
and compare_angle(robot.pose.orientation, robot_to_ball.angle, abs_tol=MAX_ANGLE_FOR_KICK)
def _robot_can_hit_ball(self, robot):
KICK_DISTANCE_MIN = ROBOT_CENTER_TO_KICKER - BALL_RADIUS * 0.5
KICK_DISTANCE_MAX = ROBOT_CENTER_TO_KICKER + BALL_RADIUS * 1.5
MAX_ANGLE_FOR_KICK = 15
ball_position = self.game_state.ball.position
robot_to_ball = robot.pose.position - ball_position
return KICK_DISTANCE_MIN < robot_to_ball.norm < KICK_DISTANCE_MAX \
and compare_angle(robot.pose.orientation, robot_to_ball.angle, abs_tol=MAX_ANGLE_FOR_KICK)
def go_behind_ball(self):
self.status_flag = Flags.WIP
orientation = (self.game_state.ball.position - self.target.position).angle
distance_behind = self._get_destination_behind_ball(self.go_behind_distance)
if (self.player.pose.position - distance_behind).norm < 200 \
and compare_angle(self.player.pose.orientation, orientation, abs_tol=0.1):
self.next_state = self.grab_ball
return CmdBuilder().addMoveTo(Pose(distance_behind, orientation),
cruise_speed=1,
ball_collision=True).build()
def go_behind_ball(self):
self.status_flag = Flags.WIP
orientation = (self.game_state.ball.position -
self.target.position).angle
distance_behind = self._get_destination_behind_ball(
self.go_behind_distance)
if (self.player.pose.position - distance_behind).norm < 200 \
and compare_angle(self.player.pose.orientation, orientation, abs_tol=0.1):
self.next_state = self.grab_ball
return CmdBuilder().addMoveTo(Pose(distance_behind, orientation),
cruise_speed=1,
ball_collision=True).build()
def grab_ball(self):
if self.position_ball_at_start is None:
self.position_ball_at_start = self.game_state.ball_position.copy()
orientation = (self.game_state.ball.position - self.target.position).angle
distance_behind = self._get_destination_behind_ball(GRAB_BALL_SPACING)
if (self.position_ball_at_start - self.game_state.ball.position).norm > BALL_DISPLACEMENT_TO_DETECT_GRABBING \
and compare_angle(self.player.pose.orientation, orientation, abs_tol=0.3):
self.next_state = self.move_ball
self.position_ball_at_start = None
self.steady_orientation = orientation
return CmdBuilder().addMoveTo(Pose(distance_behind, orientation),
cruise_speed=0.2,
ball_collision=False
).addForceDribbler().build()
def grab_ball(self):
if self.position_ball_at_start is None:
self.position_ball_at_start = self.game_state.ball_position.copy()
orientation = (self.game_state.ball.position -
self.target.position).angle
distance_behind = self._get_destination_behind_ball(GRAB_BALL_SPACING)
if (self.position_ball_at_start - self.game_state.ball.position).norm > BALL_DISPLACEMENT_TO_DETECT_GRABBING \
and compare_angle(self.player.pose.orientation, orientation, abs_tol=0.3):
self.next_state = self.move_ball
self.position_ball_at_start = None
self.steady_orientation = orientation
return CmdBuilder().addMoveTo(
Pose(distance_behind, orientation),
cruise_speed=0.2,
ball_collision=False).addForceDribbler().build()
def initialize(self):
if self.auto_update_target:
self._find_best_passing_option()
required_orientation = (self.target.position - self.game_state.ball_position).angle
dist_from_ball = (self.player.position - self.game_state.ball_position).norm
if self.get_alignment_with_ball_and_target() < 60 \
and compare_angle(self.player.pose.orientation,
required_orientation,
abs_tol=max(0.1, 0.1 * dist_from_ball/1000)):
self.next_state = self.go_behind_ball
if self._get_distance_from_ball() < KICK_DISTANCE:
self.next_state = self.kick
else:
self.next_state = self.go_behind_ball
return Idle
def go_behind_ball(self):
orientation = (self.game_state.ball_position - self.player.position).angle
ball_speed = self.game_state.ball.velocity.norm
ball_speed_modifier = (ball_speed/1000 + 1)
effective_ball_spacing = GRAB_BALL_SPACING * 3 * ball_speed_modifier
distance_behind = self.get_destination_behind_ball(effective_ball_spacing)
dist_from_ball = (self.player.position - self.game_state.ball_position).norm
if self.is_ball_going_to_collide(threshold=18) \
and compare_angle(self.player.pose.orientation, orientation,
abs_tol=max(0.1, 0.1 * dist_from_ball/100)):
self.next_state = self.wait_for_ball
else:
self.next_state = self.go_behind_ball
return CmdBuilder().addMoveTo(Pose(distance_behind, orientation),
cruise_speed=3,
end_speed=0,
ball_collision=True)\
.addChargeKicker().build()
def go_behind_ball(self):
orientation = (self.game_state.ball_position -
self.player.position).angle
ball_speed = self.game_state.ball.velocity.norm
ball_speed_modifier = (ball_speed / 1000 + 1)
effective_ball_spacing = GRAB_BALL_SPACING * 3 * ball_speed_modifier
distance_behind = self.get_destination_behind_ball(
effective_ball_spacing)
dist_from_ball = (self.player.position -
self.game_state.ball_position).norm
if self.is_ball_going_to_collide(threshold=18) \
and compare_angle(self.player.pose.orientation, orientation,
abs_tol=max(0.1, 0.1 * dist_from_ball/100)):
self.next_state = self.wait_for_ball
else:
self.next_state = self.go_behind_ball
return CmdBuilder().addMoveTo(Pose(distance_behind, orientation),
cruise_speed=3,
end_speed=0,
ball_collision=True)\
.addChargeKicker().build()
def next_position(self):
self.target_orientation = (self.target.position -
self.game_state.ball_position).angle
self.position = (
self.game_state.ball_position -
Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL)
if self.start_time is not None:
if time.time() - self.start_time >= self.rotate_time:
self.rotation_sign = self._get_direction()
if compare_angle(self.target_orientation,
(self.game_state.ball_position -
self.player.position).angle,
VALID_DIFF_ANGLE):
self.next_state = self.halt
return self._go_to_final_position()
elif time.time() - self.iter_time >= self.switch_time:
self.iter_time = time.time()
self._switch_rotation()
if (self.player.pose.position - self.position).norm < VALID_DISTANCE:
if self.start_time is None:
self.start_time = time.time()
self.iter_time = time.time()
self.ball_collision = True
self.speed = 1
self.offset_orientation += DIFF_ANGLE * self.rotation_sign
self.position = (self.game_state.ball_position -
Position.from_angle(self.offset_orientation) *
DISTANCE_FROM_BALL)
if self.start_time is not None:
orientation = self.offset_orientation if time.time() - self.start_time < \
self.rotate_time else self.target_orientation
else:
orientation = self.target_orientation
return CmdBuilder().addMoveTo(
Pose(self.position, orientation),
cruise_speed=self.speed,
ball_collision=self.ball_collision).build()
def test_compare_angle_with_same_angle():
assert compare_angle(AN_ANGLE_LESS_THAN_PI, AN_ANGLE_LESS_THAN_PI)
def check_success(self):
distance = (self.player.pose.position - self.next_pose.position).norm
if distance < POSITION_DEADZONE and compare_angle(self.player.pose.orientation, self.next_pose.orientation, abs_tol=ANGLE_TO_HALT):
return True
return False
def test_compare_angle_with_same_angle():
assert compare_angle(AN_ANGLE_LESS_THAN_PI, AN_ANGLE_LESS_THAN_PI)
def check_success(self):
distance = (self.player.pose - self.target.position).norm
return (distance < POSITION_DEADZONE) and compare_angle(
self.player.pose.orientation,
self.target.orientation,
abs_tol=ANGLE_TO_HALT)
def test_compare_angle_with_same_angle_offset_by_2pi():
assert compare_angle(AN_ANGLE_LESS_THAN_PI, AN_ANGLE_LESS_THAN_PI + 2*m.pi)
def test_compare_angle_with_different_angle():
assert not compare_angle(AN_ANGLE_LESS_THAN_PI, AN_ANGLE_GREATER_THAN_PI)
def __eq__(self, other: 'Pose') -> bool:
orientation_equal = geometry.compare_angle(self.orientation,
other.orientation,
abs_tol=ORIENTATION_ABSOLUTE_TOLERANCE)
position_equal = self.position == other.position
return position_equal and orientation_equal
def check_success(self):
distance = (self.player.pose - self.target.position).norm
return (distance < POSITION_DEADZONE) and compare_angle(self.player.pose.orientation,
self.target.orientation, abs_tol=ANGLE_TO_HALT)
def test_compare_angle_with_tolerance():
assert compare_angle(AN_ANGLE_LESS_THAN_PI, AN_ANGLE_LESS_THAN_PI+0.99, abs_tol=1)
def test_bisector_angle_between_the_intersection_is_the_same():
A_RANDOM_POINT = Position(np.random.randn(1, 1), 1)
inter = find_bisector_of_triangle(A_RANDOM_POINT, A_LINE[0], A_LINE[1])
angle1 = angle_between_three_points(A_LINE[0], A_RANDOM_POINT, inter)
angle2 = angle_between_three_points(inter, A_RANDOM_POINT, A_LINE[1])
assert compare_angle(angle1, angle2, abs_tol=0.01)
def test_compare_angle_with_tolerance():
assert compare_angle(AN_ANGLE_LESS_THAN_PI,
AN_ANGLE_LESS_THAN_PI + 0.99,
abs_tol=1)
def test_compare_angle_with_different_angle():
assert not compare_angle(AN_ANGLE_LESS_THAN_PI, AN_ANGLE_GREATER_THAN_PI)
def test_compare_angle_with_same_angle_offset_by_2pi():
assert compare_angle(AN_ANGLE_LESS_THAN_PI,
AN_ANGLE_LESS_THAN_PI + 2 * m.pi)
def test_bisector_angle_between_the_intersection_is_the_same():
A_RANDOM_POINT = Position(np.random.randn(1, 1), 1)
inter = find_bisector_of_triangle(A_RANDOM_POINT, A_LINE[0], A_LINE[1])
angle1 = angle_between_three_points(A_LINE[0], A_RANDOM_POINT, inter)
angle2 = angle_between_three_points(inter, A_RANDOM_POINT, A_LINE[1])
assert compare_angle(angle1, angle2, abs_tol=0.01)
