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/BUILD b/frc971/control_loops/drivetrain/BUILD
index 7e78c72..2bad4d9 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -288,6 +288,7 @@
hdrs = ["distance_spline.h"],
deps = [
":spline",
+ "//aos/logging",
"//frc971/control_loops:fixed_quadrature",
"//third_party/eigen",
],
@@ -309,6 +310,7 @@
deps = [
":distance_spline",
"//aos/testing:googletest",
+ "//aos/testing:test_shm",
"@com_github_gflags_gflags//:gflags",
] + cpu_select({
"amd64": [
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;
diff --git a/frc971/control_loops/drivetrain/distance_spline_test.cc b/frc971/control_loops/drivetrain/distance_spline_test.cc
index 210d4de..c9cb9f0 100644
--- a/frc971/control_loops/drivetrain/distance_spline_test.cc
+++ b/frc971/control_loops/drivetrain/distance_spline_test.cc
@@ -2,6 +2,7 @@
#include <vector>
+#include "aos/testing/test_shm.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
#if defined(SUPPORT_PLOT)
@@ -21,6 +22,7 @@
protected:
ParameterizedDistanceSplineTest()
: distance_spline_(::std::vector<Spline>(GetParam())) {}
+ ::aos::testing::TestSharedMemory shm_;
DistanceSpline distance_spline_;
};
diff --git a/frc971/control_loops/drivetrain/spline.h b/frc971/control_loops/drivetrain/spline.h
index f60eea5..e549811 100644
--- a/frc971/control_loops/drivetrain/spline.h
+++ b/frc971/control_loops/drivetrain/spline.h
@@ -11,9 +11,6 @@
// Class to hold a spline as a function of alpha. Alpha can only range between
// 0.0 and 1.0.
-// 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.
template <int N>
class NSpline {
public: