Zane kaber/movement speed (UBC-Thunderbots#3473)

Author: zuperzane

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

@@ -4,6 +4,7 @@
 #include "software/ai/hl/stp/tactic/move_primitive.h"
 #include "software/math/math_functions.h"
 
+
 Point GoalieFSM::getGoaliePositionToBlock(
     const Ball &ball, const Field &field,
     TbotsProto::GoalieTacticConfig goalie_tactic_config)
@@ -182,6 +183,55 @@ 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
+    Duration ball_intercept_time = Duration::fromSeconds(
+        (event.common.world_ptr->ball().position() - save_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;
+            }
+        }
+    }
+
     event.common.set_primitive(std::make_unique<MovePrimitive>(
         event.common.robot, goalie_pos, goalie_orientation, max_allowed_speed_mode,
         TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::OFF,

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

@@ -253,6 +253,9 @@ struct GoalieFSM
 
    private:
     static constexpr double BALL_RETRIEVED_THRESHOLD = 0.2;
+    // The step amount between speeds we check that the goalie is observed to
+    // go at during the save
+    static constexpr double GOALIE_STEP_SPEED_M_PER_S = 0.2;
     // The goalie tactic config
     TbotsProto::GoalieTacticConfig goalie_tactic_config;
     // Configuration values for inflated obstacles

src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py

@@ -29,6 +29,18 @@
         ),
         # test ball very fast misses net
         (tbots_cpp.Point(0, 0), tbots_cpp.Vector(-5, 1), tbots_cpp.Point(-4.5, 0)),
+        # test ball very fast get saved
+        (
+            tbots_cpp.Point(-2.5, 0),
+            tbots_cpp.Vector(-4.8, 1.1),
+            tbots_cpp.Point(-4.5, 0),
+        ),
+        # test ball very fast with the goalie out of position saved
+        (
+            tbots_cpp.Point(-2, 0),
+            tbots_cpp.Vector(-5.5, 1),
+            tbots_cpp.Point(-4.5, -0.1),
+        ),
         # ball slow inside friendly defense area
         (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(0, 0)),
         # ball slow inside friendly defense area