[wpimath] Optimize 2nd derivative of quintic splines (#3292)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java

@@ -10,6 +10,9 @@ public class CubicHermiteSpline extends Spline {
   private static SimpleMatrix hermiteBasis;
   private final SimpleMatrix m_coefficients;
 
+  private final ControlVector m_initialControlVector;
+  private final ControlVector m_finalControlVector;
+
   /**
    * Constructs a cubic hermite spline with the specified control vectors. Each control vector
    * contains info about the location of the point and its first derivative.
@@ -60,6 +63,10 @@ public class CubicHermiteSpline extends Spline {
       m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i));
       m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
     }
+
+    // Assign member variables.
+    m_initialControlVector = new ControlVector(xInitialControlVector, yInitialControlVector);
+    m_finalControlVector = new ControlVector(xFinalControlVector, yFinalControlVector);
   }
 
   /**
@@ -68,10 +75,30 @@ public class CubicHermiteSpline extends Spline {
    * @return The coefficients matrix.
    */
   @Override
-  protected SimpleMatrix getCoefficients() {
+  public SimpleMatrix getCoefficients() {
     return m_coefficients;
   }
 
+  /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  @Override
+  public ControlVector getInitialControlVector() {
+    return m_initialControlVector;
+  }
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  @Override
+  public ControlVector getFinalControlVector() {
+    return m_finalControlVector;
+  }
+
   /**
    * Returns the hermite basis matrix for cubic hermite spline interpolation.
    *
@@ -121,8 +148,8 @@ public class CubicHermiteSpline extends Spline {
    * @return The control vector matrix for a dimension.
    */
   private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
-    if (initialVector.length != 2 || finalVector.length != 2) {
-      throw new IllegalArgumentException("Size of vectors must be 2");
+    if (initialVector.length < 2 || finalVector.length < 2) {
+      throw new IllegalArgumentException("Size of vectors must be 2 or greater.");
     }
     return new SimpleMatrix(
         4,

wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java

@@ -10,6 +10,9 @@ public class QuinticHermiteSpline extends Spline {
   private static SimpleMatrix hermiteBasis;
   private final SimpleMatrix m_coefficients;
 
+  private final ControlVector m_initialControlVector;
+  private final ControlVector m_finalControlVector;
+
   /**
    * Constructs a quintic hermite spline with the specified control vectors. Each control vector
    * contains into about the location of the point, its first derivative, and its second derivative.
@@ -60,6 +63,10 @@ public class QuinticHermiteSpline extends Spline {
       m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i));
       m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i));
     }
+
+    // Assign member variables.
+    m_initialControlVector = new ControlVector(xInitialControlVector, yInitialControlVector);
+    m_finalControlVector = new ControlVector(xFinalControlVector, yFinalControlVector);
   }
 
   /**
@@ -68,10 +75,30 @@ public class QuinticHermiteSpline extends Spline {
    * @return The coefficients matrix.
    */
   @Override
-  protected SimpleMatrix getCoefficients() {
+  public SimpleMatrix getCoefficients() {
     return m_coefficients;
   }
 
+  /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  @Override
+  public ControlVector getInitialControlVector() {
+    return m_initialControlVector;
+  }
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  @Override
+  public ControlVector getFinalControlVector() {
+    return m_finalControlVector;
+  }
+
   /**
    * Returns the hermite basis matrix for quintic hermite spline interpolation.
    *

wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java

@@ -27,7 +27,21 @@ public abstract class Spline {
    *
    * @return The coefficients of the spline.
    */
-  protected abstract SimpleMatrix getCoefficients();
+  public abstract SimpleMatrix getCoefficients();
+
+  /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  public abstract ControlVector getInitialControlVector();
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  public abstract ControlVector getFinalControlVector();
 
   /**
    * Gets the pose and curvature at some point t on the spline.

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

@@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import java.util.Arrays;
 import java.util.List;
+import org.ejml.simple.SimpleMatrix;
 
 public final class SplineHelper {
   /** Private constructor because this is a utility class. */
@@ -217,6 +218,79 @@ public final class SplineHelper {
     return splines;
   }
 
+  /**
+   * Optimizes the curvature of 2 or more quintic splines at knot points. Overall, this reduces the
+   * integral of the absolute value of the second derivative across the set of splines.
+   *
+   * @param splines An array of un-optimized quintic splines.
+   * @return An array of optimized quintic splines.
+   */
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public static QuinticHermiteSpline[] optimizeCurvature(QuinticHermiteSpline[] splines) {
+    // If there's only spline in the array, we can't optimize anything so just return that.
+    if (splines.length < 2) {
+      return splines;
+    }
+
+    // Implements Section 4.1.2 of
+    // http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf.
+
+    // Cubic splines minimize the integral of the second derivative's absolute value. Therefore, we
+    // can create cubic splines with the same 0th and 1st derivatives and the provided quintic
+    // splines, find the second derivative of those cubic splines and then use a weighted average
+    // for the second derivatives of the quintic splines.
+
+    QuinticHermiteSpline[] optimizedSplines = new QuinticHermiteSpline[splines.length];
+    for (int i = 0; i < splines.length - 1; ++i) {
+      QuinticHermiteSpline a = splines[i];
+      QuinticHermiteSpline b = splines[i + 1];
+
+      // Get the control vectors that created the quintic splines above.
+      Spline.ControlVector aInitial = a.getInitialControlVector();
+      Spline.ControlVector aFinal = a.getFinalControlVector();
+      Spline.ControlVector bInitial = b.getInitialControlVector();
+      Spline.ControlVector bFinal = b.getFinalControlVector();
+
+      // Create cubic splines with the same control vectors.
+      CubicHermiteSpline ca = new CubicHermiteSpline(aInitial.x, aFinal.x, aInitial.y, aFinal.y);
+      CubicHermiteSpline cb = new CubicHermiteSpline(bInitial.x, bFinal.x, bInitial.y, bFinal.y);
+
+      // Calculate the second derivatives at the knot points.
+      SimpleMatrix bases = new SimpleMatrix(4, 1, true, new double[] {1, 1, 1, 1});
+      SimpleMatrix combinedA = ca.getCoefficients().mult(bases);
+
+      double ddxA = combinedA.get(4, 0);
+      double ddyA = combinedA.get(5, 0);
+      double ddxB = cb.getCoefficients().get(4, 1);
+      double ddyB = cb.getCoefficients().get(5, 1);
+
+      // Calculate the parameters for the weighted average.
+      double dAB = Math.hypot(aFinal.x[0] - aInitial.x[0], aFinal.y[0] - aInitial.y[0]);
+      double dBC = Math.hypot(bFinal.x[0] - bInitial.x[0], bFinal.y[0] - bInitial.y[0]);
+      double alpha = dBC / (dAB + dBC);
+      double beta = dAB / (dAB + dBC);
+
+      // Calculate the weighted average.
+      double ddx = alpha * ddxA + beta * ddxB;
+      double ddy = alpha * ddyA + beta * ddyB;
+
+      // Create new splines.
+      optimizedSplines[i] =
+          new QuinticHermiteSpline(
+              aInitial.x,
+              new double[] {aFinal.x[0], aFinal.x[1], ddx},
+              aInitial.y,
+              new double[] {aFinal.y[0], aFinal.y[1], ddy});
+      optimizedSplines[i + 1] =
+          new QuinticHermiteSpline(
+              new double[] {bInitial.x[0], bInitial.x[1], ddx},
+              bFinal.x,
+              new double[] {bInitial.y[0], bInitial.y[1], ddy},
+              bFinal.y);
+    }
+    return optimizedSplines;
+  }
+
   /**
    * Thomas algorithm for solving tridiagonal systems Af = d.
    *

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

@@ -207,7 +207,10 @@ public final class TrajectoryGenerator {
     // Get the spline points
     List<PoseWithCurvature> points;
     try {
-      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints));
+      points =
+          splinePointsFromSplines(
+              SplineHelper.optimizeCurvature(
+                  SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints)));
     } catch (MalformedSplineException ex) {
       reportError(ex.getMessage(), ex.getStackTrace());
       return kDoNothingTrajectory;

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

@@ -10,7 +10,9 @@ CubicHermiteSpline::CubicHermiteSpline(
     wpi::array<double, 2> xInitialControlVector,
     wpi::array<double, 2> xFinalControlVector,
     wpi::array<double, 2> yInitialControlVector,
-    wpi::array<double, 2> yFinalControlVector) {
+    wpi::array<double, 2> yFinalControlVector)
+    : m_initialControlVector{xInitialControlVector, yInitialControlVector},
+      m_finalControlVector{xFinalControlVector, yFinalControlVector} {
   const auto hermite = MakeHermiteBasis();
   const auto x =
       ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);

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

@@ -10,7 +10,9 @@ QuinticHermiteSpline::QuinticHermiteSpline(
     wpi::array<double, 3> xInitialControlVector,
     wpi::array<double, 3> xFinalControlVector,
     wpi::array<double, 3> yInitialControlVector,
-    wpi::array<double, 3> yFinalControlVector) {
+    wpi::array<double, 3> yFinalControlVector)
+    : m_initialControlVector{xInitialControlVector, yInitialControlVector},
+      m_finalControlVector{xFinalControlVector, yFinalControlVector} {
   const auto hermite = MakeHermiteBasis();
   const auto x =
       ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);

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

@@ -169,6 +169,81 @@ std::vector<QuinticHermiteSpline> SplineHelper::QuinticSplinesFromWaypoints(
   return splines;
 }
 
+std::vector<QuinticHermiteSpline> SplineHelper::OptimizeCurvature(
+    const std::vector<QuinticHermiteSpline>& splines) {
+  // If there's only one spline in the vector, we can't optimize anything so
+  // just return that.
+  if (splines.size() < 2) {
+    return splines;
+  }
+
+  // Implements Section 4.1.2 of
+  // http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf.
+
+  // Cubic splines minimize the integral of the second derivative's absolute
+  // value. Therefore, we can create cubic splines with the same 0th and 1st
+  // derivatives and the provided quintic splines, find the second derivative of
+  // those cubic splines and then use a weighted average for the second
+  // derivatives of the quintic splines.
+
+  std::vector<QuinticHermiteSpline> optimizedSplines;
+  optimizedSplines.reserve(splines.size());
+  optimizedSplines.push_back(splines[0]);
+
+  for (size_t i = 0; i < splines.size() - 1; ++i) {
+    const auto& a = splines[i];
+    const auto& b = splines[i + 1];
+
+    // Get the control vectors that created the quintic splines above.
+    const auto& aInitial = a.GetInitialControlVector();
+    const auto& aFinal = a.GetFinalControlVector();
+    const auto& bInitial = b.GetInitialControlVector();
+    const auto& bFinal = b.GetFinalControlVector();
+
+    // Create cubic splines with the same control vectors.
+    auto Trim = [](const wpi::array<double, 3>& a) {
+      return wpi::array<double, 2>{a[0], a[1]};
+    };
+    CubicHermiteSpline ca{Trim(aInitial.x), Trim(aFinal.x), Trim(aInitial.y),
+                          Trim(aFinal.y)};
+    CubicHermiteSpline cb{Trim(bInitial.x), Trim(bFinal.x), Trim(bInitial.y),
+                          Trim(bFinal.y)};
+
+    // Calculate the second derivatives at the knot points.
+    frc::Vectord<4> bases{1.0, 1.0, 1.0, 1.0};
+    frc::Vectord<6> combinedA = ca.Coefficients() * bases;
+
+    double ddxA = combinedA(4);
+    double ddyA = combinedA(5);
+    double ddxB = cb.Coefficients()(4, 1);
+    double ddyB = cb.Coefficients()(5, 1);
+
+    // Calculate the parameters for weighted average.
+    double dAB =
+        std::hypot(aFinal.x[0] - aInitial.x[0], aFinal.y[0] - aInitial.y[0]);
+    double dBC =
+        std::hypot(bFinal.x[0] - bInitial.x[0], bFinal.y[0] - bInitial.y[0]);
+    double alpha = dBC / (dAB + dBC);
+    double beta = dAB / (dAB + dBC);
+
+    // Calculate the weighted average.
+    double ddx = alpha * ddxA + beta * ddxB;
+    double ddy = alpha * ddyA + beta * ddyB;
+
+    // Create new splines.
+    optimizedSplines[i] = {aInitial.x,
+                           {aFinal.x[0], aFinal.x[1], ddx},
+                           aInitial.y,
+                           {aFinal.y[0], aFinal.y[1], ddy}};
+    optimizedSplines.push_back({{bInitial.x[0], bInitial.x[1], ddx},
+                                bFinal.x,
+                                {bInitial.y[0], bInitial.y[1], ddy},
+                                bFinal.y});
+  }
+
+  return optimizedSplines;
+}
+
 void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
                                    const std::vector<double>& b,
                                    const std::vector<double>& c,

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

@@ -121,8 +121,8 @@ Trajectory TrajectoryGenerator::GenerateTrajectory(
 
   std::vector<SplineParameterizer::PoseWithCurvature> points;
   try {
-    points = SplinePointsFromSplines(
-        SplineHelper::QuinticSplinesFromWaypoints(newWaypoints));
+    points = SplinePointsFromSplines(SplineHelper::OptimizeCurvature(
+        SplineHelper::QuinticSplinesFromWaypoints(newWaypoints)));
   } catch (SplineParameterizer::MalformedSplineException& e) {
     ReportError(e.what());
     return kDoNothingTrajectory;

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

@@ -35,16 +35,36 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
                      wpi::array<double, 2> yInitialControlVector,
                      wpi::array<double, 2> yFinalControlVector);
 
- protected:
   /**
    * Returns the coefficients matrix.
    * @return The coefficients matrix.
    */
   Matrixd<6, 3 + 1> Coefficients() const override { return m_coefficients; }
 
+  /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  const ControlVector& GetInitialControlVector() const override {
+    return m_initialControlVector;
+  }
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  const ControlVector& GetFinalControlVector() const override {
+    return m_finalControlVector;
+  }
+
  private:
   Matrixd<6, 4> m_coefficients = Matrixd<6, 4>::Zero();
 
+  ControlVector m_initialControlVector;
+  ControlVector m_finalControlVector;
+
   /**
    * Returns the hermite basis matrix for cubic hermite spline interpolation.
    * @return The hermite basis matrix for cubic hermite spline interpolation.

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

@@ -35,16 +35,36 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
                        wpi::array<double, 3> yInitialControlVector,
                        wpi::array<double, 3> yFinalControlVector);
 
- protected:
   /**
    * Returns the coefficients matrix.
    * @return The coefficients matrix.
    */
   Matrixd<6, 6> Coefficients() const override { return m_coefficients; }
 
+  /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  const ControlVector& GetInitialControlVector() const override {
+    return m_initialControlVector;
+  }
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  const ControlVector& GetFinalControlVector() const override {
+    return m_finalControlVector;
+  }
+
  private:
   Matrixd<6, 6> m_coefficients = Matrixd<6, 6>::Zero();
 
+  ControlVector m_initialControlVector;
+  ControlVector m_finalControlVector;
+
   /**
    * Returns the hermite basis matrix for quintic hermite spline interpolation.
    * @return The hermite basis matrix for quintic hermite spline interpolation.

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

@@ -94,7 +94,6 @@ class Spline {
         units::curvature_t{curvature}};
   }
 
- protected:
   /**
    * Returns the coefficients of the spline.
    *
@@ -102,6 +101,21 @@ class Spline {
    */
   virtual Matrixd<6, Degree + 1> Coefficients() const = 0;
 
+  /**
+   * Returns the initial control vector that created this spline.
+   *
+   * @return The initial control vector that created this spline.
+   */
+  virtual const ControlVector& GetInitialControlVector() const = 0;
+
+  /**
+   * Returns the final control vector that created this spline.
+   *
+   * @return The final control vector that created this spline.
+   */
+  virtual const ControlVector& GetFinalControlVector() const = 0;
+
+ protected:
   /**
    * Converts a Translation2d into a vector that is compatible with Eigen.
    *

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

@@ -77,6 +77,17 @@ class WPILIB_DLLEXPORT SplineHelper {
   static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
       const std::vector<Spline<5>::ControlVector>& controlVectors);
 
+  /**
+   * Optimizes the curvature of 2 or more quintic splines at knot points.
+   * Overall, this reduces the integral of the absolute value of the second
+   * derivative across the set of splines.
+   *
+   * @param splines A vector of un-optimized quintic splines.
+   * @return A vector of optimized quintic splines.
+   */
+  static std::vector<QuinticHermiteSpline> OptimizeCurvature(
+      const std::vector<QuinticHermiteSpline>& splines);
+
  private:
   static Spline<3>::ControlVector CubicControlVector(double scalar,
                                                      const Pose2d& point) {

wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java

@@ -172,8 +172,8 @@ class MecanumDriveOdometryTest {
     }
 
     assertEquals(
-        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
-    assertEquals(0.0, maxError, 0.3, "Incorrect max error");
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.35, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.35, "Incorrect max error");
     assertEquals(
         1.0,
         odometryDistanceTravelled / trajectoryDistanceTravelled,

wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java

@@ -7,6 +7,7 @@ package edu.wpi.first.math.trajectory;
 import static edu.wpi.first.math.util.Units.feetToMeters;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -80,4 +81,21 @@ class TrajectoryGeneratorTest {
     assertEquals(traj.getStates().size(), 1);
     assertEquals(traj.getTotalTimeSeconds(), 0);
   }
+
+  @Test
+  void testQuinticCurvatureOptimization() {
+    Trajectory t =
+        TrajectoryGenerator.generateTrajectory(
+            Arrays.asList(
+                new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
+                new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
+                new Pose2d(-1, 0, Rotation2d.fromDegrees(270)),
+                new Pose2d(0, -1, Rotation2d.fromDegrees(360)),
+                new Pose2d(1, 0, Rotation2d.fromDegrees(90))),
+            new TrajectoryConfig(2, 2));
+
+    for (int i = 1; i < t.getStates().size() - 1; ++i) {
+      assertNotEquals(0, t.getStates().get(i).curvatureRadPerMeter);
+    }
+  }
 }

wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp

@@ -41,3 +41,17 @@ TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
   ASSERT_EQ(t.States().size(), 1u);
   ASSERT_EQ(t.TotalTime(), 0_s);
 }
+
+TEST(TrajectoryGenerationTest, CurvatureOptimization) {
+  auto t = TrajectoryGenerator::GenerateTrajectory(
+      {{1_m, 0_m, 90_deg},
+       {0_m, 1_m, 180_deg},
+       {-1_m, 0_m, 270_deg},
+       {0_m, -1_m, 0_deg},
+       {1_m, 0_m, 90_deg}},
+      TrajectoryConfig{12_fps, 12_fps_sq});
+
+  for (size_t i = 1; i < t.States().size() - 1; ++i) {
+    EXPECT_NE(0, t.States()[i].curvature.to<double>());
+  }
+}