Use tracked dribble displacement from World in DribbleFSM (UBC-Thunderbots#3441)

* Use tracked dribble displacement from World in DribbleFSM

* [pre-commit.ci lite] apply automatic fixes

---------

Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com>

Author: williamckha

docs/fsm-diagrams.md

@@ -268,14 +268,14 @@ stateDiagram-v2
 classDef terminate fill:white,color:black,font-weight:bold
 direction LR
 [*] --> GetPossession
-GetPossession --> Dribble : [havePossession]\n<i>startDribble</i>
+GetPossession --> Dribble : [havePossession]\n<i>dribble</i>
 GetPossession --> GetPossession : [!havePossession]\n<i>getPossession</i>
-Dribble --> GetPossession : [lostPossession]\n<i>getPossession</i>
 Dribble --> LoseBall : [shouldLoseBall]\n<i>loseBall</i>
+Dribble --> GetPossession : [lostPossession]\n<i>getPossession</i>
 Dribble --> Dribble : [!dribblingDone]\n<i>dribble</i>
 Dribble --> Terminate:::terminate : [dribblingDone]\n<i>dribble</i>
-LoseBall --> LoseBall : [!lostPossession]\n<i>loseBall</i>
-LoseBall --> GetPossession : [lostPossession]\n<i>getPossession</i>
+LoseBall --> LoseBall : [shouldLoseBall]\n<i>loseBall</i>
+LoseBall --> GetPossession : [!shouldLoseBall]\n<i>getPossession</i>
 Terminate:::terminate --> GetPossession : [lostPossession]\n<i>getPossession</i>
 Terminate:::terminate --> Dribble : [!dribblingDone]\n<i>dribble</i>
 Terminate:::terminate --> Terminate:::terminate : <i>dribble</i>

src/proto/parameters.proto

@@ -313,11 +313,8 @@ message DribbleTacticConfig
 {
     // If ball and front of robot are separated by this amount, then we've lost
     // possession (in m)
-    required double lose_ball_possession_threshold = 1 [
-        default                   = 0.04,
-        (bounds).min_double_value = 0.0,
-        (bounds).max_double_value = 1.0
-    ];
+    required double lose_ball_possession_threshold = 1
+        [default = 0.1, (bounds).min_double_value = 0.0, (bounds).max_double_value = 1.0];
 
     // Threshold to determine if the ball is at the destination determined
     // experimentally (in m)
@@ -334,11 +331,8 @@ message DribbleTacticConfig
 
     // Maximum distance to continuously dribble the ball,
     // slightly conservative to not break the 1 meter rule (in m)
-    required double max_continuous_dribbling_distance = 4 [
-        default                   = 0.78,
-        (bounds).min_double_value = 0.0,
-        (bounds).max_double_value = 2.0
-    ];
+    required double max_continuous_dribbling_distance = 4
+        [default = 0.8, (bounds).min_double_value = 0.0, (bounds).max_double_value = 2.0];
 
     // Robot speed at which the robot is done dribbling (in m/s)
     required double robot_dribbling_done_speed = 5

src/software/ai/evaluation/keep_away.cpp

@@ -27,6 +27,13 @@ Point findKeepAwayTargetPoint(const World& world, const Pass& best_pass_so_far,
                                     field_bounds.yMax() - FIELD_SIZE_REDUCTION_M);
     Rectangle reduced_field_bounds = Rectangle(reduced_bottom_left, reduced_top_right);
 
+    Point initial_keepaway_point                      = world.ball().position();
+    const std::optional<Segment> dribble_displacement = world.getDribbleDisplacement();
+    if (dribble_displacement.has_value())
+    {
+        initial_keepaway_point = dribble_displacement->getStart();
+    }
+
     // the position rating function we want to maximize
     const auto keepaway_point_cost = [&](const std::array<double, 2>& passer_pt_array) {
         Point passer_pt(std::get<0>(passer_pt_array), std::get<1>(passer_pt_array));
@@ -36,7 +43,7 @@ Point findKeepAwayTargetPoint(const World& world, const Pass& best_pass_so_far,
     GradientDescentOptimizer<2> optimizer{PARAM_WEIGHTS};
     auto passer_pt_array = optimizer.maximize(
         keepaway_point_cost,
-        std::array<double, 2>{world.ball().position().x(), world.ball().position().y()},
+        std::array<double, 2>{initial_keepaway_point.x(), initial_keepaway_point.y()},
         GRADIENT_STEPS_PER_ITER);
     Point keepaway_target_point(std::get<0>(passer_pt_array),
                                 std::get<1>(passer_pt_array));

src/software/ai/hl/stp/tactic/dribble/dribble_fsm.cpp

@@ -155,8 +155,10 @@ void DribbleFSM::dribble(const Update &event)
 void DribbleFSM::loseBall(const Update &event)
 {
     Point ball_position = event.common.world_ptr->ball().position();
-    auto face_ball_orientation =
+
+    Angle face_ball_orientation =
         (ball_position - event.common.robot.position()).orientation();
+
     Point away_from_ball_position = robotPositionToFaceBall(
         ball_position, face_ball_orientation,
         dribble_tactic_config.lose_ball_possession_threshold() * 2);
@@ -166,14 +168,7 @@ void DribbleFSM::loseBall(const Update &event)
         TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT,
         TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::OFF,
         TbotsProto::BallCollisionType::AVOID,
-        AutoChipOrKick{AutoChipOrKickMode::AUTOKICK, 0.5}));
-}
-
-void DribbleFSM::startDribble(const Update &event)
-{
-    // update continuous_dribbling_start_point once we start dribbling
-    continuous_dribbling_start_point = event.common.world_ptr->ball().position();
-    dribble(event);
+        AutoChipOrKick{AutoChipOrKickMode::OFF, 0}));
 }
 
 bool DribbleFSM::havePossession(const Update &event)
