16 #include "frc/geometry/Pose2d.h"
24 units::compound_unit<units::radian, units::inverse<units::meter>>>;
35 using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
45 virtual ~
Spline() =
default;
55 std::array<double, (Degree + 1) / 2> x;
56 std::array<double, (Degree + 1) / 2> y;
66 Eigen::Matrix<double, Degree + 1, 1> polynomialBases;
69 for (
int i = 0; i <= Degree; i++) {
70 polynomialBases(i) = std::pow(t, Degree - i);
75 Eigen::Matrix<double, 6, 1> combined =
Coefficients() * polynomialBases;
77 double dx, dy, ddx, ddy;
92 ddx = combined(4) / t / t;
93 ddy = combined(5) / t / t;
97 const auto curvature =
98 (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
111 virtual Eigen::Matrix<double, 6, Degree + 1>
Coefficients()
const = 0;
120 return (Eigen::Vector2d() << translation.
X().to<
double>(),
121 translation.
Y().to<
double>())
132 return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1)));