frc971/control_loops/drivetrain/trajectory.cc - RealtimeRoboticsGroup/test - Gitiles

 #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
	#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