Add ability to override target rotation while path following (#418)

pathplannerlib/.vscode/settings.json

@@ -83,6 +83,9 @@
     "xstring": "cpp",
     "xtr1common": "cpp",
     "xutility": "cpp",
-    "unordered_set": "cpp"
+    "unordered_set": "cpp",
+    "forward_list": "cpp",
+    "ranges": "cpp",
+    "valarray": "cpp"
   }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java

@@ -5,9 +5,12 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import java.util.Optional;
+import java.util.function.Supplier;
 
 /** Path following controller for holonomic drive trains */
 public class PPHolonomicDriveController implements PathFollowingController {
@@ -20,6 +23,8 @@ public class PPHolonomicDriveController implements PathFollowingController {
   private Translation2d translationError = new Translation2d();
   private boolean isEnabled = true;
 
+  private static Supplier<Optional<Rotation2d>> rotationTargetOverride = null;
+
   /**
    * Constructs a HolonomicDriveController
    *
@@ -135,10 +140,15 @@ public ChassisSpeeds calculateRobotRelativeSpeeds(
         new TrapezoidProfile.Constraints(
             maxAngVel, targetState.constraints.getMaxAngularAccelerationRpsSq());
 
+    Rotation2d targetRotation = targetState.targetHolonomicRotation;
+    if (rotationTargetOverride != null) {
+      targetRotation = rotationTargetOverride.get().orElse(targetRotation);
+    }
+
     double targetRotationVel =
         rotationController.calculate(
             currentPose.getRotation().getRadians(),
-            new TrapezoidProfile.State(targetState.targetHolonomicRotation.getRadians(), 0),
+            new TrapezoidProfile.State(targetRotation.getRadians(), 0),
             rotationConstraints);
 
     return ChassisSpeeds.fromFieldRelativeSpeeds(
@@ -165,4 +175,16 @@ public double getPositionalError() {
   public boolean isHolonomic() {
     return true;
   }
+
+  /**
+   * Set a supplier that will be used to override the rotation target when path following.
+   *
+   * <p>This function should return an empty optional to use the rotation targets in the path
+   *
+   * @param rotationTargetOverride Supplier to override rotation targets
+   */
+  public static void setRotationTargetOverride(
+      Supplier<Optional<Rotation2d>> rotationTargetOverride) {
+    PPHolonomicDriveController.rotationTargetOverride = rotationTargetOverride;
+  }
 }

pathplannerlib/src/main/native/cpp/pathplanner/lib/controllers/PPHolonomicDriveController.cpp

@@ -2,6 +2,8 @@
 
 using namespace pathplanner;
 
+std::function<std::optional<frc::Rotation2d>()> PPHolonomicDriveController::rotationTargetOverride;
+
 PPHolonomicDriveController::PPHolonomicDriveController(
 		PIDConstants translationConstants, PIDConstants rotationConstants,
 		units::meters_per_second_t maxModuleSpeed,
@@ -53,6 +55,11 @@ frc::ChassisSpeeds PPHolonomicDriveController::calculateRobotRelativeSpeeds(
 	units::radians_per_second_t maxAngVel = units::math::min(angVelConstraint,
 			maxAngVelModule);
 
+	frc::Rotation2d targetRotation = referenceState.targetHolonomicRotation;
+	if (rotationTargetOverride) {
+		targetRotation = rotationTargetOverride().value_or(targetRotation);
+	}
+
 	units::radians_per_second_t targetRotationVel {
 			m_rotationController.Calculate(currentPose.Rotation().Radians(),
 					referenceState.targetHolonomicRotation.Radians(),

pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPHolonomicDriveController.h

@@ -10,6 +10,8 @@
 #include <frc/geometry/Rotation2d.h>
 #include <frc/geometry/Pose2d.h>
 #include <frc/kinematics/ChassisSpeeds.h>
+#include <functional>
+#include <optional>
 #include "pathplanner/lib/util/GeometryUtil.h"
 #include "pathplanner/lib/util/PIDConstants.h"
 #include "pathplanner/lib/util/HolonomicPathFollowerConfig.h"
@@ -81,6 +83,18 @@ class PPHolonomicDriveController: public PathFollowingController {
 		return true;
 	}
 
+	/**
+	 * Set a supplier that will be used to override the rotation target when path following.
+	 * <p>
+	 * This function should return an empty optional to use the rotation targets in the path
+	 * @param rotationTargetOverride Supplier to override rotation targets
+	 */
+	static inline void setRotationTargetOverride(
+			std::function<std::optional<frc::Rotation2d>()> rotationTargetOverride) {
+		PPHolonomicDriveController::rotationTargetOverride =
+				rotationTargetOverride;
+	}
+
 private:
 	using rpsPerMps_t = units::unit_t<units::compound_unit<units::radians_per_second, units::inverse<units::meters_per_second>>>;
 
@@ -92,5 +106,7 @@ class PPHolonomicDriveController: public PathFollowingController {
 
 	frc::Translation2d m_translationError;
 	bool m_enabled;
+
+	static std::function<std::optional<frc::Rotation2d>()> rotationTargetOverride;
 };
 }

pathplannerlib/src/test/java/com/pathplanner/lib/util/GeometryUtilTest.java

@@ -2,8 +2,6 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import org.junit.jupiter.api.Test;
 
 public class GeometryUtilTest {
@@ -17,75 +15,4 @@ public void testDoubleLerp() {
     assertEquals(0, GeometryUtil.doubleLerp(0, 1, 0), DELTA);
     assertEquals(1, GeometryUtil.doubleLerp(0, 1, 1), DELTA);
   }
-
-  @Test
-  public void testRotationLerp() {
-    assertEquals(
-        90,
-        GeometryUtil.rotationLerp(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(180), 0.5)
-            .getDegrees(),
-        DELTA);
-    assertEquals(
-        -45,
-        GeometryUtil.rotationLerp(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(-180), 0.25)
-            .getDegrees(),
-        DELTA);
-  }
-
-  @Test
-  public void testTranslationLerp() {
-    Translation2d t =
-        GeometryUtil.translationLerp(new Translation2d(2.3, 7), new Translation2d(3.5, 2.1), 0.2);
-    assertEquals(2.54, t.getX(), DELTA);
-    assertEquals(6.02, t.getY(), DELTA);
-
-    t = GeometryUtil.translationLerp(new Translation2d(-1.5, 2), new Translation2d(1.5, -3), 0.5);
-    assertEquals(0, t.getX(), DELTA);
-    assertEquals(-0.5, t.getY(), DELTA);
-  }
-
-  @Test
-  public void testQuadraticLerp() {
-    Translation2d t =
-        GeometryUtil.quadraticLerp(
-            new Translation2d(1, 2), new Translation2d(3, 4), new Translation2d(5, 6), 0.5);
-    assertEquals(3, t.getX(), DELTA);
-    assertEquals(4, t.getY(), DELTA);
-  }
-
-  @Test
-  public void testCubicLerp() {
-    Translation2d t =
-        GeometryUtil.cubicLerp(
-            new Translation2d(1, 2),
-            new Translation2d(3, 4),
-            new Translation2d(5, 6),
-            new Translation2d(7, 8),
-            0.5);
-    assertEquals(4, t.getX(), DELTA);
-    assertEquals(5, t.getY(), DELTA);
-  }
-
-  @Test
-  public void testCurveRadius() {
-    double radius1 =
-        GeometryUtil.calculateRadius(
-            new Translation2d(1.0, 1.0), new Translation2d(1.5, 1.5), new Translation2d(2.0, 1.0));
-    assertEquals(-0.5, radius1, DELTA);
-
-    double radius2 =
-        GeometryUtil.calculateRadius(
-            new Translation2d(1.0, 1.0), new Translation2d(1.5, 0.5), new Translation2d(2.0, 1.0));
-    assertEquals(0.5, radius2, DELTA);
-
-    double radius3 =
-        GeometryUtil.calculateRadius(
-            new Translation2d(1.0, 1.0), new Translation2d(1.5, 1.25), new Translation2d(2.0, 1.0));
-    assertEquals(-0.625, radius3, DELTA);
-
-    double radius4 =
-        GeometryUtil.calculateRadius(
-            new Translation2d(1.0, 1.0), new Translation2d(1.5, 0.75), new Translation2d(2.0, 1.0));
-    assertEquals(0.625, radius4, DELTA);
-  }
 }