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