[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;