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 _go_to_final_position(self):
position = self.game_state.ball_position - Position.from_angle(
self.target_orientation) * DISTANCE_FROM_BALL
return CmdBuilder().addMoveTo(
Pose(position, self.target_orientation),
cruise_speed=self.speed,
ball_collision=self.ball_collision).build()
def validate_kick(self):
if (self.game_state.ball_velocity.norm > 600) or (self._get_distance_from_ball() > KICK_SUCCEED_THRESHOLD):
self.next_state = self.halt
else:
self.next_state = self.main_state
return CmdBuilder().build()
def move(self):
if self.check_success():
self.status_flag = Flags.SUCCESS
else:
self.status_flag = Flags.WIP
return CmdBuilder().addMoveTo(self.target,
cruise_speed=self.cruise_speed).build()
def kick_charge(self):
if time.time() - self.cmd_last_time > COMMAND_DELAY:
self.next_state = self.main_state
self.cmd_last_time = time.time()
return CmdBuilder().addChargeKicker().build()
def kick(self):
self.next_state = self.validate_kick
self.tries_flag += 1
player_to_target = (self.target.position - self.player.pose.position)
behind_ball = self.game_state.ball_position - normalize(player_to_target) * (BALL_RADIUS + ROBOT_CENTER_TO_KICKER)
orientation = (self.target.position - self.game_state.ball_position).angle
return CmdBuilder().addMoveTo(Pose(behind_ball, orientation),
ball_collision=False).addKick(self.kick_force).build()
def validate_kick(self):
if self.game_state.ball.is_moving_fast() or self._get_distance_from_ball() > KICK_SUCCEED_THRESHOLD:
self.next_state = self.halt
elif self.kick_last_time - time.time() < VALIDATE_KICK_DELAY:
self.next_state = self.kick
else:
self.status_flag = Flags.INIT
self.next_state = self.go_behind_ball
return CmdBuilder().addForceDribbler().build()
def move_to_ball(self):
self.status_flag = Flags.WIP
self.target = self.game_state.our_team.available_players[self.robot_to_follow_id].pose
if (self.player.pose.position - self.target.position).norm < POSITION_DEADZONE + ROBOT_RADIUS:
self.next_state = self.halt
else:
self.next_state = self.move_to_ball
return CmdBuilder().addMoveTo(self.target).build()
def next_corner(self):
orientation = (self.points[0].position - self.player.position).angle
if (self.player.position -
self.points[0].position).norm < ROTATE_DISTANCE:
self.points.rotate()
return CmdBuilder().addMoveTo(Pose(self.points[0].position,
orientation),
cruise_speed=self.cruise_speed).build()
def kick(self):
if self._get_distance_from_ball() > 300:
self.next_state = self.halt
self.last_time = time.time()
elif time.time() - self.last_time < COMMAND_DELAY:
self.next_state = self.kick
else:
self.next_state = self.kick_charge
return CmdBuilder().addKick(KickForce.HIGH).addMoveTo(
self.target).build()
def get_away_from_ball(self):
if self._check_success(
) and self._get_distance_from_ball() > KEEPOUT_DISTANCE_FROM_BALL:
self.next_state = self.halt
self.status_flag = Flags.SUCCESS
target_to_player = normalize(self.player.position -
self.target.position)
pos_away_from_ball = 1.2 * KEEPOUT_DISTANCE_FROM_BALL * target_to_player + self.target.position
# Move to a position away from ball
return CmdBuilder().addMoveTo(
Pose(pos_away_from_ball,
self.player.orientation)).addStopDribbler().build()
def wait_for_ball_stop_spinning(self):
if self.wait_timer is None:
self.wait_timer = time.time()
if time.time() - self.wait_timer > TIME_TO_WAIT_FOR_BALL_STOP_MOVING:
self.next_state = self.get_away_from_ball
self.wait_timer = None
if self._has_ball_quit_dribbler():
self._fetch_ball()
return CmdBuilder().addMoveTo(
Pose(self.player_target_position, self.steady_orientation),
ball_collision=False,
enable_pathfinder=False).addStopDribbler().build()
def move_ball(self):
if self._check_success():
self.next_state = self.wait_for_ball_stop_spinning
elif self._has_ball_quit_dribbler():
self._fetch_ball()
ball_position = self.game_state.ball_position
ball_to_target_dir = normalize(self.target.position - ball_position)
self.player_target_position = ROBOT_CENTER_TO_KICKER * ball_to_target_dir + self.target.position
return CmdBuilder().addMoveTo(
Pose(self.player_target_position, self.steady_orientation),
cruise_speed=self.move_ball_cruise_speed,
ball_collision=False,
enable_pathfinder=False).addForceDribbler().build()
def wait_for_ball(self):
perp_vec = perpendicular(self.player.position -
self.game_state.ball.position)
component_lateral = perp_vec * np.dot(
perp_vec.array,
normalize(self.game_state.ball.velocity).array)
small_segment_len = np.sqrt(1 - component_lateral.norm**2)
latteral_move = component_lateral / small_segment_len * (
self.player.position - self.game_state.ball.position).norm
if self._get_distance_from_ball() < HAS_BALL_DISTANCE:
self.next_state = self.halt
return self.halt()
if not self.is_ball_going_to_collide(threshold=18):
self.next_state = self.wait_for_ball
return CmdBuilder().build()
orientation = (self.game_state.ball_position -
self.player.position).angle
return CmdBuilder().addMoveTo(
Pose(self.player.position + latteral_move.view(Position),
orientation),
cruise_speed=3,
end_speed=0,
ball_collision=False).addChargeKicker().build()
def grab_ball(self):
if not self.is_ball_going_to_collide(threshold=18):
self.next_state = self.go_behind_ball
if self._get_distance_from_ball() < HAS_BALL_DISTANCE:
self.next_state = self.halt
return self.halt()
orientation = (self.game_state.ball_position -
self.player.position).angle
distance_behind = self.get_destination_behind_ball(GRAB_BALL_SPACING)
return CmdBuilder().addMoveTo(
Pose(distance_behind, orientation),
cruise_speed=3,
end_speed=0,
ball_collision=False).addChargeKicker().build()
def kick(self):
if self.auto_update_target:
self._find_best_passing_option()
if self.get_alignment_with_ball_and_target() > 45:
self.next_state = self.go_behind_ball
return self.go_behind_ball()
self.next_state = self.validate_kick
player_to_target = (self.target.position - self.player.pose.position)
position_behind_ball = self.game_state.ball_position + normalize(player_to_target) * ROBOT_CENTER_TO_KICKER
required_orientation = (self.target.position - self.game_state.ball_position).angle
return CmdBuilder().addMoveTo(Pose(position_behind_ball, required_orientation), ball_collision=False)\
.addKick(self.kick_force)\
.addForceDribbler().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.auto_update_target:
self._find_best_passing_option()
if self.get_alignment_with_ball_and_target() > 45:
self.next_state = self.go_behind_ball
if self._get_distance_from_ball() < KICK_DISTANCE:
self.next_state = self.kick
self.kick_last_time = time.time()
required_orientation = (self.target.position - self.game_state.ball_position).angle
position_behind_ball = self.get_destination_behind_ball(GRAB_BALL_SPACING)
return CmdBuilder().addMoveTo(Pose(position_behind_ball, required_orientation), ball_collision=False)\
.addForceDribbler()\
.addKick(self.kick_force)\
.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 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 main_state(self):
target_player = closest_player_to_point(self.game_state.ball_position,
our_team=False).player
orientation_opponent = np.array([
math.cos(target_player.pose.orientation),
math.sin(target_player.pose.orientation)
])
destination_position = target_player.pose.position + self.distance * orientation_opponent
ball_to_player = self.game_state.ball_position - self.player.pose.orientation
destination_orientation = ball_to_player.angle
if self.check_success():
self.status_flag = Flags.SUCCESS
else:
self.status_flag = Flags.WIP
return CmdBuilder().addMoveTo(
Pose(destination_position, destination_orientation),
cruise_speed=self.cruise_speed,
ball_collision=self.ball_collision,
end_speed=self.end_speed).addChargeKicker().addForceDribbler(
).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 kick_charge(self):
if time.time() - self.last_time > COMMAND_DELAY:
self.next_state = self.get_behind_ball
self.last_time = time.time()
return CmdBuilder().addKick().addForceDribbler().build()