@@ -210,8 +205,10 @@ bool DribbleFSM::dribblingDone(const Update &event)
 
 bool DribbleFSM::shouldLoseBall(const Update &event)
 {
-    Point ball_position = event.common.world_ptr->ball().position();
+    std::optional<Segment> dribble_displacement =
+        event.common.world_ptr->getDribbleDisplacement();
     return (!event.control_params.allow_excessive_dribbling &&
-            !comparePoints(ball_position, continuous_dribbling_start_point,
-                           dribble_tactic_config.max_continuous_dribbling_distance()));
+            dribble_displacement.has_value() &&
+            dribble_displacement->length() >=
+                dribble_tactic_config.max_continuous_dribbling_distance());
 }

src/software/ai/hl/stp/tactic/dribble/dribble_fsm.h

@@ -23,8 +23,7 @@ struct DribbleFSM
      * @param dribble_tactic_config The config to fetch parameters from
      */
     explicit DribbleFSM(TbotsProto::DribbleTacticConfig dribble_tactic_config)
-        : dribble_tactic_config(dribble_tactic_config),
-          continuous_dribbling_start_point(Point())
+        : dribble_tactic_config(dribble_tactic_config)
     {
     }
 
@@ -124,13 +123,6 @@ struct DribbleFSM
      */
     void dribble(const Update &event);
 
-    /**
-     * Start dribbling
-     *
-     * @param event DribbleFSM::Update
-     */
-    void startDribble(const Update &event);
-
     /**
      * Action to lose possession of the ball
      *
@@ -189,28 +181,26 @@ struct DribbleFSM
         DEFINE_SML_GUARD(lostPossession)
         DEFINE_SML_GUARD(dribblingDone)
         DEFINE_SML_GUARD(shouldLoseBall)
-        DEFINE_SML_ACTION(startDribble)
         DEFINE_SML_ACTION(loseBall)
         DEFINE_SML_ACTION(getPossession)
         DEFINE_SML_ACTION(dribble)
 
         return make_transition_table(
             // src_state + event [guard] / action = dest_state
-            *GetPossession_S + Update_E[havePossession_G] / startDribble_A = Dribble_S,
+            *GetPossession_S + Update_E[havePossession_G] / dribble_A = Dribble_S,
             GetPossession_S + Update_E[!havePossession_G] / getPossession_A,
-            Dribble_S + Update_E[lostPossession_G] / getPossession_A = GetPossession_S,
             Dribble_S + Update_E[shouldLoseBall_G] / loseBall_A      = LoseBall_S,
+            Dribble_S + Update_E[lostPossession_G] / getPossession_A = GetPossession_S,
             Dribble_S + Update_E[!dribblingDone_G] / dribble_A,
             Dribble_S + Update_E[dribblingDone_G] / dribble_A = X,
-            LoseBall_S + Update_E[!lostPossession_G] / loseBall_A,
-            LoseBall_S + Update_E[lostPossession_G] / getPossession_A = GetPossession_S,
-            X + Update_E[lostPossession_G] / getPossession_A          = GetPossession_S,
-            X + Update_E[!dribblingDone_G] / dribble_A                = Dribble_S,
-            X + Update_E / dribble_A                                  = X);
+            LoseBall_S + Update_E[shouldLoseBall_G] / loseBall_A,
+            LoseBall_S + Update_E[!shouldLoseBall_G] / getPossession_A = GetPossession_S,
+            X + Update_E[lostPossession_G] / getPossession_A           = GetPossession_S,
+            X + Update_E[!dribblingDone_G] / dribble_A                 = Dribble_S,
+            X + Update_E / dribble_A                                   = X);
     }
 
    private:
     // the dribble tactic config
     TbotsProto::DribbleTacticConfig dribble_tactic_config;
-    Point continuous_dribbling_start_point;
 };