Check that the end of the splines are continuous
Check that we are continuous to the second derivitive of the spline.
This doesn't allow d alpha/ dd to not be the same across the junction.
ie, it's overly conservative. That's a fine starting assumption.
Change-Id: I927565229e26df07a5c8b085c9e97c55da82c28b
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index f07f5ee..1572526 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -1,5 +1,6 @@
#include "frc971/control_loops/drivetrain/distance_spline.h"
+#include "aos/logging/logging.h"
#include "frc971/control_loops/drivetrain/spline.h"
namespace frc971 {
@@ -11,6 +12,46 @@
::std::vector<double> distances;
distances.push_back(0.0);
+ if (splines_.size() > 1) {
+ // We've got a multispline to follow!
+ // Confirm that the ends line up to the correct number of derivitives.
+ for (size_t i = 1; i < splines_.size(); ++i) {
+ const Spline &spline0 = splines_[i - 1];
+ const Spline &spline1 = splines_[i];
+
+ const ::Eigen::Matrix<double, 2, 1> end0 = spline0.Point(1.0);
+ const ::Eigen::Matrix<double, 2, 1> start1 = spline1.Point(0.0);
+
+ if (!end0.isApprox(start1, 1e-6)) {
+ LOG(ERROR, "Splines %d and %d don't line up. [%f, %f] != [%f, %f]\n",
+ static_cast<int>(i - 1), static_cast<int>(i), end0(0, 0),
+ end0(1, 0), start1(0, 0), start1(1, 0));
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> dend0 = spline0.DPoint(1.0);
+ const ::Eigen::Matrix<double, 2, 1> dstart1 = spline1.DPoint(0.0);
+
+ if (!dend0.isApprox(dstart1, 1e-6)) {
+ LOG(ERROR,
+ "Splines %d and %d don't line up in the first derivitive. [%f, "
+ "%f] != [%f, %f]\n",
+ static_cast<int>(i - 1), static_cast<int>(i), dend0(0, 0),
+ dend0(1, 0), dstart1(0, 0), dstart1(1, 0));
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> ddend0 = spline0.DDPoint(1.0);
+ const ::Eigen::Matrix<double, 2, 1> ddstart1 = spline1.DDPoint(0.0);
+
+ if (!ddend0.isApprox(ddstart1, 1e-6)) {
+ LOG(ERROR,
+ "Splines %d and %d don't line up in the second derivitive. [%f, "
+ "%f] != [%f, %f]\n",
+ static_cast<int>(i - 1), static_cast<int>(i), ddend0(0, 0),
+ ddend0(1, 0), ddstart1(0, 0), ddstart1(1, 0));
+ }
+ }
+ }
+
const double dalpha =
static_cast<double>(splines_.size()) / static_cast<double>(num_alpha - 1);
double last_alpha = 0.0;