[wpimath] Fix quintic spline generation from control vectors (#2762)

This does not introduce any breaking changes for teams that used the TrajectoryGenerator API for
quintic splines with poses.

The GetQuinticControlVectorsFromWaypoints() method was removed because it is not possible for us (or
would require breaking changes to the shape of the splines) to generate only one quintic control vector
per Pose2d.  Users who actually have control vectors directly (i.e. not from Pose2d objects, but a
dashboard such as PathWeaver) should have the number of control vectors correspond to the number of
"waypoints" and can call GetQuinticSplinesFromControlVectors() directly.

wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java

@@ -7,7 +7,6 @@
 
 package edu.wpi.first.wpilibj.spline;
 
-import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
 
@@ -53,26 +52,28 @@ public final class SplineHelper {
   }
 
   /**
-   * Returns quintic control vectors from a set of waypoints.
+   * Returns quintic splines from a set of waypoints.
    *
    * @param waypoints The waypoints
-   * @return List of control vectors
+   * @return List of splines.
    */
-  public static List<Spline.ControlVector> getQuinticControlVectorsFromWaypoints(
-      List<Pose2d> waypoints
-  ) {
-    List<Spline.ControlVector> vectors = new ArrayList<>();
-    for (int i = 0; i < waypoints.size() - 1; i++) {
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(List<Pose2d> waypoints) {
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.size() - 1];
+    for (int i = 0; i < waypoints.size() - 1; ++i) {
       var p0 = waypoints.get(i);
       var p1 = waypoints.get(i + 1);
 
       // This just makes the splines look better.
       final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
 
-      vectors.add(getQuinticControlVector(scalar, p0));
-      vectors.add(getQuinticControlVector(scalar, p1));
+      var controlVecA = getQuinticControlVector(scalar, p0);
+      var controlVecB = getQuinticControlVector(scalar, p1);
+
+      splines[i]
+          = new QuinticHermiteSpline(controlVecA.x, controlVecB.x, controlVecA.y, controlVecB.y);
     }
-    return vectors;
+    return splines;
   }
 
   /**
@@ -216,15 +217,14 @@ public final class SplineHelper {
   @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
   public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
       Spline.ControlVector[] controlVectors) {
-    // There are twice as many control vectors are there are splines.
-    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length / 2];
-    for (int i = 0; i < controlVectors.length - 1; i += 2) {
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
+    for (int i = 0; i < controlVectors.length - 1; i++) {
       var xInitial = controlVectors[i].x;
       var xFinal = controlVectors[i + 1].x;
       var yInitial = controlVectors[i].y;
       var yFinal = controlVectors[i + 1].y;
-      splines[i / 2] = new QuinticHermiteSpline(xInitial, xFinal,
-          yInitial, yFinal);
+      splines[i] = new QuinticHermiteSpline(xInitial, xFinal,
+                                            yInitial, yFinal);
     }
     return splines;
   }

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java

@@ -196,9 +196,38 @@ public final class TrajectoryGenerator {
    */
   @SuppressWarnings("LocalVariableName")
   public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
-    var originalList = SplineHelper.getQuinticControlVectorsFromWaypoints(waypoints);
-    var newList = new ControlVectorList(originalList);
-    return generateTrajectory(newList, config);
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+
+    List<Pose2d> newWaypoints = new ArrayList<>();
+    if (config.isReversed()) {
+      for (Pose2d originalWaypoint : waypoints) {
+        newWaypoints.add(originalWaypoint.plus(flip));
+      }
+    } else {
+      newWaypoints.addAll(waypoints);
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
+        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
+        config.getMaxAcceleration(), config.isReversed());
   }
 
   /**

wpimath/src/main/native/cpp/spline/SplineHelper.cpp

@@ -127,9 +127,7 @@ std::vector<QuinticHermiteSpline>
 SplineHelper::QuinticSplinesFromControlVectors(
     const std::vector<Spline<5>::ControlVector>& controlVectors) {
   std::vector<QuinticHermiteSpline> splines;
-  // There are twice as many control vectors are there are splines,
-  // so we increment the counter by 2.
-  for (size_t i = 0; i < controlVectors.size() - 1; i += 2) {
+  for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
     auto& xInitial = controlVectors[i].x;
     auto& yInitial = controlVectors[i].y;
     auto& xFinal = controlVectors[i + 1].x;
@@ -160,10 +158,10 @@ SplineHelper::CubicControlVectorsFromWaypoints(
   return {initialCV, finalCV};
 }
 
-std::vector<Spline<5>::ControlVector>
-SplineHelper::QuinticControlVectorsFromWaypoints(
+std::vector<QuinticHermiteSpline> SplineHelper::QuinticSplinesFromWaypoints(
     const std::vector<Pose2d>& waypoints) {
-  std::vector<Spline<5>::ControlVector> vectors;
+  std::vector<QuinticHermiteSpline> splines;
+  splines.reserve(waypoints.size() - 1);
   for (size_t i = 0; i < waypoints.size() - 1; ++i) {
     auto& p0 = waypoints[i];
     auto& p1 = waypoints[i + 1];
@@ -172,10 +170,12 @@ SplineHelper::QuinticControlVectorsFromWaypoints(
     const auto scalar =
         1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
 
-    vectors.push_back(QuinticControlVector(scalar, p0));
-    vectors.push_back(QuinticControlVector(scalar, p1));
+    auto controlVectorA = QuinticControlVector(scalar, p0);
+    auto controlVectorB = QuinticControlVector(scalar, p1);
+    splines.emplace_back(controlVectorA.x, controlVectorB.x, controlVectorA.y,
+                         controlVectorB.y);
   }
-  return vectors;
+  return splines;
 }
 
 void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,

wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp

@@ -113,6 +113,30 @@ Trajectory TrajectoryGenerator::GenerateTrajectory(
 
 Trajectory TrajectoryGenerator::GenerateTrajectory(
     const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
-  return GenerateTrajectory(
-      SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config);
+  auto newWaypoints = waypoints;
+  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+  if (config.IsReversed())
+    for (auto& waypoint : newWaypoints) waypoint += flip;
+
+  std::vector<SplineParameterizer::PoseWithCurvature> points;
+  try {
+    points = SplinePointsFromSplines(
+        SplineHelper::QuinticSplinesFromWaypoints(newWaypoints));
+  } catch (SplineParameterizer::MalformedSplineException& e) {
+    ReportError(e.what());
+    return kDoNothingTrajectory;
+  }
+
+  // After trajectory generation, flip theta back so it's relative to the
+  // field. Also fix curvature.
+  if (config.IsReversed()) {
+    for (auto& point : points) {
+      point = {point.first + flip, -point.second};
+    }
+  }
+
+  return TrajectoryParameterizer::TimeParameterizeTrajectory(
+      points, config.Constraints(), config.StartVelocity(),
+      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+      config.IsReversed());
 }

wpimath/src/main/native/include/frc/spline/SplineHelper.h

@@ -36,13 +36,13 @@ class SplineHelper {
       const Pose2d& end);
 
   /**
-   * Returns quintic control vectors from a set of waypoints.
+   * Returns quintic splines from a set of waypoints.
    *
    * @param waypoints The waypoints
-   * @return List of control vectors
+   * @return List of quintic splines.
    */
-  static std::vector<Spline<5>::ControlVector>
-  QuinticControlVectorsFromWaypoints(const std::vector<Pose2d>& waypoints);
+  static std::vector<QuinticHermiteSpline> QuinticSplinesFromWaypoints(
+      const std::vector<Pose2d>& waypoints);
 
   /**
    * Returns a set of cubic splines corresponding to the provided control

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

@@ -31,9 +31,7 @@ class QuinticHermiteSplineTest {
     //var start = System.nanoTime();
 
     // Generate and parameterize the spline.
-    var controlVectors = SplineHelper.getQuinticControlVectorsFromWaypoints(List.of(a, b));
-    var spline = SplineHelper.getQuinticSplinesFromControlVectors(
-        controlVectors.toArray(new Spline.ControlVector[0]))[0];
+    var spline = SplineHelper.getQuinticSplinesFromWaypoints(List.of(a, b))[0];
     var poses = SplineParameterizer.parameterize(spline);
 
     // End the timer.

wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp

@@ -27,8 +27,7 @@ class QuinticHermiteSplineTest : public ::testing::Test {
     const auto start = std::chrono::high_resolution_clock::now();
 
     // Generate and parameterize the spline.
-    const auto spline = SplineHelper::QuinticSplinesFromControlVectors(
-        SplineHelper::QuinticControlVectorsFromWaypoints({a, b}))[0];
+    const auto spline = SplineHelper::QuinticSplinesFromWaypoints({a, b})[0];
     const auto poses = SplineParameterizer::Parameterize(spline);
 
     // End timer.