Create an iterative XVA plan for trajectory.
Change-Id: I10d274049cd1a96add42f05cb6644521e2162927
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 64b4612..c9582e6 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -362,31 +362,32 @@
return result;
}
-::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
- ::std::chrono::nanoseconds dt) {
+::Eigen::Matrix<double, 3, 1> Trajectory::GetNextXVA(
+ ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state) {
double dt_float =
::std::chrono::duration_cast<::std::chrono::duration<double>>(dt).count();
- double t = 0.0;
- ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
+ // TODO(austin): This feels like something that should be pulled out into
+ // a library for re-use.
+ *state = RungeKutta([this](const ::Eigen::Matrix<double, 2, 1> x) {
+ ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
+ return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
+ }, *state, dt_float);
+
+ ::Eigen::Matrix<double, 3, 1> result = FFAcceleration((*state)(0));
+ (*state)(1) = result(1);
+ return result;
+}
+
+::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
+ ::std::chrono::nanoseconds dt) {
+ ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
::std::vector<::Eigen::Matrix<double, 3, 1>> result;
result.emplace_back(FFAcceleration(0));
result.back()(1) = 0.0;
- while (state(0) < length() - 1e-4) {
- t += dt_float;
-
- // TODO(austin): This feels like something that should be pulled out into
- // a library for re-use.
- state = RungeKutta(
- [this](const ::Eigen::Matrix<double, 2, 1> x) {
- ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
- return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
- },
- state, dt_float);
-
- result.emplace_back(FFAcceleration(state(0)));
- state(1) = result.back()(1);
+ while (!is_at_end(state)) {
+ result.emplace_back(GetNextXVA(dt, &state));
}
return result;
}
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index d3468b3..66dcba1 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -98,6 +98,11 @@
// Returns the feed forwards voltage for an explicit distance.
::Eigen::Matrix<double, 2, 1> FFVoltage(double distance);
+ // Returns whether a state represents a state at the end of the spline.
+ bool is_at_end(::Eigen::Matrix<double, 2, 1> state) const {
+ return state(0) > length() - 1e-4;
+ }
+
// Returns the length of the path in meters.
double length() const { return spline_->length(); }
@@ -151,6 +156,10 @@
const ::Eigen::DiagonalMatrix<double, 5> &Q,
const ::Eigen::DiagonalMatrix<double, 2> &R) const;
+ // Return the next position, velocity, acceleration based on the current
+ // state. Updates the passed in state for the next iteration.
+ ::Eigen::Matrix<double, 3, 1> GetNextXVA(
+ ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state);
::std::vector<::Eigen::Matrix<double, 3, 1>> PlanXVA(
::std::chrono::nanoseconds dt);
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index eb1664a..f65adf1 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -265,6 +265,15 @@
1.2e-2);
}
+// Tests that Iteratively computing the XVA plan is the same as precomputing it.
+TEST_P(ParameterizedSplineTest, IterativeXVA) {
+ ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
+ for (size_t i = 1; i < length_plan_xva_.size(); ++i) {
+ ::Eigen::Matrix<double, 3, 1> xva = trajectory_->GetNextXVA(kDt, &state);
+ EXPECT_LT((length_plan_xva_[i] - xva).norm(), 1e-2);
+ }
+}
+
SplineTestParams MakeSplineTestParams(struct SplineTestParams params) {
return params;
}