[wpimath] Optimize 2nd derivative of quintic splines

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.
@@ -61,6 +64,10 @@ public CubicHermiteSpline(
       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);
   }
 
   /**
@@ -69,10 +76,30 @@ public CubicHermiteSpline(
    * @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.
    *
@@ -122,8 +149,8 @@ private SimpleMatrix makeHermiteBasis() {
    * @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.
@@ -61,6 +64,10 @@ public QuinticHermiteSpline(
       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);
   }
 
   /**
@@ -69,10 +76,30 @@ public QuinticHermiteSpline(
    * @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.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. */
@@ -219,6 +220,79 @@ public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
     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

@@ -209,7 +209,10 @@ public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryCo
     // 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

@@ -6,6 +6,8 @@
 
 #include <cstddef>
 
+#include "frc/StateSpaceUtil.h"
+
 using namespace frc;
 
 std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
@@ -175,6 +177,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.
+    auto bases = frc::MakeMatrix<4, 1>(1.0, 1.0, 1.0, 1.0);
+    auto 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;