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

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

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) {