Add a 4 point spline to 6 point spline function
This makes a 6th order spline with the same shape as the provided 4th
order spline.
Change-Id: I14cf6ca0560c5a99ece2152c444dc7509dc7a34f
diff --git a/frc971/control_loops/drivetrain/spline.cc b/frc971/control_loops/drivetrain/spline.cc
index 941d520..609d3b1 100644
--- a/frc971/control_loops/drivetrain/spline.cc
+++ b/frc971/control_loops/drivetrain/spline.cc
@@ -4,6 +4,38 @@
namespace control_loops {
namespace drivetrain {
+::Eigen::Matrix<double, 2, 6> Spline4To6(
+ const ::Eigen::Matrix<double, 2, 4> &control_points) {
+ ::Eigen::Matrix<double, 2, 6> new_control_points;
+ // a' = a
+ // b' = (2a + 3b) / 5
+ // c' = (a + 6b + 3c) / 10
+ // d' = (d + 6c + 3b) / 10
+ // e' = (2d + 3c) / 5
+ // f' = d
+ new_control_points.block<2, 1>(0, 0) = control_points.block<2, 1>(0, 0);
+ new_control_points.block<2, 1>(0, 1) =
+ (2.0 * control_points.block<2, 1>(0, 0) +
+ 3.0 * control_points.block<2, 1>(0, 1)) /
+ 5.0;
+ new_control_points.block<2, 1>(0, 2) =
+ (control_points.block<2, 1>(0, 0) +
+ 6.0 * control_points.block<2, 1>(0, 1) +
+ 3.0 * control_points.block<2, 1>(0, 2)) /
+ 10.0;
+ new_control_points.block<2, 1>(0, 3) =
+ (control_points.block<2, 1>(0, 3) +
+ 6.0 * control_points.block<2, 1>(0, 2) +
+ 3.0 * control_points.block<2, 1>(0, 1)) /
+ 10.0;
+ new_control_points.block<2, 1>(0, 4) =
+ (2.0 * control_points.block<2, 1>(0, 3) +
+ 3.0 * control_points.block<2, 1>(0, 2)) /
+ 5.0;
+ new_control_points.block<2, 1>(0, 5) = control_points.block<2, 1>(0, 3);
+ return new_control_points;
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/spline.h b/frc971/control_loops/drivetrain/spline.h
index 7a77082..c5b94cc 100644
--- a/frc971/control_loops/drivetrain/spline.h
+++ b/frc971/control_loops/drivetrain/spline.h
@@ -150,6 +150,10 @@
1.0 / magdxy2 * (dx * dddy - dy * dddx);
}
+ const ::Eigen::Matrix<double, 2, N> &control_points() const {
+ return control_points_;
+ }
+
private:
const ::Eigen::Matrix<double, 2, N> control_points_;
@@ -163,6 +167,10 @@
typedef NSpline<4> Spline;
+// Converts a 4 control point spline into
+::Eigen::Matrix<double, 2, 6> Spline4To6(
+ const ::Eigen::Matrix<double, 2, 4> &control_points);
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/spline_test.cc b/frc971/control_loops/drivetrain/spline_test.cc
index 9aa3325..3ba856d 100644
--- a/frc971/control_loops/drivetrain/spline_test.cc
+++ b/frc971/control_loops/drivetrain/spline_test.cc
@@ -17,10 +17,15 @@
class SplineTest : public ::testing::Test {
protected:
SplineTest()
- : spline_((::Eigen::Matrix<double, 2, 4>() << 0.0, 0.5, 0.5, 1.0, 0.0,
- 0.0, 1.0, 1.0)
- .finished()) {}
- Spline spline_;
+ : control_points_((::Eigen::Matrix<double, 2, 4>() << 0.0, 0.5, 0.5, 1.0,
+ 0.0, 0.0, 1.0, 1.0)
+ .finished()),
+ spline4_(control_points_),
+ spline6_(Spline4To6(control_points_)) {}
+
+ ::Eigen::Matrix<double, 2, 4> control_points_;
+ NSpline<4> spline4_;
+ NSpline<6> spline6_;
};
// Tests that the derivitives of xy integrate back up to the position.
@@ -36,18 +41,18 @@
::std::vector<double> idy_plot;
const int num_points = 10000;
- ::Eigen::Matrix<double, 2, 1> point = spline_.Point(0.0);
- ::Eigen::Matrix<double, 2, 1> dpoint = spline_.DPoint(0.0);
- ::Eigen::Matrix<double, 2, 1> ddpoint = spline_.DDPoint(0.0);
+ ::Eigen::Matrix<double, 2, 1> point = spline6_.Point(0.0);
+ ::Eigen::Matrix<double, 2, 1> dpoint = spline6_.DPoint(0.0);
+ ::Eigen::Matrix<double, 2, 1> ddpoint = spline6_.DDPoint(0.0);
const double dalpha = 1.0 / static_cast<double>(num_points - 1);
for (int i = 0; i < num_points; ++i) {
const double alpha =
1.0 * static_cast<double>(i) / static_cast<double>(num_points - 1);
- const ::Eigen::Matrix<double, 2, 1> expected_point = spline_.Point(alpha);
- const ::Eigen::Matrix<double, 2, 1> expected_dpoint = spline_.DPoint(alpha);
+ const ::Eigen::Matrix<double, 2, 1> expected_point = spline6_.Point(alpha);
+ const ::Eigen::Matrix<double, 2, 1> expected_dpoint = spline6_.DPoint(alpha);
const ::Eigen::Matrix<double, 2, 1> expected_ddpoint =
- spline_.DDPoint(alpha);
+ spline6_.DDPoint(alpha);
alphas_plot.push_back(alpha);
x_plot.push_back(expected_point(0));
@@ -72,7 +77,7 @@
point += dpoint * dalpha;
dpoint += ddpoint * dalpha;
- ddpoint += spline_.DDDPoint(alpha) * dalpha;
+ ddpoint += spline6_.DDDPoint(alpha) * dalpha;
}
// Conditionally plot the functions and their integrals to aid debugging.
@@ -88,6 +93,10 @@
matplotlibcpp::plot(alphas_plot, idy_plot, {{"label", "idy"}});
matplotlibcpp::legend();
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(x_plot, y_plot, {{"label", "spline"}});
+ matplotlibcpp::legend();
+
matplotlibcpp::show();
}
}
@@ -101,15 +110,15 @@
::std::vector<double> idtheta_plot;
const int num_points = 10000;
- double theta = spline_.Theta(0.0);
- double dtheta = spline_.DTheta(0.0);
+ double theta = spline6_.Theta(0.0);
+ double dtheta = spline6_.DTheta(0.0);
const double dalpha = 1.0 / static_cast<double>(num_points - 1);
for (int i = 0; i < num_points; ++i) {
const double alpha =
1.0 * static_cast<double>(i) / static_cast<double>(num_points - 1);
- const double expected_theta = spline_.Theta(alpha);
- const double expected_dtheta = spline_.DTheta(alpha);
+ const double expected_theta = spline6_.Theta(alpha);
+ const double expected_dtheta = spline6_.DTheta(alpha);
alphas_plot.push_back(alpha);
theta_plot.push_back(expected_theta);
@@ -126,7 +135,7 @@
}
theta += dtheta * dalpha;
- dtheta += spline_.DDTheta(alpha) * dalpha;
+ dtheta += spline6_.DDTheta(alpha) * dalpha;
}
// Conditionally plot the functions and their integrals to aid debugging.
@@ -142,6 +151,75 @@
}
}
+// Tests that a 4 point spline has the same points as a 6 point spline built
+// with Spline4To6.
+TEST_F(SplineTest, FourToSixSpline) {
+ const int num_points = 10000;
+
+ ::std::vector<double> alphas_plot;
+ ::std::vector<double> x_plot;
+ ::std::vector<double> y_plot;
+
+ const double dalpha = 1.0 / static_cast<double>(num_points - 1);
+ for (int i = 0; i < num_points; ++i) {
+ const double alpha = dalpha * static_cast<double>(i);
+
+ const ::Eigen::Matrix<double, 2, 1> expected_point = spline4_.Point(alpha);
+ const ::Eigen::Matrix<double, 2, 1> expected_dpoint =
+ spline4_.DPoint(alpha);
+ const ::Eigen::Matrix<double, 2, 1> expected_ddpoint =
+ spline4_.DDPoint(alpha);
+ const ::Eigen::Matrix<double, 2, 1> expected_dddpoint =
+ spline4_.DDDPoint(alpha);
+
+ const ::Eigen::Matrix<double, 2, 1> point = spline6_.Point(alpha);
+ const ::Eigen::Matrix<double, 2, 1> dpoint = spline6_.DPoint(alpha);
+ const ::Eigen::Matrix<double, 2, 1> ddpoint = spline6_.DDPoint(alpha);
+ const ::Eigen::Matrix<double, 2, 1> dddpoint = spline6_.DDDPoint(alpha);
+
+ alphas_plot.push_back(alpha);
+ x_plot.push_back(point(0));
+ y_plot.push_back(point(1));
+
+ EXPECT_LT((point - expected_point).norm(), 1e-9) << ": At alpha " << alpha;
+ EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-9) << ": At alpha "
+ << alpha;
+ EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-9) << ": At alpha "
+ << alpha;
+ EXPECT_LT((dddpoint - expected_dddpoint).norm(), 1e-9) << ": At alpha "
+ << alpha;
+ }
+
+ // Conditionally plot the functions and their integrals to aid debugging.
+ if (FLAGS_plot) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(alphas_plot, x_plot, {{"label", "x"}});
+ matplotlibcpp::plot(alphas_plot, y_plot, {{"label", "y"}});
+ matplotlibcpp::legend();
+
+ ::std::vector<double> control4x;
+ ::std::vector<double> control4y;
+ ::std::vector<double> control6x;
+ ::std::vector<double> control6y;
+ for (int i = 0; i < 4; ++i) {
+ control4x.push_back(spline4_.control_points()(0, i));
+ control4y.push_back(spline4_.control_points()(1, i));
+ }
+ for (int i = 0; i < 6; ++i) {
+ control6x.push_back(spline6_.control_points()(0, i));
+ control6y.push_back(spline6_.control_points()(1, i));
+ }
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(x_plot, y_plot, {{"label", "spline"}});
+ matplotlibcpp::plot(control4x, control4y, {{"label", "4 control points"}});
+ matplotlibcpp::plot(control6x, control6y, {{"label", "6 control points"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::show();
+ }
+}
+
} // namespace testing
} // namespace drivetrain
} // namespace control_loops