Copy WPILib's CubicHermiteSpline as a differential initial guess

trajoptlib/include/trajopt/path/PathBuilder.hpp

@@ -13,6 +13,7 @@
 #include "trajopt/geometry/Translation2.hpp"
 #include "trajopt/path/Path.hpp"
 #include "trajopt/util/GenerateLinearInitialGuess.hpp"
+#include "trajopt/util/GenerateSplineInitialGuess.hpp"
 #include "trajopt/util/SymbolExports.hpp"
 
 namespace trajopt {
@@ -222,6 +223,17 @@ class TRAJOPT_DLLEXPORT PathBuilder {
                                                 controlIntervalCounts);
   }
 
+  /**
+   * Calculate a discrete, spline initial guess of the x, y, and heading of the
+   * robot that goes through each segment.
+   *
+   * @return the initial guess, as a solution
+   */
+  Solution CalculateSplineInitialGuess() const {
+    return GenerateSplineInitialGuess<Solution>(initialGuessPoints,
+                                                controlIntervalCounts);
+  }
+
  protected:
   /// The path.
   Path<Drivetrain, Solution> path;

trajoptlib/include/trajopt/spline/CubicHermitePoseSplineHolonomic.hpp

@@ -0,0 +1,103 @@
+// Copyright (c) TrajoptLib contributors
+
+#pragma once
+
+#include <array>
+#include <utility>
+
+#include "trajopt/geometry/Pose2.hpp"
+#include "trajopt/geometry/Rotation2.hpp"
+#include "trajopt/geometry/Translation2.hpp"
+#include "trajopt/spline/CubicHermiteSpline.hpp"
+#include "trajopt/spline/CubicHermiteSpline1d.hpp"
+#include "trajopt/util/SymbolExports.hpp"
+
+namespace trajopt {
+
+/**
+ * Represents a cubic pose spline, which is a specific implementation of a cubic
+ * hermite spline.
+ */
+class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic : CubicHermiteSpline {
+ public:
+  /// Pose2d with curvature.
+  using PoseWithCurvature = std::pair<Pose2d, double>;
+
+  /**
+   * Constructs a cubic pose spline.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   *     the x dimension.
+   * @param xFinalControlVector The control vector for the final point in
+   *     the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   *     the y dimension.
+   * @param yFinalControlVector The control vector for the final point in
+   *     the y dimension.
+   * @param r0 Initial heading.
+   * @param r1 Final heading.
+   */
+  CubicHermitePoseSplineHolonomic(std::array<double, 2> xInitialControlVector,
+                                  std::array<double, 2> xFinalControlVector,
+                                  std::array<double, 2> yInitialControlVector,
+                                  std::array<double, 2> yFinalControlVector,
+                                  Rotation2d r0, Rotation2d r1)
+      : CubicHermiteSpline(xInitialControlVector, xFinalControlVector,
+                           yInitialControlVector, yFinalControlVector),
+        r0(r0),
+        theta(0.0, (-r0).RotateBy(r1).Radians(), 0, 0) {}
+
+  /**
+   * Return course at point t.
+   *
+   * @param t The point t
+   * @return The course at point t.
+   */
+  Rotation2d GetCourse(double t) const {
+    const PoseWithCurvature splinePoint =
+        CubicHermiteSpline::GetPoint(t).value();
+    return splinePoint.first.Rotation();
+  }
+
+  /**
+   * Return heading at point t.
+   *
+   * @param t The point t
+   * @return The heading at point t.
+   */
+  Rotation2d GetHeading(double t) const {
+    return r0.RotateBy(Rotation2d{theta.GetPosition(t)});
+  }
+
+  /**
+   * Return heading rate at point t.
+   *
+   * @param t The point t
+   * @return The heading rate at point t.
+   */
+  double GetHeadingRate(double t) const { return theta.GetVelocity(t); }
+
+  /**
+   * Gets the pose and curvature at some point t on the spline.
+   *
+   * @param t The point t
+   * @param isDifferential Whether the drivetrain is a differential drive.
+   * @return The pose and curvature at that point.
+   */
+  std::optional<PoseWithCurvature> GetPoint(double t,
+                                            bool isDifferential) const {
+    if (isDifferential) {
+      return CubicHermiteSpline::GetPoint(t);
+    } else {
+      const auto splinePoint = CubicHermiteSpline::GetPoint(t).value();
+      return PoseWithCurvature{{splinePoint.first.Translation(), GetHeading(t)},
+                               splinePoint.second};
+    }
+  }
+
+ private:
+  Rotation2d r0;
+  CubicHermiteSpline1d theta;
+};
+
+}  // namespace trajopt

trajoptlib/include/trajopt/spline/CubicHermiteSpline.hpp

@@ -0,0 +1,130 @@
+// Copyright (c) TrajoptLib contributors
+
+#pragma once
+
+#include <array>
+
+#include <Eigen/Core>
+
+#include "trajopt/spline/Spline.hpp"
+#include "trajopt/util/SymbolExports.hpp"
+
+namespace trajopt {
+
+/**
+ * Represents a hermite spline of degree 3.
+ */
+class TRAJOPT_DLLEXPORT CubicHermiteSpline : public Spline<3> {
+ public:
+  /**
+   * 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.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   * the x dimension.
+   * @param xFinalControlVector The control vector for the final point in
+   * the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   * the y dimension.
+   * @param yFinalControlVector The control vector for the final point in
+   * the y dimension.
+   */
+  CubicHermiteSpline(std::array<double, 2> xInitialControlVector,
+                     std::array<double, 2> xFinalControlVector,
+                     std::array<double, 2> yInitialControlVector,
+                     std::array<double, 2> yFinalControlVector)
+      : m_initialControlVector{xInitialControlVector, yInitialControlVector},
+        m_finalControlVector{xFinalControlVector, yFinalControlVector} {
+    // Calculate the basis matrix for cubic Hermite spline interpolation.
+    //
+    // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
+    // the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀.
+    //
+    // P(i)    = P(0)  = a₀
+    // P'(i)   = P'(0) = a₁
+    // P(i+1)  = P(1)  = a₃ + a₂ + a₁ + a₀
+    // P'(i+1) = P'(1) = 3a₃ + 2a₂ + a₁
+    //
+    // [P(i)   ] = [0 0 0 1][a₃]
+    // [P'(i)  ] = [0 0 1 0][a₂]
+    // [P(i+1) ] = [1 1 1 1][a₁]
+    // [P'(i+1)] = [3 2 1 0][a₀]
+    //
+    // To solve for the coefficients, we can invert the 4x4 matrix and move it
+    // to the other side of the equation.
+    //
+    // [a₃] = [ 2  1 -2  1][P(i)   ]
+    // [a₂] = [-3 -2  3 -1][P'(i)  ]
+    // [a₁] = [ 0  1  0  0][P(i+1) ]
+    // [a₀] = [ 1  0  0  0][P'(i+1)]
+    constexpr Eigen::Matrix4d basis{{+2.0, +1.0, -2.0, +1.0},
+                                    {-3.0, -2.0, +3.0, -1.0},
+                                    {+0.0, +1.0, +0.0, +0.0},
+                                    {+1.0, +0.0, +0.0, +0.0}};
+
+    Eigen::Vector4d x{m_initialControlVector.x[0], m_initialControlVector.x[1],
+                      m_finalControlVector.x[0], m_finalControlVector.x[1]};
+    Eigen::Vector4d y{m_initialControlVector.y[0], m_initialControlVector.y[1],
+                      m_finalControlVector.y[0], m_finalControlVector.y[1]};
+
+    // Populate rows 0 and 1 with coefficients
+    m_coefficients.template block<1, 4>(0, 0) = basis * x;
+    m_coefficients.template block<1, 4>(1, 0) = basis * y;
+
+    // Populate rows 2 and 3 with the derivatives of the equations above
+    for (int i = 0; i < 4; i++) {
+      // Here, we are multiplying by (3 - i) to manually take the derivative.
+      // The power of the term in index 0 is 3, index 1 is 2 and so on. To find
+      // the coefficient of the derivative, we can use the power rule and
+      // multiply the existing coefficient by its power.
+      m_coefficients.template block<2, 1>(2, i) =
+          m_coefficients.template block<2, 1>(0, i) * (3 - i);
+    }
+
+    // Populate rows 4 and 5 with the second derivatives
+    for (int i = 0; i < 3; i++) {
+      // Here, we are multiplying by (2 - i) to manually take the derivative.
+      // The power of the term in index 0 is 2, index 1 is 1 and so on. To find
+      // the coefficient of the derivative, we can use the power rule and
+      // multiply the existing coefficient by its power.
+      m_coefficients.template block<2, 1>(4, i) =
+          m_coefficients.template block<2, 1>(2, i) * (2 - i);
+    }
+  }
+
+  /**
+   * Returns the coefficients matrix.
+   *
+   * @return The coefficients matrix.
+   */
+  const Eigen::Matrix<double, 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:
+  Eigen::Matrix<double, 6, 4> m_coefficients;
+
+  ControlVector m_initialControlVector;
+  ControlVector m_finalControlVector;
+};
+
+}  // namespace trajopt

trajoptlib/include/trajopt/spline/CubicHermiteSpline1d.hpp

@@ -0,0 +1,74 @@
+// Copyright (c) TrajoptLib contributors
+
+#pragma once
+
+#include "trajopt/util/SymbolExports.hpp"
+
+namespace trajopt {
+
+/**
+ * 1D cubic hermite spline.
+ */
+class TRAJOPT_DLLEXPORT CubicHermiteSpline1d {
+ public:
+  /**
+   * Constructs a 1D cubic hermite spline.
+   *
+   * @param p0 The initial position.
+   * @param p1 The final position.
+   * @param v0 The initial velocity.
+   * @param v1 The final velocity.
+   */
+  constexpr CubicHermiteSpline1d(double p0, double p1, double v0, double v1)
+      : a{v0 + v1 + 2 * p0 - 2 * p1},
+        b{-2 * v0 - v1 - 3 * p0 + 3 * p1},
+        c{v0},
+        d{p0} {}
+
+  /**
+   * Return the position at point t.
+   *
+   * @param t The point t
+   * @return The position at point t.
+   */
+  constexpr double GetPosition(double t) const {
+    double t2 = t * t;
+    double t3 = t2 * t;
+    return a * t3 + b * t2 + c * t + d;
+  }
+
+  /**
+   * Return the velocity at point t.
+   *
+   * @param t The point t
+   * @return The velocity at point t.
+   */
+  constexpr double GetVelocity(double t) const {
+    return 3 * a * t * t + 2 * b * t + c;
+  }
+
+  /**
+   * Return the acceleration at point t.
+   *
+   * @param t The point t
+   * @return The acceleration at point t.
+   */
+  constexpr double GetAcceleration(double t) const { return 6 * a * t + 2 * b; }
+
+  /**
+   * Return the jerk at point t.
+   *
+   * @param t The point t
+   * @return The jerk at point t.
+   */
+  constexpr double GetJerk([[maybe_unused]] double t) const { return 6 * a; }
+
+ private:
+  // Coefficients of the cubic polynomial
+  double a;
+  double b;
+  double c;
+  double d;
+};
+
+}  // namespace trajopt