Merge "Added Comment to clarify sendtime timestamp"
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index c08599b..c37f379 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -277,3 +277,28 @@
"@com_github_gflags_gflags//:gflags",
],
)
+
+cc_library(
+ name = "distance_spline",
+ srcs = ["distance_spline.cc"],
+ hdrs = ["distance_spline.h"],
+ deps = [
+ ":spline",
+ "//frc971/control_loops:fixed_quadrature",
+ "//third_party/eigen",
+ ],
+)
+
+cc_test(
+ name = "distance_spline_test",
+ srcs = [
+ "distance_spline_test.cc",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":distance_spline",
+ "//aos/testing:googletest",
+ "//third_party/matplotlib-cpp",
+ "@com_github_gflags_gflags//:gflags",
+ ],
+)
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
new file mode 100644
index 0000000..2ac3e62
--- /dev/null
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -0,0 +1,80 @@
+#include "frc971/control_loops/drivetrain/distance_spline.h"
+
+#include "frc971/control_loops/drivetrain/spline.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+DistanceSpline::DistanceSpline(const Spline &spline, int num_alpha)
+ : spline_(spline) {
+ distances_.push_back(0.0);
+ const double dalpha = 1.0 / static_cast<double>(num_alpha - 1);
+
+ double last_alpha = 0.0;
+ for (int i = 1; i < num_alpha; ++i) {
+ const double alpha = dalpha * i;
+ distances_.push_back(
+ distances_.back() +
+ GaussianQuadrature5(
+ [this](double alpha) { return this->spline_.DPoint(alpha).norm(); },
+ last_alpha, alpha));
+ last_alpha = alpha;
+ }
+}
+
+::Eigen::Matrix<double, 2, 1> DistanceSpline::DDXY(double distance) const {
+ const double alpha = DistanceToAlpha(distance);
+ const ::Eigen::Matrix<double, 2, 1> dspline_point = spline_.DPoint(alpha);
+ const ::Eigen::Matrix<double, 2, 1> ddspline_point = spline_.DDPoint(alpha);
+
+ const double squared_norm = dspline_point.squaredNorm();
+
+ return ddspline_point / squared_norm -
+ dspline_point * (dspline_point(0) * ddspline_point(0) +
+ dspline_point(1) * ddspline_point(1)) /
+ ::std::pow(squared_norm, 2);
+}
+
+double DistanceSpline::DDTheta(double distance) const {
+ const double alpha = DistanceToAlpha(distance);
+
+ // TODO(austin): We are re-computing DPoint here even worse
+ const ::Eigen::Matrix<double, 2, 1> dspline_point = spline_.DPoint(alpha);
+ const ::Eigen::Matrix<double, 2, 1> ddspline_point = spline_.DDPoint(alpha);
+
+ const double dtheta = spline_.DTheta(alpha);
+ const double ddtheta = spline_.DDTheta(alpha);
+
+ const double squared_norm = dspline_point.squaredNorm();
+
+ return ddtheta / squared_norm -
+ dtheta * (dspline_point(0) * ddspline_point(0) +
+ dspline_point(1) * ddspline_point(1)) /
+ ::std::pow(squared_norm, 2);
+}
+
+double DistanceSpline::DistanceToAlpha(double distance) const {
+ if (distance <= 0.0) {
+ return 0.0;
+ }
+ if (distance >= length()) {
+ return 1.0;
+ }
+
+ // Find the distance right below our number using a binary search.
+ size_t after = ::std::distance(
+ distances_.begin(),
+ ::std::lower_bound(distances_.begin(), distances_.end(), distance));
+ size_t before = after - 1;
+ const double distance_step_size =
+ (1.0 / static_cast<double>(distances_.size() - 1));
+
+ return (distance - distances_[before]) /
+ (distances_[after] - distances_[before]) * distance_step_size +
+ static_cast<double>(before) * distance_step_size;
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
new file mode 100644
index 0000000..3c1824c
--- /dev/null
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -0,0 +1,66 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DISTANCE_SPLINE_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DISTANCE_SPLINE_H_
+
+#include <vector>
+
+#include "Eigen/Dense"
+#include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/fixed_quadrature.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+// Class to hold a spline as a function of distance.
+class DistanceSpline {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ DistanceSpline(const Spline &spline, int num_alpha = 100);
+
+ // Returns a point on the spline as a function of distance.
+ ::Eigen::Matrix<double, 2, 1> XY(double distance) const {
+ return spline_.Point(DistanceToAlpha(distance));
+ }
+
+ // Returns the velocity as a function of distance.
+ ::Eigen::Matrix<double, 2, 1> DXY(double distance) const {
+ return spline_.DPoint(DistanceToAlpha(distance)).normalized();
+ }
+
+ // Returns the acceleration as a function of distance.
+ ::Eigen::Matrix<double, 2, 1> DDXY(double distance) const;
+
+ // Returns the heading as a function of distance.
+ double Theta(double distance) const {
+ return spline_.Theta(DistanceToAlpha(distance));
+ }
+
+ // Returns the angular velocity as a function of distance.
+ double DTheta(double distance) const {
+ // TODO(austin): We are re-computing DPoint here!
+ const double alpha = DistanceToAlpha(distance);
+ return spline_.DTheta(alpha) / spline_.DPoint(alpha).norm();
+ }
+
+ // Returns the angular acceleration as a function of distance.
+ double DDTheta(double distance) const;
+
+ // Returns the length of the path in meters.
+ double length() const { return distances_.back(); }
+
+ private:
+ // Computes alpha for a distance
+ double DistanceToAlpha(double distance) const;
+
+ // The spline we are converting to a distance.
+ const Spline spline_;
+ // An interpolation table of distances evenly distributed in alpha.
+ ::std::vector<double> distances_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DISTANCE_SPLINE_H_
diff --git a/frc971/control_loops/drivetrain/distance_spline_test.cc b/frc971/control_loops/drivetrain/distance_spline_test.cc
new file mode 100644
index 0000000..dd33214
--- /dev/null
+++ b/frc971/control_loops/drivetrain/distance_spline_test.cc
@@ -0,0 +1,145 @@
+#include "frc971/control_loops/drivetrain/distance_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 DistanceSplineTest : public ::testing::Test {
+ protected:
+ DistanceSplineTest()
+ : distance_spline_(Spline((::Eigen::Matrix<double, 2, 4>() << 0.0, 0.5,
+ 0.5, 1.0, 0.0, 0.0, 1.0, 1.0)
+ .finished())) {}
+ DistanceSpline distance_spline_;
+};
+
+// Tests that the derivitives of xy integrate back up to the position.
+TEST_F(DistanceSplineTest, XYIntegral) {
+ ::std::vector<double> distances_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 = distance_spline_.XY(0.0);
+ ::Eigen::Matrix<double, 2, 1> dpoint = distance_spline_.DXY(0.0);
+
+ const double ddistance =
+ distance_spline_.length() / static_cast<double>(num_points - 1);
+ for (int i = 0; i < num_points; ++i) {
+ const double distance = ddistance * static_cast<double>(i);
+ const ::Eigen::Matrix<double, 2, 1> expected_point =
+ distance_spline_.XY(distance);
+ const ::Eigen::Matrix<double, 2, 1> expected_dpoint =
+ distance_spline_.DXY(distance);
+
+ distances_plot.push_back(distance);
+ 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 distance "
+ << distance;
+ EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-2) << ": At distance "
+ << distance;
+
+ // We need to record the starting state without integrating.
+ if (i == 0) {
+ continue;
+ }
+
+ point += dpoint * ddistance;
+ dpoint += distance_spline_.DDXY(distance) * ddistance;
+ }
+
+ // Conditionally plot the functions and their integrals to aid debugging.
+ if (FLAGS_plot) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(distances_plot, x_plot, {{"label", "x"}});
+ matplotlibcpp::plot(distances_plot, ix_plot, {{"label", "ix"}});
+ matplotlibcpp::plot(distances_plot, y_plot, {{"label", "y"}});
+ matplotlibcpp::plot(distances_plot, iy_plot, {{"label", "iy"}});
+ matplotlibcpp::plot(distances_plot, dx_plot, {{"label", "dx"}});
+ matplotlibcpp::plot(distances_plot, idx_plot, {{"label", "idx"}});
+ matplotlibcpp::plot(distances_plot, dy_plot, {{"label", "dy"}});
+ matplotlibcpp::plot(distances_plot, idy_plot, {{"label", "idy"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::show();
+ }
+}
+
+// Tests that the derivitives of xy integrate back up to the position.
+TEST_F(DistanceSplineTest, ThetaIntegral) {
+ ::std::vector<double> distances_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 = distance_spline_.Theta(0.0);
+ double dtheta = distance_spline_.DTheta(0.0);
+
+ const double ddistance =
+ distance_spline_.length() / static_cast<double>(num_points - 1);
+ for (int i = 0; i < num_points; ++i) {
+ const double distance = ddistance * static_cast<double>(i);
+ const double expected_theta = distance_spline_.Theta(distance);
+ const double expected_dtheta = distance_spline_.DTheta(distance);
+
+ distances_plot.push_back(distance);
+ 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 distance " << distance;
+ EXPECT_NEAR(expected_dtheta, dtheta, 1e-2) << ": At distance " << distance;
+
+ // We need to record the starting state without integrating.
+ if (i == 0) {
+ continue;
+ }
+
+ theta += dtheta * ddistance;
+ dtheta += distance_spline_.DDTheta(distance) * ddistance;
+ }
+
+ // Conditionally plot the functions and their integrals to aid debugging.
+ if (FLAGS_plot) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(distances_plot, theta_plot, {{"label", "theta"}});
+ matplotlibcpp::plot(distances_plot, itheta_plot, {{"label", "itheta"}});
+ matplotlibcpp::plot(distances_plot, dtheta_plot, {{"label", "dtheta"}});
+ matplotlibcpp::plot(distances_plot, idtheta_plot, {{"label", "idtheta"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::show();
+ }
+}
+
+} // namespace testing
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971