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 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 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_same_angle_offset_by_2pi(): assert compare_angle(AN_ANGLE_LESS_THAN_PI, AN_ANGLE_LESS_THAN_PI + 2 * m.pi)