Allow defenders to intercept passes without slowing down (UBC-Thunderbots#3479)

Author: zuperzane

src/software/ai/evaluation/intercept.cpp

@@ -87,3 +87,55 @@ std::optional<std::pair<Point, Duration>> findBestInterceptForBall(const Ball &b
 
     return std::make_pair(best_ball_intercept_pos, time_to_ball_pos);
 }
+
+Point findOvershootInterceptPosition(const Robot &robot, const Point intercept_position,
+                                     const Field &field, Duration ball_intercept_time,
+                                     double step_speed, bool restrict_to_defense_area)
+{
+    Point new_destination = intercept_position;
+    Point base_position   = intercept_position;
+    Point best_position   = intercept_position;
+    double final_speed    = step_speed;
+    bool finished         = false;
+    double max_speed      = robot.robotConstants().robot_max_speed_m_per_s;
+    double max_acc        = robot.robotConstants().robot_max_acceleration_m_per_s_2;
+
+    while (!finished)
+    {
+        Vector final_velocity =
+            (new_destination - robot.position()).normalize(final_speed);
+
+        double extra_length = (final_speed * final_speed) / (2.0 * max_acc);
+        new_destination     = base_position + final_velocity.normalize(extra_length);
+
+        bool in_bounds  = contains(field.fieldBoundary(), new_destination);
+        bool in_defense = field.pointInFriendlyDefenseArea(new_destination);
+
+        if ((!restrict_to_defense_area ^ in_defense) && in_bounds)
+        {
+            if (restrict_to_defense_area)
+            {
+                best_position = new_destination;
+            }
+
+            if (robot.getTimeToPosition(base_position, final_velocity) <
+                ball_intercept_time)
+            {
+                best_position = new_destination;
+                finished      = true;
+            }
+        }
+        else
+        {
+            finished = true;
+        }
+
+        final_speed += step_speed;
+        if (final_speed > max_speed)
+        {
+            finished = true;
+        }
+    }
+
+    return best_position;
+}

src/software/ai/evaluation/intercept.h

@@ -22,3 +22,22 @@
 std::optional<std::pair<Point, Duration>> findBestInterceptForBall(const Ball &ball,
                                                                    const Field &field,
                                                                    const Robot &robot);
+
+
+/**
+ * Attempts to find a reachable overshoot destination for intercepting the ball,
+ * adjusting final speed in steps up to the robot's max speed.
+ *
+ * @param robot The robot attempting to intercept.
+ * @param intercept_position The ideal intercept point without overshoot.
+ * @param field The field.
+ * @param ball_intercept_time Time the ball will take to reach the base position.
+ * @param step_speed Speed increment for each overshoot iteration.
+ * @param restrict_to_defense_area Whether to restrict overshoot to the friendly defense
+ * area.
+ * @return The best reachable intercept point (possibly overshot), or base_position if no
+ * improvement.
+ */
+Point findOvershootInterceptPosition(const Robot &robot, const Point intercept_position,
+                                     const Field &field, Duration ball_intercept_time,
+                                     double step_speed, bool restrict_to_defense_area);

src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp

@@ -1,6 +1,7 @@
 #include "software/ai/hl/stp/tactic/goalie/goalie_fsm.h"
 
 #include "software/ai/evaluation/find_open_areas.h"
+#include "software/ai/evaluation/intercept.h"
 #include "software/ai/hl/stp/tactic/move_primitive.h"
 #include "software/math/math_functions.h"
 
@@ -183,53 +184,18 @@ void GoalieFSM::panic(const Update &event)
     Angle goalie_orientation =
         (event.common.world_ptr->ball().position() - goalie_pos).orientation();
 
-    Point save_pos = goalie_pos;
-    // First we are trying to determine how much time do we have to reach the ball
+    // Here we are determining how much time do we have to reach the ball
     Duration ball_intercept_time = Duration::fromSeconds(
-        (event.common.world_ptr->ball().position() - save_pos).length() /
+        (event.common.world_ptr->ball().position() - goalie_pos).length() /
         (std::max(std::numeric_limits<double>::epsilon(),
                   event.common.world_ptr->ball().velocity().length())));
 
+
     if (event.common.robot.getTimeToPosition(goalie_pos) > ball_intercept_time)
     {
-        Point new_destination = save_pos;
-        double final_speed    = GOALIE_STEP_SPEED_M_PER_S;
-        bool finished         = false;
-        double max_speed = event.common.robot.robotConstants().robot_max_speed_m_per_s;
-        double max_acc =
-            event.common.robot.robotConstants().robot_max_acceleration_m_per_s_2;
-        while (!finished)
-        {
-            Vector final_velocity =
-                (new_destination - event.common.robot.position()).normalize(final_speed);
-
-            // What's happening here is we are using v_o^2=2*d*a to determine the extra
-            // distance the goalie will be forced to travel if they intercept the ball
-            // with final_speed and then immediately decelerate
-            double extra_length = (final_speed * final_speed) / (2.0 * max_acc);
-            new_destination     = save_pos + final_velocity.normalize(extra_length);
-            if (event.common.world_ptr->field().pointInFriendlyDefenseArea(
-                    new_destination))
-            {
-                goalie_pos = new_destination;
-                if (event.common.robot.getTimeToPosition(save_pos, final_velocity) <
-                    ball_intercept_time)
-                {
-                    // If we found that the robot can make it before the ball we consider
-                    // the while loop finished
-                    finished = true;
-                }
-            }
-            else
-            {
-                finished = true;
-            }
-            final_speed += GOALIE_STEP_SPEED_M_PER_S;
-            if (final_speed > max_speed)
-            {
-                finished = true;
-            }
-        }
+        goalie_pos = findOvershootInterceptPosition(
+            event.common.robot, goalie_pos, event.common.world_ptr->field(),
+            ball_intercept_time, GOALIE_STEP_SPEED_M_PER_S, true);
     }
 
     event.common.set_primitive(std::make_unique<MovePrimitive>(

src/software/ai/hl/stp/tactic/pass_defender/pass_defender_fsm.cpp

@@ -1,6 +1,7 @@
 #include "software/ai/hl/stp/tactic/pass_defender/pass_defender_fsm.h"
 
 #include "proto/message_translation/tbots_protobuf.h"
+#include "software/ai/evaluation/intercept.h"
 #include "software/ai/hl/stp/tactic/move_primitive.h"
 #include "software/geom/algorithms/closest_point.h"
 
@@ -69,6 +70,30 @@ void PassDefenderFSM::interceptBall(const Update& event)
             // that the defender can move to and intercept the pass
             intercept_position = closestPoint(
                 robot_position, Line(ball.position(), ball.position() + ball.velocity()));
+
+            // Here we are using v_f^2=v_0^2+2*a*d to calculate the final
+            // speed and after that the average speed
+            double ball_vel = (event.common.world_ptr->ball().velocity().length());
+            double ball_dis =
+                (event.common.world_ptr->ball().position() - intercept_position).length();
+            double final_vel =
+                sqrt(ball_vel * ball_vel +
+                     2 * ball_dis *
+                         BALL_ROLLING_FRICTION_DECELERATION_METERS_PER_SECOND_SQUARED);
+            double avg_vel               = (final_vel + ball_vel) / 2.0;
+            Duration ball_intercept_time = Duration::fromSeconds(
+                ball_dis / (std::max(std::numeric_limits<double>::epsilon(), avg_vel)));
+
+            // Here we check if we can make it in time to the position and stop in time
+            // otherwise we will attempt to overshoot the position and intercept it
+            if (event.common.robot.getTimeToPosition(intercept_position) >
+                ball_intercept_time)
+            {
+                intercept_position = findOvershootInterceptPosition(
+                    event.common.robot, intercept_position,
+                    event.common.world_ptr->field(), ball_intercept_time,
+                    DEFENDER_STEP_SPEED_M_PER_S, false);
+            }
         }
 
         auto face_ball_orientation = (ball.position() - robot_position).orientation();

src/software/ai/hl/stp/tactic/pass_defender/pass_defender_fsm.h

@@ -40,7 +40,7 @@ struct PassDefenderFSM : public DefenderFSMBase
 
     // The maximum angle difference to determine if ball has been kicked in
     // the approximate direction of the defender
-    static constexpr Angle MAX_PASS_ANGLE_DIFFERENCE = Angle::fromDegrees(30);
+    static constexpr Angle MAX_PASS_ANGLE_DIFFERENCE = Angle::fromDegrees(35);
 
     // The minimum angle difference between a ball's trajectory and
     // pass_orientation for which we can consider a pass to be deflected
@@ -139,5 +139,8 @@ struct PassDefenderFSM : public DefenderFSMBase
 
    private:
     Angle pass_orientation;
+    // The step amount between speeds we check that the defender is observed to
+    // go at during the interception
+    static constexpr double DEFENDER_STEP_SPEED_M_PER_S = 0.2;
     TbotsProto::PassDefenderConfig pass_defender_config;
 };

src/software/ai/hl/stp/tactic/pass_defender/pass_defender_tactic_test.py

@@ -209,6 +209,14 @@ def test_avoid_intercept_scenario(
             tbots_cpp.Point(2.2, 0),
             True,
         ),
+        # Intercept faster pass angled away from defender, steal
+        (
+            tbots_cpp.Point(2, 0),
+            tbots_cpp.Vector(-4, 0),
+            tbots_cpp.Point(-2, 1.25),
+            tbots_cpp.Point(2.2, 0),
+            True,
+        ),
         # Intercept diagonal pass, steal
         (
             tbots_cpp.Point(-1, -3),
@@ -217,6 +225,14 @@ def test_avoid_intercept_scenario(
             tbots_cpp.Point(-0.8, 0),
             True,
         ),
+        # Intercept faster diagonal pass, steal
+        (
+            tbots_cpp.Point(0.75, 0),
+            tbots_cpp.Vector(-3.5, -2),
+            tbots_cpp.Point(-3, 0),
+            tbots_cpp.Point(1, 0),
+            True,
+        ),
         # Enemy too close, no steal
         (
             tbots_cpp.Point(0, 0),