Add cubic beizer spline class and tests
Change-Id: Ice1d1f3301b3b3a0faef03240bb1afc6de5f4112
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index abc8aaf..6dbee52 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -10,8 +10,8 @@
],
deps = [
":drivetrain_queue",
- "//aos/controls:replay_control_loop",
"//aos:init",
+ "//aos/controls:replay_control_loop",
"//frc971/queues:gyro",
],
)
@@ -66,7 +66,7 @@
"//aos/controls:polytope",
"//aos/logging:matrix_logging",
"//aos/logging:queue_logging",
- "//aos/robot_state:robot_state",
+ "//aos/robot_state",
"//aos/util:log_interval",
"//aos/util:trapezoid_profile",
"//frc971:shifter_hall_effect",
@@ -91,7 +91,7 @@
"//aos/controls:polytope",
"//aos/logging:matrix_logging",
"//aos/logging:queue_logging",
- "//aos/robot_state:robot_state",
+ "//aos/robot_state",
"//aos/util:log_interval",
"//frc971/control_loops:coerce_goal",
"//frc971/control_loops:state_feedback_loop",
@@ -257,3 +257,26 @@
"//frc971/control_loops:state_feedback_loop",
],
)
+
+cc_library(
+ name = "spline",
+ srcs = ["spline.cc"],
+ hdrs = ["spline.h"],
+ deps = [
+ "//third_party/eigen",
+ ],
+)
+
+cc_test(
+ name = "spline_test",
+ srcs = [
+ "spline_test.cc",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":spline",
+ "//aos/testing:googletest",
+ "//third_party/matplotlib-cpp",
+ "@com_github_gflags_gflags//:gflags",
+ ],
+)
diff --git a/frc971/control_loops/drivetrain/spline.cc b/frc971/control_loops/drivetrain/spline.cc
new file mode 100644
index 0000000..f75c6eb
--- /dev/null
+++ b/frc971/control_loops/drivetrain/spline.cc
@@ -0,0 +1,48 @@
+#include "frc971/control_loops/drivetrain/spline.h"
+
+namespace frc971 {
+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
new file mode 100644
index 0000000..4d8533f
--- /dev/null
+++ b/frc971/control_loops/drivetrain/spline.h
@@ -0,0 +1,82 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINE_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINE_H_
+
+#include "Eigen/Dense"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+// 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.
+class Spline {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ // 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) {}
+
+ // 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();
+ }
+
+ // 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();
+ }
+
+ // 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();
+ }
+
+ // 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();
+ }
+
+ // Returns theta for a given alpha.
+ double Theta(double alpha) const;
+
+ // Returns dtheta/dalpha for a given alpha.
+ double DTheta(double alpha) const;
+
+ // Returns d^2 theta/dalpha^2 for a given alpha.
+ double DDTheta(double alpha) const;
+
+ private:
+ ::Eigen::Matrix<double, 2, 4> control_points_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINE_H_
diff --git a/frc971/control_loops/drivetrain/spline_test.cc b/frc971/control_loops/drivetrain/spline_test.cc
new file mode 100644
index 0000000..9aa3325
--- /dev/null
+++ b/frc971/control_loops/drivetrain/spline_test.cc
@@ -0,0 +1,148 @@
+#include "frc971/control_loops/drivetrain/spline.h"
+
+#include <vector>
+
+#include "gflags/gflags.h"
+#include "gtest/gtest.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+
+DEFINE_bool(plot, false, "If true, plot");
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+// Test fixture with a spline from 0, 0 to 1, 1
+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_;
+};
+
+// Tests that the derivitives of xy integrate back up to the position.
+TEST_F(SplineTest, XYIntegral) {
+ ::std::vector<double> alphas_plot;
+ ::std::vector<double> x_plot;
+ ::std::vector<double> y_plot;
+ ::std::vector<double> ix_plot;
+ ::std::vector<double> iy_plot;
+ ::std::vector<double> dx_plot;
+ ::std::vector<double> dy_plot;
+ ::std::vector<double> idx_plot;
+ ::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);
+
+ 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_ddpoint =
+ spline_.DDPoint(alpha);
+
+ alphas_plot.push_back(alpha);
+ x_plot.push_back(expected_point(0));
+ y_plot.push_back(expected_point(1));
+ ix_plot.push_back(point(0));
+ iy_plot.push_back(point(1));
+ dx_plot.push_back(expected_dpoint(0));
+ dy_plot.push_back(expected_dpoint(1));
+ idx_plot.push_back(dpoint(0));
+ idy_plot.push_back(dpoint(1));
+
+ EXPECT_LT((point - expected_point).norm(), 1e-2) << ": At alpha " << alpha;
+ EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-2) << ": At alpha "
+ << alpha;
+ EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-2) << ": At alpha "
+ << alpha;
+
+ // We need to record the starting state without integrating.
+ if (i == 0) {
+ continue;
+ }
+
+ point += dpoint * dalpha;
+ dpoint += ddpoint * dalpha;
+ ddpoint += spline_.DDDPoint(alpha) * dalpha;
+ }
+
+ // 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, ix_plot, {{"label", "ix"}});
+ matplotlibcpp::plot(alphas_plot, y_plot, {{"label", "y"}});
+ matplotlibcpp::plot(alphas_plot, iy_plot, {{"label", "iy"}});
+ matplotlibcpp::plot(alphas_plot, dx_plot, {{"label", "dx"}});
+ matplotlibcpp::plot(alphas_plot, idx_plot, {{"label", "idx"}});
+ matplotlibcpp::plot(alphas_plot, dy_plot, {{"label", "dy"}});
+ matplotlibcpp::plot(alphas_plot, idy_plot, {{"label", "idy"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::show();
+ }
+}
+
+// Tests that the derivitives of theta integrate back up to the angle.
+TEST_F(SplineTest, ThetaIntegral) {
+ ::std::vector<double> alphas_plot;
+ ::std::vector<double> theta_plot;
+ ::std::vector<double> itheta_plot;
+ ::std::vector<double> dtheta_plot;
+ ::std::vector<double> idtheta_plot;
+
+ const int num_points = 10000;
+ double theta = spline_.Theta(0.0);
+ double dtheta = spline_.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);
+
+ alphas_plot.push_back(alpha);
+ theta_plot.push_back(expected_theta);
+ itheta_plot.push_back(theta);
+ dtheta_plot.push_back(expected_dtheta);
+ idtheta_plot.push_back(dtheta);
+
+ EXPECT_NEAR(expected_theta, theta, 1e-2) << ": At alpha " << alpha;
+ EXPECT_NEAR(expected_dtheta, dtheta, 1e-2) << ": At alpha " << alpha;
+
+ // We need to record the starting state without integrating.
+ if (i == 0) {
+ continue;
+ }
+
+ theta += dtheta * dalpha;
+ dtheta += spline_.DDTheta(alpha) * dalpha;
+ }
+
+ // Conditionally plot the functions and their integrals to aid debugging.
+ if (FLAGS_plot) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(alphas_plot, theta_plot, {{"label", "theta"}});
+ matplotlibcpp::plot(alphas_plot, itheta_plot, {{"label", "itheta"}});
+ matplotlibcpp::plot(alphas_plot, dtheta_plot, {{"label", "dtheta"}});
+ matplotlibcpp::plot(alphas_plot, idtheta_plot, {{"label", "idtheta"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::show();
+ }
+}
+
+} // namespace testing
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971