Refactor TrajectoryGenerator (#1972)

wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java

@@ -14,8 +14,8 @@ import org.junit.jupiter.api.Test;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Twist2d;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
@@ -24,6 +24,16 @@ class RamseteControllerTest {
   private static final double kTolerance = 1 / 12.0;
   private static final double kAngularTolerance = Math.toRadians(2);
 
+  private static double boundRadians(double value) {
+    while (value > Math.PI) {
+      value -= Math.PI * 2;
+    }
+    while (value <= -Math.PI) {
+      value += Math.PI * 2;
+    }
+    return value;
+  }
+
   @Test
   @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
   void testReachesReference() {
@@ -33,8 +43,8 @@ class RamseteControllerTest {
     final var waypoints = new ArrayList<Pose2d>();
     waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
     waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
-    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints,
-        new ArrayList<TrajectoryConstraint>(), 0, 0, 8.8, 0.1, false);
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
 
     final double kDt = 0.02;
     final var totalTime = trajectory.getTotalTimeSeconds();
@@ -54,23 +64,13 @@ class RamseteControllerTest {
     final var finalRobotPose = robotPose;
     assertAll(
         () -> assertEquals(endPose.getTranslation().getX(), finalRobotPose.getTranslation().getX(),
-                           kTolerance),
+            kTolerance),
         () -> assertEquals(endPose.getTranslation().getY(), finalRobotPose.getTranslation().getY(),
-                           kTolerance),
+            kTolerance),
         () -> assertEquals(0.0,
             boundRadians(endPose.getRotation().getRadians()
-              - finalRobotPose.getRotation().getRadians()),
+                - finalRobotPose.getRotation().getRadians()),
             kAngularTolerance)
     );
   }
-
-  private static double boundRadians(double value) {
-    while (value > Math.PI) {
-      value -= Math.PI * 2;
-    }
-    while (value <= -Math.PI) {
-      value += Math.PI * 2;
-    }
-    return value;
-  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java

@@ -32,8 +32,13 @@ class CubicHermiteSplineTest {
     var start = System.nanoTime();
 
     // Generate and parameterize the spline.
+    var controlVectors =
+        SplineHelper.getCubicControlVectorsFromWaypoints(a,
+            waypoints.toArray(new Translation2d[0]), b);
     var splines
-        = SplineHelper.getCubicSplinesFromWaypoints(a, waypoints.toArray(new Translation2d[0]), b);
+        = SplineHelper.getCubicSplinesFromControlVectors(
+        controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]);
+
     var poses = new ArrayList<PoseWithCurvature>();
 
     poses.add(splines[0].getPoint(0.0));

wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java

@@ -7,6 +7,8 @@
 
 package edu.wpi.first.wpilibj.spline;
 
+import java.util.List;
+
 import org.junit.jupiter.api.Test;
 
 import edu.wpi.first.wpilibj.geometry.Pose2d;
@@ -27,7 +29,9 @@ class QuinticHermiteSplineTest {
     var start = System.nanoTime();
 
     // Generate and parameterize the spline.
-    var spline = SplineHelper.getQuinticSplinesFromWaypoints(new Pose2d[]{a, b})[0];
+    var controlVectors = SplineHelper.getQuinticControlVectorsFromWaypoints(List.of(a, b));
+    var spline = SplineHelper.getQuinticSplinesFromControlVectors(
+        controlVectors.toArray(new Spline.ControlVector[0]))[0];
     var poses = SplineParameterizer.parameterize(spline);
 
     // End the timer.

wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java

@@ -24,8 +24,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
 
 class TrajectoryGeneratorTest {
   static Trajectory getTrajectory(List<TrajectoryConstraint> constraints) {
-    final double startVelocity = 0;
-    final double endVelocity = 0;
     final double maxVelocity = feetToMeters(12.0);
     final double maxAccel = feetToMeters(12);
 
@@ -45,15 +43,11 @@ class TrajectoryGeneratorTest {
             Rotation2d.fromDegrees(-90))));
     waypoints.add(crossScale);
 
-    return TrajectoryGenerator.generateTrajectory(
-        waypoints,
-        constraints,
-        startVelocity,
-        endVelocity,
-        maxVelocity,
-        maxAccel,
-        true
-    );
+    TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAccel)
+        .setReversed(true)
+        .addConstraints(constraints);
+
+    return TrajectoryGenerator.generateTrajectory(waypoints, config);
   }
 
   @Test