Adding dynamic thresholding to pivot when kicking

soccer/src/soccer/planning/planner/rotate_path_planner.cpp

@@ -19,6 +19,11 @@
 using namespace rj_geometry;
 
 Trajectory RotatePathPlanner::plan(const PlanRequest& request) {
+    if (!cached_angle_change_ && request.trigger_mode == RobotIntent::TriggerMode::AT_END) {
+        double target_distance =
+            (request.motion_command.target.position - request.start.pose.position()).mag();
+        isDoneAngleChangeThresh = max(pow(2, -target_distance), 0.01);
+    }
     update_state();
     switch (current_state_) {
         case PIVOT:
@@ -35,7 +40,7 @@
         return;
     }
     current_state_ = abs(cached_angle_change_.value()) <
-                             degrees_to_radians(static_cast<float>(kIsDoneAngleChangeThresh))
+                             degrees_to_radians(static_cast<float>(isDoneAngleChangeThresh))
                          ? END
                          : PIVOT;
 }
@@ -70,7 +75,7 @@
     Trajectory path{};
 
     if (cached_target_angle_.has_value() &&
-        (*cached_target_angle_ - target_angle) < degrees_to_radians(kIsDoneAngleChangeThresh)) {
+        (*cached_target_angle_ - target_angle) < degrees_to_radians(isDoneAngleChangeThresh)) {
         if (cached_path_) {
             path = cached_path_.value();
         } else {

soccer/src/soccer/planning/planner/rotate_path_planner.hpp

@@ -47,6 +47,6 @@
     enum State { PIVOT, END };
     RotatePathPlanner::State current_state_ = RotatePathPlanner::State::PIVOT;
 
-    static constexpr double kIsDoneAngleChangeThresh{1.0};
+    double isDoneAngleChangeThresh{1.0};
 };
 }  // namespace planning