Template Spline with the number of control points.
It's just about as hard to go from 4 to 6 as 4 to N... Parker provided
me with the algorithm.
Change-Id: I8bc665b8798d4c31a58c17fa9f67fcf7b601b987
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index ac6e2d3..9bb335c 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -263,6 +263,7 @@
srcs = ["spline.cc"],
hdrs = ["spline.h"],
deps = [
+ "//frc971/control_loops:binomial",
"//third_party/eigen",
],
)
diff --git a/frc971/control_loops/drivetrain/spline.cc b/frc971/control_loops/drivetrain/spline.cc
index f75c6eb..941d520 100644
--- a/frc971/control_loops/drivetrain/spline.cc
+++ b/frc971/control_loops/drivetrain/spline.cc
@@ -4,45 +4,6 @@
namespace control_loops {
namespace drivetrain {
-// TODO(austin): We are re-computing dpoints a lot here. Figure out how to pass
-// them in.
-double Spline::Theta(double alpha) const {
- const ::Eigen::Matrix<double, 2, 1> dp = DPoint(alpha);
- return ::std::atan2(dp(1), dp(0));
-}
-
-double Spline::DTheta(double alpha) const {
- const ::Eigen::Matrix<double, 2, 1> dp = DPoint(alpha);
- const ::Eigen::Matrix<double, 2, 1> ddp = DDPoint(alpha);
- const double dx = dp(0);
- const double dy = dp(1);
-
- const double ddx = ddp(0);
- const double ddy = ddp(1);
-
- return 1.0 / (::std::pow(dx, 2) + ::std::pow(dy, 2)) * (dx * ddy - dy * ddx);
-}
-
-double Spline::DDTheta(double alpha) const {
- const ::Eigen::Matrix<double, 2, 1> dp = DPoint(alpha);
- const ::Eigen::Matrix<double, 2, 1> ddp = DDPoint(alpha);
- const ::Eigen::Matrix<double, 2, 1> dddp = DDDPoint(alpha);
- const double dx = dp(0);
- const double dy = dp(1);
-
- const double ddx = ddp(0);
- const double ddy = ddp(1);
-
- const double dddx = dddp(0);
- const double dddy = dddp(1);
-
- const double magdxy2 = ::std::pow(dx, 2) + ::std::pow(dy, 2);
-
- return -1.0 / (::std::pow(magdxy2, 2)) * (dx * ddy - dy * ddx) * 2.0 *
- (dy * ddy + dx * ddx) +
- 1.0 / magdxy2 * (dx * dddy - dy * dddx);
-}
-
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/spline.h b/frc971/control_loops/drivetrain/spline.h
index 4d8533f..7a77082 100644
--- a/frc971/control_loops/drivetrain/spline.h
+++ b/frc971/control_loops/drivetrain/spline.h
@@ -3,6 +3,8 @@
#include "Eigen/Dense"
+#include "frc971/control_loops/binomial.h"
+
namespace frc971 {
namespace control_loops {
namespace drivetrain {
@@ -12,69 +14,155 @@
// TODO(austin): Need to be able to represent splines which have more than 2
// control points at some point. Or splines chained together. This is close
// enough for now.
-class Spline {
+template <int N>
+class NSpline {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ // Computes the matrix to multiply by [1, a, a^2, ...] to evaluate the spline.
+ template <int D>
+ static ::Eigen::Matrix<double, 2, N - D> SplinePolynomial(
+ const ::Eigen::Matrix<double, 2, N> &control_points) {
+ ::Eigen::Matrix<double, 2, N> polynomial =
+ ::Eigen::Matrix<double, 2, N>::Zero();
+ // Compute this in pieces. Start by the coefficients of each control point.
+ for (int i = 0; i < N; ++i) {
+ // Binomial(N - 1, i) * (1 - t) ^ (N - i - 1) * (t ^ i) * P[i]
+ const double binomial = Binomial(N - 1, i);
+ const int one_minus_t_power = N - i - 1;
+ // Then iterate over the powers of t and add the pieces to the polynomial
+ // matrix.
+ for (int j = i; j < N; ++j) {
+ // j is the power of t we are placing in the polynomial matrix.
+ // k is the power of t in the (1 - t) expression that we need to
+ // evaluate.
+ const int k = j - i;
+ const double tscalar =
+ binomial * Binomial(one_minus_t_power, k) * ::std::pow(-1.0, k);
+ polynomial.template block<2, 1>(0, j) +=
+ control_points.template block<2, 1>(0, i) * tscalar;
+ }
+ }
+
+ // Now, compute the derivative requested.
+ for (int i = D; i < N; ++i) {
+ // Start with t^i, and multiply i, i-1, i-2 until you have done this
+ // Derivative times.
+ double scalar = 1.0;
+ for (int j = i; j > i - D; --j) {
+ scalar *= j;
+ }
+ polynomial.template block<2, 1>(0, i) =
+ polynomial.template block<2, 1>(0, i) * scalar;
+ }
+ return polynomial.template block<2, N - D>(0, D);
+ }
+
+ // Computes an order M-1 polynomial matrix in alpha. [1, alpha, alpha^2, ...]
+ template <int M>
+ static ::Eigen::Matrix<double, M, 1> AlphaPolynomial(const double alpha) {
+ ::Eigen::Matrix<double, M, 1> polynomial =
+ ::Eigen::Matrix<double, M, 1>::Zero();
+ polynomial(0) = 1.0;
+ for (int i = 1; i < M; ++i) {
+ polynomial(i) = polynomial(i - 1) * alpha;
+ }
+ return polynomial;
+ }
+
// Constructs a spline. control_points is a matrix of start, control1,
- // control2, end.
- Spline(::Eigen::Matrix<double, 2, 4> control_points)
- : control_points_(control_points) {}
+ // control2, ..., end.
+ NSpline(::Eigen::Matrix<double, 2, N> control_points)
+ : control_points_(control_points),
+ spline_polynomial_(SplinePolynomial<0>(control_points_)),
+ dspline_polynomial_(SplinePolynomial<1>(control_points_)),
+ ddspline_polynomial_(SplinePolynomial<2>(control_points_)),
+ dddspline_polynomial_(SplinePolynomial<3>(control_points_)) {}
// Returns the xy coordiate of the spline for a given alpha.
::Eigen::Matrix<double, 2, 1> Point(double alpha) const {
- return control_points_ *
- (::Eigen::Matrix<double, 4, 1>() << ::std::pow((1.0 - alpha), 3),
- 3.0 * ::std::pow((1.0 - alpha), 2) * alpha,
- 3.0 * (1.0 - alpha) * ::std::pow(alpha, 2), ::std::pow(alpha, 3))
- .finished();
+ return spline_polynomial_ * AlphaPolynomial<N>(alpha);
}
// Returns the dspline/dalpha for a given alpha.
::Eigen::Matrix<double, 2, 1> DPoint(double alpha) const {
- return control_points_ *
- (::Eigen::Matrix<double, 4, 1>()
- << -3.0 * ::std::pow((1.0 - alpha), 2),
- 3.0 * ::std::pow((1.0 - alpha), 2) +
- -2.0 * 3.0 * (1.0 - alpha) * alpha,
- -3.0 * ::std::pow(alpha, 2) + 2.0 * 3.0 * (1.0 - alpha) * alpha,
- 3.0 * ::std::pow(alpha, 2))
- .finished();
+ return dspline_polynomial_ * AlphaPolynomial<N - 1>(alpha);
}
// Returns the d^2spline/dalpha^2 for a given alpha.
::Eigen::Matrix<double, 2, 1> DDPoint(double alpha) const {
- return control_points_ *
- (::Eigen::Matrix<double, 4, 1>() << 2.0 * 3.0 * (1.0 - alpha),
- -2.0 * 3.0 * (1.0 - alpha) + -2.0 * 3.0 * (1.0 - alpha) +
- 2.0 * 3.0 * alpha,
- -2.0 * 3.0 * alpha + 2.0 * 3.0 * (1.0 - alpha) - 2.0 * 3.0 * alpha,
- 2.0 * 3.0 * alpha)
- .finished();
+ return ddspline_polynomial_ * AlphaPolynomial<N - 2>(alpha);
}
// Returns the d^3spline/dalpha^3 for a given alpha.
- ::Eigen::Matrix<double, 2, 1> DDDPoint(double /*alpha*/) const {
- return control_points_ *
- (::Eigen::Matrix<double, 4, 1>() << -2.0 * 3.0,
- 2.0 * 3.0 + 2.0 * 3.0 + 2.0 * 3.0,
- -2.0 * 3.0 - 2.0 * 3.0 - 2.0 * 3.0, 2.0 * 3.0)
- .finished();
+ ::Eigen::Matrix<double, 2, 1> DDDPoint(double alpha) const {
+ return dddspline_polynomial_ * AlphaPolynomial<N - 3>(alpha);
}
// Returns theta for a given alpha.
- double Theta(double alpha) const;
+ double Theta(double alpha) const {
+ const ::Eigen::Matrix<double, 2, 1> dp = DPoint(alpha);
+ return ::std::atan2(dp(1), dp(0));
+ }
// Returns dtheta/dalpha for a given alpha.
- double DTheta(double alpha) const;
+ double DTheta(double alpha) const {
+ const ::Eigen::Matrix<double, N - 1, 1> alpha_polynomial =
+ AlphaPolynomial<N - 1>(alpha);
+
+ const ::Eigen::Matrix<double, 2, 1> dp =
+ dspline_polynomial_ * alpha_polynomial;
+ const ::Eigen::Matrix<double, 2, 1> ddp =
+ ddspline_polynomial_ * alpha_polynomial.template block<N - 2, 1>(0, 0);
+ const double dx = dp(0);
+ const double dy = dp(1);
+
+ const double ddx = ddp(0);
+ const double ddy = ddp(1);
+
+ return 1.0 / (::std::pow(dx, 2) + ::std::pow(dy, 2)) *
+ (dx * ddy - dy * ddx);
+ }
// Returns d^2 theta/dalpha^2 for a given alpha.
- double DDTheta(double alpha) const;
+ double DDTheta(double alpha) const {
+ const ::Eigen::Matrix<double, N - 1, 1> alpha_polynomial =
+ AlphaPolynomial<N - 1>(alpha);
+ const ::Eigen::Matrix<double, 2, 1> dp =
+ dspline_polynomial_ * alpha_polynomial;
+ const ::Eigen::Matrix<double, 2, 1> ddp =
+ ddspline_polynomial_ * alpha_polynomial.template block<N - 2, 1>(0, 0);
+ const ::Eigen::Matrix<double, 2, 1> dddp =
+ dddspline_polynomial_ * alpha_polynomial.template block<N - 3, 1>(0, 0);
+ const double dx = dp(0);
+ const double dy = dp(1);
+
+ const double ddx = ddp(0);
+ const double ddy = ddp(1);
+
+ const double dddx = dddp(0);
+ const double dddy = dddp(1);
+
+ const double magdxy2 = ::std::pow(dx, 2) + ::std::pow(dy, 2);
+
+ return -1.0 / (::std::pow(magdxy2, 2)) * (dx * ddy - dy * ddx) * 2.0 *
+ (dy * ddy + dx * ddx) +
+ 1.0 / magdxy2 * (dx * dddy - dy * dddx);
+ }
private:
- ::Eigen::Matrix<double, 2, 4> control_points_;
+ const ::Eigen::Matrix<double, 2, N> control_points_;
+
+ // Each of these polynomials gets multiplied by [x^(n-1), x^(n-2), ..., x, 1]
+ // depending on the size of the polynomial.
+ const ::Eigen::Matrix<double, 2, N> spline_polynomial_;
+ const ::Eigen::Matrix<double, 2, N - 1> dspline_polynomial_;
+ const ::Eigen::Matrix<double, 2, N - 2> ddspline_polynomial_;
+ const ::Eigen::Matrix<double, 2, N - 3> dddspline_polynomial_;
};
+typedef NSpline<4> Spline;
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971