Merge pull request #480 from RoboCupULaval/feature/ajustement_go_kick

Feature/ajustement go kick

Engine/filters/path_smoother.py

@@ -9,7 +9,6 @@
 
 def path_smoother(path, speed, end_speed) -> Tuple[Path, float]:
 
-
     if len(path) < 3:
         return path, end_speed
 
@@ -25,12 +24,11 @@ def path_smoother(path, speed, end_speed) -> Tuple[Path, float]:
         point_list = [path.start, p4, p5]
     turn_radius, _ = compute_turn_radius(*point_list, speed, acc=MAX_LINEAR_ACCELERATION)
     next_speed = speed_in_corner(turn_radius, acc=MAX_LINEAR_ACCELERATION)
-    #print(next_speed)
     return Path.from_sequence(point_list), next_speed
 
 
 def compute_circle_points(p1, p2, p3, speed: float, acc: float) -> Tuple[Position, Position]:
-    turn_radius, deviation_from_path = compute_turn_radius(p1, p2, p3, speed, acc)
+    turn_radius, deviation_from_path = compute_turn_radius(p1, p2, p3, speed, acc=acc)
     distance_on_segment = sqrt((deviation_from_path + turn_radius) ** 2 - turn_radius ** 2)
     p4 = point_on_segment(p2, p1, distance_on_segment)
     p5 = point_on_segment(p2, p3, distance_on_segment)
@@ -45,25 +43,35 @@ def speed_in_corner(radius: float, acc: float) -> float:
     return speed
 
 
-def compute_turn_radius(p1, p2, p3, speed:float , max_deviation:float=100, acc:float=MAX_LINEAR_ACCELERATION) -> Tuple[float, float]:
+def compute_turn_radius(p1, p2, p3, speed: float, max_deviation: float=50, acc: float=MAX_LINEAR_ACCELERATION) -> Tuple[float, float]:
     """Assume the raw path is p1->p2->p3.
        Deviation is compute from p2 to the circle with a line passing by the center of the circle."""
 
     radius_at_const_speed = speed ** 2 / acc
     path_angle = wrap_to_pi((p3 - p2).angle - (p1 - p2).angle)
     const_speed_deviation = deviation(radius_at_const_speed, path_angle)
     if const_speed_deviation < max_deviation:
-        deviation_from_path = const_speed_deviation
+        speed_deviation = const_speed_deviation
         turn_radius = radius_at_const_speed
     else:
-        deviation_from_path = max_deviation
-        turn_radius = deviation_from_path / (1 / sin(path_angle / 2) - 1)
-
-    return abs(turn_radius), deviation_from_path
+        speed *= 0.95
+        radius = speed ** 2 / acc
+        speed_deviation = deviation(radius, path_angle)
+        turn_radius = speed_deviation / (1 / sin(path_angle / 2) - 1)
+        while speed_deviation > max_deviation:
+            speed *= 0.9
+            radius = speed ** 2 / acc
+            speed_deviation = deviation(radius, path_angle)
+            turn_radius = speed_deviation / (1 / sin(path_angle / 2) - 1)
+            if abs(speed) < 0.01:
+                speed_deviation = 0
+                turn_radius = 0
+                break
+    return abs(turn_radius), speed_deviation
 
 
 def deviation(radius: float, theta: float) -> float:
-    return abs(radius / sin(theta / 2) - radius) if sin(theta/2) != 0 else 0
+    return abs(radius / sin(theta / 2)) - radius if sin(theta/2) != 0 else 0
 
 
 def point_on_segment(start: Position, end: Position, distance: float):

ai/STA/Tactic/go_kick.py

@@ -100,24 +100,26 @@ def grab_ball(self):
         if self._get_distance_from_ball() < KICK_DISTANCE:
             self.next_state = self.kick
             self.kick_last_time = time.time()
-
+        ball_speed = self.game_state.ball.velocity.norm
         orientation = (self.target.position - self.game_state.ball_position).angle
-        distance_behind = self.get_destination_behind_ball(GRAB_BALL_SPACING)
+        distance_behind = self.get_destination_behind_ball(GRAB_BALL_SPACING * (1 + ball_speed / 1000))
         return CmdBuilder().addMoveTo(Pose(distance_behind, orientation),
                                       cruise_speed=3,
                                       ball_collision=False)\
                            .addForceDribbler()\
                            .addChargeKicker()\
-                           .addKick(self.kick_force)\
                            .build()
 
     def kick(self):
         if self.auto_update_target:
             self._find_best_passing_option()
+        if not self.is_able_to_grab_ball_directly(0.8):
+            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)
-        behind_ball = self.game_state.ball_position - normalize(player_to_target) * (BALL_RADIUS + ROBOT_CENTER_TO_KICKER)
+        behind_ball = self.game_state.ball_position + normalize(player_to_target) * (ROBOT_CENTER_TO_KICKER)
         orientation = (self.target.position - self.game_state.ball_position).angle
 
         return CmdBuilder().addMoveTo(Pose(behind_ball, orientation),
@@ -187,5 +189,4 @@ def is_able_to_grab_ball_directly(self, threshold):
         vec_target_to_ball = normalize(self.game_state.ball.position - self.target.position)
         alignement_behind = np.dot(vec_target_to_ball.array,
                                    (normalize(self.player.position - self.game_state.ball_position)).array)
-
         return threshold < alignement_behind

tests/STA/perfect_sim.py

@@ -71,7 +71,7 @@ def _apply_cmd(self, ia_cmd):
             self.has_charge_kick = True
 
     def _robot_can_hit_ball(self, robot):
-        KICK_DISTANCE_MIN = ROBOT_CENTER_TO_KICKER + BALL_RADIUS * 0.5
+        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