Merge pull request #479 from RoboCupULaval/fix/goal_kick_outside

Fix/goal kick outside

Author: PhiBabin

Engine/robot.py

@@ -5,7 +5,7 @@
 
 MAX_LINEAR_SPEED = 4000  # mm/s
 MAX_LINEAR_ACCELERATION = 4000  # mm/s^2
-MAX_ANGULAR_SPEED = 100  # rad/s
+MAX_ANGULAR_SPEED = 50  # rad/s
 MAX_ANGULAR_ACC = 3  # rad/s^2
 MIN_LINEAR_SPEED = 200  # mm/s Speed near zero, but still move the robot
 

Util/geometry.py

@@ -122,6 +122,10 @@ def pad(cls, area, padding=0):
     def from_limits(cls, top, bottom, right, left):
         return cls(Position(left, top), Position(right, bottom))
 
+    @classmethod
+    def flip_x(cls, area):
+        return Area.from_limits(area.top, area.bottom, -area.left, -area.right)
+
 
 def find_bisector_of_triangle(c, a, b):
     """

ai/GameDomainObjects/field.py

@@ -164,8 +164,7 @@ def _update_field_const(self):
         self.our_goal_area = Area(self.field_lines["RightPenaltyStretch"].p2,
                                   self.field_lines["RightFieldLeftPenaltyStretch"].p1)
 
-        self.their_goal_area = Area(self.field_lines["RightPenaltyStretch"].p2.flip_x(),
-                                    self.field_lines["RightFieldLeftPenaltyStretch"].p1.flip_x())
+        self.their_goal_area = Area.flip_x(self.our_goal_area)
 
         self.goal_line = Line(p1=Position(self.our_goal_x, +self.goal_width / 2),
                                   p2=Position(self.our_goal_x, -self.goal_width / 2))
@@ -174,6 +173,11 @@ def _update_field_const(self):
                                   p2=Position(self.our_goal_x, -self.goal_width / 2))
         self.their_goal_line = Line(p1=Position(self.their_goal_x, +self.goal_width / 2),
                                     p2=Position(self.their_goal_x, -self.goal_width / 2))
+        self.behind_our_goal_line = Area.from_limits(self.our_goal_area.top,
+                                                     self.our_goal_area.bottom,
+                                                     self.our_goal_area.right + 50 * self.goal_depth,
+                                                     self.our_goal_area.left)
+        self.behind_their_goal_line = Area.flip_x(self.behind_our_goal_line)
 
         self.free_kick_avoid_area = Area.pad(self.their_goal_area,
                                              INDIRECT_KICK_OFFSET + KEEPOUT_DISTANCE_FROM_GOAL)

ai/STA/Strategy/smart_stop.py

@@ -48,11 +48,11 @@ def obstacles(self):
     def required_roles(cls):
         return [Role.GOALKEEPER,
                 Role.FIRST_ATTACK,
-                Role.FIRST_DEFENCE]
+                Role.SECOND_ATTACK]
 
     @classmethod
     def optional_roles(cls):
-        return [Role.SECOND_ATTACK,
+        return [Role.FIRST_DEFENCE,
                 Role.MIDDLE,
                 Role.SECOND_DEFENCE]
 

ai/STA/Tactic/align_to_defense_wall.py

@@ -134,7 +134,9 @@ def go_kick(self):
         if self.go_kick_tactic is None:
             self.go_kick_tactic = GoKick(self.game_state, self.player, target=self.game_state.field.their_goal_pose)
 
-        if not self._should_ball_be_kick_by_wall() or self.game_state.field.is_ball_in_our_goal_area():
+        if not self._should_ball_be_kick_by_wall() \
+                or self.game_state.field.is_ball_in_our_goal_area() \
+                or not self._is_closest_not_goaler(self.player):
             self.go_kick_tactic = None
             self.next_state = self.main_state
             return Idle

ai/STA/Tactic/go_to_position.py

@@ -2,6 +2,7 @@
 
 from typing import List
 
+from Debug.debug_command_factory import DebugCommandFactory, VIOLET
 from Util import Pose, Position
 from Util.ai_command import CmdBuilder
 from Util.constant import POSITION_DEADZONE, ANGLE_TO_HALT
@@ -34,3 +35,4 @@ 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)
+

ai/STA/Tactic/goalkeeper.py

@@ -5,15 +5,15 @@
 
 from Debug.debug_command_factory import DebugCommandFactory
 from ai.Algorithm.evaluation_module import player_with_ball, player_pointing_toward_point, \
-    player_pointing_toward_segment
+    player_pointing_toward_segment, closest_players_to_point
 
 __author__ = 'RoboCupULaval'
 
 from typing import List
 
 from Util import Pose, Position
 from Util.ai_command import MoveTo, Idle
-from Util.constant import ROBOT_RADIUS, KEEPOUT_DISTANCE_FROM_GOAL
+from Util.constant import ROBOT_RADIUS, KEEPOUT_DISTANCE_FROM_GOAL, ROBOT_DIAMETER
 from Util.geometry import intersection_line_and_circle, intersection_between_lines, \
     closest_point_on_segment, find_bisector_of_triangle, Area
 from ai.GameDomainObjects import Player
@@ -54,7 +54,7 @@ def defense_dumb(self):
 
     def defense(self):
         # Prepare to block the ball
-        if self.game_state.field.is_ball_in_our_goal_area() and self.game_state.ball.is_immobile():
+        if self._is_ball_safe_to_kick() and self.game_state.ball.is_immobile():
             self.next_state = self.clear
 
         if self._ball_going_toward_goal():
@@ -104,28 +104,38 @@ def intercept(self):
                       end_speed=0,
                       ball_collision=False)
 
-    def _ball_going_toward_goal(self):
-        upper_angle = (self.game_state.ball.position - self.GOAL_LINE.p2).angle + 5 * np.pi / 180.0
-        lower_angle = (self.game_state.ball.position - self.GOAL_LINE.p1).angle - 5 * np.pi / 180.0
-        ball_speed = self.game_state.ball.velocity.norm
-        return (ball_speed > self.DANGER_BALL_VELOCITY and self.game_state.ball.velocity.x > 0) or \
-               (ball_speed > self.MOVING_BALL_VELOCITY and upper_angle <= self.game_state.ball.velocity.angle <= lower_angle)
-
     def clear(self):
         # Move the ball to outside of the penality zone
         if self.go_kick_tactic is None:
             self.go_kick_tactic = GoKick(self.game_state,
                                          self.player,
                                          auto_update_target=True,
                                          go_behind_distance=1.2*GRAB_BALL_SPACING,
-                                         forbidden_areas=self.forbidden_areas) # make it easier
-        if not self.game_state.field.is_ball_in_our_goal_area():
+                                         forbidden_areas=self.forbidden_areas)  # make it easier
+        if not self._is_ball_safe_to_kick():
             self.next_state = self.defense
             self.go_kick_tactic = None
             return Idle
         else:
             return self.go_kick_tactic.exec()
 
+    def _ball_going_toward_goal(self):
+        upper_angle = (self.game_state.ball.position - self.GOAL_LINE.p2).angle + 5 * np.pi / 180.0
+        lower_angle = (self.game_state.ball.position - self.GOAL_LINE.p1).angle - 5 * np.pi / 180.0
+        ball_speed = self.game_state.ball.velocity.norm
+        return (ball_speed > self.DANGER_BALL_VELOCITY and self.game_state.ball.velocity.x > 0) or \
+               (ball_speed > self.MOVING_BALL_VELOCITY and upper_angle <= self.game_state.ball.velocity.angle <= lower_angle)
+
+    def _is_ball_safe_to_kick(self):
+        # Since defender can not kick the ball while inside the goal there are position where the ball is unreachable
+        # The goalee must leave the goal area and kick the ball
+        goal_area = self.game_state.field.our_goal_area
+        width = KEEPOUT_DISTANCE_FROM_GOAL + ROBOT_DIAMETER
+        area_in_front_of_goal = Area.from_limits(goal_area.top, goal_area.bottom,
+                                                 goal_area.left, goal_area.left - width)
+        return self.game_state.field.is_ball_in_our_goal_area() or \
+               area_in_front_of_goal.point_inside(self.game_state.ball.position) and self._no_enemy_around_ball()
+
     def _best_target_into_goal(self):
 
         enemy_player_with_ball = player_with_ball(min_dist_from_ball=200, our_team=False)
@@ -159,3 +169,10 @@ def debug_cmd(self):
         else:
             return []
 
+    def _no_enemy_around_ball(self):
+        closest = closest_players_to_point(self.game_state.ball_position, our_team=False)
+        if len(closest) == 0:
+            return True
+        DANGEROUS_ENEMY_MIN_DISTANCE = 500
+        return closest[0].distance > DANGEROUS_ENEMY_MIN_DISTANCE
+

ai/STA/Tactic/tactic.py

@@ -3,7 +3,7 @@
 
 import logging
 
-from Debug.debug_command_factory import DebugCommandFactory
+from Debug.debug_command_factory import DebugCommandFactory, VIOLET
 from Util import Pose, Position
 from Util.ai_command import AICommand
 from Util.constant import ROBOT_RADIUS, KEEPOUT_DISTANCE_FROM_GOAL
@@ -56,11 +56,14 @@ def __init__(self, game_state: GameState, player: Player, target: Optional[Pose]
                                          field.bottom,
                                          field.left - field.boundary_width,
                                          field.left - 100 * field.boundary_width)
+            # Those limits are for ulaval local only
             areas = [
                 top_area,
                 bottom_area,
                 right_area,
-                left_area]
+                left_area,
+                self.field.behind_our_goal_line,
+                self.field.behind_their_goal_line]
             #areas = []
             self.forbidden_areas = [Area.pad(area, KEEPOUT_DISTANCE_FROM_GOAL) for area in areas]
             self.forbidden_areas += [self.game_state.field.their_goal_forbidden_area,
@@ -86,13 +89,19 @@ def exec(self) -> AICommand:
     def _check_for_forbidden_area(self, next_ai_command):
         old_target_position = next_ai_command.target.position
         target_to_position = Line(self.player.position, next_ai_command.target.position)
+        closest_new_target = None
         for area in self.forbidden_areas:
             if old_target_position in area:
                 target_position = self._find_best_next_target(area, old_target_position, self.player.position, target_to_position)
                 new_target = Pose(target_position, next_ai_command.target.orientation)
 
-                # This trailing _ is not for protected access, it was add to avoid a name conflict with the function replace ;)
-                return next_ai_command._replace(target=new_target)
+                if closest_new_target is None \
+                        or (closest_new_target - self.player.position).norm > (new_target - self.player.position).norm:
+                    closest_new_target = new_target
+
+        if closest_new_target is not None:
+            # This trailing _ is not for protected access, it was add to avoid a name conflict with the function replace
+            return next_ai_command._replace(target=closest_new_target )
         return next_ai_command
 
     def _find_best_next_target(self, area: Area, old_target_position, player_position: Position, target_to_position: Line):
@@ -109,12 +118,18 @@ def _find_best_next_target(self, area: Area, old_target_position, player_positio
                 return intersections[1]
 
     def debug_cmd(self):
-        #return [DebugCommandFactory().area(area) for area in self.forbidden_areas]
-        return []
+        cmds = []
+        [cmds.extend(DebugCommandFactory().area(area, color=VIOLET)) for area in self.forbidden_areas]
+        return cmds
+        #return []
 
     @classmethod
     def name(cls):
         return cls.__name__
 
+    @property
+    def field(self):
+        return self.game_state.field
+
     def __str__(self):
         return self.__class__.__name__