Implement the C++ drivetrain trajectory optimizer
This implements the curvature, forwards, and backwards passes, and adds
a test which makes sure the feed forwards gets us close enough to the
end. Also adds a plotting tool (trajectory_plot) to simulate everything
and tune.
Change-Id: I9f8d6088893cc0b7263b3ff0d79667c027604700
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 7f87cca..432dd0e 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -303,3 +303,53 @@
"@com_github_gflags_gflags//:gflags",
],
)
+
+cc_library(
+ name = "trajectory",
+ srcs = ["trajectory.cc"],
+ hdrs = ["trajectory.h"],
+ deps = [
+ ":distance_spline",
+ ":drivetrain_config",
+ "//aos/logging:matrix_logging",
+ "//frc971/control_loops:c2d",
+ "//frc971/control_loops:dlqr",
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:runge_kutta",
+ "//frc971/control_loops:state_feedback_loop",
+ "//third_party/eigen",
+ ],
+)
+
+cc_binary(
+ name = "trajectory_plot",
+ srcs = [
+ "trajectory_plot.cc",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":distance_spline",
+ ":trajectory",
+ "//aos/logging:implementations",
+ "//aos/logging:matrix_logging",
+ "//aos/network:team_number",
+ "//third_party/eigen",
+ "//third_party/matplotlib-cpp",
+ "//y2016/control_loops/drivetrain:drivetrain_base",
+ "@com_github_gflags_gflags//:gflags",
+ ],
+)
+
+cc_test(
+ name = "trajectory_test",
+ srcs = [
+ "trajectory_test.cc",
+ ],
+ deps = [
+ ":trajectory",
+ "//aos/testing:googletest",
+ "//aos/testing:test_shm",
+ "//y2016:constants",
+ "//y2016/control_loops/drivetrain:polydrivetrain_plants",
+ ],
+)
diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index 3c1824c..7dea1dc 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -43,6 +43,10 @@
return spline_.DTheta(alpha) / spline_.DPoint(alpha).norm();
}
+ double DThetaDt(double distance, double velocity) const {
+ return DTheta(distance) * velocity;
+ }
+
// Returns the angular acceleration as a function of distance.
double DDTheta(double distance) const;
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
new file mode 100644
index 0000000..2c07992
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -0,0 +1,378 @@
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+#include <chrono>
+
+#include "Eigen/Dense"
+#include "aos/logging/matrix_logging.h"
+#include "frc971/control_loops/dlqr.h"
+#include "frc971/control_loops/c2d.h"
+#include "frc971/control_loops/drivetrain/distance_spline.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/hybrid_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+Trajectory::Trajectory(const DistanceSpline *spline,
+ const DrivetrainConfig<double> &config, double vmax,
+ int num_distance)
+ : spline_(spline),
+ velocity_drivetrain_(
+ ::std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
+ StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>(
+ new StateFeedbackLoop<2, 2, 2, double,
+ StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>(
+ config.make_hybrid_drivetrain_velocity_loop()))),
+ robot_radius_l_(config.robot_radius),
+ robot_radius_r_(config.robot_radius),
+ longitudal_acceleration_(3.0),
+ lateral_acceleration_(2.0),
+ Tlr_to_la_((::Eigen::Matrix<double, 2, 2>() << 0.5, 0.5,
+ -1.0 / (robot_radius_l_ + robot_radius_r_),
+ 1.0 / (robot_radius_l_ + robot_radius_r_))
+ .finished()),
+ Tla_to_lr_(Tlr_to_la_.inverse()),
+ plan_(num_distance, vmax) {}
+
+void Trajectory::LateralAccelPass() {
+ for (size_t i = 0; i < plan_.size(); ++i) {
+ const double distance = Distance(i);
+ plan_[i] = ::std::min(plan_[i], LateralVelocityCurvature(distance));
+ }
+}
+
+// TODO(austin): Deduplicate this potentially with the backward accel function.
+// Need to sort out how the max velocity limit is going to work since the
+// velocity and acceleration need to match at all points.
+// TODO(austin): Accel check the wheels instead of the center of mass.
+double Trajectory::ForwardAcceleration(const double x, const double v) {
+ ::Eigen::Matrix<double, 2, 1> K3;
+ ::Eigen::Matrix<double, 2, 1> K4;
+ ::Eigen::Matrix<double, 2, 1> K5;
+ K345(x, &K3, &K4, &K5);
+
+ const ::Eigen::Matrix<double, 2, 1> C = K3 * v * v + K4 * v;
+ // Now, solve for all a's and find the best one which meets our criteria.
+ double maxa = -::std::numeric_limits<double>::infinity();
+ for (const double a : {(voltage_limit_ - C(0, 0)) / K5(0, 0),
+ (voltage_limit_ - C(1, 0)) / K5(1, 0),
+ (-voltage_limit_ - C(0, 0)) / K5(0, 0),
+ (-voltage_limit_ - C(1, 0)) / K5(1, 0)}) {
+ const ::Eigen::Matrix<double, 2, 1> U = K5 * a + K3 * v * v + K4 * v;
+ if ((U.array().abs() < voltage_limit_ + 1e-6).all()) {
+ maxa = ::std::max(maxa, a);
+ }
+ }
+
+ // Then, assume an acceleration oval and stay inside it.
+ const double lateral_acceleration = v * v * spline_->DDXY(x).norm();
+ const double squared =
+ 1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_, 2.0);
+ // If we would end up with an imaginary number, cap us at 0 acceleration.
+ // TODO(austin): Investigate when this happens, why, and fix it.
+ if (squared < 0.0) {
+ LOG(ERROR, "Imaginary %f, d %f\n", squared, x);
+ return 0.0;
+ }
+ const double longitudal_acceleration =
+ ::std::sqrt(squared) * longitudal_acceleration_;
+ return ::std::min(longitudal_acceleration, maxa);
+}
+
+void Trajectory::ForwardPass() {
+ plan_[0] = 0.0;
+ const double delta_distance = Distance(1) - Distance(0);
+ for (size_t i = 0; i < plan_.size() - 1; ++i) {
+ const double distance = Distance(i);
+
+ // Integrate our acceleration forward one step.
+ plan_[i + 1] = ::std::min(
+ plan_[i + 1],
+ IntegrateAccelForDistance(
+ [this](double x, double v) { return ForwardAcceleration(x, v); },
+ plan_[i], distance, delta_distance));
+ }
+}
+
+double Trajectory::BackwardAcceleration(double x, double v) {
+ ::Eigen::Matrix<double, 2, 1> K3;
+ ::Eigen::Matrix<double, 2, 1> K4;
+ ::Eigen::Matrix<double, 2, 1> K5;
+ K345(x, &K3, &K4, &K5);
+
+ // Now, solve for all a's and find the best one which meets our criteria.
+ const ::Eigen::Matrix<double, 2, 1> C = K3 * v * v + K4 * v;
+ double mina = ::std::numeric_limits<double>::infinity();
+ for (const double a : {(voltage_limit_ - C(0, 0)) / K5(0, 0),
+ (voltage_limit_ - C(1, 0)) / K5(1, 0),
+ (-voltage_limit_ - C(0, 0)) / K5(0, 0),
+ (-voltage_limit_ - C(1, 0)) / K5(1, 0)}) {
+ const ::Eigen::Matrix<double, 2, 1> U = K5 * a + K3 * v * v + K4 * v;
+ if ((U.array().abs() < voltage_limit_ + 1e-6).all()) {
+ mina = ::std::min(mina, a);
+ }
+ }
+
+ // Then, assume an acceleration oval and stay inside it.
+ const double lateral_acceleration = v * v * spline_->DDXY(x).norm();
+ const double squared =
+ 1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_, 2.0);
+ // If we would end up with an imaginary number, cap us at 0 acceleration.
+ // TODO(austin): Investigate when this happens, why, and fix it.
+ if (squared < 0.0) {
+ LOG(ERROR, "Imaginary %f, d %f\n", squared, x);
+ return 0.0;
+ }
+ const double longitudal_acceleration =
+ -::std::sqrt(squared) * longitudal_acceleration_;
+ return ::std::max(longitudal_acceleration, mina);
+}
+
+void Trajectory::BackwardPass() {
+ const double delta_distance = Distance(0) - Distance(1);
+ plan_.back() = 0.0;
+ for (size_t i = plan_.size() - 1; i > 0; --i) {
+ const double distance = Distance(i);
+
+ // Integrate our deceleration back one step.
+ plan_[i - 1] = ::std::min(
+ plan_[i - 1],
+ IntegrateAccelForDistance(
+ [this](double x, double v) { return BackwardAcceleration(x, v); },
+ plan_[i], distance, delta_distance));
+ }
+}
+
+::Eigen::Matrix<double, 3, 1> Trajectory::FFAcceleration(double distance) {
+ size_t before_index;
+ size_t after_index;
+ if (distance < Distance(1)) {
+ // Within the first step.
+ after_index = 1;
+ // Make sure we don't end up off the beginning of the curve.
+ if (distance < 0.0) {
+ distance = 0.0;
+ }
+ } else if (distance > Distance(plan_.size() - 2)) {
+ // Within the last step.
+ after_index = plan_.size() - 1;
+ // Make sure we don't end up off the end of the curve.
+ if (distance > length()) {
+ distance = length();
+ }
+ } else {
+ // Otherwise do the calculation normally.
+ after_index = static_cast<size_t>(
+ ::std::ceil(distance / length() * (plan_.size() - 1)));
+ }
+ before_index = after_index - 1;
+ const double before_distance = Distance(before_index);
+ const double after_distance = Distance(after_index);
+
+ // Now, compute the velocity that we could have if we accelerated from the
+ // previous step and decelerated from the next step. The min will tell us
+ // which is in effect.
+ const double velocity_forwards = IntegrateAccelForDistance(
+ [this](double x, double v) { return ForwardAcceleration(x, v); },
+ plan_[before_index], before_distance, distance - before_distance);
+ const double velocity_backward = IntegrateAccelForDistance(
+ [this](double x, double v) { return BackwardAcceleration(x, v); },
+ plan_[after_index], after_distance, distance - after_distance);
+
+ // And then also make sure we aren't curvature limited.
+ const double vcurvature = LateralVelocityCurvature(distance);
+
+ double acceleration;
+ double velocity;
+ if (vcurvature < velocity_forwards && vcurvature < velocity_backward) {
+ // If we are curvature limited, we can't accelerate.
+ velocity = vcurvature;
+ acceleration = 0.0;
+ } else if (velocity_forwards < velocity_backward) {
+ // Otherwise, pick the acceleration and velocity from the forward pass if it
+ // was the predominate factor in this step.
+ velocity = velocity_forwards;
+ acceleration = ForwardAcceleration(distance, velocity);
+ } else {
+ // Otherwise, pick the acceleration and velocity from the backward pass if
+ // it was the predominate factor in this step.
+ velocity = velocity_backward;
+ acceleration = BackwardAcceleration(distance, velocity);
+ }
+ return (::Eigen::Matrix<double, 3, 1>() << distance, velocity, acceleration)
+ .finished();
+}
+
+::Eigen::Matrix<double, 2, 1> Trajectory::FFVoltage(double distance) {
+ const Eigen::Matrix<double, 3, 1> xva = FFAcceleration(distance);
+ const double velocity = xva(1);
+ const double acceleration = xva(2);
+ const double current_ddtheta = spline_->DDTheta(distance);
+ const double current_dtheta = spline_->DTheta(distance);
+ // We've now got the equation:
+ // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
+ const ::Eigen::Matrix<double, 2, 1> my_K2 = K2(current_dtheta);
+
+ const ::Eigen::Matrix<double, 2, 2> B_inverse =
+ velocity_drivetrain_->plant().coefficients().B_continuous.inverse();
+
+ // Now, rephrase it as K5 a + K3 v^2 + K4 v = U
+ const ::Eigen::Matrix<double, 2, 1> K5 = B_inverse * my_K2;
+ const ::Eigen::Matrix<double, 2, 1> K3 = B_inverse * K1(current_ddtheta);
+ const ::Eigen::Matrix<double, 2, 1> K4 =
+ -B_inverse * velocity_drivetrain_->plant().coefficients().A_continuous *
+ my_K2;
+
+ return K5 * acceleration + K3 * velocity * velocity + K4 * velocity;
+}
+
+const ::std::vector<double> Trajectory::Distances() const {
+ ::std::vector<double> d;
+ d.reserve(plan_.size());
+ for (size_t i = 0; i < plan_.size(); ++i) {
+ d.push_back(Distance(i));
+ }
+ return d;
+}
+
+::Eigen::Matrix<double, 5, 5> Trajectory::ALinearizedContinuous(
+ const ::Eigen::Matrix<double, 5, 1> &state) const {
+
+ const double sintheta = ::std::sin(state(2));
+ const double costheta = ::std::cos(state(2));
+ const ::Eigen::Matrix<double, 2, 1> linear_angular =
+ Tlr_to_la_ * state.block<2, 1>(3, 0);
+
+ // When stopped, just roll with a min velocity.
+ double linear_velocity = 0.0;
+ constexpr double kMinVelocity = 0.1;
+ if (::std::abs(linear_angular(0)) < kMinVelocity / 100.0) {
+ linear_velocity = 0.1;
+ } else if (::std::abs(linear_angular(0)) > kMinVelocity) {
+ linear_velocity = linear_angular(0);
+ } else if (linear_angular(0) > 0) {
+ linear_velocity = kMinVelocity;
+ } else if (linear_angular(0) < 0) {
+ linear_velocity = -kMinVelocity;
+ }
+
+ ::Eigen::Matrix<double, 5, 5> result = ::Eigen::Matrix<double, 5, 5>::Zero();
+ result(0, 2) = -sintheta * linear_velocity;
+ result(0, 3) = 0.5 * costheta;
+ result(0, 4) = 0.5 * costheta;
+
+ result(1, 2) = costheta * linear_velocity;
+ result(1, 3) = 0.5 * sintheta;
+ result(1, 4) = 0.5 * sintheta;
+
+ result(2, 3) = Tlr_to_la_(1, 0);
+ result(2, 4) = Tlr_to_la_(1, 1);
+
+ result.block<2, 2>(3, 3) =
+ velocity_drivetrain_->plant().coefficients().A_continuous;
+ return result;
+}
+
+::Eigen::Matrix<double, 5, 2> Trajectory::BLinearizedContinuous() const {
+ ::Eigen::Matrix<double, 5, 2> result = ::Eigen::Matrix<double, 5, 2>::Zero();
+ result.block<2, 2>(3, 0) =
+ velocity_drivetrain_->plant().coefficients().B_continuous;
+ return result;
+}
+
+void Trajectory::AB(const ::Eigen::Matrix<double, 5, 1> &state,
+ ::std::chrono::nanoseconds dt,
+ ::Eigen::Matrix<double, 5, 5> *A,
+ ::Eigen::Matrix<double, 5, 2> *B) const {
+ ::Eigen::Matrix<double, 5, 5> A_linearized_continuous =
+ ALinearizedContinuous(state);
+ ::Eigen::Matrix<double, 5, 2> B_linearized_continuous =
+ BLinearizedContinuous();
+
+ // Now, convert it to discrete.
+ controls::C2D(A_linearized_continuous, B_linearized_continuous,
+ dt, A, B);
+}
+
+::Eigen::Matrix<double, 2, 5> Trajectory::KForState(
+ const ::Eigen::Matrix<double, 5, 1> &state, ::std::chrono::nanoseconds dt,
+ const ::Eigen::DiagonalMatrix<double, 5> &Q,
+ const ::Eigen::DiagonalMatrix<double, 2> &R) const {
+ ::Eigen::Matrix<double, 5, 5> A;
+ ::Eigen::Matrix<double, 5, 2> B;
+ AB(state, dt, &A, &B);
+
+ ::Eigen::Matrix<double, 5, 5> S = ::Eigen::Matrix<double, 5, 5>::Zero();
+ ::Eigen::Matrix<double, 2, 5> K = ::Eigen::Matrix<double, 2, 5>::Zero();
+
+ int info = ::frc971::controls::dlqr<5, 2>(A, B, Q, R, &K, &S);
+ if (info == 0) {
+ LOG_MATRIX(INFO, "K", K);
+ } else {
+ LOG(ERROR, "Failed to solve %d, controllability: %d\n", info,
+ controls::Controllability(A, B));
+ // TODO(austin): Can we be more clever here? Use the last one? We should
+ // collect more info about when this breaks down from logs.
+ K = ::Eigen::Matrix<double, 2, 5>::Zero();
+ }
+ ::Eigen::EigenSolver<::Eigen::Matrix<double, 5, 5>> eigensolver(A - B * K);
+ const auto eigenvalues = eigensolver.eigenvalues();
+ LOG(DEBUG,
+ "Eigenvalues: (%f + %fj), (%f + %fj), (%f + %fj), (%f + %fj), (%f + "
+ "%fj)\n",
+ eigenvalues(0).real(), eigenvalues(0).imag(), eigenvalues(1).real(),
+ eigenvalues(1).imag(), eigenvalues(2).real(), eigenvalues(2).imag(),
+ eigenvalues(3).real(), eigenvalues(3).imag(), eigenvalues(4).real(),
+ eigenvalues(4).imag());
+ return K;
+}
+
+const ::Eigen::Matrix<double, 5, 1> Trajectory::GoalState(double distance,
+ double velocity) {
+ ::Eigen::Matrix<double, 5, 1> result;
+ result.block<2, 1>(0, 0) = spline_->XY(distance);
+ result(2, 0) = spline_->Theta(distance);
+
+ result.block<2, 1>(3, 0) = Tla_to_lr_ *
+ (::Eigen::Matrix<double, 2, 1>() << velocity,
+ spline_->DThetaDt(distance, velocity))
+ .finished();
+ return result;
+}
+
+::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
+ ::std::chrono::nanoseconds dt) {
+ 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();
+
+ ::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);
+ }
+ return result;
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
new file mode 100644
index 0000000..a42476a
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -0,0 +1,239 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_H_
+
+#include <chrono>
+
+#include "Eigen/Dense"
+#include "frc971/control_loops/drivetrain/distance_spline.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/hybrid_state_feedback_loop.h"
+#include "frc971/control_loops/runge_kutta.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+template <typename F>
+double IntegrateAccelForDistance(const F &fn, double v, double x, double dx) {
+ // Use a trick from
+ // https://www.johndcook.com/blog/2012/02/21/care-and-treatment-of-singularities/
+ const double a0 = fn(x, v);
+
+ return (RungeKutta(
+ [&fn, &a0](double t, double y) {
+ // Since we know that a0 == a(0) and that they are asymtotically
+ // the same at 0, we know that the limit is 0 at 0. This is
+ // true because when starting from a stop, under sane
+ // accelerations, we can assume that we will start with a
+ // constant acceleration. So, hard-code it.
+ if (::std::abs(y) < 1e-6) {
+ return 0.0;
+ }
+ return (fn(t, y) - a0) / y;
+ },
+ v, x, dx) -
+ v) +
+ ::std::sqrt(2.0 * a0 * dx + v * v);
+}
+
+// Class to plan and hold the velocity plan for a spline.
+class Trajectory {
+ public:
+ Trajectory(const DistanceSpline *spline,
+ const DrivetrainConfig<double> &config,
+ double vmax = 10.0, int num_distance = 500);
+ // Sets the plan longitudal acceleration limit
+ void set_longitudal_acceleration(double longitudal_acceleration) {
+ longitudal_acceleration_ = longitudal_acceleration;
+ }
+ // Sets the plan lateral acceleration limit
+ void set_lateral_acceleration(double lateral_acceleration) {
+ lateral_acceleration_ = lateral_acceleration;
+ }
+ // Sets the voltage limit
+ void set_voltage_limit(double voltage_limit) {
+ voltage_limit_ = voltage_limit;
+ }
+
+ // Returns the velocity limit for a defined distance.
+ double LateralVelocityCurvature(double distance) const {
+ return ::std::sqrt(lateral_acceleration_ / spline_->DDXY(distance).norm());
+ }
+
+ // Runs the lateral acceleration (curvature) pass on the plan.
+ void LateralAccelPass();
+
+ // Returns the forward acceleration for a distance along the spline taking
+ // into account the lateral acceleration, longitudinal acceleration, and
+ // voltage limits.
+ double ForwardAcceleration(const double x, const double v);
+
+ // Runs the forwards pass, setting the starting velocity to 0 m/s
+ void ForwardPass();
+
+ // Returns the backwards acceleration for a distance along the spline taking
+ // into account the lateral acceleration, longitudinal acceleration, and
+ // voltage limits.
+ double BackwardAcceleration(double x, double v);
+
+ // Runs the forwards pass, setting the ending velocity to 0 m/s
+ void BackwardPass();
+
+ // Runs all the planning passes.
+ void Plan() {
+ LateralAccelPass();
+ ForwardPass();
+ BackwardPass();
+ }
+
+ // Returns the feed forwards position, velocity, acceleration for an explicit
+ // distance.
+ ::Eigen::Matrix<double, 3, 1> FFAcceleration(double distance);
+
+ // Returns the feed forwards voltage for an explicit distance.
+ ::Eigen::Matrix<double, 2, 1> FFVoltage(double distance);
+
+ // Returns the length of the path in meters.
+ double length() const { return spline_->length(); }
+
+ // Returns a list of the distances. Mostly useful for plotting.
+ const ::std::vector<double> Distances() const;
+ // Returns the distance for an index in the plan.
+ double Distance(int index) const {
+ return static_cast<double>(index) * length() /
+ static_cast<double>(plan_.size() - 1);
+ }
+
+ // Returns the plan. This is the pathwise velocity as a function of distance.
+ // To get the distance for an index, use the Distance(index) function provided
+ // with the index.
+ const ::std::vector<double> plan() const { return plan_; }
+
+ // Returns the left, right to linear, angular transformation matrix.
+ const ::Eigen::Matrix<double, 2, 2> &Tlr_to_la() const { return Tlr_to_la_; }
+ // Returns the linear, angular to left, right transformation matrix.
+ const ::Eigen::Matrix<double, 2, 2> &Tla_to_lr() const { return Tla_to_lr_; }
+
+ // Returns the goal state as a function of path distance, velocity.
+ const ::Eigen::Matrix<double, 5, 1> GoalState(double distance,
+ double velocity);
+
+ // Returns the velocity drivetrain in use.
+ const StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>
+ &velocity_drivetrain() const {
+ return *velocity_drivetrain_;
+ }
+
+ // Returns the continuous statespace A and B matricies for [x, y, theta, vl,
+ // vr] for the linearized system (around the provided state).
+ ::Eigen::Matrix<double, 5, 5> ALinearizedContinuous(
+ const ::Eigen::Matrix<double, 5, 1> &state) const;
+ ::Eigen::Matrix<double, 5, 2> BLinearizedContinuous() const;
+
+ // Returns the discrete time A and B matricies for the provided state,
+ // assuming the provided timestep.
+ void AB(const ::Eigen::Matrix<double, 5, 1> &state,
+ ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 5, 5> *A,
+ ::Eigen::Matrix<double, 5, 2> *B) const;
+
+ // Returns the lqr controller for the current state, timestep, and Q and R
+ // gains.
+ // TODO(austin): This feels like it should live somewhere else, but I'm not
+ // sure where. So, throw it here...
+ ::Eigen::Matrix<double, 2, 5> KForState(
+ const ::Eigen::Matrix<double, 5, 1> &state, ::std::chrono::nanoseconds dt,
+ const ::Eigen::DiagonalMatrix<double, 5> &Q,
+ const ::Eigen::DiagonalMatrix<double, 2> &R) const;
+
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> PlanXVA(
+ ::std::chrono::nanoseconds dt);
+
+ private:
+ // Computes alpha for a distance.
+ double DistanceToAlpha(double distance) const;
+
+ // Returns K1 and K2.
+ // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
+ const ::Eigen::Matrix<double, 2, 1> K1(double current_ddtheta) const {
+ return (::Eigen::Matrix<double, 2, 1>()
+ << -robot_radius_l_ * current_ddtheta,
+ robot_radius_r_ * current_ddtheta)
+ .finished();
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> K2(double current_dtheta) const {
+ return (::Eigen::Matrix<double, 2, 1>()
+ << 1.0 - robot_radius_l_ * current_dtheta,
+ 1.0 + robot_radius_r_ * current_dtheta)
+ .finished();
+ }
+
+ // Computes K3, K4, and K5 for the provided distance.
+ // K5 a + K3 v^2 + K4 v = U
+ void K345(const double x, ::Eigen::Matrix<double, 2, 1> *K3,
+ ::Eigen::Matrix<double, 2, 1> *K4,
+ ::Eigen::Matrix<double, 2, 1> *K5) {
+ const double current_ddtheta = spline_->DDTheta(x);
+ const double current_dtheta = spline_->DTheta(x);
+ // We've now got the equation:
+ // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
+ const ::Eigen::Matrix<double, 2, 1> my_K2 = K2(current_dtheta);
+
+ const ::Eigen::Matrix<double, 2, 2> B_inverse =
+ velocity_drivetrain_->plant().coefficients().B_continuous.inverse();
+
+ // Now, rephrase it as K5 a + K3 v^2 + K4 v = U
+ *K3 = B_inverse * K1(current_ddtheta);
+ *K4 = -B_inverse *
+ velocity_drivetrain_->plant().coefficients().A_continuous * my_K2;
+ *K5 = B_inverse * my_K2;
+ }
+
+ // The spline we are planning for.
+ const DistanceSpline *spline_;
+ // The drivetrain we are controlling.
+ ::std::unique_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>
+ velocity_drivetrain_;
+
+ // Left and right robot radiuses.
+ const double robot_radius_l_;
+ const double robot_radius_r_;
+ // Acceleration limits.
+ double longitudal_acceleration_;
+ double lateral_acceleration_;
+ // Transformation matrix from left, right to linear, angular
+ const ::Eigen::Matrix<double, 2, 2> Tlr_to_la_;
+ // Transformation matrix from linear, angular to left, right
+ const ::Eigen::Matrix<double, 2, 2> Tla_to_lr_;
+ // Velocities in the plan (distance for each index is defined by distance())
+ ::std::vector<double> plan_;
+ // Plan voltage limit.
+ double voltage_limit_ = 12.0;
+};
+
+// Returns the continuous time dynamics given the [x, y, theta, vl, vr] state
+// and the [vl, vr] input voltage.
+inline ::Eigen::Matrix<double, 5, 1> ContinuousDynamics(
+ const StateFeedbackHybridPlant<2, 2, 2> &velocity_drivetrain,
+ const ::Eigen::Matrix<double, 2, 2> &Tlr_to_la,
+ const ::Eigen::Matrix<double, 5, 1> X,
+ const ::Eigen::Matrix<double, 2, 1> U) {
+ const auto &velocity = X.block<2, 1>(3, 0);
+ const double theta = X(2);
+ ::Eigen::Matrix<double, 2, 1> la = Tlr_to_la * velocity;
+ return (::Eigen::Matrix<double, 5, 1>() << ::std::cos(theta) * la(0),
+ ::std::sin(theta) * la(0), la(1),
+ (velocity_drivetrain.coefficients().A_continuous * velocity +
+ velocity_drivetrain.coefficients().B_continuous * U))
+ .finished();
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_H_
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
new file mode 100644
index 0000000..d9c12f4
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -0,0 +1,241 @@
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+#include <chrono>
+
+#include "aos/logging/implementations.h"
+#include "aos/logging/matrix_logging.h"
+#include "aos/network/team_number.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/dlqr.h"
+#include "gflags/gflags.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#include "y2016/control_loops/drivetrain/drivetrain_base.h"
+
+// Notes:
+// Basic ideas from spline following are from Jared Russell and
+// http://msc.fe.uni-lj.si/Papers/Chapter10_MobileRobotsNewResearch_Lepetic2005.pdf
+//
+// For the future, I'd like to use the following to measure distance to the path.
+// http://home.eps.hw.ac.uk/~ab226/papers/dist.pdf
+//
+// LQR controller was inspired by
+// https://calhoun.nps.edu/bitstream/handle/10945/40159/kanayama_a_stable.pdf
+//
+// I ended up just taking the jacobian of the dynamics. That gives me a tangent
+// plane to design a LQR controller around. That works because we have a good
+// feed forwards and a good idea where the robot will be next time so we only
+// need to handle disturbances.
+//
+// https://photos.google.com/share/AF1QipPl34MOTPem2QmmTC3B21dL7GV2_HjxnseRrqxgR60TUasyIPliIuWmnH3yxuSNZw?key=cVhZLUYycXBIZlNTRy10cjZlWm0tcmlqQl9MTE13
+
+DEFINE_bool(plot, true, "If true, plot");
+DEFINE_double(qx, 0.05, "Q_xpos");
+DEFINE_double(qy, 0.05, "Q_ypos");
+DEFINE_double(qt, 0.2, "Q_thetapos");
+DEFINE_double(qv, 0.5, "Q_vel");
+
+DEFINE_double(dx, 0.0, "Amount to disturb x at the start");
+DEFINE_double(dy, 0.0, "Amount to disturb y at the start");
+DEFINE_double(dt, 0.0, "Amount to disturb theta at the start");
+DEFINE_double(dvl, 0.0, "Amount to disturb vl at the start");
+DEFINE_double(dvr, 0.0, "Amount to disturb vr at the start");
+
+DEFINE_double(forward, 1.0, "Amount to drive forwards");
+
+namespace chrono = ::std::chrono;
+
+namespace frc971 {
+namespace control_loops {
+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()));
+ Trajectory trajectory(
+ &distance_spline,
+ ::y2016::control_loops::drivetrain::GetDrivetrainConfig());
+ trajectory.set_lateral_acceleration(2.0);
+ trajectory.set_longitudal_acceleration(1.0);
+
+ // Grab the spline.
+ ::std::vector<double> distances = trajectory.Distances();
+ ::std::vector<double> spline_x;
+ ::std::vector<double> spline_y;
+ ::std::vector<double> spline_theta;
+
+ for (const double distance : distances) {
+ const ::Eigen::Matrix<double, 2, 1> point = distance_spline.XY(distance);
+ const double theta = distance_spline.Theta(distance);
+ spline_x.push_back(point(0));
+ spline_y.push_back(point(1));
+ spline_theta.push_back(theta);
+ }
+
+ // Compute the velocity plan.
+ ::aos::monotonic_clock::time_point start_time = ::aos::monotonic_clock::now();
+ ::std::vector<double> initial_plan = trajectory.plan();
+ trajectory.LateralAccelPass();
+ ::std::vector<double> curvature_plan = trajectory.plan();
+ trajectory.ForwardPass();
+ ::std::vector<double> forward_plan = trajectory.plan();
+ trajectory.BackwardPass();
+
+ ::aos::monotonic_clock::time_point end_time = ::aos::monotonic_clock::now();
+ ::std::vector<double> backward_plan = trajectory.plan();
+
+ LOG(INFO, "Took %fms to plan\n",
+ chrono::duration_cast<chrono::duration<double, ::std::milli>>(end_time -
+ start_time)
+ .count());
+
+ // Now, compute the xva as a function of time.
+ ::std::vector<double> length_plan_t;
+ ::std::vector<double> length_plan_x;
+ ::std::vector<double> length_plan_v;
+ ::std::vector<double> length_plan_a;
+ ::std::vector<double> length_plan_vl;
+ ::std::vector<double> length_plan_vr;
+ const chrono::nanoseconds kDt = chrono::microseconds(5050);
+ const double kDtDouble =
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(kDt)
+ .count();
+ {
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva =
+ trajectory.PlanXVA(kDt);
+ for (size_t i = 0; i < length_plan_xva.size(); ++i) {
+ length_plan_t.push_back(i * kDtDouble);
+ length_plan_x.push_back(length_plan_xva[i](0));
+ length_plan_v.push_back(length_plan_xva[i](1));
+ length_plan_a.push_back(length_plan_xva[i](2));
+
+ ::Eigen::Matrix<double, 2, 1> U =
+ trajectory.FFVoltage(length_plan_xva[i](0));
+ length_plan_vl.push_back(U(0));
+ length_plan_vr.push_back(U(1));
+ }
+ }
+
+ const double kXPos = FLAGS_qx;
+ const double kYPos = FLAGS_qy;
+ const double kThetaPos = FLAGS_qt;
+ const double kVel = FLAGS_qv;
+ const ::Eigen::DiagonalMatrix<double, 5> Q =
+ (::Eigen::DiagonalMatrix<double, 5>().diagonal()
+ << 1.0 / ::std::pow(kXPos, 2),
+ 1.0 / ::std::pow(kYPos, 2), 1.0 / ::std::pow(kThetaPos, 2),
+ 1.0 / ::std::pow(kVel, 2), 1.0 / ::std::pow(kVel, 2))
+ .finished()
+ .asDiagonal();
+
+ const ::Eigen::DiagonalMatrix<double, 2> R =
+ (::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 1.0 / ::std::pow(12.0, 2),
+ 1.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal();
+
+ ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
+ state(0, 0) = FLAGS_dx;
+ state(1, 0) = FLAGS_dy;
+ state(2, 0) = FLAGS_dt;
+ state(3, 0) = FLAGS_dvl;
+ state(4, 0) = FLAGS_dvr;
+ ::std::vector<double> simulation_t = length_plan_t;
+ ::std::vector<double> simulation_x;
+ ::std::vector<double> error_x;
+ ::std::vector<double> simulation_y;
+ ::std::vector<double> error_y;
+ ::std::vector<double> simulation_theta;
+ ::std::vector<double> error_theta;
+ ::std::vector<double> simulation_velocity_l;
+ ::std::vector<double> error_velocity_l;
+ ::std::vector<double> simulation_velocity_r;
+ ::std::vector<double> error_velocity_r;
+ ::std::vector<double> simulation_ul;
+ ::std::vector<double> simulation_ur;
+ for (size_t i = 0; i < length_plan_t.size(); ++i) {
+ const double distance = length_plan_x[i];
+ const double velocity = length_plan_v[i];
+ const ::Eigen::Matrix<double, 5, 1> goal_state =
+ trajectory.GoalState(distance, velocity);
+
+ const ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+
+ simulation_x.push_back(state(0));
+ simulation_y.push_back(state(1));
+ simulation_theta.push_back(state(2));
+ simulation_velocity_l.push_back(state(3));
+ simulation_velocity_r.push_back(state(4));
+
+ error_x.push_back(state_error(0));
+ error_y.push_back(state_error(1));
+ error_theta.push_back(state_error(2));
+ error_velocity_l.push_back(state_error(3));
+ error_velocity_r.push_back(state_error(4));
+
+ ::Eigen::Matrix<double, 2, 5> K =
+ trajectory.KForState(state, chrono::microseconds(5050), Q, R);
+
+ const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory.FFVoltage(distance);
+ const ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
+ const ::Eigen::Matrix<double, 2, 1> U = U_ff + U_fb;
+ state = RungeKuttaU(
+ [&trajectory](const ::Eigen::Matrix<double, 5, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return ContinuousDynamics(trajectory.velocity_drivetrain().plant(),
+ trajectory.Tlr_to_la(), X, U);
+ },
+ state, U, kDtDouble);
+
+ simulation_ul.push_back(U(0));
+ simulation_ur.push_back(U(1));
+ }
+
+ if (FLAGS_plot) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(distances, backward_plan, {{"label", "backward"}});
+ matplotlibcpp::plot(distances, forward_plan, {{"label", "forward"}});
+ matplotlibcpp::plot(distances, curvature_plan, {{"label", "lateral"}});
+ matplotlibcpp::plot(distances, initial_plan, {{"label", "initial"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(length_plan_t, length_plan_x, {{"label", "x"}});
+ matplotlibcpp::plot(length_plan_t, length_plan_v, {{"label", "v"}});
+ matplotlibcpp::plot(length_plan_t, length_plan_a, {{"label", "a"}});
+ matplotlibcpp::plot(length_plan_t, length_plan_vl, {{"label", "Vl"}});
+ matplotlibcpp::plot(length_plan_t, length_plan_vr, {{"label", "Vr"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(length_plan_t, error_x, {{"label", "x error"}});
+ matplotlibcpp::plot(length_plan_t, error_y, {{"label", "y error"}});
+ matplotlibcpp::plot(length_plan_t, error_theta, {{"label", "theta error"}});
+ matplotlibcpp::plot(length_plan_t, error_velocity_l,
+ {{"label", "velocityl error"}});
+ matplotlibcpp::plot(length_plan_t, error_velocity_r,
+ {{"label", "velocityr error"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(spline_x, spline_y, {{"label", "spline"}});
+ matplotlibcpp::plot(simulation_x, simulation_y, {{"label", "robot"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::show();
+ }
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+int main(int argc, char **argv) {
+ ::gflags::ParseCommandLineFlags(&argc, &argv, false);
+ ::aos::logging::Init();
+ ::aos::network::OverrideTeamNumber(971);
+ ::frc971::control_loops::drivetrain::Main();
+ return 0;
+}
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
new file mode 100644
index 0000000..0d3a6db
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -0,0 +1,179 @@
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+#include <chrono>
+#include <vector>
+
+#include "aos/testing/test_shm.h"
+#include "gtest/gtest.h"
+#include "y2016/constants.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
+#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+namespace chrono = ::std::chrono;
+
+double a(double /*v*/, double /*x*/) { return 2.0; }
+
+// Tests that the derivitives of xy integrate back up to the position.
+TEST(IntegrateAccelForDistanceTest, IntegrateAccelForDistance) {
+ double v = 0.0;
+ const size_t count = 10;
+ const double dx = 4.0 / static_cast<double>(count);
+ for (size_t i = 0; i < count; ++i) {
+ v = IntegrateAccelForDistance(a, v, 0.0, dx);
+ }
+ EXPECT_NEAR(4.0, v, 1e-8);
+}
+
+const constants::ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25,
+ 0.75};
+
+// TODO(austin): factor this out of drivetrain_lib_test.cc
+const DrivetrainConfig<double> &GetDrivetrainConfig() {
+ static DrivetrainConfig<double> kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
+ IMUType::IMU_X,
+ ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeHybridVelocityDrivetrainLoop,
+
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(::y2016::control_loops::drivetrain::kDt)),
+ ::y2016::control_loops::drivetrain::kRobotRadius,
+ ::y2016::control_loops::drivetrain::kWheelRadius,
+ ::y2016::control_loops::drivetrain::kV,
+
+ ::y2016::control_loops::drivetrain::kHighGearRatio,
+ ::y2016::control_loops::drivetrain::kLowGearRatio,
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter,
+ false,
+ 0,
+
+ 0.25,
+ 1.00,
+ 1.00};
+
+ return kDrivetrainConfig;
+};
+
+class SplineTest : public ::testing::Test {
+ protected:
+ ::aos::testing::TestSharedMemory shm_;
+ static constexpr chrono::nanoseconds kDt =
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(::y2016::control_loops::drivetrain::kDt));
+
+ DistanceSpline distance_spline_;
+ Trajectory trajectory_;
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva_;
+
+ SplineTest()
+ : distance_spline_(Spline((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2,
+ -0.2, 1.0, 0.0, 0.0, 1.0, 1.0)
+ .finished())),
+ trajectory_(&distance_spline_, GetDrivetrainConfig()) {
+ trajectory_.set_lateral_acceleration(2.0);
+ trajectory_.set_longitudal_acceleration(1.0);
+ trajectory_.LateralAccelPass();
+ trajectory_.ForwardPass();
+ trajectory_.BackwardPass();
+ length_plan_xva_ = trajectory_.PlanXVA(kDt);
+ }
+
+ static constexpr double kXPos = 0.05;
+ static constexpr double kYPos = 0.05;
+ static constexpr double kThetaPos = 0.2;
+ static constexpr double kVel = 0.5;
+ const ::Eigen::DiagonalMatrix<double, 5> Q =
+ (::Eigen::DiagonalMatrix<double, 5>().diagonal()
+ << 1.0 / ::std::pow(kXPos, 2),
+ 1.0 / ::std::pow(kYPos, 2), 1.0 / ::std::pow(kThetaPos, 2),
+ 1.0 / ::std::pow(kVel, 2), 1.0 / ::std::pow(kVel, 2))
+ .finished()
+ .asDiagonal();
+
+ const ::Eigen::DiagonalMatrix<double, 2> R =
+ (::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 1.0 / ::std::pow(12.0, 2),
+ 1.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal();
+};
+
+constexpr chrono::nanoseconds SplineTest::kDt;
+
+// Tests that following a spline with feed forwards only gets pretty darn close
+// to the right point.
+TEST_F(SplineTest, FFSpline) {
+ ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
+
+ for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
+ const double distance = length_plan_xva_[i](0);
+
+ const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_.FFVoltage(distance);
+ const ::Eigen::Matrix<double, 2, 1> U = U_ff;
+ state = RungeKuttaU(
+ [this](const ::Eigen::Matrix<double, 5, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return ContinuousDynamics(trajectory_.velocity_drivetrain().plant(),
+ trajectory_.Tlr_to_la(), X, U);
+ },
+ state, U, ::y2016::control_loops::drivetrain::kDt);
+ }
+
+ EXPECT_LT((state - trajectory_.GoalState(trajectory_.length(), 0.0)).norm(),
+ 2e-2);
+}
+
+// Tests that following a spline with both feed forwards and feed back gets
+// pretty darn close to the right point.
+TEST_F(SplineTest, FBSpline) {
+ ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
+
+ for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
+ const double distance = length_plan_xva_[i](0);
+ const double velocity = length_plan_xva_[i](1);
+ const ::Eigen::Matrix<double, 5, 1> goal_state =
+ trajectory_.GoalState(distance, velocity);
+
+ const ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+
+ const ::Eigen::Matrix<double, 2, 5> K =
+ trajectory_.KForState(state, SplineTest::kDt, Q, R);
+
+ const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_.FFVoltage(distance);
+ const ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
+ const ::Eigen::Matrix<double, 2, 1> U = U_ff + U_fb;
+ state = RungeKuttaU(
+ [this](const ::Eigen::Matrix<double, 5, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return ContinuousDynamics(trajectory_.velocity_drivetrain().plant(),
+ trajectory_.Tlr_to_la(), X, U);
+ },
+ state, U, ::y2016::control_loops::drivetrain::kDt);
+ }
+
+ EXPECT_LT((state - trajectory_.GoalState(trajectory_.length(), 0.0)).norm(),
+ 1.1e-2);
+}
+
+// TODO(austin): Try a velocity limited plan at some point.
+//
+// TODO(austin): Handle saturation. 254 does this by just not going that
+// fast... We want to maybe replan when we get behind, or something. Maybe
+// stop moving the setpoint like our 2018 arm?
+
+} // namespace testing
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index 585d6b8..d045f45 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -491,11 +491,8 @@
accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
(-12.0 - C[1, 0]) / K5[1, 0]]
- maxa = 0.0
+ maxa = -float('inf')
for a in accelerations:
- if a < 0.0:
- continue
-
U = K5 * a + K3 * v * v + K4 * v
if not (numpy.abs(U) > 12.0 + 1e-6).any():
maxa = max(maxa, a)
@@ -537,11 +534,8 @@
accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
(-12.0 - C[1, 0]) / K5[1, 0]]
- mina = 0.0
+ mina = float('inf')
for a in accelerations:
- if a > 0.0:
- continue
-
U = K5 * a + K3 * v * v + K4 * v
if not (numpy.abs(U) > 12.0 + 1e-6).any():
mina = min(mina, a)