Add double jointed arm trajectory with roll
This is strongly inspired by the 2018 arm code and drivetrain splines,
but is different enough to not be worth making the same.
Change-Id: I9d840b65596454a2e42a1b4a082ea1549e5b4ae2
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index c125fac..0ef2cf9 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -78,3 +78,50 @@
"//frc971/control_loops/double_jointed_arm:dynamics",
],
)
+
+cc_library(
+ name = "trajectory",
+ srcs = ["trajectory.cc"],
+ hdrs = ["trajectory.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//frc971/control_loops:binomial",
+ "//frc971/control_loops:dlqr",
+ "//frc971/control_loops:fixed_quadrature",
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops/double_jointed_arm:dynamics",
+ "//frc971/control_loops/double_jointed_arm:ekf",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "trajectory_test",
+ srcs = ["trajectory_test.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":arm_constants",
+ ":trajectory",
+ "//aos/testing:googletest",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
+ ],
+)
+
+cc_binary(
+ name = "trajectory_plot",
+ srcs = [
+ "trajectory_plot.cc",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":arm_constants",
+ ":trajectory",
+ "//frc971/analysis:in_process_plotter",
+ "//frc971/control_loops:binomial",
+ "//frc971/control_loops:fixed_quadrature",
+ "//frc971/control_loops/double_jointed_arm:ekf",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
+ "@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
diff --git a/y2023/control_loops/superstructure/arm/trajectory.cc b/y2023/control_loops/superstructure/arm/trajectory.cc
new file mode 100644
index 0000000..ad6720f
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/trajectory.cc
@@ -0,0 +1,955 @@
+#include "y2023/control_loops/superstructure/arm/trajectory.h"
+
+#include "Eigen/Dense"
+#include "aos/logging/logging.h"
+#include "frc971/control_loops/dlqr.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/jacobian.h"
+#include "frc971/control_loops/runge_kutta.h"
+#include "gflags/gflags.h"
+
+DEFINE_double(lqr_proximal_pos, 0.15, "Position LQR gain");
+DEFINE_double(lqr_proximal_vel, 4.0, "Velocity LQR gain");
+DEFINE_double(lqr_distal_pos, 0.20, "Position LQR gain");
+DEFINE_double(lqr_distal_vel, 4.0, "Velocity LQR gain");
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+::Eigen::Matrix<double, 3, 1> CosSpline::Theta(double alpha) const {
+ ::Eigen::Matrix<double, 3, 1> result;
+ result.block<2, 1>(0, 0) = spline_.Theta(alpha);
+
+ const std::pair<AlphaTheta, AlphaTheta> roll_points = RollPoints(alpha);
+ const double alpha_percent =
+ (alpha - roll_points.first.alpha) /
+ (roll_points.second.alpha - roll_points.first.alpha);
+
+ result(2) = (0.5 - 0.5 * std::cos(alpha_percent * M_PI)) *
+ (roll_points.second.theta - roll_points.first.theta) +
+ roll_points.first.theta;
+ return result;
+}
+
+::Eigen::Matrix<double, 3, 1> CosSpline::Omega(double alpha) const {
+ ::Eigen::Matrix<double, 3, 1> result;
+ result.block<2, 1>(0, 0) = spline_.Omega(alpha);
+
+ const std::pair<AlphaTheta, AlphaTheta> roll_points = RollPoints(alpha);
+ const double dalpha = (roll_points.second.alpha - roll_points.first.alpha);
+ const double alpha_percent = (alpha - roll_points.first.alpha) / dalpha;
+
+ result(2) = 0.5 * std::sin(alpha_percent * M_PI) *
+ (roll_points.second.theta - roll_points.first.theta) * M_PI /
+ dalpha;
+ return result;
+}
+
+::Eigen::Matrix<double, 3, 1> CosSpline::Alpha(double alpha) const {
+ ::Eigen::Matrix<double, 3, 1> result;
+ result.block<2, 1>(0, 0) = spline_.Alpha(alpha);
+
+ const std::pair<AlphaTheta, AlphaTheta> roll_points = RollPoints(alpha);
+ const double dalpha = (roll_points.second.alpha - roll_points.first.alpha);
+ const double alpha_percent = (alpha - roll_points.first.alpha) / dalpha;
+
+ result(2) = 0.5 * std::cos(alpha_percent * M_PI) *
+ (roll_points.second.theta - roll_points.first.theta) * M_PI *
+ M_PI / dalpha / dalpha;
+ return result;
+}
+
+std::pair<CosSpline::AlphaTheta, CosSpline::AlphaTheta> CosSpline::RollPoints(
+ double alpha) const {
+ if (alpha <= 0.0) {
+ return std::make_pair(roll_[0], roll_[1]);
+ }
+ if (alpha >= 1.0) {
+ return std::make_pair(roll_[roll_.size() - 2], roll_[roll_.size() - 1]);
+ }
+
+ // Find the distance right below our number using a binary search.
+ size_t after = ::std::distance(
+ roll_.begin(), ::std::lower_bound(roll_.begin(), roll_.end(), alpha,
+ [](AlphaTheta at, double alpha) {
+ return at.alpha < alpha;
+ }));
+ DCHECK_GT(after, 0u);
+ size_t before = after - 1;
+ DCHECK_LT(before, roll_.size());
+ return std::make_pair(roll_[before], roll_[after]);
+}
+
+CosSpline CosSpline::Reversed() const {
+ std::vector<AlphaTheta> new_roll(roll_.size());
+ for (size_t i = 0; i < roll_.size(); ++i) {
+ new_roll[roll_.size() - 1 - i] = {1.0 - roll_[i].alpha, roll_[i].theta};
+ }
+
+ Eigen::Matrix<double, 2, 4> new_control_points;
+ new_control_points = spline_.control_points().rowwise().reverse();
+ return CosSpline(NSpline<4, 2>(new_control_points), std::move(new_roll));
+}
+
+::Eigen::Matrix<double, 3, 1> Path::Theta(double distance) const {
+ const double alpha = DistanceToAlpha(distance);
+ return spline_.Theta(alpha);
+}
+
+::Eigen::Matrix<double, 3, 1> Path::Omega(double distance) const {
+ const double alpha = DistanceToAlpha(distance);
+ return spline_.Omega(alpha).normalized();
+}
+
+::Eigen::Matrix<double, 3, 1> Path::Alpha(double distance) const {
+ const double alpha = DistanceToAlpha(distance);
+ const ::Eigen::Matrix<double, 3, 1> dspline_point = spline_.Omega(alpha);
+ const ::Eigen::Matrix<double, 3, 1> ddspline_point = spline_.Alpha(alpha);
+
+ const double squared_norm = dspline_point.squaredNorm();
+
+ return ddspline_point / squared_norm -
+ dspline_point * (dspline_point.transpose() * ddspline_point) /
+ ::std::pow(squared_norm, 2);
+}
+
+std::vector<float> Path::BuildDistances(size_t num_alpha) {
+ num_alpha = num_alpha ? num_alpha : 100;
+ std::vector<float> distances;
+ distances.push_back(0.0);
+
+ const double dalpha = 1.0 / static_cast<double>(num_alpha - 1);
+ double last_alpha = 0.0;
+ 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_.Omega(alpha).norm(); },
+ last_alpha, alpha));
+ last_alpha = alpha;
+ }
+ return distances;
+}
+
+double Path::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));
+
+ const double alpha = (distance - distances()[before]) /
+ (distances()[after] - distances()[before]) *
+ distance_step_size +
+ static_cast<double>(before) * distance_step_size;
+ CHECK_GT(alpha, 0.0);
+ CHECK_LT(alpha, 1.0);
+ return alpha;
+}
+
+Path Path::Reversed() const { return Path(spline_.Reversed()); }
+
+::std::unique_ptr<Path> Path::Reversed(::std::unique_ptr<Path> p) {
+ return ::std::make_unique<Path>(p->Reversed());
+}
+
+double Trajectory::MaxCurvatureSpeed(
+ double goal_distance, const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,
+ double plan_vmax) {
+ // TODO(austin): We are looking up the index in the path 3 times here.
+ const ::Eigen::Matrix<double, 3, 1> theta = path().Theta(goal_distance);
+ const ::Eigen::Matrix<double, 3, 1> omega = path().Omega(goal_distance);
+ const ::Eigen::Matrix<double, 3, 1> alpha = path().Alpha(goal_distance);
+
+ const ::Eigen::Matrix<double, 4, 1> X =
+ (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
+ .finished();
+
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2;
+
+ const ::Eigen::Matrix<double, 2, 1> gravity_volts =
+ dynamics_->K3_inverse() * dynamics_->GravityTorque(X);
+
+ dynamics_->NormilizedMatriciesForState(X, &K1, &K2);
+ const ::Eigen::Matrix<double, 2, 2> omega_square =
+ (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
+ .finished();
+
+ // Here, we can say that
+ // d^2/dt^2 theta = d^2/dd^2 theta(d) * (d d/dt)^2
+ // Normalize so that the max accel is 1, and take magnitudes. This
+ // gives us the max velocity we can be at each point due to
+ // curvature.
+ double min_good_ddot =
+ ::std::sqrt(1.0 / ::std::max(0.001, (alpha_unitizer * alpha).norm()));
+
+ // Now, solve for the max speed we can follow the path without hitting the
+ // vmax limit. This makes it such that we can always hold vmax and follow
+ // the path. This helps when there are cases where holding constant speed
+ // into a reducing radius turn will saturate our voltage limit. Technically,
+ // there are cases where faster paths exist, but they are a lot harder to
+ // solve due to the nonlinearities.
+ const ::Eigen::Matrix<double, 2, 1> vk1 =
+ dynamics_->K3_inverse() * (K1 * alpha.block<2, 1>(0, 0) +
+ K2 * omega_square * omega.block<2, 1>(0, 0));
+ const ::Eigen::Matrix<double, 2, 1> vk2 =
+ dynamics_->K3_inverse() * dynamics_->K4() * omega.block<2, 1>(0, 0);
+
+ // Loop through all the various vmin, plan_vmax combinations.
+ for (const double c : {-plan_vmax, plan_vmax}) {
+ // Also loop through saturating theta0 and theta1
+ for (const ::std::tuple<double, double, double> &abgravity :
+ {::std::tuple<double, double, double>{vk1(0), vk2(0),
+ gravity_volts(0)},
+ ::std::tuple<double, double, double>{vk1(1), vk2(1),
+ gravity_volts(1)}}) {
+ const double a = ::std::get<0>(abgravity);
+ const double b = ::std::get<1>(abgravity);
+ const double gravity = ::std::get<2>(abgravity);
+ const double sqrt_number = b * b - 4.0 * a * (c - gravity);
+
+ // Throw out imaginary solutions to the quadratic.
+ if (sqrt_number > 0) {
+ const double sqrt_result = ::std::sqrt(sqrt_number);
+ const double ddot1 = (-b + sqrt_result) / (2.0 * a);
+ const double ddot2 = (-b - sqrt_result) / (2.0 * a);
+ // Loop through both solutions.
+ for (const double ddot : {ddot1, ddot2}) {
+ const ::Eigen::Matrix<double, 2, 1> U =
+ vk1 * ddot * ddot + vk2 * ddot - gravity_volts;
+
+ // Finally, make sure the velocity is positive and valid.
+ if ((U.array().abs() <= plan_vmax + 1e-6).all() && ddot > 0.0) {
+ min_good_ddot = ::std::min(min_good_ddot, ddot);
+ }
+ }
+ }
+ }
+ }
+
+ return min_good_ddot;
+}
+
+::std::vector<double> Trajectory::CurvatureOptimizationPass(
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax) {
+ ::std::vector<double> max_dvelocity_unfiltered;
+ max_dvelocity_unfiltered.reserve(num_plan_points_);
+ for (size_t i = 0; i < num_plan_points_; ++i) {
+ const double distance = DistanceForIndex(i);
+
+ max_dvelocity_unfiltered.push_back(
+ MaxCurvatureSpeed(distance, alpha_unitizer, plan_vmax));
+ }
+ return max_dvelocity_unfiltered;
+}
+
+double Trajectory::FeasableForwardsAcceleration(
+ double goal_distance, double goal_velocity, double /*plan_vmax*/,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const {
+ const ::Eigen::Matrix<double, 3, 1> omega = path().Omega(goal_distance);
+ const ::Eigen::Matrix<double, 3, 1> alpha = path().Alpha(goal_distance);
+
+ return ::std::sqrt(
+ ::std::max(0.0, 1.0 - ::std::pow((alpha_unitizer * alpha).norm() *
+ goal_velocity * goal_velocity,
+ 2))) /
+ (alpha_unitizer * omega).norm();
+}
+
+double Trajectory::FeasableForwardsVoltage(
+ double goal_distance, double goal_velocity, double plan_vmax,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const {
+ // TODO(austin): We are looking up the index in the path 3 times here.
+ const ::Eigen::Matrix<double, 3, 1> theta = path().Theta(goal_distance);
+ const ::Eigen::Matrix<double, 3, 1> omega = path().Omega(goal_distance);
+ const ::Eigen::Matrix<double, 3, 1> alpha = path().Alpha(goal_distance);
+
+ const ::Eigen::Matrix<double, 4, 1> arm_X =
+ (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
+ .finished();
+
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2_normalized;
+
+ dynamics_->NormilizedMatriciesForState(arm_X, &K1, &K2_normalized);
+
+ const ::Eigen::Matrix<double, 2, 2> omega_square =
+ (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
+ .finished();
+
+ // The derivitives of f(d) wrt t are:
+ // theta = f(d)
+ // dtheta/dt = d/dd f(d) * dd/dt
+ // d^2 theta/dt^2 = d^2/dd^2 f(d) * (dd/dt)^2 + d^2d/dt^2 * d/dd f(d)
+ //
+ // And, our arm dynamics are:
+ // K1 d^2 theta/dt^2 + K2_normalized * diag(dtheta/dt) * dtheta/dt = k3 * V -
+ // K4 * dtheta/dt
+ //
+ // And our roll dynamics are:
+ //
+ // d^2 theta2/dt^2 = A[1, 1] d theta2/dt + B[1, 0] * V
+ //
+ // Plug the derivitives in to our dynamics, solve for d^2d/dt^2, and ship it!
+ //
+ // We want things in the form V = k_constant + k_scalar * d^2d/dt^2
+
+ const ::Eigen::Matrix<double, 3, 1> k_constant =
+ (::Eigen::Matrix<double, 3, 1>()
+ << (dynamics_->K3_inverse() *
+ ((K1 * alpha.block<2, 1>(0, 0) +
+ K2_normalized * omega_square * omega.block<2, 1>(0, 0)) *
+ goal_velocity * goal_velocity +
+ dynamics_->K4() * omega.block<2, 1>(0, 0) * goal_velocity -
+ dynamics_->GravityTorque(arm_X))),
+ ((alpha(2, 0) * goal_velocity * goal_velocity -
+ roll_->coefficients().A_continuous(1, 1) * omega(2, 0) *
+ goal_velocity) /
+ roll_->coefficients().B_continuous(1, 0)))
+ .finished();
+
+ const ::Eigen::Matrix<double, 3, 1> k_scalar =
+ (::Eigen::Matrix<double, 3, 1>()
+ << dynamics_->K3_inverse() * K1 * omega.block<2, 1>(0, 0),
+ (omega(2, 0) / roll_->coefficients().B_continuous(1, 0)))
+ .finished();
+
+ const double constraint_goal_acceleration =
+ ::std::sqrt(
+ ::std::max(0.0, 1.0 - ::std::pow((alpha_unitizer * alpha).norm() *
+ goal_velocity * goal_velocity,
+ 2))) /
+ (alpha_unitizer * omega).norm();
+
+ double min_goal_acceleration = ::std::numeric_limits<double>::infinity();
+ double max_goal_acceleration = -::std::numeric_limits<double>::infinity();
+ for (double c : {-plan_vmax, plan_vmax}) {
+ for (const ::std::pair<double, double> &ab :
+ {::std::pair<double, double>{k_constant(0, 0), k_scalar(0, 0)},
+ ::std::pair<double, double>{k_constant(1, 0), k_scalar(1, 0)},
+ ::std::pair<double, double>{k_constant(2, 0), k_scalar(2, 0)}}) {
+ const double a = ab.first;
+ const double b = ab.second;
+ const double voltage_accel = (c - a) / b;
+ const ::Eigen::Matrix<double, 3, 1> U =
+ k_constant + k_scalar * voltage_accel;
+
+ VLOG(1) << "Trying, U is " << U.transpose() << ", accel "
+ << voltage_accel;
+ if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
+ min_goal_acceleration = std::min(voltage_accel, min_goal_acceleration);
+ max_goal_acceleration = std::max(voltage_accel, max_goal_acceleration);
+ }
+ }
+ }
+ if (min_goal_acceleration == ::std::numeric_limits<double>::infinity() ||
+ max_goal_acceleration == -::std::numeric_limits<double>::infinity()) {
+ // TODO(austin): The math above doesn't always give a valid solution.
+ // This happens when things get pretty ill-conditioned. Figure out what to
+ // do about it.
+ VLOG(1)
+ << "Failed to find a valid accel at distance " << goal_distance
+ << ", voltage "
+ << (k_constant + k_scalar * constraint_goal_acceleration).transpose()
+ << ", accel " << constraint_goal_acceleration << " vs vmax "
+ << plan_vmax;
+ return constraint_goal_acceleration;
+ }
+
+ const double goal_acceleration =
+ std::abs(max_goal_acceleration - constraint_goal_acceleration) <
+ std::abs(min_goal_acceleration - constraint_goal_acceleration)
+ ? max_goal_acceleration
+ : min_goal_acceleration;
+
+ if (min_goal_acceleration < constraint_goal_acceleration &&
+ constraint_goal_acceleration < max_goal_acceleration) {
+ VLOG(1)
+ << "Solved valid accel at distance " << goal_distance << ", voltage "
+ << (k_constant + k_scalar * constraint_goal_acceleration).transpose()
+ << ", accel " << constraint_goal_acceleration
+ << ", overall accel limited, U limit is " << goal_acceleration
+ << " with U of "
+ << (k_constant + k_scalar * goal_acceleration).transpose();
+
+ if (!((k_constant + k_scalar * constraint_goal_acceleration)
+ .array()
+ .abs() <= plan_vmax + 1e-6)
+ .all()) {
+ LOG(FATAL) << "Accel in range, but constraint voltage out of range.";
+ }
+ return constraint_goal_acceleration;
+ }
+
+ VLOG(1) << "Solved valid accel at distance " << goal_distance << ", voltage "
+ << (k_constant + k_scalar * goal_acceleration).transpose()
+ << ", decel limits [" << min_goal_acceleration << ", "
+ << max_goal_acceleration << "], picked " << goal_acceleration
+ << " voltage limited, constraint accel "
+ << constraint_goal_acceleration << " constraint voltage "
+ << (k_constant + k_scalar * constraint_goal_acceleration).transpose();
+
+ return goal_acceleration;
+}
+
+double Trajectory::FeasableBackwardsAcceleration(
+ double goal_distance, double goal_velocity, double /*plan_vmax*/,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const {
+ const ::Eigen::Matrix<double, 3, 1> omega = path().Omega(goal_distance);
+ const ::Eigen::Matrix<double, 3, 1> alpha = path().Alpha(goal_distance);
+
+ return ::std::sqrt(
+ ::std::max(0.0, 1.0 - ::std::pow(((alpha_unitizer * alpha).norm() *
+ goal_velocity * goal_velocity),
+ 2))) /
+ (alpha_unitizer * omega).norm();
+}
+
+double Trajectory::FeasableBackwardsVoltage(
+ double goal_distance, double goal_velocity, double plan_vmax,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const {
+ const ::Eigen::Matrix<double, 3, 1> theta = path().Theta(goal_distance);
+ const ::Eigen::Matrix<double, 3, 1> omega = path().Omega(goal_distance);
+ const ::Eigen::Matrix<double, 3, 1> alpha = path().Alpha(goal_distance);
+ const ::Eigen::Matrix<double, 4, 1> arm_X =
+ (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
+ .finished();
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2_normalized;
+
+ dynamics_->NormilizedMatriciesForState(arm_X, &K1, &K2_normalized);
+
+ const ::Eigen::Matrix<double, 2, 2> omega_square =
+ (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
+ .finished();
+
+ const ::Eigen::Matrix<double, 3, 1> k_constant =
+ (::Eigen::Matrix<double, 3, 1>()
+ << dynamics_->K3_inverse() *
+ ((K1 * alpha.block<2, 1>(0, 0) +
+ K2_normalized * omega_square * omega.block<2, 1>(0, 0)) *
+ goal_velocity * goal_velocity +
+ dynamics_->K4() * omega.block<2, 1>(0, 0) * goal_velocity -
+ dynamics_->GravityTorque(arm_X)),
+ ((alpha(2, 0) * goal_velocity * goal_velocity -
+ roll_->coefficients().A_continuous(1, 1) * omega(2, 0) *
+ goal_velocity) /
+ roll_->coefficients().B_continuous(1, 0)))
+ .finished();
+ const ::Eigen::Matrix<double, 3, 1> k_scalar =
+ (::Eigen::Matrix<double, 3, 1>()
+ << dynamics_->K3_inverse() * K1 * omega.block<2, 1>(0, 0),
+ (omega(2, 0) / roll_->coefficients().B_continuous(1, 0)))
+ .finished();
+
+ const double constraint_goal_acceleration =
+ ::std::sqrt(
+ ::std::max(0.0, 1.0 - ::std::pow(((alpha_unitizer * alpha).norm() *
+ goal_velocity * goal_velocity),
+ 2))) /
+ (alpha_unitizer * omega).norm();
+
+ double min_goal_acceleration = ::std::numeric_limits<double>::infinity();
+ double max_goal_acceleration = -::std::numeric_limits<double>::infinity();
+ for (double c : {-plan_vmax, plan_vmax}) {
+ for (const ::std::pair<double, double> &ab :
+ {::std::pair<double, double>{k_constant(0, 0), k_scalar(0, 0)},
+ ::std::pair<double, double>{k_constant(1, 0), k_scalar(1, 0)},
+ ::std::pair<double, double>{k_constant(2, 0), k_scalar(2, 0)}}) {
+ const double a = ab.first;
+ const double b = ab.second;
+ // This time, we are doing the other pass. So, find all the
+ // decelerations (and flip them) to find the prior velocity.
+ const double voltage_accel = (c - a) / b;
+
+ const ::Eigen::Matrix<double, 3, 1> U =
+ k_constant + k_scalar * voltage_accel;
+
+ // TODO(austin): This doesn't always give a valid solution. It really
+ // should. Figure out why.
+ VLOG(1) << "Trying, U is " << U.transpose() << ", accel "
+ << voltage_accel;
+ if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
+ min_goal_acceleration = std::min(-voltage_accel, min_goal_acceleration);
+ max_goal_acceleration = std::max(-voltage_accel, max_goal_acceleration);
+ }
+ }
+ }
+
+ if (min_goal_acceleration == ::std::numeric_limits<double>::infinity() ||
+ max_goal_acceleration == -::std::numeric_limits<double>::infinity()) {
+ VLOG(1)
+ << "Failed to find a valid decel at distance " << goal_distance
+ << ", voltage "
+ << (k_constant + k_scalar * constraint_goal_acceleration).transpose()
+ << ", accel " << constraint_goal_acceleration << " vs vmax "
+ << plan_vmax;
+ return constraint_goal_acceleration;
+ }
+
+ if (min_goal_acceleration < constraint_goal_acceleration &&
+ constraint_goal_acceleration < max_goal_acceleration) {
+ VLOG(1)
+ << "Solved valid decel at distance " << goal_distance << ", voltage "
+ << (k_constant - k_scalar * constraint_goal_acceleration).transpose()
+ << ", decel " << min_goal_acceleration
+ << " <= " << constraint_goal_acceleration
+ << " <= " << max_goal_acceleration << ", accel limited";
+
+ if (!((k_constant - k_scalar * constraint_goal_acceleration)
+ .array()
+ .abs() <= plan_vmax + 1e-6)
+ .all()) {
+ LOG(FATAL) << "Accel in range, but constraint voltage out of range.";
+ }
+
+ return constraint_goal_acceleration;
+ }
+
+ const double goal_acceleration =
+ std::abs(max_goal_acceleration - constraint_goal_acceleration) <
+ std::abs(min_goal_acceleration - constraint_goal_acceleration)
+ ? max_goal_acceleration
+ : min_goal_acceleration;
+
+ VLOG(1) << "Solved valid decel at distance " << goal_distance << ", voltage "
+ << (k_constant - k_scalar * goal_acceleration).transpose()
+ << ", decel limits [" << min_goal_acceleration << ", "
+ << max_goal_acceleration << "], picked " << goal_acceleration
+ << " voltage limited, constraint accel "
+ << constraint_goal_acceleration << " constraint voltage "
+ << (k_constant - k_scalar * constraint_goal_acceleration).transpose();
+
+ return goal_acceleration;
+}
+
+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 (frc971::control_loops::RungeKuttaSteps(
+ [&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, 10) -
+ v) +
+ std::sqrt(2.0 * a0 * dx + v * v);
+}
+
+::std::vector<double> Trajectory::BackwardsOptimizationAccelerationPass(
+ const ::std::vector<double> &max_dvelocity,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax) {
+ ::std::vector<double> max_dvelocity_back_pass = max_dvelocity;
+
+ // Now, iterate over the list of velocities and constrain the acceleration.
+ for (int i = num_plan_points_ - 2; i >= 0; --i) {
+ const double previous_velocity = max_dvelocity_back_pass[i + 1];
+ const double previous_distance = DistanceForIndex(i + 1);
+
+ // Now, integrate with respect to distance (not time like normal).
+ const double integrated_velocity = IntegrateAccelForDistance(
+ [&](double x, double v) {
+ return FeasableBackwardsAcceleration(x, v, plan_vmax, alpha_unitizer);
+ },
+ previous_velocity, previous_distance, step_size_);
+ max_dvelocity_back_pass[i] =
+ ::std::min(integrated_velocity, max_dvelocity_back_pass[i]);
+ }
+
+ return max_dvelocity_back_pass;
+}
+
+::std::vector<double> Trajectory::BackwardsOptimizationVoltagePass(
+ const ::std::vector<double> &max_dvelocity,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax) {
+ ::std::vector<double> max_dvelocity_back_pass = max_dvelocity;
+
+ // Now, iterate over the list of velocities and constrain the acceleration.
+ for (int i = num_plan_points_ - 2; i >= 0; --i) {
+ const double previous_velocity = max_dvelocity_back_pass[i + 1];
+ const double previous_distance = DistanceForIndex(i + 1);
+
+ // Now, integrate with respect to distance (not time like normal).
+ const double integrated_velocity = IntegrateAccelForDistance(
+ [&](double x, double v) {
+ return FeasableBackwardsVoltage(x, v, plan_vmax, alpha_unitizer);
+ },
+ previous_velocity, previous_distance, step_size_);
+ max_dvelocity_back_pass[i] =
+ ::std::min(integrated_velocity, max_dvelocity_back_pass[i]);
+ }
+
+ return max_dvelocity_back_pass;
+}
+
+::std::vector<double> Trajectory::ForwardsOptimizationAccelerationPass(
+ const ::std::vector<double> &max_dvelocity,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax) {
+ ::std::vector<double> max_dvelocity_forwards_pass = max_dvelocity;
+ // Now, iterate over the list of velocities and constrain the acceleration.
+ for (size_t i = 1; i < num_plan_points_; ++i) {
+ const double previous_velocity = max_dvelocity_forwards_pass[i - 1];
+ const double previous_distance = DistanceForIndex(i - 1);
+
+ // Now, integrate with respect to distance (not time like normal).
+ const double integrated_velocity = IntegrateAccelForDistance(
+ [&](double x, double v) {
+ return FeasableForwardsAcceleration(x, v, plan_vmax, alpha_unitizer);
+ },
+ previous_velocity, previous_distance, step_size_);
+
+ max_dvelocity_forwards_pass[i] =
+ ::std::min(integrated_velocity, max_dvelocity_forwards_pass[i]);
+ }
+
+ return max_dvelocity_forwards_pass;
+}
+
+::std::vector<double> Trajectory::ForwardsOptimizationVoltagePass(
+ const ::std::vector<double> &max_dvelocity,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax) {
+ ::std::vector<double> max_dvelocity_forwards_pass = max_dvelocity;
+ // Now, iterate over the list of velocities and constrain the acceleration.
+ for (size_t i = 1; i < num_plan_points_; ++i) {
+ const double previous_velocity = max_dvelocity_forwards_pass[i - 1];
+ const double previous_distance = DistanceForIndex(i - 1);
+
+ // Now, integrate with respect to distance (not time like normal).
+ const double integrated_velocity = IntegrateAccelForDistance(
+ [&](double x, double v) {
+ return FeasableForwardsVoltage(x, v, plan_vmax, alpha_unitizer);
+ },
+ previous_velocity, previous_distance, step_size_);
+
+ max_dvelocity_forwards_pass[i] =
+ ::std::min(integrated_velocity, max_dvelocity_forwards_pass[i]);
+ }
+
+ return max_dvelocity_forwards_pass;
+}
+
+void TrajectoryFollower::Reset() {
+ next_goal_ = last_goal_ = goal_ = ::Eigen::Matrix<double, 2, 1>::Zero();
+ U_unsaturated_ = U_ff_ = U_ = ::Eigen::Matrix<double, 3, 1>::Zero();
+ goal_acceleration_ = 0.0;
+ saturation_fraction_along_path_ = 0.0;
+ omega_.setZero();
+ if (trajectory_ != nullptr) {
+ set_theta(trajectory_->ThetaT(goal_(0)));
+ }
+}
+
+::Eigen::Matrix<double, 2, 6> TrajectoryFollower::ArmK_at_state(
+ const Eigen::Ref<const ::Eigen::Matrix<double, 6, 1>> arm_X,
+ const Eigen::Ref<const ::Eigen::Matrix<double, 2, 1>> arm_U) {
+ const double kProximalPos = FLAGS_lqr_proximal_pos;
+ const double kProximalVel = FLAGS_lqr_proximal_vel;
+ const double kDistalPos = FLAGS_lqr_distal_pos;
+ const double kDistalVel = FLAGS_lqr_distal_vel;
+ const ::Eigen::DiagonalMatrix<double, 4> Q =
+ (::Eigen::DiagonalMatrix<double, 4>().diagonal()
+ << 1.0 / ::std::pow(kProximalPos, 2),
+ 1.0 / ::std::pow(kProximalVel, 2), 1.0 / ::std::pow(kDistalPos, 2),
+ 1.0 / ::std::pow(kDistalVel, 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();
+
+ const auto x_blocked = arm_X.block<4, 1>(0, 0);
+
+ const ::Eigen::Matrix<double, 4, 4> final_A =
+ ::frc971::control_loops::NumericalJacobianX<4, 2>(
+ [this](const auto &x_blocked, const auto &U, double dt) {
+ return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
+ },
+ x_blocked, arm_U, 0.00505);
+
+ const ::Eigen::Matrix<double, 4, 2> final_B =
+ ::frc971::control_loops::NumericalJacobianU<4, 2>(
+ [this](const auto &x_blocked, const auto &U, double dt) {
+ return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
+ },
+ x_blocked, arm_U, 0.00505);
+
+ ::Eigen::Matrix<double, 4, 4> S;
+ ::Eigen::Matrix<double, 2, 4> sub_K;
+ if (::frc971::controls::dlqr<4, 2>(final_A, final_B, Q, R, &sub_K, &S) == 0) {
+ ::Eigen::EigenSolver<::Eigen::Matrix<double, 4, 4>> eigensolver(
+ final_A - final_B * sub_K);
+
+ ::Eigen::Matrix<double, 2, 6> K;
+ K.setZero();
+ K.block<2, 4>(0, 0) = sub_K;
+ K(0, 4) = 1.0;
+ K(1, 5) = 1.0;
+ failed_solutions_ = 0;
+ last_K_ = K;
+ } else {
+ ++failed_solutions_;
+ }
+ return last_K_;
+}
+
+void TrajectoryFollower::USaturationSearch(
+ double goal_distance, double last_goal_distance, double goal_velocity,
+ double last_goal_velocity, double saturation_fraction_along_path,
+ const ::Eigen::Matrix<double, 2, 6> &arm_K,
+ const ::Eigen::Matrix<double, 9, 1> &X, const Trajectory &trajectory,
+ ::Eigen::Matrix<double, 3, 1> *U, double *saturation_goal_velocity,
+ double *saturation_goal_acceleration) {
+ double saturation_goal_distance =
+ ((goal_distance - last_goal_distance) * saturation_fraction_along_path +
+ last_goal_distance);
+
+ const ::Eigen::Matrix<double, 3, 1> theta_t =
+ trajectory.ThetaT(saturation_goal_distance);
+ *saturation_goal_velocity = trajectory.InterpolateVelocity(
+ saturation_goal_distance, last_goal_distance, goal_distance,
+ last_goal_velocity, goal_velocity);
+ *saturation_goal_acceleration = trajectory.InterpolateAcceleration(
+ last_goal_distance, goal_distance, last_goal_velocity, goal_velocity);
+ const ::Eigen::Matrix<double, 3, 1> omega_t =
+ trajectory.OmegaT(saturation_goal_distance, *saturation_goal_velocity);
+ const ::Eigen::Matrix<double, 3, 1> alpha_t =
+ trajectory.AlphaT(saturation_goal_distance, *saturation_goal_velocity,
+ *saturation_goal_acceleration);
+ const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+
+ const ::Eigen::Matrix<double, 3, 1> U_ff = ComputeFF_U(R, omega_t, alpha_t);
+
+ *U = U_ff;
+ U->block<2, 1>(0, 0) += arm_K * (R.block<6, 1>(0, 0) - X.block<6, 1>(0, 0));
+ U->block<1, 1>(2, 0) +=
+ roll_->controller().K() * (R.block<3, 1>(6, 0) - X.block<3, 1>(6, 0));
+}
+
+::Eigen::Matrix<double, 2, 1> TrajectoryFollower::PlanNextGoal(
+ const ::Eigen::Matrix<double, 2, 1> &goal, double plan_vmax, double dt) {
+ // Figure out where we would be if we were to accelerate as fast as
+ // our constraints allow.
+ ::Eigen::Matrix<double, 2, 1> next_goal = ::frc971::control_loops::RungeKutta(
+ [this, &plan_vmax](const ::Eigen::Matrix<double, 2, 1> &X) {
+ return (::Eigen::Matrix<double, 2, 1>() << X(1),
+ trajectory_->FeasableForwardsVoltage(
+ X(0), X(1), plan_vmax, trajectory_->alpha_unitizer()))
+ .finished();
+ },
+ goal, dt);
+
+ // Min that with the backwards pass velocity in case the backwards pass
+ // wants us to slow down.
+ const double next_trajectory_velocity =
+ trajectory_->GetDVelocity(next_goal(0));
+ if (next_trajectory_velocity < next_goal(1)) {
+ next_goal(1) = next_trajectory_velocity;
+ // And then recompute how far to go with our new trajectory in mind.
+ double goal_acceleration = trajectory_->InterpolateAcceleration(
+ goal(0), next_goal(0), goal(1), next_goal(1));
+ next_goal(0) = (goal(0) + goal(1) * dt + 0.5 * dt * dt * goal_acceleration);
+ next_goal(1) = trajectory_->GetDVelocity(next_goal(0));
+ }
+ return next_goal;
+}
+
+Eigen::Matrix<double, 3, 1> TrajectoryFollower::ComputeFF_U(
+ const Eigen::Matrix<double, 9, 1> &X,
+ const Eigen::Matrix<double, 3, 1> &omega_t,
+ const Eigen::Matrix<double, 3, 1> &alpha_t) const {
+ Eigen::Matrix<double, 3, 1> result;
+ result.block<2, 1>(0, 0) =
+ dynamics_->FF_U(X.block<4, 1>(0, 0), omega_t.block<2, 1>(0, 0),
+ alpha_t.block<2, 1>(0, 0));
+ result(2, 0) =
+ (alpha_t(2, 0) -
+ roll_->plant().coefficients().A_continuous(1, 1) * omega_t(2, 0)) /
+ roll_->plant().coefficients().B_continuous(1, 0);
+
+ return result;
+}
+
+void TrajectoryFollower::Update(const ::Eigen::Matrix<double, 9, 1> &X,
+ bool disabled, double dt, double plan_vmax,
+ double voltage_limit) {
+ // TODO(austin): Separate voltage limit for shoulder for no path.
+ last_goal_ = goal_;
+
+ if (!has_path()) {
+ // No path, so go to the last theta (no velocity).
+ last_goal_.setZero();
+ next_goal_.setZero();
+ goal_.setZero();
+ goal_acceleration_ = 0.0;
+ saturation_fraction_along_path_ = 0.0;
+
+ if (disabled) {
+ U_ff_.setZero();
+ U_.setZero();
+ U_unsaturated_.setZero();
+ } else {
+ const ::Eigen::Matrix<double, 9, 1> R =
+ Trajectory::R(theta_, ::Eigen::Matrix<double, 3, 1>::Zero());
+
+ U_ff_ = ComputeFF_U(X, ::Eigen::Matrix<double, 3, 1>::Zero(),
+ ::Eigen::Matrix<double, 3, 1>::Zero());
+
+ const ::Eigen::Matrix<double, 2, 6> arm_K =
+ ArmK_at_state(X.block<6, 1>(0, 0), U_ff_.block<2, 1>(0, 0));
+
+ U_unsaturated_.block<2, 1>(0, 0) =
+ U_ff_.block<2, 1>(0, 0) +
+ arm_K * (R.block<6, 1>(0, 0) - X.block<6, 1>(0, 0));
+ U_unsaturated_(2, 0) =
+ U_ff_(2, 0) +
+ roll_->controller().K() * (R.block<3, 1>(6, 0) - X.block<3, 1>(6, 0));
+
+ U_ = U_unsaturated_;
+
+ U_ = U_.array().max(-voltage_limit).min(voltage_limit);
+ }
+ return;
+ }
+
+ if (disabled) {
+ // If we are disabled, it's likely for a bit of time. So, lets freeze
+ // ourselves on the path (accept the previous motion, but then zero out the
+ // velocity). Set all outputs to 0 as well.
+ next_goal_(1) = 0.0;
+ goal_ = next_goal_;
+ goal_acceleration_ = 0.0;
+ U_unsaturated_.setZero();
+ U_.setZero();
+ U_ff_.setZero();
+ saturation_fraction_along_path_ = 1.0;
+ theta_ = trajectory_->ThetaT(goal_(0));
+ omega_.setZero();
+ return;
+ }
+
+ // To avoid exposing the new goals before the outer code has a chance to
+ // querry the internal state, move to the new goals here.
+ goal_ = next_goal_;
+
+ if (::std::abs(goal_(0) - path()->length()) < 1e-2) {
+ // If we go backwards along the path near the goal, snap us to the end
+ // point or we'll never actually finish.
+ if (goal_acceleration_ * dt + goal_(1) < 0.0 ||
+ goal_(0) > path()->length()) {
+ goal_(0) = path()->length();
+ goal_(1) = 0.0;
+ }
+ }
+
+ if (goal_(0) == path()->length()) {
+ next_goal_(0) = goal_(0);
+ next_goal_(1) = 0.0;
+ goal_acceleration_ = 0.0;
+ } else {
+ // Figure out where we would be if we were to accelerate as fast as
+ // our constraints allow.
+ next_goal_ = PlanNextGoal(goal_, plan_vmax, dt);
+
+ goal_acceleration_ = trajectory_->InterpolateAcceleration(
+ goal_(0), next_goal_(0), goal_(1), next_goal_(1));
+ }
+
+ const ::Eigen::Matrix<double, 3, 1> theta_t = trajectory_->ThetaT(goal_(0));
+ const ::Eigen::Matrix<double, 3, 1> omega_t =
+ trajectory_->OmegaT(goal_(0), goal_(1));
+ const ::Eigen::Matrix<double, 3, 1> alpha_t =
+ trajectory_->AlphaT(goal_(0), goal_(1), goal_acceleration_);
+
+ const ::Eigen::Matrix<double, 9, 1> R = Trajectory::R(theta_t, omega_t);
+
+ U_ff_ = ComputeFF_U(R, omega_t, alpha_t);
+
+ const ::Eigen::Matrix<double, 2, 6> arm_K =
+ ArmK_at_state(X.block<6, 1>(0, 0), U_ff_.block<2, 1>(0, 0));
+ U_unsaturated_ = U_ff_;
+ U_unsaturated_.block<2, 1>(0, 0) +=
+ arm_K * (R.block<6, 1>(0, 0) - X.block<6, 1>(0, 0));
+ U_unsaturated_.block<1, 1>(2, 0) +=
+ roll_->controller().K() * (R.block<3, 1>(6, 0) - X.block<3, 1>(6, 0));
+ U_ = U_unsaturated_;
+
+ // Ok, now we know if we are staturated or not. If we are, time to search
+ // between here and our previous goal either until we find a state where we
+ // aren't saturated, or we are really close to our starting point.
+ saturation_fraction_along_path_ = 1.0;
+ if ((U_.array().abs() > voltage_limit).any()) {
+ // Saturated. Let's do a binary search.
+ double step_size;
+ if ((goal_(0) - last_goal_(0)) < 1e-8) {
+ // print "Not bothering to move"
+ // Avoid the divide by 0 when interpolating. Just don't move since we
+ // are saturated.
+ saturation_fraction_along_path_ = 0.0;
+ step_size = 0.0;
+ } else {
+ saturation_fraction_along_path_ = 0.5;
+ step_size = 0.5;
+ }
+
+ // Pull us back to the previous point until we aren't saturated anymore.
+ double saturation_goal_velocity;
+ double saturation_goal_acceleration;
+ while (step_size > 0.01) {
+ USaturationSearch(goal_(0), last_goal_(0), goal_(1), last_goal_(1),
+ saturation_fraction_along_path_, arm_K, X, *trajectory_,
+ &U_, &saturation_goal_velocity,
+ &saturation_goal_acceleration);
+ step_size = step_size * 0.5;
+ if ((U_.array().abs() > voltage_limit).any()) {
+ saturation_fraction_along_path_ -= step_size;
+ } else {
+ saturation_fraction_along_path_ += step_size;
+ }
+ }
+
+ goal_(0) = ((goal_(0) - last_goal_(0)) * saturation_fraction_along_path_ +
+ last_goal_(0));
+ goal_(1) = saturation_goal_velocity;
+
+ next_goal_ = PlanNextGoal(goal_, plan_vmax, dt);
+
+ goal_acceleration_ = trajectory_->InterpolateAcceleration(
+ goal_(0), next_goal_(0), goal_(1), next_goal_(1));
+
+ U_ = U_.array().max(-voltage_limit).min(voltage_limit);
+ }
+ theta_ = trajectory_->ThetaT(goal_(0));
+ omega_ = trajectory_->OmegaT(goal_(0), goal_(1));
+}
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023
diff --git a/y2023/control_loops/superstructure/arm/trajectory.h b/y2023/control_loops/superstructure/arm/trajectory.h
new file mode 100644
index 0000000..17ec3aa
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/trajectory.h
@@ -0,0 +1,614 @@
+#ifndef Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
+#define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
+
+#include <array>
+#include <initializer_list>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "Eigen/Dense"
+#include "frc971/control_loops/binomial.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/fixed_quadrature.h"
+#include "frc971/control_loops/hybrid_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+using frc971::control_loops::Binomial;
+using frc971::control_loops::GaussianQuadrature5;
+
+template <int N, int M>
+class NSpline {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ // Computes the characteristic matrix of a given spline order. This is an
+ // upper triangular matrix rather than lower because our splines are
+ // represented as rows rather than columns.
+ // Each row represents the impact of each point with increasing powers of
+ // alpha. Row i, column j contains the effect point i has with the j'th power
+ // of alpha.
+ static ::Eigen::Matrix<double, N, N> SplineMatrix() {
+ ::Eigen::Matrix<double, N, N> matrix =
+ ::Eigen::Matrix<double, N, N>::Zero();
+
+ for (int i = 0; i < N; ++i) {
+ // Binomial(N - 1, i) * (1 - t) ^ (N - i - 1) * (t ^ i) * P[i]
+ const double binomial = Binomial(N - 1, i);
+ const int one_minus_t_power = N - i - 1;
+
+ // Then iterate over the powers of t and add the pieces to the matrix.
+ for (int j = i; j < N; ++j) {
+ // j is the power of t we are placing in the matrix.
+ // k is the power of t in the (1 - t) expression that we need to
+ // evaluate.
+ const int k = j - i;
+ const double tscalar =
+ binomial * Binomial(one_minus_t_power, k) * ::std::pow(-1.0, k);
+ matrix(i, j) = tscalar;
+ }
+ }
+ return matrix;
+ }
+
+ // Computes the matrix to multiply by [1, a, a^2, ...] to evaluate the spline.
+ template <int D>
+ static ::Eigen::Matrix<double, M, N - D> SplinePolynomial(
+ const ::Eigen::Matrix<double, M, N> &control_points) {
+ // We use rows for the spline, so the multiplication looks "backwards"
+ ::Eigen::Matrix<double, M, N> polynomial = control_points * SplineMatrix();
+
+ // Now, compute the derivative requested.
+ for (int i = D; i < N; ++i) {
+ // Start with t^i, and multiply i, i-1, i-2 until you have done this
+ // Derivative times.
+ double scalar = 1.0;
+ for (int j = i; j > i - D; --j) {
+ scalar *= j;
+ }
+ polynomial.template block<M, 1>(0, i) =
+ polynomial.template block<M, 1>(0, i) * scalar;
+ }
+ return polynomial.template block<M, N - D>(0, D);
+ }
+
+ // Computes an order K-1 polynomial matrix in alpha. [1, alpha, alpha^2, ...]
+ template <int K>
+ static ::Eigen::Matrix<double, K, 1> AlphaPolynomial(double alpha) {
+ alpha = std::min(1.0, std::max(0.0, alpha));
+ ::Eigen::Matrix<double, K, 1> polynomial =
+ ::Eigen::Matrix<double, K, 1>::Zero();
+ polynomial(0) = 1.0;
+ for (int i = 1; i < K; ++i) {
+ polynomial(i) = polynomial(i - 1) * alpha;
+ }
+ return polynomial;
+ }
+
+ // Constructs a spline. control_points is a matrix of start, control1,
+ // control2, ..., end.
+ NSpline(::Eigen::Matrix<double, M, N> control_points)
+ : control_points_(control_points),
+ spline_polynomial_(SplinePolynomial<0>(control_points_)),
+ dspline_polynomial_(SplinePolynomial<1>(control_points_)),
+ ddspline_polynomial_(SplinePolynomial<2>(control_points_)) {}
+
+ // Returns the xy coordiate of the spline for a given alpha.
+ ::Eigen::Matrix<double, M, 1> Theta(double alpha) const {
+ return spline_polynomial_ * AlphaPolynomial<N>(alpha);
+ }
+
+ // Returns the dspline/dalpha for a given alpha.
+ ::Eigen::Matrix<double, M, 1> Omega(double alpha) const {
+ return dspline_polynomial_ * AlphaPolynomial<N - 1>(alpha);
+ }
+
+ // Returns the d^2spline/dalpha^2 for a given alpha.
+ ::Eigen::Matrix<double, M, 1> Alpha(double alpha) const {
+ return ddspline_polynomial_ * AlphaPolynomial<N - 2>(alpha);
+ }
+
+ const ::Eigen::Matrix<double, M, N> &control_points() const {
+ return control_points_;
+ }
+
+ private:
+ const ::Eigen::Matrix<double, M, N> control_points_;
+
+ // Each of these polynomials gets multiplied by [x^(n-1), x^(n-2), ..., x, 1]
+ // depending on the size of the polynomial.
+ const ::Eigen::Matrix<double, M, N> spline_polynomial_;
+ const ::Eigen::Matrix<double, M, N - 1> dspline_polynomial_;
+ const ::Eigen::Matrix<double, M, N - 2> ddspline_polynomial_;
+};
+
+// Add a cos wave to the bottom of the 2d spline to handle the roll.
+class CosSpline {
+ public:
+ // Struct defining pairs of alphas and thetas.
+ struct AlphaTheta {
+ double alpha;
+ double theta;
+ };
+
+ CosSpline(NSpline<4, 2> spline, std::vector<AlphaTheta> roll)
+ : spline_(spline), roll_(std::move(roll)) {
+ CHECK_GE(roll_.size(), 2u);
+ CHECK_EQ(roll_[0].alpha, 0.0);
+ CHECK_EQ(roll_[roll_.size() - 1].alpha, 1.0);
+ }
+
+ // Returns the xy coordiate of the spline for a given alpha.
+ ::Eigen::Matrix<double, 3, 1> Theta(double alpha) const;
+
+ // Returns the dspline/dalpha for a given alpha.
+ ::Eigen::Matrix<double, 3, 1> Omega(double alpha) const;
+
+ // Returns the d^2spline/dalpha^2 for a given alpha.
+ ::Eigen::Matrix<double, 3, 1> Alpha(double alpha) const;
+
+ CosSpline Reversed() const;
+
+ private:
+ NSpline<4, 2> spline_;
+ const std::vector<AlphaTheta> roll_;
+
+ // Returns the two control points for the roll for an alpha.
+ std::pair<AlphaTheta, AlphaTheta> RollPoints(double alpha) const;
+};
+
+// Class to hold a spline as a function of distance.
+class Path {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ Path(const CosSpline &spline, int num_alpha = 100)
+ : spline_(spline), distances_(BuildDistances(num_alpha)) {}
+
+ virtual ~Path() {}
+
+ // Returns a point on the spline as a function of distance.
+ ::Eigen::Matrix<double, 3, 1> Theta(double distance) const;
+
+ // Returns the velocity as a function of distance.
+ ::Eigen::Matrix<double, 3, 1> Omega(double distance) const;
+
+ // Returns the acceleration as a function of distance.
+ ::Eigen::Matrix<double, 3, 1> Alpha(double distance) const;
+
+ // Returns the length of the path in meters.
+ double length() const { return distances().back(); }
+
+ const absl::Span<const float> distances() const { return distances_; }
+
+ Path Reversed() const;
+ static ::std::unique_ptr<Path> Reversed(::std::unique_ptr<Path> p);
+
+ private:
+ std::vector<float> BuildDistances(size_t num_alpha);
+
+ // Computes alpha for a distance
+ double DistanceToAlpha(double distance) const;
+
+ const CosSpline spline_;
+ // An interpolation table of distances evenly distributed in alpha.
+ const ::std::vector<float> distances_;
+};
+
+namespace testing {
+class TrajectoryTest_IndicesForDistanceTest_Test;
+} // namespace testing
+
+// A trajectory is a path and a set of velocities as a function of distance.
+class Trajectory {
+ public:
+ // Constructs a trajectory (but doesn't calculate it) given a path and a step
+ // size.
+ Trajectory(const frc971::control_loops::arm::Dynamics *dynamics,
+ const StateFeedbackHybridPlant<3, 1, 1> *roll,
+ ::std::unique_ptr<const Path> path, double gridsize)
+ : dynamics_(dynamics),
+ roll_(roll),
+ path_(::std::move(path)),
+ num_plan_points_(
+ static_cast<size_t>(::std::ceil(path_->length() / gridsize) + 1)),
+ step_size_(path_->length() /
+ static_cast<double>(num_plan_points_ - 1)) {
+ alpha_unitizer_.setZero();
+ }
+
+ // Optimizes the trajectory. The path will adhere to the constraints that
+ // || angular acceleration * alpha_unitizer || < 1, and the applied voltage <
+ // plan_vmax.
+ void OptimizeTrajectory(const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,
+ double plan_vmax) {
+ if (path_ == nullptr) {
+ abort();
+ }
+ alpha_unitizer_ = alpha_unitizer;
+
+ max_dvelocity_unfiltered_ =
+ CurvatureOptimizationPass(alpha_unitizer, plan_vmax);
+
+ // We need to start and end the trajectory with 0 velocity.
+ max_dvelocity_unfiltered_[0] = 0.0;
+ max_dvelocity_unfiltered_[max_dvelocity_unfiltered_.size() - 1] = 0.0;
+
+ max_dvelocity_backwards_accel_ = BackwardsOptimizationAccelerationPass(
+ max_dvelocity_unfiltered_, alpha_unitizer, plan_vmax);
+ max_dvelocity_forwards_accel_ = ForwardsOptimizationAccelerationPass(
+ max_dvelocity_backwards_accel_, alpha_unitizer, plan_vmax);
+
+ max_dvelocity_backwards_voltage_ = BackwardsOptimizationVoltagePass(
+ max_dvelocity_forwards_accel_, alpha_unitizer, plan_vmax);
+
+ max_dvelocity_forwards_voltage_ = ForwardsOptimizationVoltagePass(
+ max_dvelocity_backwards_voltage_, alpha_unitizer, plan_vmax);
+ }
+
+ // Returns an array of the distances used in the plan. The starting point
+ // (0.0), and end point (path->length()) are included in the array.
+ ::std::vector<double> DistanceArray() const {
+ ::std::vector<double> result;
+ result.reserve(num_plan_points_);
+ for (size_t i = 0; i < num_plan_points_; ++i) {
+ result.push_back(DistanceForIndex(i));
+ }
+ return result;
+ }
+
+ // Computes the maximum velocity that we can follow the path while adhering to
+ // the constraints that || angular acceleration * alpha_unitizer || < 1, and
+ // the applied voltage < plan_vmax. Returns the velocities.
+ ::std::vector<double> CurvatureOptimizationPass(
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax);
+
+ double MaxCurvatureSpeed(double goal_distance,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,
+ double plan_vmax);
+
+ // Computes the maximum forwards feasable acceleration at a given position
+ // while adhering to the plan_vmax and alpha_unitizer constraints.
+ // This gives us the maximum path distance acceleration (d^2d/dt^2) for any
+ // initial position and velocity for the forwards path.
+ double FeasableForwardsAcceleration(
+ double goal_distance, double goal_velocity, double plan_vmax,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const;
+ double FeasableForwardsVoltage(
+ double goal_distance, double goal_velocity, double plan_vmax,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const;
+
+ // Computes the maximum backwards feasable acceleration at a given position
+ // while adhering to the plan_vmax and alpha_unitizer constraints.
+ // This gives us the maximum path distance acceleration (d^2d/dt^2) for any
+ // initial position and velocity for the backwards path.
+ // Note: positive acceleration means speed up while going in the negative
+ // direction on the path.
+ double FeasableBackwardsAcceleration(
+ double goal_distance, double goal_velocity, double plan_vmax,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const;
+ double FeasableBackwardsVoltage(
+ double goal_distance, double goal_velocity, double plan_vmax,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const;
+
+ // Executes the backwards path optimization pass.
+ ::std::vector<double> BackwardsOptimizationAccelerationPass(
+ const ::std::vector<double> &max_dvelocity,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax);
+ ::std::vector<double> BackwardsOptimizationVoltagePass(
+ const ::std::vector<double> &max_dvelocity,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax);
+
+ // Executes the forwards path optimization pass.
+ ::std::vector<double> ForwardsOptimizationAccelerationPass(
+ const ::std::vector<double> &max_dvelocity,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax);
+ ::std::vector<double> ForwardsOptimizationVoltagePass(
+ const ::std::vector<double> &max_dvelocity,
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax);
+
+ // Returns the number of points used for the plan.
+ size_t num_plan_points() const { return num_plan_points_; }
+
+ // Returns the curvature only velocity plan.
+ const ::std::vector<double> &max_dvelocity_unfiltered() const {
+ return max_dvelocity_unfiltered_;
+ }
+ // Returns the backwards pass + curvature plan.
+ const ::std::vector<double> &max_dvelocity_backward_accel() const {
+ return max_dvelocity_backwards_accel_;
+ }
+ const ::std::vector<double> &max_dvelocity_backward_voltage() const {
+ return max_dvelocity_backwards_voltage_;
+ }
+ // Returns the full plan. This isn't useful at runtime since we don't want to
+ // be tied to a specific acceleration plan when we hit saturation.
+ const ::std::vector<double> &max_dvelocity_forwards_accel() const {
+ return max_dvelocity_forwards_accel_;
+ }
+ const ::std::vector<double> &max_dvelocity_forwards_voltage() const {
+ return max_dvelocity_forwards_voltage_;
+ }
+
+ // Returns the interpolated velocity at the distance d along the path. The
+ // math assumes constant acceleration from point (d0, v0), to point (d1, v1),
+ // and that we are at point d between the two.
+ static double InterpolateVelocity(double d, double d0, double d1, double v0,
+ double v1) {
+ // We don't support negative velocities. Report 0 velocity in that case.
+ // TODO(austin): Verify this doesn't show up in the real world.
+ if (v0 < 0 || v1 < 0) {
+ return 0.0;
+ }
+ if (d <= d0) {
+ return v0;
+ }
+ if (d >= d1) {
+ return v1;
+ }
+ return ::std::sqrt(v0 * v0 + (v1 * v1 - v0 * v0) * (d - d0) / (d1 - d0));
+ }
+
+ // Computes the path distance velocity of the plan as a function of the
+ // distance.
+ double GetDVelocity(double distance) const {
+ return GetDVelocity(distance, max_dvelocity_forwards_voltage_);
+ }
+ double GetDVelocity(double d, const ::std::vector<double> &plan) const {
+ ::std::pair<size_t, size_t> indices = IndicesForDistance(d);
+ const double v0 = plan[indices.first];
+ const double v1 = plan[indices.second];
+ const double d0 = DistanceForIndex(indices.first);
+ const double d1 = DistanceForIndex(indices.second);
+ return InterpolateVelocity(d, d0, d1, v0, v1);
+ }
+
+ // Computes the path distance acceleration of the plan as a function of the
+ // distance.
+ double GetDAcceleration(double distance) const {
+ return GetDAcceleration(distance, max_dvelocity_forwards_voltage_);
+ }
+ double GetDAcceleration(double distance,
+ const ::std::vector<double> &plan) const {
+ ::std::pair<size_t, size_t> indices = IndicesForDistance(distance);
+ const double v0 = plan[indices.first];
+ const double v1 = plan[indices.second];
+ const double d0 = DistanceForIndex(indices.first);
+ const double d1 = DistanceForIndex(indices.second);
+ return InterpolateAcceleration(d0, d1, v0, v1);
+ }
+
+ // Returns the acceleration along the path segment assuming constant
+ // acceleration.
+ static double InterpolateAcceleration(double d0, double d1, double v0,
+ double v1) {
+ return 0.5 * (::std::pow(v1, 2) - ::std::pow(v0, 2)) / (d1 - d0);
+ }
+
+ ::Eigen::Matrix<double, 3, 1> ThetaT(double d) const {
+ return path_->Theta(d);
+ }
+
+ // Returns d theta/dt at a specified path distance and velocity.
+ ::Eigen::Matrix<double, 3, 1> OmegaT(double distance, double velocity) const {
+ if (distance > path_->length() || distance < 0.0) {
+ return ::Eigen::Matrix<double, 3, 1>::Zero();
+ } else {
+ return path_->Omega(distance) * velocity;
+ }
+ }
+
+ // Returns d^2 theta/dt^2 at a specified path distance, velocity and
+ // acceleration.
+ ::Eigen::Matrix<double, 3, 1> AlphaT(double distance, double velocity,
+ double acceleration) const {
+ if (distance > path_->length() || distance < 0.0) {
+ return ::Eigen::Matrix<double, 3, 1>::Zero();
+ } else {
+ return path_->Alpha(distance) * ::std::pow(velocity, 2) +
+ path_->Omega(distance) * acceleration;
+ }
+ }
+
+ // Converts a theta and omega vector to a full state vector.
+ static ::Eigen::Matrix<double, 9, 1> R(
+ ::Eigen::Matrix<double, 3, 1> theta_t,
+ ::Eigen::Matrix<double, 3, 1> omega_t) {
+ return (::Eigen::Matrix<double, 9, 1>() << theta_t(0, 0), omega_t(0, 0),
+ theta_t(1, 0), omega_t(1, 0), 0.0, 0.0, theta_t(2, 0),
+ omega_t(2, 0), 0.0)
+ .finished();
+ }
+
+ const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer() const {
+ return alpha_unitizer_;
+ }
+
+ const Path &path() const { return *path_; }
+
+ private:
+ friend class testing::TrajectoryTest_IndicesForDistanceTest_Test;
+
+ // Returns the distance along the path for a specific index in the plan.
+ double DistanceForIndex(size_t index) const {
+ return static_cast<double>(index) * step_size_;
+ }
+
+ // Returns the before and after indices for a specific distance in the plan.
+ ::std::pair<size_t, size_t> IndicesForDistance(double distance) const {
+ const double path_length = path_->length();
+ if (distance <= 0.0) {
+ return ::std::pair<size_t, size_t>(0, 1);
+ }
+ const size_t lower_index =
+ ::std::min(static_cast<size_t>(num_plan_points_ - 2),
+ static_cast<size_t>(::std::floor((num_plan_points_ - 1) *
+ distance / path_length)));
+
+ return ::std::pair<size_t, size_t>(lower_index, lower_index + 1);
+ }
+
+ const frc971::control_loops::arm::Dynamics *dynamics_;
+ const StateFeedbackHybridPlant<3, 1, 1> *roll_;
+
+ // The path to follow.
+ ::std::unique_ptr<const Path> path_;
+ // The number of points in the plan.
+ const size_t num_plan_points_;
+ // A cached version of the step size since we need this a *lot*.
+ const double step_size_;
+
+ ::std::vector<double> max_dvelocity_unfiltered_;
+ ::std::vector<double> max_dvelocity_backwards_voltage_;
+ ::std::vector<double> max_dvelocity_backwards_accel_;
+ ::std::vector<double> max_dvelocity_forwards_accel_;
+ ::std::vector<double> max_dvelocity_forwards_voltage_;
+
+ ::Eigen::Matrix<double, 3, 3> alpha_unitizer_;
+};
+
+// This class tracks the current goal along trajectories and paths.
+class TrajectoryFollower {
+ public:
+ TrajectoryFollower(const frc971::control_loops::arm::Dynamics *dynamics,
+ const StateFeedbackLoop<3, 1, 1, double,
+ StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>> *roll,
+ const ::Eigen::Matrix<double, 3, 1> &theta)
+ : dynamics_(dynamics), roll_(roll), trajectory_(nullptr), theta_(theta) {
+ omega_.setZero();
+ last_K_.setZero();
+ Reset();
+ }
+
+ TrajectoryFollower(const frc971::control_loops::arm::Dynamics *dynamics,
+ const StateFeedbackLoop<3, 1, 1, double,
+ StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>> *roll,
+ Trajectory *const trajectory)
+ : dynamics_(dynamics), roll_(roll), trajectory_(trajectory) {
+ last_K_.setZero();
+ Reset();
+ }
+
+ bool has_path() const { return trajectory_ != nullptr; }
+
+ void set_theta(const ::Eigen::Matrix<double, 3, 1> &theta) { theta_ = theta; }
+
+ // Returns the goal distance along the path.
+ const ::Eigen::Matrix<double, 2, 1> &goal() const { return goal_; }
+ double goal(int i) const { return goal_(i); }
+
+ // Starts over at the beginning of the path.
+ void Reset();
+
+ // Switches paths and starts at the beginning of the path.
+ void SwitchTrajectory(const Trajectory *trajectory) {
+ trajectory_ = trajectory;
+ Reset();
+ }
+
+ // Returns the controller gain at the provided state.
+ ::Eigen::Matrix<double, 2, 6> ArmK_at_state(
+ const Eigen::Ref<const ::Eigen::Matrix<double, 6, 1>> arm_X,
+ const Eigen::Ref<const ::Eigen::Matrix<double, 2, 1>> arm_U);
+
+ // Returns the voltage, velocity and acceleration if we were to be partially
+ // along the path.
+ void USaturationSearch(double goal_distance, double last_goal_distance,
+ double goal_velocity, double last_goal_velocity,
+ double saturation_fraction_along_path,
+ const ::Eigen::Matrix<double, 2, 6> &K,
+ const ::Eigen::Matrix<double, 9, 1> &X,
+ const Trajectory &trajectory,
+ ::Eigen::Matrix<double, 3, 1> *U,
+ double *saturation_goal_velocity,
+ double *saturation_goal_acceleration);
+
+ // Returns the next goal given a planning plan_vmax and timestep. This
+ // ignores the backwards pass.
+ ::Eigen::Matrix<double, 2, 1> PlanNextGoal(
+ const ::Eigen::Matrix<double, 2, 1> &goal, double plan_vmax, double dt);
+
+ // Plans the next cycle and updates the internal state for consumption.
+ void Update(const ::Eigen::Matrix<double, 9, 1> &X, bool disabled, double dt,
+ double plan_vmax, double voltage_limit);
+
+ // Returns the goal acceleration for this cycle.
+ double goal_acceleration() const { return goal_acceleration_; }
+
+ // Returns U(s) for this cycle.
+ const ::Eigen::Matrix<double, 3, 1> &U() const { return U_; }
+ double U(int i) const { return U_(i); }
+ const ::Eigen::Matrix<double, 3, 1> &U_unsaturated() const {
+ return U_unsaturated_;
+ }
+ const ::Eigen::Matrix<double, 3, 1> &U_ff() const { return U_ff_; }
+
+ double saturation_fraction_along_path() const {
+ return saturation_fraction_along_path_;
+ }
+
+ const ::Eigen::Matrix<double, 3, 1> &theta() const { return theta_; }
+ double theta(int i) const { return theta_(i); }
+ const ::Eigen::Matrix<double, 3, 1> &omega() const { return omega_; }
+ double omega(int i) const { return omega_(i); }
+
+ // Distance left on the path before we get to the end of the path.
+ double path_distance_to_go() const {
+ if (has_path()) {
+ return ::std::max(0.0, path()->length() - goal_(0));
+ } else {
+ return 0.0;
+ }
+ }
+
+ const Path *path() const { return &trajectory_->path(); }
+
+ int failed_solutions() const { return failed_solutions_; }
+
+ Eigen::Matrix<double, 3, 1> ComputeFF_U(
+ const Eigen::Matrix<double, 9, 1> &X,
+ const Eigen::Matrix<double, 3, 1> &omega_t,
+ const Eigen::Matrix<double, 3, 1> &alpha_t) const;
+
+ private:
+ const frc971::control_loops::arm::Dynamics *dynamics_;
+ const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>> *roll_;
+ // The trajectory plan.
+ const Trajectory *trajectory_ = nullptr;
+
+ // The current goal.
+ ::Eigen::Matrix<double, 2, 1> goal_;
+ // The previously executed goal.
+ ::Eigen::Matrix<double, 2, 1> last_goal_;
+ // The goal to use next cycle. We always plan 1 cycle ahead.
+ ::Eigen::Matrix<double, 2, 1> next_goal_;
+
+ ::Eigen::Matrix<double, 3, 1> U_;
+ ::Eigen::Matrix<double, 3, 1> U_unsaturated_;
+ ::Eigen::Matrix<double, 3, 1> U_ff_;
+ double goal_acceleration_ = 0.0;
+
+ double saturation_fraction_along_path_ = 1.0;
+
+ // Holds the last valid goal position for when we loose our path.
+ ::Eigen::Matrix<double, 3, 1> theta_;
+ ::Eigen::Matrix<double, 3, 1> omega_;
+
+ ::Eigen::Matrix<double, 2, 6> last_K_;
+ int failed_solutions_ = 0;
+};
+
+} // namespace arm
+} // namespace control_loops
+} // namespace frc971
+} // namespace y2023
+
+#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
diff --git a/y2023/control_loops/superstructure/arm/trajectory_plot.cc b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
new file mode 100644
index 0000000..2b05a30
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
@@ -0,0 +1,535 @@
+#include "aos/init.h"
+#include "frc971/analysis/in_process_plotter.h"
+#include "frc971/control_loops/binomial.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
+#include "frc971/control_loops/fixed_quadrature.h"
+#include "gflags/gflags.h"
+#include "y2023/control_loops/superstructure/arm/arm_constants.h"
+#include "y2023/control_loops/superstructure/arm/trajectory.h"
+#include "y2023/control_loops/superstructure/roll/integral_hybrid_roll_plant.h"
+#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
+
+DEFINE_bool(forwards, true, "If true, run the forwards simulation.");
+DEFINE_bool(plot, true, "If true, plot");
+DEFINE_bool(plot_thetas, true, "If true, plot the angles");
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+using frc971::control_loops::MatrixGaussianQuadrature5;
+
+void Main() {
+ frc971::control_loops::arm::Dynamics dynamics(kArmConstants);
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>
+ hybrid_roll = superstructure::roll::MakeIntegralHybridRollLoop();
+
+ Eigen::Matrix<double, 3, 4> spline_params;
+
+ spline_params << 0.30426338, 0.42813912, 0.64902386, 0.55127045, -1.73611082,
+ -1.64478944, -1.44763868, -1.22624244, 0.1, 0.2, 0.3, 0.5;
+ LOG(INFO) << "Spline " << spline_params;
+ NSpline<4, 2> spline(spline_params.block<2, 4>(0, 0));
+ CosSpline cos_spline(spline,
+ {{0.0, 0.1}, {0.3, 0.1}, {0.7, 0.2}, {1.0, 0.2}});
+ Path distance_spline(cos_spline, 100);
+
+ Trajectory trajectory(&dynamics, &hybrid_roll.plant(),
+ std::make_unique<Path>(cos_spline), 0.001);
+
+ constexpr double kAlpha0Max = 20.0;
+ constexpr double kAlpha1Max = 30.0;
+ constexpr double kAlpha2Max = 40.0;
+ constexpr double vmax = 9.0;
+ constexpr double sim_dt = 0.00505;
+
+ const ::Eigen::DiagonalMatrix<double, 3> alpha_unitizer(
+ (::Eigen::DiagonalMatrix<double, 3>().diagonal() << (1.0 / kAlpha0Max),
+ (1.0 / kAlpha1Max), (1.0 / kAlpha2Max))
+ .finished());
+ trajectory.OptimizeTrajectory(alpha_unitizer, vmax);
+
+ const ::std::vector<double> distance_array = trajectory.DistanceArray();
+
+ ::std::vector<double> theta0_array;
+ ::std::vector<double> theta1_array;
+ ::std::vector<double> theta2_array;
+ ::std::vector<double> omega0_array;
+ ::std::vector<double> omega1_array;
+ ::std::vector<double> omega2_array;
+ ::std::vector<double> alpha0_array;
+ ::std::vector<double> alpha1_array;
+ ::std::vector<double> alpha2_array;
+
+ ::std::vector<double> integrated_distance;
+ ::std::vector<double> integrated_theta0_array;
+ ::std::vector<double> integrated_theta1_array;
+ ::std::vector<double> integrated_theta2_array;
+ ::std::vector<double> integrated_omega0_array;
+ ::std::vector<double> integrated_omega1_array;
+ ::std::vector<double> integrated_omega2_array;
+
+ ::Eigen::Matrix<double, 3, 1> integrated_theta = distance_spline.Theta(0.0);
+ ::Eigen::Matrix<double, 3, 1> integrated_omega = distance_spline.Omega(0.0);
+
+ // Plot the splines and their integrals to check consistency.
+ for (const double d : distance_array) {
+ const ::Eigen::Matrix<double, 3, 1> theta = distance_spline.Theta(d);
+ const ::Eigen::Matrix<double, 3, 1> omega = distance_spline.Omega(d);
+ const ::Eigen::Matrix<double, 3, 1> alpha = distance_spline.Alpha(d);
+ theta0_array.push_back(theta(0, 0));
+ theta1_array.push_back(theta(1, 0));
+ theta2_array.push_back(theta(2, 0));
+ omega0_array.push_back(omega(0, 0));
+ omega1_array.push_back(omega(1, 0));
+ omega2_array.push_back(omega(2, 0));
+ alpha0_array.push_back(alpha(0, 0));
+ alpha1_array.push_back(alpha(1, 0));
+ alpha2_array.push_back(alpha(2, 0));
+ }
+
+ const double dd = distance_spline.length() / 1000.0;
+ for (double d = 0; d <= distance_spline.length(); d += dd) {
+ integrated_distance.push_back(d);
+ integrated_omega0_array.push_back(integrated_omega(0));
+ integrated_omega1_array.push_back(integrated_omega(1));
+ integrated_omega2_array.push_back(integrated_omega(2));
+ integrated_theta0_array.push_back(integrated_theta(0));
+ integrated_theta1_array.push_back(integrated_theta(1));
+ integrated_theta2_array.push_back(integrated_theta(2));
+
+ integrated_theta += MatrixGaussianQuadrature5<3>(
+ [&](double distance) { return distance_spline.Omega(distance); }, d,
+ d + dd);
+ integrated_omega += MatrixGaussianQuadrature5<3>(
+ [&](double distance) { return distance_spline.Alpha(distance); }, d,
+ d + dd);
+ }
+
+ // Next step: see what U is as a function of distance for all the passes.
+ ::std::vector<double> Uff0_distance_array_curvature;
+ ::std::vector<double> Uff1_distance_array_curvature;
+ ::std::vector<double> Uff2_distance_array_curvature;
+ ::std::vector<double> Uff0_distance_array_backwards_accel_only;
+ ::std::vector<double> Uff1_distance_array_backwards_accel_only;
+ ::std::vector<double> Uff2_distance_array_backwards_accel_only;
+ ::std::vector<double> Uff0_distance_array_forwards_accel_only;
+ ::std::vector<double> Uff1_distance_array_forwards_accel_only;
+ ::std::vector<double> Uff2_distance_array_forwards_accel_only;
+ ::std::vector<double> Uff0_distance_array_backwards_voltage_only;
+ ::std::vector<double> Uff1_distance_array_backwards_voltage_only;
+ ::std::vector<double> Uff2_distance_array_backwards_voltage_only;
+ ::std::vector<double> Uff0_distance_array_forwards_voltage_only;
+ ::std::vector<double> Uff1_distance_array_forwards_voltage_only;
+ ::std::vector<double> Uff2_distance_array_forwards_voltage_only;
+
+ TrajectoryFollower follower(&dynamics, &hybrid_roll, &trajectory);
+
+ for (const double distance : distance_array) {
+ const ::Eigen::Matrix<double, 3, 1> theta_t = trajectory.ThetaT(distance);
+
+ {
+ const double goal_velocity = trajectory.GetDVelocity(
+ distance, trajectory.max_dvelocity_unfiltered());
+ const double goal_acceleration = trajectory.GetDAcceleration(
+ distance, trajectory.max_dvelocity_unfiltered());
+ const ::Eigen::Matrix<double, 3, 1> omega_t =
+ trajectory.OmegaT(distance, goal_velocity);
+ const ::Eigen::Matrix<double, 3, 1> alpha_t =
+ trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+ const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+ const ::Eigen::Matrix<double, 3, 1> U =
+ follower.ComputeFF_U(R, omega_t, alpha_t).array().max(-20).min(20);
+
+ Uff0_distance_array_curvature.push_back(U(0));
+ Uff1_distance_array_curvature.push_back(U(1));
+ Uff2_distance_array_curvature.push_back(U(2));
+ }
+ {
+ const double goal_velocity = trajectory.GetDVelocity(
+ distance, trajectory.max_dvelocity_backward_accel());
+ const double goal_acceleration = trajectory.GetDAcceleration(
+ distance, trajectory.max_dvelocity_backward_accel());
+ const ::Eigen::Matrix<double, 3, 1> omega_t =
+ trajectory.OmegaT(distance, goal_velocity);
+ const ::Eigen::Matrix<double, 3, 1> alpha_t =
+ trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+ const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+ const ::Eigen::Matrix<double, 3, 1> U =
+ follower.ComputeFF_U(R, omega_t, alpha_t).array().max(-20).min(20);
+
+ Uff0_distance_array_backwards_accel_only.push_back(U(0));
+ Uff1_distance_array_backwards_accel_only.push_back(U(1));
+ Uff2_distance_array_backwards_accel_only.push_back(U(2));
+ }
+ {
+ const double goal_velocity = trajectory.GetDVelocity(
+ distance, trajectory.max_dvelocity_forwards_accel());
+ const double goal_acceleration = trajectory.GetDAcceleration(
+ distance, trajectory.max_dvelocity_forwards_accel());
+ const ::Eigen::Matrix<double, 3, 1> omega_t =
+ trajectory.OmegaT(distance, goal_velocity);
+ const ::Eigen::Matrix<double, 3, 1> alpha_t =
+ trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+ const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+ const ::Eigen::Matrix<double, 3, 1> U =
+ follower.ComputeFF_U(R, omega_t, alpha_t).array().max(-20).min(20);
+
+ Uff0_distance_array_forwards_accel_only.push_back(U(0));
+ Uff1_distance_array_forwards_accel_only.push_back(U(1));
+ Uff2_distance_array_forwards_accel_only.push_back(U(2));
+ }
+ {
+ const double goal_velocity = trajectory.GetDVelocity(
+ distance, trajectory.max_dvelocity_backward_voltage());
+ const double goal_acceleration = trajectory.GetDAcceleration(
+ distance, trajectory.max_dvelocity_backward_voltage());
+ const ::Eigen::Matrix<double, 3, 1> omega_t =
+ trajectory.OmegaT(distance, goal_velocity);
+ const ::Eigen::Matrix<double, 3, 1> alpha_t =
+ trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+ const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+ const ::Eigen::Matrix<double, 3, 1> U =
+ follower.ComputeFF_U(R, omega_t, alpha_t).array().max(-20).min(20);
+
+ Uff0_distance_array_backwards_voltage_only.push_back(U(0));
+ Uff1_distance_array_backwards_voltage_only.push_back(U(1));
+ Uff2_distance_array_backwards_voltage_only.push_back(U(2));
+ }
+ {
+ const double goal_velocity = trajectory.GetDVelocity(
+ distance, trajectory.max_dvelocity_forwards_voltage());
+ const double goal_acceleration = trajectory.GetDAcceleration(
+ distance, trajectory.max_dvelocity_forwards_voltage());
+ const ::Eigen::Matrix<double, 3, 1> omega_t =
+ trajectory.OmegaT(distance, goal_velocity);
+ const ::Eigen::Matrix<double, 3, 1> alpha_t =
+ trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+ const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+ const ::Eigen::Matrix<double, 3, 1> U =
+ follower.ComputeFF_U(R, omega_t, alpha_t).array().max(-20).min(20);
+
+ Uff0_distance_array_forwards_voltage_only.push_back(U(0));
+ Uff1_distance_array_forwards_voltage_only.push_back(U(1));
+ Uff2_distance_array_forwards_voltage_only.push_back(U(2));
+ }
+ }
+
+ double t = 0;
+ ::Eigen::Matrix<double, 4, 1> arm_X;
+ ::Eigen::Matrix<double, 2, 1> roll_X;
+ {
+ ::Eigen::Matrix<double, 3, 1> theta_t = trajectory.ThetaT(0.0);
+ arm_X << theta_t(0), 0.0, theta_t(1), 0.0;
+ roll_X << theta_t(2), 0.0;
+ }
+
+ ::std::vector<double> t_array;
+ ::std::vector<double> theta0_goal_t_array;
+ ::std::vector<double> theta1_goal_t_array;
+ ::std::vector<double> theta2_goal_t_array;
+ ::std::vector<double> omega0_goal_t_array;
+ ::std::vector<double> omega1_goal_t_array;
+ ::std::vector<double> omega2_goal_t_array;
+ ::std::vector<double> alpha0_goal_t_array;
+ ::std::vector<double> alpha1_goal_t_array;
+ ::std::vector<double> alpha2_goal_t_array;
+ ::std::vector<double> theta0_t_array;
+ ::std::vector<double> omega0_t_array;
+ ::std::vector<double> theta1_t_array;
+ ::std::vector<double> omega1_t_array;
+ ::std::vector<double> theta2_t_array;
+ ::std::vector<double> omega2_t_array;
+ ::std::vector<double> distance_t_array;
+ ::std::vector<double> velocity_t_array;
+ ::std::vector<double> acceleration_t_array;
+ ::std::vector<double> u0_unsaturated_array;
+ ::std::vector<double> u1_unsaturated_array;
+ ::std::vector<double> u2_unsaturated_array;
+ ::std::vector<double> alpha0_t_array;
+ ::std::vector<double> alpha1_t_array;
+ ::std::vector<double> alpha2_t_array;
+ ::std::vector<double> uff0_array;
+ ::std::vector<double> uff1_array;
+ ::std::vector<double> uff2_array;
+ ::std::vector<double> u0_array;
+ ::std::vector<double> u1_array;
+ ::std::vector<double> u2_array;
+ ::std::vector<double> theta0_hat_t_array;
+ ::std::vector<double> omega0_hat_t_array;
+ ::std::vector<double> theta1_hat_t_array;
+ ::std::vector<double> omega1_hat_t_array;
+ ::std::vector<double> theta2_hat_t_array;
+ ::std::vector<double> omega2_hat_t_array;
+ ::std::vector<double> torque0_hat_t_array;
+ ::std::vector<double> torque1_hat_t_array;
+ ::std::vector<double> torque2_hat_t_array;
+
+ // Now follow the trajectory.
+ frc971::control_loops::arm::EKF arm_ekf(&dynamics);
+ arm_ekf.Reset(arm_X);
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackPlant<3, 1, 1>,
+ StateFeedbackObserver<3, 1, 1>>
+ roll = superstructure::roll::MakeIntegralRollLoop();
+ roll.mutable_X_hat().setZero();
+ roll.mutable_X_hat().block<2, 1>(0, 0) = roll_X;
+
+ ::std::cout << "Reset P: " << arm_ekf.P_reset() << ::std::endl;
+ ::std::cout << "Stabilized P: " << arm_ekf.P_half_converged() << ::std::endl;
+ ::std::cout << "Really stabilized P: " << arm_ekf.P_converged()
+ << ::std::endl;
+
+ while (t < 1.2) {
+ t_array.push_back(t);
+ arm_ekf.Correct(
+ (::Eigen::Matrix<double, 2, 1>() << arm_X(0), arm_X(2)).finished(),
+ sim_dt);
+ roll.Correct((::Eigen::Matrix<double, 1, 1>() << roll_X(0)).finished());
+ follower.Update(
+ (Eigen::Matrix<double, 9, 1>() << arm_ekf.X_hat(), roll.X_hat())
+ .finished(),
+ false, sim_dt, vmax, 12.0);
+
+ const ::Eigen::Matrix<double, 3, 1> theta_t =
+ trajectory.ThetaT(follower.goal()(0));
+ const ::Eigen::Matrix<double, 3, 1> omega_t =
+ trajectory.OmegaT(follower.goal()(0), follower.goal()(1));
+ const ::Eigen::Matrix<double, 3, 1> alpha_t = trajectory.AlphaT(
+ follower.goal()(0), follower.goal()(1), follower.goal_acceleration());
+
+ theta0_goal_t_array.push_back(theta_t(0));
+ theta1_goal_t_array.push_back(theta_t(1));
+ theta2_goal_t_array.push_back(theta_t(2));
+ omega0_goal_t_array.push_back(omega_t(0));
+ omega1_goal_t_array.push_back(omega_t(1));
+ omega2_goal_t_array.push_back(omega_t(2));
+ alpha0_goal_t_array.push_back(alpha_t(0));
+ alpha1_goal_t_array.push_back(alpha_t(1));
+ alpha2_goal_t_array.push_back(alpha_t(2));
+ theta0_t_array.push_back(arm_X(0));
+ omega0_t_array.push_back(arm_X(1));
+ theta1_t_array.push_back(arm_X(2));
+ omega1_t_array.push_back(arm_X(3));
+ theta2_t_array.push_back(roll_X(0));
+ omega2_t_array.push_back(roll_X(1));
+ theta0_hat_t_array.push_back(arm_ekf.X_hat(0));
+ omega0_hat_t_array.push_back(arm_ekf.X_hat(1));
+ torque0_hat_t_array.push_back(arm_ekf.X_hat(4));
+ theta1_hat_t_array.push_back(arm_ekf.X_hat(2));
+ omega1_hat_t_array.push_back(arm_ekf.X_hat(3));
+ torque1_hat_t_array.push_back(arm_ekf.X_hat(5));
+
+ theta2_hat_t_array.push_back(roll.X_hat(0));
+ omega2_hat_t_array.push_back(roll.X_hat(1));
+ torque2_hat_t_array.push_back(roll.X_hat(2));
+
+ distance_t_array.push_back(follower.goal()(0));
+ velocity_t_array.push_back(follower.goal()(1));
+ acceleration_t_array.push_back(follower.goal_acceleration());
+
+ u0_unsaturated_array.push_back(follower.U_unsaturated()(0));
+ u1_unsaturated_array.push_back(follower.U_unsaturated()(1));
+ u2_unsaturated_array.push_back(follower.U_unsaturated()(2));
+
+ ::Eigen::Matrix<double, 3, 1> actual_U = follower.U();
+ // Add in a disturbance force to see how well the arm learns it.
+ // actual_U(0) += 1.0;
+
+ const ::Eigen::Matrix<double, 4, 1> arm_xdot =
+ dynamics.Acceleration(arm_X, actual_U.block<2, 1>(0, 0));
+ const ::Eigen::Matrix<double, 2, 1> roll_xdot =
+ hybrid_roll.plant().coefficients().A_continuous.block<2, 2>(0, 0) *
+ roll_X +
+ hybrid_roll.plant().coefficients().B_continuous.block<2, 1>(0, 0) *
+ actual_U.block<1, 1>(2, 0);
+
+ arm_X = dynamics.UnboundedDiscreteDynamics(
+ arm_X, actual_U.block<2, 1>(0, 0), sim_dt);
+ arm_ekf.Predict(follower.U().block<2, 1>(0, 0), sim_dt);
+ roll_X =
+ roll.plant()
+ .Update((Eigen::Matrix<double, 3, 1>() << roll_X, 0.0).finished(),
+ follower.U().block<1, 1>(2, 0))
+ .block<2, 1>(0, 0);
+ roll.UpdateObserver(follower.U().block<1, 1>(2, 0),
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<double>(sim_dt)));
+
+ alpha0_t_array.push_back(arm_xdot(1));
+ alpha1_t_array.push_back(arm_xdot(3));
+ alpha2_t_array.push_back(roll_xdot(1));
+
+ uff0_array.push_back(follower.U_ff()(0));
+ uff1_array.push_back(follower.U_ff()(1));
+ uff2_array.push_back(follower.U_ff()(2));
+ u0_array.push_back(follower.U()(0));
+ u1_array.push_back(follower.U()(1));
+ u2_array.push_back(follower.U()(2));
+
+ t += sim_dt;
+ }
+
+ if (FLAGS_plot) {
+ frc971::analysis::Plotter plotter;
+
+ plotter.AddFigure();
+ plotter.Title("Input spline");
+ plotter.AddLine(distance_array, theta0_array, "theta0");
+ plotter.AddLine(distance_array, theta1_array, "theta1");
+ plotter.AddLine(distance_array, theta2_array, "theta2");
+ plotter.AddLine(distance_array, omega0_array, "omega0");
+ plotter.AddLine(distance_array, omega1_array, "omega1");
+ plotter.AddLine(distance_array, omega2_array, "omega2");
+ plotter.AddLine(distance_array, alpha0_array, "alpha0");
+ plotter.AddLine(distance_array, alpha1_array, "alpha1");
+ plotter.AddLine(distance_array, alpha2_array, "alpha2");
+
+ plotter.AddLine(integrated_distance, integrated_theta0_array,
+ "integrated theta0");
+ plotter.AddLine(integrated_distance, integrated_theta1_array,
+ "integrated theta1");
+ plotter.AddLine(integrated_distance, integrated_theta2_array,
+ "integrated theta2");
+ plotter.AddLine(integrated_distance, integrated_omega0_array,
+ "integrated omega0");
+ plotter.AddLine(integrated_distance, integrated_omega1_array,
+ "integrated omega1");
+ plotter.AddLine(integrated_distance, integrated_omega2_array,
+ "integrated omega2");
+ plotter.Publish();
+
+ plotter.AddFigure();
+ plotter.Title("Trajectory");
+ plotter.AddLine(theta0_array, theta1_array, "desired path");
+ plotter.AddLine(theta0_t_array, theta1_t_array, "actual path");
+ plotter.Publish();
+
+ plotter.AddFigure();
+ plotter.Title("Solver passes");
+ plotter.AddLine(distance_array, trajectory.max_dvelocity_unfiltered(),
+ "pass0");
+ plotter.AddLine(distance_array, trajectory.max_dvelocity_backward_accel(),
+ "passb accel");
+ plotter.AddLine(distance_array, trajectory.max_dvelocity_forwards_accel(),
+ "passf accel");
+ plotter.AddLine(distance_array, trajectory.max_dvelocity_backward_voltage(),
+ "passb voltage");
+ plotter.AddLine(distance_array, trajectory.max_dvelocity_forwards_voltage(),
+ "passf voltage");
+ plotter.Publish();
+
+ plotter.AddFigure();
+ plotter.Title("Time Goals");
+ plotter.AddLine(t_array, alpha0_goal_t_array, "alpha0_t_goal");
+ plotter.AddLine(t_array, alpha0_t_array, "alpha0_t");
+ plotter.AddLine(t_array, alpha1_goal_t_array, "alpha1_t_goal");
+ plotter.AddLine(t_array, alpha1_t_array, "alpha1_t");
+ plotter.AddLine(t_array, alpha2_goal_t_array, "alpha2_t_goal");
+ plotter.AddLine(t_array, alpha2_t_array, "alpha2_t");
+ plotter.AddLine(t_array, distance_t_array, "distance_t");
+ plotter.AddLine(t_array, velocity_t_array, "velocity_t");
+ plotter.AddLine(t_array, acceleration_t_array, "acceleration_t");
+ plotter.Publish();
+
+ plotter.AddFigure();
+ plotter.Title("Angular Velocities");
+ plotter.AddLine(t_array, omega0_goal_t_array, "omega0_t_goal");
+ plotter.AddLine(t_array, omega0_t_array, "omega0_t");
+ plotter.AddLine(t_array, omega0_hat_t_array, "omega0_hat_t");
+
+ plotter.AddLine(t_array, omega1_goal_t_array, "omega1_t_goal");
+ plotter.AddLine(t_array, omega1_t_array, "omega1_t");
+ plotter.AddLine(t_array, omega1_hat_t_array, "omega1_hat_t");
+
+ plotter.AddLine(t_array, omega2_goal_t_array, "omega2_t_goal");
+ plotter.AddLine(t_array, omega2_t_array, "omega2_t");
+ plotter.AddLine(t_array, omega2_hat_t_array, "omega2_hat_t");
+ plotter.Publish();
+
+ plotter.AddFigure();
+ plotter.Title("Voltages");
+ plotter.AddLine(t_array, u0_unsaturated_array, "u0_full");
+ plotter.AddLine(t_array, u0_array, "u0");
+ plotter.AddLine(t_array, uff0_array, "uff0");
+ plotter.AddLine(t_array, u1_unsaturated_array, "u1_full");
+ plotter.AddLine(t_array, u1_array, "u1");
+ plotter.AddLine(t_array, uff1_array, "uff1");
+ plotter.AddLine(t_array, u2_unsaturated_array, "u2_full");
+ plotter.AddLine(t_array, u2_array, "u2");
+ plotter.AddLine(t_array, uff2_array, "uff2");
+ plotter.AddLine(t_array, torque0_hat_t_array, "torque0_hat");
+ plotter.AddLine(t_array, torque1_hat_t_array, "torque1_hat");
+ plotter.AddLine(t_array, torque2_hat_t_array, "torque2_hat");
+ plotter.Publish();
+
+ if (FLAGS_plot_thetas) {
+ plotter.AddFigure();
+ plotter.Title("Angles");
+ plotter.AddLine(t_array, theta0_goal_t_array, "theta0_t_goal");
+ plotter.AddLine(t_array, theta0_t_array, "theta0_t");
+ plotter.AddLine(t_array, theta0_hat_t_array, "theta0_hat_t");
+ plotter.AddLine(t_array, theta1_goal_t_array, "theta1_t_goal");
+ plotter.AddLine(t_array, theta1_t_array, "theta1_t");
+ plotter.AddLine(t_array, theta1_hat_t_array, "theta1_hat_t");
+ plotter.AddLine(t_array, theta2_goal_t_array, "theta2_t_goal");
+ plotter.AddLine(t_array, theta2_t_array, "theta2_t");
+ plotter.AddLine(t_array, theta2_hat_t_array, "theta2_hat_t");
+ plotter.Publish();
+ }
+
+ plotter.AddFigure();
+ plotter.Title("ff for distance");
+ plotter.AddLine(distance_array, Uff0_distance_array_forwards_voltage_only,
+ "ff0");
+ plotter.AddLine(distance_array, Uff1_distance_array_forwards_voltage_only,
+ "ff1");
+ plotter.AddLine(distance_array, Uff2_distance_array_forwards_voltage_only,
+ "ff2");
+
+ plotter.AddLine(distance_array, Uff0_distance_array_backwards_voltage_only,
+ "ff0_back voltage");
+ plotter.AddLine(distance_array, Uff1_distance_array_backwards_voltage_only,
+ "ff1_back voltage");
+ plotter.AddLine(distance_array, Uff2_distance_array_backwards_voltage_only,
+ "ff2_back voltage");
+
+ plotter.AddLine(distance_array, Uff0_distance_array_forwards_accel_only,
+ "ff0_forward accel");
+ plotter.AddLine(distance_array, Uff1_distance_array_forwards_accel_only,
+ "ff1_forward accel");
+ plotter.AddLine(distance_array, Uff2_distance_array_forwards_accel_only,
+ "ff2_forward accel");
+
+ plotter.AddLine(distance_array, Uff0_distance_array_backwards_accel_only,
+ "ff0_back accel");
+ plotter.AddLine(distance_array, Uff1_distance_array_backwards_accel_only,
+ "ff1_back accel");
+ plotter.AddLine(distance_array, Uff2_distance_array_backwards_accel_only,
+ "ff2_back accel");
+
+ plotter.AddLine(distance_array, Uff0_distance_array_curvature, "ff0_curve");
+ plotter.AddLine(distance_array, Uff1_distance_array_curvature, "ff1_curve");
+ plotter.AddLine(distance_array, Uff2_distance_array_curvature, "ff2_curve");
+
+ plotter.Publish();
+ plotter.Spin();
+ }
+}
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+ ::y2023::control_loops::superstructure::arm::Main();
+ return 0;
+}
diff --git a/y2023/control_loops/superstructure/arm/trajectory_test.cc b/y2023/control_loops/superstructure/arm/trajectory_test.cc
new file mode 100644
index 0000000..bcf338c
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/trajectory_test.cc
@@ -0,0 +1,232 @@
+#include "y2023/control_loops/superstructure/arm/trajectory.h"
+
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
+#include "gtest/gtest.h"
+#include "y2023/control_loops/superstructure/arm/arm_constants.h"
+#include "y2023/control_loops/superstructure/roll/integral_hybrid_roll_plant.h"
+#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+namespace testing {
+
+using frc971::control_loops::MatrixGaussianQuadrature5;
+using frc971::control_loops::arm::Dynamics;
+using frc971::control_loops::arm::EKF;
+
+// Tests that we can pull out values along the path.
+TEST(TrajectoryTest, Theta) {
+ Eigen::Matrix<double, 2, 4> spline_params;
+ spline_params << 0.3, 0.4, 0.6, 0.5, -1.7, -1.6, -1.4, -1.2;
+
+ NSpline<4, 2> spline(spline_params);
+ CosSpline cos_spline(spline,
+ {{0.0, 0.1}, {0.3, 0.1}, {0.7, 0.2}, {1.0, 0.2}});
+
+ EXPECT_TRUE(cos_spline.Theta(-1.0).isApprox(
+ (Eigen::Matrix<double, 3, 1>() << 0.3, -1.7, 0.1).finished()))
+ << cos_spline.Theta(-1.0).transpose();
+
+ EXPECT_TRUE(cos_spline.Theta(0.0).isApprox(
+ (Eigen::Matrix<double, 3, 1>() << 0.3, -1.7, 0.1).finished()))
+ << cos_spline.Theta(0.0).transpose();
+
+ EXPECT_TRUE(cos_spline.Theta(0.3).isApprox(
+ (Eigen::Matrix<double, 3, 1>() << 0.4062, -1.5857, 0.1).finished()))
+ << cos_spline.Theta(0.3).transpose();
+
+ EXPECT_TRUE(cos_spline.Theta(0.5).isApprox(
+ (Eigen::Matrix<double, 3, 1>() << 0.475, -1.4875, 0.15).finished()))
+ << cos_spline.Theta(0.5).transpose();
+
+ EXPECT_TRUE(cos_spline.Theta(0.7).isApprox(
+ (Eigen::Matrix<double, 3, 1>() << 0.5198, -1.3773, 0.2).finished()))
+ << cos_spline.Theta(0.7).transpose();
+
+ EXPECT_TRUE(cos_spline.Theta(1.0).isApprox(
+ (Eigen::Matrix<double, 3, 1>() << 0.5, -1.2, 0.2).finished()))
+ << cos_spline.Theta(1.0).transpose();
+
+ EXPECT_TRUE(cos_spline.Theta(2.0).isApprox(
+ (Eigen::Matrix<double, 3, 1>() << 0.5, -1.2, 0.2).finished()))
+ << cos_spline.Theta(2.0).transpose();
+}
+
+// Tests that the integral of alpha and omega matches the functions they were
+// differentiated from on Path.
+TEST(TrajectoryTest, IntegrateAccel) {
+ Eigen::Matrix<double, 2, 4> spline_params;
+ spline_params << 0.3, 0.4, 0.6, 0.5, -1.7, -1.6, -1.4, -1.2;
+ NSpline<4, 2> spline(spline_params);
+ CosSpline cos_spline(spline,
+ {{0.0, 0.1}, {0.3, 0.1}, {0.7, 0.2}, {1.0, 0.2}});
+ Path distance_spline(cos_spline, 100);
+
+ Eigen::Matrix<double, 3, 1> integrated_theta = distance_spline.Theta(0.0);
+ Eigen::Matrix<double, 3, 1> integrated_omega = distance_spline.Omega(0.0);
+
+ constexpr size_t kSlices = 1000;
+ for (size_t i = 0; i < kSlices; ++i) {
+ const double d = i * distance_spline.length() / kSlices;
+ const double next_d = (i + 1) * distance_spline.length() / kSlices;
+
+ integrated_theta += MatrixGaussianQuadrature5<3>(
+ [&](double distance) { return distance_spline.Omega(distance); }, d,
+ next_d);
+ integrated_omega += MatrixGaussianQuadrature5<3>(
+ [&](double distance) { return distance_spline.Alpha(distance); }, d,
+ next_d);
+
+ EXPECT_TRUE(integrated_theta.isApprox(distance_spline.Theta(next_d), 1e-3))
+ << ": Got " << integrated_theta.transpose() << ", expected "
+ << distance_spline.Theta(next_d).transpose();
+ EXPECT_TRUE(integrated_omega.isApprox(distance_spline.Omega(next_d), 1e-3))
+ << ": Got " << integrated_omega.transpose() << ", expected "
+ << distance_spline.Omega(next_d).transpose();
+ }
+}
+
+// Tests that we can correctly interpolate velocities between two points
+TEST(TrajectoryTest, InterpolateVelocity) {
+ // x = 0.5 * a * t^2
+ // v = a * t
+ // a = 2.0
+ EXPECT_EQ(0.0, Trajectory::InterpolateVelocity(0.0, 0.0, 1.0, 0.0, 2.0));
+ EXPECT_EQ(2.0, Trajectory::InterpolateVelocity(1.0, 0.0, 1.0, 0.0, 2.0));
+ EXPECT_EQ(0.0, Trajectory::InterpolateVelocity(-1.0, 0.0, 1.0, 0.0, 2.0));
+ EXPECT_EQ(2.0, Trajectory::InterpolateVelocity(20.0, 0.0, 1.0, 0.0, 2.0));
+}
+
+// Tests that we can correctly interpolate velocities between two points
+TEST(TrajectoryTest, InterpolateAcceleration) {
+ // x = 0.5 * a * t^2
+ // v = a * t
+ // a = 2.0
+ EXPECT_EQ(2.0, Trajectory::InterpolateAcceleration(0.0, 1.0, 0.0, 2.0));
+}
+
+std::unique_ptr<Path> MakeDemoPath() {
+ Eigen::Matrix<double, 2, 4> spline_params;
+ spline_params << 0.3, 0.4, 0.6, 0.5, -1.7, -1.6, -1.4, -1.2;
+ NSpline<4, 2> spline(spline_params);
+ CosSpline cos_spline(spline,
+ {{0.0, 0.1}, {0.3, 0.1}, {0.7, 0.2}, {1.0, 0.2}});
+ return std::make_unique<Path>(cos_spline, 100);
+}
+
+// Tests that we can correctly interpolate velocities between two points
+TEST(TrajectoryTest, ReversedPath) {
+ // Tests that a reversed path is actually reversed.
+ ::std::unique_ptr<Path> path = MakeDemoPath();
+ ::std::unique_ptr<Path> reversed_path = Path::Reversed(MakeDemoPath());
+
+ EXPECT_NEAR(path->length(), reversed_path->length(), 1e-6);
+
+ for (double d = 0; d < path->length(); d += 0.01) {
+ EXPECT_LT(
+ (path->Theta(d) - reversed_path->Theta(path->length() - d)).norm(),
+ 1e-5);
+ EXPECT_LT(
+ (path->Omega(d) + reversed_path->Omega(path->length() - d)).norm(),
+ 1e-5);
+ EXPECT_LT(
+ (path->Alpha(d) - reversed_path->Alpha(path->length() - d)).norm(),
+ 1e-5);
+ }
+}
+
+// Tests that we can follow a path. Look at :trajectory_plot if you want to see
+// the path.
+TEST(TrajectoryTest, RunTrajectory) {
+ Dynamics dynamics(kArmConstants);
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>
+ hybrid_roll = y2023::control_loops::superstructure::roll::
+ MakeIntegralHybridRollLoop();
+ ::std::unique_ptr<Path> path = MakeDemoPath();
+ Trajectory trajectory(&dynamics, &hybrid_roll.plant(), ::std::move(path),
+ 0.001);
+
+ constexpr double kAlpha0Max = 40.0;
+ constexpr double kAlpha1Max = 60.0;
+ constexpr double kAlpha2Max = 60.0;
+ constexpr double vmax = 11.95;
+
+ const ::Eigen::DiagonalMatrix<double, 3> alpha_unitizer(
+ (::Eigen::DiagonalMatrix<double, 3>().diagonal() << (1.0 / kAlpha0Max),
+ (1.0 / kAlpha1Max), (1.0 / kAlpha2Max))
+ .finished());
+ trajectory.OptimizeTrajectory(alpha_unitizer, vmax);
+
+ double t = 0;
+ ::Eigen::Matrix<double, 4, 1> arm_X;
+ ::Eigen::Matrix<double, 2, 1> roll_X;
+ {
+ ::Eigen::Matrix<double, 3, 1> theta_t = trajectory.ThetaT(0.0);
+ arm_X << theta_t(0), 0.0, theta_t(1), 0.0;
+ roll_X << theta_t(2), 0.0;
+ }
+
+ EKF arm_ekf(&dynamics);
+ arm_ekf.Reset(arm_X);
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackPlant<3, 1, 1>,
+ StateFeedbackObserver<3, 1, 1>>
+ roll = y2023::control_loops::superstructure::roll::MakeIntegralRollLoop();
+ roll.mutable_X_hat().setZero();
+ roll.mutable_X_hat().block<2, 1>(0, 0) = roll_X;
+
+ TrajectoryFollower follower(&dynamics, &hybrid_roll, &trajectory);
+ constexpr double sim_dt = 0.00505;
+ while (t < 1.0) {
+ arm_ekf.Correct(
+ (::Eigen::Matrix<double, 2, 1>() << arm_X(0), arm_X(2)).finished(),
+ sim_dt);
+ roll.Correct((::Eigen::Matrix<double, 1, 1>() << roll_X(0)).finished());
+ follower.Update(
+ (Eigen::Matrix<double, 9, 1>() << arm_ekf.X_hat(), roll.X_hat())
+ .finished(),
+ false, sim_dt, vmax, 12.0);
+
+ arm_X = dynamics.UnboundedDiscreteDynamics(
+ arm_X, follower.U().block<2, 1>(0, 0), sim_dt);
+ arm_ekf.Predict(follower.U().block<2, 1>(0, 0), sim_dt);
+
+ roll_X =
+ roll.plant()
+ .Update((Eigen::Matrix<double, 3, 1>() << roll_X, 0.0).finished(),
+ follower.U().block<1, 1>(2, 0))
+ .block<2, 1>(0, 0);
+ roll.UpdateObserver(follower.U().block<1, 1>(2, 0),
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<double>(sim_dt)));
+ t += sim_dt;
+ }
+
+ ::Eigen::Matrix<double, 4, 1> final_arm_X;
+ ::Eigen::Matrix<double, 2, 1> final_roll_X;
+ ::Eigen::Matrix<double, 3, 1> final_theta_t =
+ trajectory.ThetaT(trajectory.path().length());
+ final_arm_X << final_theta_t(0), 0.0, final_theta_t(1), 0.0;
+ final_roll_X << final_theta_t(2), 0.0;
+
+ // Verify that we got to the end.
+ EXPECT_TRUE(arm_X.isApprox(final_arm_X, 0.01))
+ << ": X is " << arm_X.transpose() << " final_X is "
+ << final_arm_X.transpose();
+ EXPECT_TRUE(roll_X.isApprox(final_roll_X, 0.01))
+ << ": X is " << roll_X.transpose() << " final_X is "
+ << final_roll_X.transpose();
+
+ // Verify that our goal is at the end.
+ EXPECT_TRUE(
+ final_theta_t.isApprox(trajectory.path().Theta(follower.goal(0))));
+}
+
+} // namespace testing
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023