Follow multiple 6 splines
Convert from a 4 to a 6 spline by default, and use a vector of them.
This requires continuity in the angular velocity of the spline that is
not yet checked.
Change-Id: I31487d03916f4831095d922745ba78507895a683
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index 2ac3e62..f07f5ee 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -6,27 +6,44 @@
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);
+::std::vector<double> DistanceSpline::BuildDistances(size_t num_alpha) {
+ num_alpha = num_alpha == 0 ? 100 * splines_.size() : num_alpha;
+ ::std::vector<double> distances;
+ distances.push_back(0.0);
+ const double dalpha =
+ static_cast<double>(splines_.size()) / static_cast<double>(num_alpha - 1);
double last_alpha = 0.0;
- for (int i = 1; i < num_alpha; ++i) {
+ for (size_t 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));
+ distances.push_back(distances.back() +
+ GaussianQuadrature5(
+ [this](double alpha) {
+ const size_t spline_index = ::std::min(
+ static_cast<size_t>(::std::floor(alpha)),
+ splines_.size() - 1);
+ return this->splines_[spline_index]
+ .DPoint(alpha - spline_index)
+ .norm();
+ },
+ last_alpha, alpha));
last_alpha = alpha;
}
+ return distances;
}
+DistanceSpline::DistanceSpline(::std::vector<Spline> &&splines, int num_alpha)
+ : splines_(::std::move(splines)), distances_(BuildDistances(num_alpha)) {}
+
+DistanceSpline::DistanceSpline(const Spline &spline, int num_alpha)
+ : splines_({spline}), distances_(BuildDistances(num_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 AlphaAndIndex a = DistanceToAlpha(distance);
+ const ::Eigen::Matrix<double, 2, 1> dspline_point =
+ splines_[a.index].DPoint(a.alpha);
+ const ::Eigen::Matrix<double, 2, 1> ddspline_point =
+ splines_[a.index].DDPoint(a.alpha);
const double squared_norm = dspline_point.squaredNorm();
@@ -37,14 +54,16 @@
}
double DistanceSpline::DDTheta(double distance) const {
- const double alpha = DistanceToAlpha(distance);
+ const AlphaAndIndex a = 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 ::Eigen::Matrix<double, 2, 1> dspline_point =
+ splines_[a.index].DPoint(a.alpha);
+ const ::Eigen::Matrix<double, 2, 1> ddspline_point =
+ splines_[a.index].DDPoint(a.alpha);
- const double dtheta = spline_.DTheta(alpha);
- const double ddtheta = spline_.DDTheta(alpha);
+ const double dtheta = splines_[a.index].DTheta(a.alpha);
+ const double ddtheta = splines_[a.index].DDTheta(a.alpha);
const double squared_norm = dspline_point.squaredNorm();
@@ -54,12 +73,13 @@
::std::pow(squared_norm, 2);
}
-double DistanceSpline::DistanceToAlpha(double distance) const {
+DistanceSpline::AlphaAndIndex DistanceSpline::DistanceToAlpha(
+ double distance) const {
if (distance <= 0.0) {
- return 0.0;
+ return {0, 0.0};
}
if (distance >= length()) {
- return 1.0;
+ return {splines_.size() - 1, 1.0};
}
// Find the distance right below our number using a binary search.
@@ -68,11 +88,15 @@
::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));
+ (splines_.size() / static_cast<double>(distances_.size() - 1));
- return (distance - distances_[before]) /
- (distances_[after] - distances_[before]) * distance_step_size +
- static_cast<double>(before) * distance_step_size;
+ const double alpha = (distance - distances_[before]) /
+ (distances_[after] - distances_[before]) *
+ distance_step_size +
+ static_cast<double>(before) * distance_step_size;
+ const size_t index = static_cast<size_t>(::std::floor(alpha));
+
+ return {index, alpha - index};
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index 7dea1dc..1c9f37d 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -16,16 +16,19 @@
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- DistanceSpline(const Spline &spline, int num_alpha = 100);
+ DistanceSpline(const Spline &spline, int num_alpha = 0);
+ DistanceSpline(::std::vector<Spline> &&splines, int num_alpha = 0);
// 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));
+ const AlphaAndIndex a = DistanceToAlpha(distance);
+ return splines_[a.index].Point(a.alpha);
}
// Returns the velocity as a function of distance.
::Eigen::Matrix<double, 2, 1> DXY(double distance) const {
- return spline_.DPoint(DistanceToAlpha(distance)).normalized();
+ const AlphaAndIndex a = DistanceToAlpha(distance);
+ return splines_[a.index].DPoint(a.alpha).normalized();
}
// Returns the acceleration as a function of distance.
@@ -33,14 +36,16 @@
// Returns the heading as a function of distance.
double Theta(double distance) const {
- return spline_.Theta(DistanceToAlpha(distance));
+ const AlphaAndIndex a = DistanceToAlpha(distance);
+ return splines_[a.index].Theta(a.alpha);
}
// 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();
+ const AlphaAndIndex a = DistanceToAlpha(distance);
+ const Spline &spline = splines_[a.index];
+ return spline.DTheta(a.alpha) / spline.DPoint(a.alpha).norm();
}
double DThetaDt(double distance, double velocity) const {
@@ -54,13 +59,20 @@
double length() const { return distances_.back(); }
private:
+ struct AlphaAndIndex {
+ size_t index;
+ double alpha;
+ };
+
// Computes alpha for a distance
- double DistanceToAlpha(double distance) const;
+ AlphaAndIndex DistanceToAlpha(double distance) const;
+
+ ::std::vector<double> BuildDistances(size_t num_alpha);
// The spline we are converting to a distance.
- const Spline spline_;
+ const ::std::vector<Spline> splines_;
// An interpolation table of distances evenly distributed in alpha.
- ::std::vector<double> distances_;
+ const ::std::vector<double> distances_;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/distance_spline_test.cc b/frc971/control_loops/drivetrain/distance_spline_test.cc
index 86c6d8b..210d4de 100644
--- a/frc971/control_loops/drivetrain/distance_spline_test.cc
+++ b/frc971/control_loops/drivetrain/distance_spline_test.cc
@@ -16,17 +16,16 @@
namespace testing {
// Test fixture with a spline from 0, 0 to 1, 1
-class DistanceSplineTest : public ::testing::Test {
+class ParameterizedDistanceSplineTest
+ : public ::testing::TestWithParam<::std::vector<Spline>> {
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())) {}
+ ParameterizedDistanceSplineTest()
+ : distance_spline_(::std::vector<Spline>(GetParam())) {}
DistanceSpline distance_spline_;
};
// Tests that the derivitives of xy integrate back up to the position.
-TEST_F(DistanceSplineTest, XYIntegral) {
+TEST_P(ParameterizedDistanceSplineTest, XYIntegral) {
::std::vector<double> distances_plot;
::std::vector<double> x_plot;
::std::vector<double> y_plot;
@@ -98,7 +97,7 @@
}
// Tests that the derivitives of xy integrate back up to the position.
-TEST_F(DistanceSplineTest, ThetaIntegral) {
+TEST_P(ParameterizedDistanceSplineTest, ThetaIntegral) {
::std::vector<double> distances_plot;
::std::vector<double> theta_plot;
::std::vector<double> itheta_plot;
@@ -149,6 +148,21 @@
#endif
}
+INSTANTIATE_TEST_CASE_P(
+ DistanceSplineTest, ParameterizedDistanceSplineTest,
+ ::testing::Values(
+ ::std::vector<Spline>(
+ {Spline(Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 0.5,
+ 0.5, 1.0, 0.0, 0.0, 1.0, 1.0)
+ .finished()))}),
+ ::std::vector<Spline>(
+ {Spline(Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 0.5,
+ 0.5, 1.0, 0.0, 0.0, 1.0, 1.0)
+ .finished())),
+ Spline(Spline4To6((::Eigen::Matrix<double, 2, 4>() << 1.0, 1.5,
+ 1.5, 2.0, 1.0, 1.0, 0.0, 0.0)
+ .finished()))})));
+
} // namespace testing
} // namespace drivetrain
} // namespace control_loops
diff --git a/frc971/control_loops/drivetrain/spline.h b/frc971/control_loops/drivetrain/spline.h
index c5b94cc..f60eea5 100644
--- a/frc971/control_loops/drivetrain/spline.h
+++ b/frc971/control_loops/drivetrain/spline.h
@@ -165,12 +165,23 @@
const ::Eigen::Matrix<double, 2, N - 3> dddspline_polynomial_;
};
-typedef NSpline<4> Spline;
+typedef NSpline<6> Spline;
// Converts a 4 control point spline into
::Eigen::Matrix<double, 2, 6> Spline4To6(
const ::Eigen::Matrix<double, 2, 4> &control_points);
+template <int N>
+::Eigen::Matrix<double, 2, N> TranslateSpline(
+ const ::Eigen::Matrix<double, 2, N> &control_points,
+ const ::Eigen::Matrix<double, 2, 1> translation) {
+ ::Eigen::Matrix<double, 2, N> ans = control_points;
+ for (size_t i = 0; i < N; ++i) {
+ ans.template block<2, 1>(0, i) += translation;
+ }
+ return ans;
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 4e082ee..fec90f1 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -49,10 +49,10 @@
namespace drivetrain {
void Main() {
- DistanceSpline distance_spline(
- Spline((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2 * FLAGS_forward,
- -0.2 * FLAGS_forward, FLAGS_forward, 0.0, 0.0, 1.0, 1.0)
- .finished()));
+ DistanceSpline distance_spline(Spline(
+ Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2 * FLAGS_forward,
+ -0.2 * FLAGS_forward, FLAGS_forward, 0.0, 0.0, 1.0, 1.0)
+ .finished())));
Trajectory trajectory(
&distance_spline,
::y2016::control_loops::drivetrain::GetDrivetrainConfig());
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index 89b5ffb..65848f0 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -101,7 +101,7 @@
void SetUp() {
distance_spline_ = ::std::unique_ptr<DistanceSpline>(
- new DistanceSpline(Spline(GetParam().control_points)));
+ new DistanceSpline(Spline(Spline4To6(GetParam().control_points))));
trajectory_ = ::std::unique_ptr<Trajectory>(
new Trajectory(distance_spline_.get(), GetDrivetrainConfig(),
GetParam().velocity_limit));