Refactor year-agnostic catapult code to frc971
This breaks up the y2022 catapult code into multiple files and moves
them into the frc971 folder. Year-specific parameters are now
provided via the constructors, and the goal message is moved into
frc971 as well.
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: I4ea720ae62a7c6c229d6c24a1f08edd7bc5b9728
diff --git a/frc971/control_loops/catapult/BUILD b/frc971/control_loops/catapult/BUILD
new file mode 100644
index 0000000..1235044
--- /dev/null
+++ b/frc971/control_loops/catapult/BUILD
@@ -0,0 +1,86 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+
+package(default_visibility = ["//visibility:public"])
+
+static_flatbuffer(
+ name = "catapult_goal_fbs",
+ srcs = [
+ "catapult_goal.fbs",
+ ],
+ deps = [
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ ],
+)
+
+cc_library(
+ name = "catapult_controller",
+ srcs =
+ [
+ "catapult_controller.cc",
+ ],
+ hdrs =
+ [
+ "catapult_controller.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps =
+ [
+ ":mpc_problem_generator",
+ ],
+)
+
+cc_library(
+ name = "mpc_problem",
+ srcs =
+ [
+ "mpc_problem.cc",
+ ],
+ hdrs =
+ [
+ "mpc_problem.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps =
+ [
+ "//aos:realtime",
+ "//aos/time",
+ "//third_party/osqp-cpp",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "mpc_problem_generator",
+ srcs =
+ [
+ "mpc_problem_generator.cc",
+ ],
+ hdrs =
+ [
+ "mpc_problem_generator.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps =
+ [
+ ":mpc_problem",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
+
+cc_library(
+ name = "catapult",
+ srcs = [
+ "catapult.cc",
+ ],
+ hdrs =
+ [
+ "catapult.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":catapult_controller",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//frc971/control_loops/catapult:catapult_goal_fbs",
+ "//frc971/zeroing:pot_and_absolute_encoder",
+ ],
+)
diff --git a/frc971/control_loops/catapult/catapult.cc b/frc971/control_loops/catapult/catapult.cc
new file mode 100644
index 0000000..2e1867c
--- /dev/null
+++ b/frc971/control_loops/catapult/catapult.cc
@@ -0,0 +1,138 @@
+#include "frc971/control_loops/catapult/catapult.h"
+
+namespace frc971::control_loops::catapult {
+
+const flatbuffers::Offset<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+Catapult::Iterate(const CatapultGoal *catapult_goal,
+ const PotAndAbsolutePosition *position,
+ double battery_voltage, double *catapult_voltage, bool fire,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *return_goal =
+ catapult_goal != nullptr && catapult_goal->has_return_position()
+ ? catapult_goal->return_position()
+ : nullptr;
+
+ const bool catapult_disabled =
+ catapult_.Correct(return_goal, position, catapult_voltage == nullptr);
+
+ if (catapult_disabled) {
+ catapult_state_ = CatapultState::PROFILE;
+ } else if (catapult_.running() && catapult_goal != nullptr && fire &&
+ !last_firing_) {
+ catapult_state_ = CatapultState::FIRING;
+ latched_shot_position = catapult_goal->shot_position();
+ latched_shot_velocity = catapult_goal->shot_velocity();
+ }
+
+ // Don't update last_firing_ if the catapult is disabled, so that we actually
+ // end up firing once it's enabled
+ if (catapult_.running() && !catapult_disabled) {
+ last_firing_ = fire;
+ }
+
+ use_profile_ = true;
+
+ switch (catapult_state_) {
+ case CatapultState::FIRING: {
+ // Select the ball controller. We should only be firing if we have a
+ // ball, or at least should only care about the shot accuracy.
+ catapult_.set_controller_index(0);
+ // Ok, so we've now corrected. Next step is to run the MPC.
+ //
+ // Since there is a unit delay between when we ask for a U and the
+ // hardware applies it, we need to run the optimizer for the position at
+ // the *next* control loop cycle.
+
+ Eigen::Vector3d next_X = catapult_.estimated_state();
+ for (int i = catapult_.controller().plant().coefficients().delayed_u;
+ i > 1; --i) {
+ next_X = catapult_.controller().plant().A() * next_X +
+ catapult_.controller().plant().B() *
+ catapult_.controller().observer().last_U(i - 1);
+ }
+
+ catapult_mpc_.SetState(
+ next_X.block<2, 1>(0, 0),
+ Eigen::Vector2d(latched_shot_position, latched_shot_velocity));
+
+ const bool solved = catapult_mpc_.Solve();
+ current_horizon_ = catapult_mpc_.current_horizon();
+ const bool started = catapult_mpc_.started();
+ if (solved || started) {
+ std::optional<double> solution = catapult_mpc_.Next();
+
+ if (!solution.has_value()) {
+ CHECK_NOTNULL(catapult_voltage);
+ *catapult_voltage = 0.0;
+ if (catapult_mpc_.started()) {
+ ++shot_count_;
+ // Finished the catapult, time to fire.
+ catapult_state_ = CatapultState::RESETTING;
+ }
+ } else {
+ // TODO(austin): Voltage error?
+ CHECK_NOTNULL(catapult_voltage);
+ if (current_horizon_ == 1) {
+ battery_voltage = 12.0;
+ }
+ *catapult_voltage = std::max(
+ 0.0, std::min(12.0, (*solution - 0.0 * next_X(2, 0)) * 12.0 /
+ std::max(battery_voltage, 8.0)));
+ use_profile_ = false;
+ }
+ } else {
+ if (!fire) {
+ // Eh, didn't manage to solve before it was time to fire. Give up.
+ catapult_state_ = CatapultState::PROFILE;
+ }
+ }
+
+ if (!use_profile_) {
+ catapult_.ForceGoal(catapult_.estimated_position(),
+ catapult_.estimated_velocity());
+ }
+ }
+ if (catapult_state_ != CatapultState::RESETTING) {
+ break;
+ } else {
+ [[fallthrough]];
+ }
+
+ case CatapultState::RESETTING:
+ if (catapult_.controller().R(1, 0) > 7.0) {
+ catapult_.AdjustProfile(7.0, 2000.0);
+ } else if (catapult_.controller().R(1, 0) > 0.0) {
+ catapult_.AdjustProfile(7.0, 1000.0);
+ } else {
+ catapult_state_ = CatapultState::PROFILE;
+ }
+ [[fallthrough]];
+
+ case CatapultState::PROFILE:
+ break;
+ }
+
+ if (use_profile_) {
+ if (catapult_state_ != CatapultState::FIRING) {
+ catapult_mpc_.Reset();
+ }
+ // Select the controller designed for when we have no ball.
+ catapult_.set_controller_index(1);
+
+ current_horizon_ = 0u;
+ const double output_voltage = catapult_.UpdateController(catapult_disabled);
+ if (catapult_voltage != nullptr) {
+ *catapult_voltage = output_voltage;
+ }
+ }
+
+ catapult_.UpdateObserver(catapult_voltage != nullptr
+ ? (*catapult_voltage * battery_voltage / 12.0)
+ : 0.0);
+
+ return catapult_.MakeStatus(fbb);
+}
+
+} // namespace frc971::control_loops::catapult
\ No newline at end of file
diff --git a/frc971/control_loops/catapult/catapult.h b/frc971/control_loops/catapult/catapult.h
new file mode 100644
index 0000000..e84ea88
--- /dev/null
+++ b/frc971/control_loops/catapult/catapult.h
@@ -0,0 +1,79 @@
+#ifndef FRC971_CONTROL_LOOPS_CATAPULT_CATAPULT_H_
+#define FRC971_CONTROL_LOOPS_CATAPULT_CATAPULT_H_
+
+#include "frc971/control_loops/catapult/catapult_controller.h"
+#include "frc971/control_loops/catapult/catapult_goal_generated.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace catapult {
+
+// Class to handle transitioning between both the profiled subsystem and the MPC
+// for shooting.
+class Catapult {
+ public:
+ Catapult(frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ catapult_params,
+ StateFeedbackPlant<2, 1, 1> plant)
+ : catapult_(catapult_params), catapult_mpc_(std::move(plant), 30) {}
+
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
+ // Resets all state for when WPILib restarts.
+ void Reset() { catapult_.Reset(); }
+
+ void Estop() { catapult_.Estop(); }
+
+ bool zeroed() const { return catapult_.zeroed(); }
+ bool estopped() const { return catapult_.estopped(); }
+ double solve_time() const { return catapult_mpc_.solve_time(); }
+
+ uint8_t mpc_horizon() const { return current_horizon_; }
+
+ bool mpc_active() const { return !use_profile_; }
+
+ // Returns the number of shots taken.
+ int shot_count() const { return shot_count_; }
+
+ // Returns the estimated position
+ double estimated_position() const { return catapult_.estimated_position(); }
+
+ // Runs either the MPC or the profiled subsystem depending on if we are
+ // shooting or not. Returns the status.
+ const flatbuffers::Offset<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+ Iterate(const CatapultGoal *catapult_goal,
+ const PotAndAbsolutePosition *position, double battery_voltage,
+ double *catapult_voltage, bool fire,
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ private:
+ PotAndAbsoluteEncoderSubsystem catapult_;
+
+ frc971::control_loops::catapult::CatapultController catapult_mpc_;
+
+ enum CatapultState { PROFILE, FIRING, RESETTING };
+
+ CatapultState catapult_state_ = CatapultState::PROFILE;
+
+ double latched_shot_position = 0.0;
+ double latched_shot_velocity = 0.0;
+
+ bool last_firing_ = false;
+ bool use_profile_ = true;
+
+ int shot_count_ = 0;
+ uint8_t current_horizon_ = 0u;
+};
+
+} // namespace catapult
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_CATAPULT_CATAPULT_H_
diff --git a/frc971/control_loops/catapult/catapult_controller.cc b/frc971/control_loops/catapult/catapult_controller.cc
new file mode 100644
index 0000000..a4187f7
--- /dev/null
+++ b/frc971/control_loops/catapult/catapult_controller.cc
@@ -0,0 +1,62 @@
+#include "frc971/control_loops/catapult/catapult_controller.h"
+
+namespace frc971::control_loops::catapult {
+
+CatapultController::CatapultController(StateFeedbackPlant<2, 1, 1> plant,
+ size_t horizon)
+ : generator_(std::move(plant), horizon) {
+ problems_.reserve(generator_.horizon());
+ for (size_t i = generator_.horizon(); i > 0; --i) {
+ problems_.emplace_back(generator_.MakeProblem(i));
+ }
+
+ Reset();
+}
+
+void CatapultController::Reset() {
+ current_controller_ = 0;
+ solve_time_ = 0.0;
+}
+
+void CatapultController::SetState(Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final) {
+ if (current_controller_ >= problems_.size()) {
+ return;
+ }
+ problems_[current_controller_]->SetState(X_initial, X_final);
+}
+
+bool CatapultController::Solve() {
+ if (current_controller_ >= problems_.size()) {
+ return true;
+ }
+ const bool result = problems_[current_controller_]->Solve();
+ solve_time_ = problems_[current_controller_]->solve_time();
+ return result;
+}
+
+std::optional<double> CatapultController::Next() {
+ if (current_controller_ >= problems_.size()) {
+ return std::nullopt;
+ }
+
+ double u;
+ size_t solution_number = 0;
+ if (current_controller_ == 0u) {
+ while (solution_number < problems_[current_controller_]->horizon() &&
+ problems_[current_controller_]->U(solution_number) < 0.01) {
+ u = problems_[current_controller_]->U(solution_number);
+ ++solution_number;
+ }
+ }
+ u = problems_[current_controller_]->U(solution_number);
+
+ if (current_controller_ + 1u + solution_number < problems_.size()) {
+ problems_[current_controller_ + solution_number + 1]->WarmStart(
+ *problems_[current_controller_]);
+ }
+ current_controller_ += 1u + solution_number;
+ return u;
+}
+
+} // namespace frc971::control_loops::catapult
\ No newline at end of file
diff --git a/frc971/control_loops/catapult/catapult_controller.h b/frc971/control_loops/catapult/catapult_controller.h
new file mode 100644
index 0000000..3a666f9
--- /dev/null
+++ b/frc971/control_loops/catapult/catapult_controller.h
@@ -0,0 +1,65 @@
+#include "frc971/control_loops/catapult/mpc_problem_generator.h"
+// A class to hold all the state needed to manage the catapult MPC solvers for
+// repeated shots.
+//
+// The solver may take a couple of cycles to get everything converged and ready.
+// The flow is as follows:
+// 1) Reset() the state for the new problem.
+// 2) Update to the current state with SetState()
+// 3) Call Solve(). This will return true if it is ready to be executed, false
+// if it needs more iterations to fully converge.
+// 4) Next() returns the current optimal control output and advances the
+// pointers to the next problem.
+// 5) Go back to 2 for the next cycle.
+
+namespace frc971 {
+namespace control_loops {
+namespace catapult {
+
+class CatapultController {
+ public:
+ CatapultController(StateFeedbackPlant<2, 1, 1> plant, size_t horizon);
+
+ // Starts back over at the first controller.
+ void Reset();
+
+ // Updates our current and final states for the current controller.
+ void SetState(Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final);
+
+ // Solves! Returns true if the solution converged and osqp was happy.
+ bool Solve();
+
+ // Returns the time in seconds it last took Solve to run.
+ double solve_time() const { return solve_time_; }
+
+ // Returns the horizon of the current controller.
+ size_t current_horizon() const {
+ if (current_controller_ >= problems_.size()) {
+ return 0u;
+ } else {
+ return problems_[current_controller_]->horizon();
+ }
+ }
+
+ // Returns the controller value if there is a controller to run, or nullopt if
+ // we finished the last controller. Advances the controller pointer to the
+ // next controller and warms up the next controller.
+ std::optional<double> Next();
+
+ // Returns true if Next has been called and a controller has been used. Reset
+ // starts over.
+ bool started() const { return current_controller_ != 0u; }
+
+ private:
+ CatapultProblemGenerator generator_;
+
+ std::vector<std::unique_ptr<MPCProblem>> problems_;
+
+ size_t current_controller_ = 0;
+ double solve_time_ = 0.0;
+};
+
+} // namespace catapult
+} // namespace control_loops
+} // namespace frc971
\ No newline at end of file
diff --git a/frc971/control_loops/catapult/catapult_goal.fbs b/frc971/control_loops/catapult/catapult_goal.fbs
new file mode 100644
index 0000000..3e59455
--- /dev/null
+++ b/frc971/control_loops/catapult/catapult_goal.fbs
@@ -0,0 +1,17 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace frc971.control_loops.catapult;
+
+table CatapultGoal {
+ // Old fire flag, only kept for backwards-compatability with logs.
+ // Use the fire flag in the root Goal instead
+ fire:bool (id: 0, deprecated);
+
+ // The target shot position and velocity. If these are provided before fire
+ // is called, the optimizer can pre-compute the trajectory.
+ shot_position:double (id: 1);
+ shot_velocity:double (id: 2);
+
+ // The target position to return the catapult to when not shooting.
+ return_position:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);
+}
\ No newline at end of file
diff --git a/y2022/control_loops/superstructure/catapult/mpc.tex b/frc971/control_loops/catapult/mpc.tex
similarity index 100%
rename from y2022/control_loops/superstructure/catapult/mpc.tex
rename to frc971/control_loops/catapult/mpc.tex
diff --git a/frc971/control_loops/catapult/mpc_problem.cc b/frc971/control_loops/catapult/mpc_problem.cc
new file mode 100644
index 0000000..23859cd
--- /dev/null
+++ b/frc971/control_loops/catapult/mpc_problem.cc
@@ -0,0 +1,95 @@
+#include "frc971/control_loops/catapult/mpc_problem.h"
+
+namespace frc971::control_loops::catapult {
+namespace chrono = std::chrono;
+
+namespace {
+osqp::OsqpInstance MakeInstance(
+ size_t horizon, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P) {
+ osqp::OsqpInstance instance;
+ instance.objective_matrix = P.sparseView();
+
+ instance.constraint_matrix =
+ Eigen::SparseMatrix<double, Eigen::ColMajor, osqp::c_int>(horizon,
+ horizon);
+ instance.constraint_matrix.setIdentity();
+
+ instance.lower_bounds =
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(horizon, 1);
+ instance.upper_bounds =
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Ones(horizon, 1) * 12.0;
+ return instance;
+}
+
+} // namespace
+MPCProblem::MPCProblem(size_t horizon,
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P,
+ Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q,
+ Eigen::Matrix<double, 2, 2> Af,
+ Eigen::Matrix<double, Eigen::Dynamic, 2> final_q)
+ : horizon_(horizon),
+ accel_q_(std::move(accel_q)),
+ Af_(std::move(Af)),
+ final_q_(std::move(final_q)),
+ instance_(MakeInstance(horizon, std::move(P))) {
+ // Start with a representative problem.
+ Eigen::Matrix<double, 2, 1> X_initial(0.0, 0.0);
+ Eigen::Matrix<double, 2, 1> X_final(2.0, 25.0);
+
+ objective_vector_ =
+ X_initial(1, 0) * accel_q_ + final_q_ * (Af_ * X_initial - X_final);
+ instance_.objective_vector = objective_vector_;
+ settings_.max_iter = 25;
+ settings_.check_termination = 5;
+ settings_.warm_start = 1;
+ // TODO(austin): Do we need this scaling thing? It makes it not solve
+ // sometimes... I'm pretty certain by giving it a decently formed problem to
+ // initialize with, it will not try doing crazy things with the scaling
+ // internally.
+ settings_.scaling = 0;
+ auto status = solver_.Init(instance_, settings_);
+ CHECK(status.ok()) << status;
+}
+
+void MPCProblem::SetState(Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final) {
+ X_initial_ = X_initial;
+ X_final_ = X_final;
+ // If we mark this noalias(), it won't re-allocate the vector each time.
+ objective_vector_.noalias() =
+ X_initial(1, 0) * accel_q_ + final_q_ * (Af_ * X_initial - X_final);
+
+ auto status = solver_.SetObjectiveVector(objective_vector_);
+ CHECK(status.ok()) << status;
+}
+
+bool MPCProblem::Solve() {
+ const aos::monotonic_clock::time_point start_time =
+ aos::monotonic_clock::now();
+ osqp::OsqpExitCode exit_code = solver_.Solve();
+ const aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
+ VLOG(1) << "OSQP solved in "
+ << std::chrono::duration<double>(end_time - start_time).count();
+ solve_time_ = std::chrono::duration<double>(end_time - start_time).count();
+ // TODO(austin): Dump the exit codes out as an enum for logging.
+ //
+ // TODO(austin): The dual problem doesn't appear to be converging on all
+ // problems. Are we phrasing something wrong?
+
+ // TODO(austin): Set a time limit so we can't run forever, and signal back
+ // when we hit our limit.
+ return exit_code == osqp::OsqpExitCode::kOptimal;
+}
+
+void MPCProblem::WarmStart(const MPCProblem &p) {
+ CHECK_GE(p.horizon(), horizon())
+ << ": Can only copy a bigger problem's solution into a smaller problem.";
+ auto status = solver_.SetPrimalWarmStart(p.solver_.primal_solution().block(
+ p.horizon() - horizon(), 0, horizon(), 1));
+ CHECK(status.ok()) << status;
+ status = solver_.SetDualWarmStart(p.solver_.dual_solution().block(
+ p.horizon() - horizon(), 0, horizon(), 1));
+ CHECK(status.ok()) << status;
+}
+
+} // namespace frc971::control_loops::catapult
\ No newline at end of file
diff --git a/frc971/control_loops/catapult/mpc_problem.h b/frc971/control_loops/catapult/mpc_problem.h
new file mode 100644
index 0000000..04e3936
--- /dev/null
+++ b/frc971/control_loops/catapult/mpc_problem.h
@@ -0,0 +1,72 @@
+#include "Eigen/Dense"
+#include "Eigen/Sparse"
+#include "glog/logging.h"
+
+#include "aos/realtime.h"
+#include "aos/time/time.h"
+#include "osqp++.h"
+#include "osqp.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace catapult {
+
+// MPC problem for a specified horizon. This contains all the state for the
+// solver, setters to modify the current and target state, and a way to fetch
+// the solution.
+class MPCProblem {
+ public:
+ MPCProblem(size_t horizon,
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P,
+ Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q,
+ Eigen::Matrix<double, 2, 2> Af,
+ Eigen::Matrix<double, Eigen::Dynamic, 2> final_q);
+
+ MPCProblem(MPCProblem const &) = delete;
+ void operator=(MPCProblem const &x) = delete;
+
+ // Sets the current and final state. This keeps the problem in tact and
+ // doesn't recreate it, so it will be fast.
+ void SetState(Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final);
+
+ // Solves our problem.
+ bool Solve();
+
+ double solve_time() const { return solve_time_; }
+
+ // Returns the solution that the solver found when Solve was last called.
+ double U(size_t i) const { return solver_.primal_solution()(i); }
+
+ // Returns the number of U's to be solved.
+ size_t horizon() const { return horizon_; }
+
+ // Warm starts the optimizer with the provided solution to make it solve
+ // faster.
+ void WarmStart(const MPCProblem &p);
+
+ private:
+ // The number of u's to solve for.
+ const size_t horizon_;
+
+ // The problem statement variables needed by SetState to update q.
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q_;
+ const Eigen::Matrix<double, 2, 2> Af_;
+ const Eigen::Matrix<double, Eigen::Dynamic, 2> final_q_;
+
+ Eigen::Matrix<double, 2, 1> X_initial_;
+ Eigen::Matrix<double, 2, 1> X_final_;
+
+ Eigen::Matrix<double, Eigen::Dynamic, 1> objective_vector_;
+
+ // Solver state.
+ osqp::OsqpInstance instance_;
+ osqp::OsqpSolver solver_;
+ osqp::OsqpSettings settings_;
+
+ double solve_time_ = 0;
+};
+
+} // namespace catapult
+} // namespace control_loops
+} // namespace frc971
\ No newline at end of file
diff --git a/frc971/control_loops/catapult/mpc_problem_generator.cc b/frc971/control_loops/catapult/mpc_problem_generator.cc
new file mode 100644
index 0000000..3b36318
--- /dev/null
+++ b/frc971/control_loops/catapult/mpc_problem_generator.cc
@@ -0,0 +1,168 @@
+#include "frc971/control_loops/catapult/mpc_problem_generator.h"
+
+namespace frc971::control_loops::catapult {
+namespace chrono = std::chrono;
+
+CatapultProblemGenerator::CatapultProblemGenerator(
+ StateFeedbackPlant<2, 1, 1> plant, size_t horizon)
+ : plant_(std::move(plant)),
+ horizon_(horizon),
+ Q_final_(
+ (Eigen::DiagonalMatrix<double, 2>().diagonal() << 10000.0, 10000.0)
+ .finished()),
+ As_(MakeAs()),
+ Bs_(MakeBs()),
+ m_(Makem()),
+ M_(MakeM()),
+ W_(MakeW()),
+ w_(Makew()),
+ Pi_(MakePi()),
+ WM_(W_ * M_),
+ Wmpw_(W_ * m_ + w_) {}
+
+std::unique_ptr<MPCProblem> CatapultProblemGenerator::MakeProblem(
+ size_t horizon) {
+ return std::make_unique<MPCProblem>(
+ horizon, P(horizon), accel_q(horizon), Af(horizon),
+ (2.0 * Q_final_ * Bf(horizon)).transpose());
+}
+
+const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::P(size_t horizon) {
+ CHECK_GT(horizon, 0u);
+ CHECK_LE(horizon, horizon_);
+ return 2.0 * (WM_.block(0, 0, horizon, horizon).transpose() * Pi(horizon) *
+ WM_.block(0, 0, horizon, horizon) +
+ Bf(horizon).transpose() * Q_final_ * Bf(horizon));
+}
+
+const Eigen::Matrix<double, Eigen::Dynamic, 1> CatapultProblemGenerator::q(
+ size_t horizon, Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final) {
+ CHECK_GT(horizon, 0u);
+ CHECK_LE(horizon, horizon_);
+ return 2.0 * X_initial(1, 0) * accel_q(horizon) +
+ 2.0 * ((Af(horizon) * X_initial - X_final).transpose() * Q_final_ *
+ Bf(horizon))
+ .transpose();
+}
+
+const Eigen::Matrix<double, Eigen::Dynamic, 1>
+CatapultProblemGenerator::accel_q(size_t horizon) {
+ return 2.0 * ((Wmpw_.block(0, 0, horizon, 1)).transpose() * Pi(horizon) *
+ WM_.block(0, 0, horizon, horizon))
+ .transpose();
+}
+
+const Eigen::Matrix<double, 2, 2> CatapultProblemGenerator::Af(size_t horizon) {
+ CHECK_GT(horizon, 0u);
+ CHECK_LE(horizon, horizon_);
+ return As_.block<2, 2>(2 * (horizon - 1), 0);
+}
+
+const Eigen::Matrix<double, 2, Eigen::Dynamic> CatapultProblemGenerator::Bf(
+ size_t horizon) {
+ CHECK_GT(horizon, 0u);
+ CHECK_LE(horizon, horizon_);
+ return Bs_.block(2 * (horizon - 1), 0, 2, horizon);
+}
+
+const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::Pi(size_t horizon) {
+ CHECK_GT(horizon, 0u);
+ CHECK_LE(horizon, horizon_);
+ return Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>(Pi_).block(
+ horizon_ - horizon, horizon_ - horizon, horizon, horizon);
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, 2> CatapultProblemGenerator::MakeAs() {
+ Eigen::Matrix<double, Eigen::Dynamic, 2> As =
+ Eigen::Matrix<double, Eigen::Dynamic, 2>::Zero(horizon_ * 2, 2);
+ for (size_t i = 0; i < horizon_; ++i) {
+ if (i == 0) {
+ As.block<2, 2>(0, 0) = plant_.A();
+ } else {
+ As.block<2, 2>(i * 2, 0) = plant_.A() * As.block<2, 2>((i - 1) * 2, 0);
+ }
+ }
+ return As;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::MakeBs() {
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Bs =
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(horizon_ * 2,
+ horizon_);
+ for (size_t i = 0; i < horizon_; ++i) {
+ for (size_t j = 0; j < i + 1; ++j) {
+ if (i == j) {
+ Bs.block<2, 1>(i * 2, j) = plant_.B();
+ } else {
+ Bs.block<2, 1>(i * 2, j) =
+ As_.block<2, 2>((i - j - 1) * 2, 0) * plant_.B();
+ }
+ }
+ }
+ return Bs;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, 1> CatapultProblemGenerator::Makem() {
+ Eigen::Matrix<double, Eigen::Dynamic, 1> m =
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(horizon_, 1);
+ for (size_t i = 0; i < horizon_; ++i) {
+ m(i, 0) = As_(1 + 2 * i, 1);
+ }
+ return m;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::MakeM() {
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> M =
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(horizon_,
+ horizon_);
+ for (size_t i = 0; i < horizon_; ++i) {
+ for (size_t j = 0; j < horizon_; ++j) {
+ M(i, j) = Bs_(2 * i + 1, j);
+ }
+ }
+ return M;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::MakeW() {
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> W =
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Identity(horizon_,
+ horizon_);
+ for (size_t i = 0; i < horizon_ - 1; ++i) {
+ W(i + 1, i) = -1.0;
+ }
+ W /= std::chrono::duration<double>(plant_.dt()).count();
+ return W;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, 1> CatapultProblemGenerator::Makew() {
+ Eigen::Matrix<double, Eigen::Dynamic, 1> w =
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(horizon_, 1);
+ w(0, 0) = -1.0 / std::chrono::duration<double>(plant_.dt()).count();
+ return w;
+}
+
+Eigen::DiagonalMatrix<double, Eigen::Dynamic>
+CatapultProblemGenerator::MakePi() {
+ Eigen::DiagonalMatrix<double, Eigen::Dynamic> Pi(horizon_);
+ for (size_t i = 0; i < horizon_; ++i) {
+ Pi.diagonal()(i) =
+ std::pow(0.01, 2.0) +
+ std::pow(0.02 * std::max(0.0, (20 - ((int)horizon_ - (int)i)) / 20.),
+ 2.0);
+ }
+ return Pi;
+}
+
+Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
+CatapultProblemGenerator::MakeP() {
+ return 2.0 * (M_.transpose() * W_.transpose() * Pi_ * W_ * M_ +
+ Bf(horizon_).transpose() * Q_final_ * Bf(horizon_));
+}
+
+} // namespace frc971::control_loops::catapult
\ No newline at end of file
diff --git a/frc971/control_loops/catapult/mpc_problem_generator.h b/frc971/control_loops/catapult/mpc_problem_generator.h
new file mode 100644
index 0000000..2fbaf66
--- /dev/null
+++ b/frc971/control_loops/catapult/mpc_problem_generator.h
@@ -0,0 +1,69 @@
+#include "frc971/control_loops/catapult/mpc_problem.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace catapult {
+
+// Decently efficient problem generator for multiple horizons given a max
+// horizon to solve for.
+//
+// The math is documented in mpc.tex
+class CatapultProblemGenerator {
+ public:
+ // Builds a problem generator for the specified max horizon and caches a lot
+ // of the state.
+ CatapultProblemGenerator(StateFeedbackPlant<2, 1, 1> plant, size_t horizon);
+
+ // Returns the maximum horizon.
+ size_t horizon() const { return horizon_; }
+
+ // Makes a problem for the specificed horizon.
+ std::unique_ptr<MPCProblem> MakeProblem(size_t horizon);
+
+ // Returns the P and Q matrices for the problem statement.
+ // cost = 0.5 X.T P X + q.T X
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P(size_t horizon);
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> q(
+ size_t horizon, Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final);
+
+ private:
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q(size_t horizon);
+
+ const Eigen::Matrix<double, 2, 2> Af(size_t horizon);
+ const Eigen::Matrix<double, 2, Eigen::Dynamic> Bf(size_t horizon);
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Pi(
+ size_t horizon);
+
+ // These functions are used in the constructor to build up the matrices below.
+ Eigen::Matrix<double, Eigen::Dynamic, 2> MakeAs();
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeBs();
+ Eigen::Matrix<double, Eigen::Dynamic, 1> Makem();
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeM();
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeW();
+ Eigen::Matrix<double, Eigen::Dynamic, 1> Makew();
+ Eigen::DiagonalMatrix<double, Eigen::Dynamic> MakePi();
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeP();
+
+ const StateFeedbackPlant<2, 1, 1> plant_;
+ const size_t horizon_;
+
+ const Eigen::DiagonalMatrix<double, 2> Q_final_;
+
+ const Eigen::Matrix<double, Eigen::Dynamic, 2> As_;
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Bs_;
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> m_;
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> M_;
+
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> W_;
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> w_;
+ const Eigen::DiagonalMatrix<double, Eigen::Dynamic> Pi_;
+
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> WM_;
+ const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Wmpw_;
+};
+
+} // namespace catapult
+} // namespace control_loops
+} // namespace frc971
\ No newline at end of file
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 3c90434..988aaa9 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -256,8 +256,12 @@
static const int kNumInputs = number_of_inputs;
private:
- Eigen::Matrix<Scalar, number_of_states, 1> X_;
- Eigen::Matrix<Scalar, number_of_outputs, 1> Y_;
+ // X_ and Y_ must be explicitly initialized to avoid having gcc complain about
+ // uninitialized values when using the move constructor.
+ Eigen::Matrix<Scalar, number_of_states, 1> X_ =
+ Eigen::Matrix<Scalar, number_of_states, 1>::Zero();
+ Eigen::Matrix<Scalar, number_of_outputs, 1> Y_ =
+ Eigen::Matrix<Scalar, number_of_outputs, 1>::Zero();
Eigen::Matrix<Scalar, number_of_inputs, Eigen::Dynamic> last_U_;
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index 41f7e04..ec650ac 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -32,6 +32,7 @@
using ::frc971::ProfileParametersT;
using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::catapult::CatapultGoal;
using frc971::control_loops::drivetrain::LocalizerControl;
namespace chrono = ::std::chrono;
@@ -401,11 +402,11 @@
*builder.fbb(), kCatapultReturnPosition,
CreateProfileParameters(*builder.fbb(), 9.0, 50.0));
- superstructure::CatapultGoal::Builder catapult_goal_builder(*builder.fbb());
+ CatapultGoal::Builder catapult_goal_builder(*builder.fbb());
catapult_goal_builder.add_shot_position(0.03);
catapult_goal_builder.add_shot_velocity(18.0);
catapult_goal_builder.add_return_position(catapult_return_position_offset);
- flatbuffers::Offset<superstructure::CatapultGoal> catapult_goal_offset =
+ flatbuffers::Offset<CatapultGoal> catapult_goal_offset =
catapult_goal_builder.Finish();
superstructure::Goal::Builder superstructure_builder =
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index 0f2737d..52b4db0 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -12,6 +12,7 @@
deps = [
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/catapult:catapult_goal_fbs",
],
)
@@ -80,10 +81,10 @@
"//aos:flatbuffer_merge",
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
+ "//frc971/control_loops/catapult",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/zeroing:pot_and_absolute_encoder",
"//y2022:constants",
- "//y2022/control_loops/superstructure/catapult",
"//y2022/control_loops/superstructure/turret:aiming",
"//y2022/vision:ball_color_fbs",
],
diff --git a/y2022/control_loops/superstructure/catapult/BUILD b/y2022/control_loops/superstructure/catapult/BUILD
index 36e34bd..84827bf 100644
--- a/y2022/control_loops/superstructure/catapult/BUILD
+++ b/y2022/control_loops/superstructure/catapult/BUILD
@@ -29,35 +29,15 @@
],
)
-cc_library(
- name = "catapult",
- srcs = [
- "catapult.cc",
- ],
- hdrs = [
- "catapult.h",
- ],
- visibility = ["//visibility:public"],
- deps = [
- ":catapult_plants",
- "//aos:realtime",
- "//frc971/zeroing:pot_and_absolute_encoder",
- "//third_party/osqp-cpp",
- "//y2022:constants",
- "//y2022/control_loops/superstructure:superstructure_goal_fbs",
- "//y2022/control_loops/superstructure:superstructure_position_fbs",
- "//y2022/control_loops/superstructure:superstructure_status_fbs",
- ],
-)
-
cc_test(
name = "catapult_test",
srcs = [
"catapult_test.cc",
],
deps = [
- ":catapult",
+ ":catapult_plants",
"//aos/testing:googletest",
+ "//frc971/control_loops/catapult",
],
)
@@ -67,7 +47,8 @@
"catapult_main.cc",
],
deps = [
- ":catapult",
+ ":catapult_plants",
"//aos:init",
+ "//frc971/control_loops/catapult",
],
)
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
deleted file mode 100644
index 493bb46..0000000
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ /dev/null
@@ -1,453 +0,0 @@
-#include "y2022/control_loops/superstructure/catapult/catapult.h"
-
-#include "Eigen/Dense"
-#include "Eigen/Sparse"
-#include "glog/logging.h"
-
-#include "aos/realtime.h"
-#include "aos/time/time.h"
-#include "osqp++.h"
-#include "osqp.h"
-#include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
-
-namespace y2022::control_loops::superstructure::catapult {
-namespace chrono = std::chrono;
-
-namespace {
-osqp::OsqpInstance MakeInstance(
- size_t horizon, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P) {
- osqp::OsqpInstance instance;
- instance.objective_matrix = P.sparseView();
-
- instance.constraint_matrix =
- Eigen::SparseMatrix<double, Eigen::ColMajor, osqp::c_int>(horizon,
- horizon);
- instance.constraint_matrix.setIdentity();
-
- instance.lower_bounds =
- Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(horizon, 1);
- instance.upper_bounds =
- Eigen::Matrix<double, Eigen::Dynamic, 1>::Ones(horizon, 1) * 12.0;
- return instance;
-}
-} // namespace
-
-MPCProblem::MPCProblem(size_t horizon,
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P,
- Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q,
- Eigen::Matrix<double, 2, 2> Af,
- Eigen::Matrix<double, Eigen::Dynamic, 2> final_q)
- : horizon_(horizon),
- accel_q_(std::move(accel_q)),
- Af_(std::move(Af)),
- final_q_(std::move(final_q)),
- instance_(MakeInstance(horizon, std::move(P))) {
- // Start with a representative problem.
- Eigen::Matrix<double, 2, 1> X_initial(0.0, 0.0);
- Eigen::Matrix<double, 2, 1> X_final(2.0, 25.0);
-
- objective_vector_ =
- X_initial(1, 0) * accel_q_ + final_q_ * (Af_ * X_initial - X_final);
- instance_.objective_vector = objective_vector_;
- settings_.max_iter = 25;
- settings_.check_termination = 5;
- settings_.warm_start = 1;
- // TODO(austin): Do we need this scaling thing? It makes it not solve
- // sometimes... I'm pretty certain by giving it a decently formed problem to
- // initialize with, it will not try doing crazy things with the scaling
- // internally.
- settings_.scaling = 0;
- auto status = solver_.Init(instance_, settings_);
- CHECK(status.ok()) << status;
-}
-
-void MPCProblem::SetState(Eigen::Matrix<double, 2, 1> X_initial,
- Eigen::Matrix<double, 2, 1> X_final) {
- X_initial_ = X_initial;
- X_final_ = X_final;
- // If we mark this noalias(), it won't re-allocate the vector each time.
- objective_vector_.noalias() =
- X_initial(1, 0) * accel_q_ + final_q_ * (Af_ * X_initial - X_final);
-
- auto status = solver_.SetObjectiveVector(objective_vector_);
- CHECK(status.ok()) << status;
-}
-
-bool MPCProblem::Solve() {
- const aos::monotonic_clock::time_point start_time =
- aos::monotonic_clock::now();
- osqp::OsqpExitCode exit_code = solver_.Solve();
- const aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
- VLOG(1) << "OSQP solved in "
- << std::chrono::duration<double>(end_time - start_time).count();
- solve_time_ = std::chrono::duration<double>(end_time - start_time).count();
- // TODO(austin): Dump the exit codes out as an enum for logging.
- //
- // TODO(austin): The dual problem doesn't appear to be converging on all
- // problems. Are we phrasing something wrong?
-
- // TODO(austin): Set a time limit so we can't run forever, and signal back
- // when we hit our limit.
- return exit_code == osqp::OsqpExitCode::kOptimal;
-}
-
-void MPCProblem::WarmStart(const MPCProblem &p) {
- CHECK_GE(p.horizon(), horizon())
- << ": Can only copy a bigger problem's solution into a smaller problem.";
- auto status = solver_.SetPrimalWarmStart(p.solver_.primal_solution().block(
- p.horizon() - horizon(), 0, horizon(), 1));
- CHECK(status.ok()) << status;
- status = solver_.SetDualWarmStart(p.solver_.dual_solution().block(
- p.horizon() - horizon(), 0, horizon(), 1));
- CHECK(status.ok()) << status;
-}
-
-CatapultProblemGenerator::CatapultProblemGenerator(size_t horizon)
- : plant_(MakeCatapultPlant()),
- horizon_(horizon),
- Q_final_(
- (Eigen::DiagonalMatrix<double, 2>().diagonal() << 10000.0, 10000.0)
- .finished()),
- As_(MakeAs()),
- Bs_(MakeBs()),
- m_(Makem()),
- M_(MakeM()),
- W_(MakeW()),
- w_(Makew()),
- Pi_(MakePi()),
- WM_(W_ * M_),
- Wmpw_(W_ * m_ + w_) {}
-
-std::unique_ptr<MPCProblem> CatapultProblemGenerator::MakeProblem(
- size_t horizon) {
- return std::make_unique<MPCProblem>(
- horizon, P(horizon), accel_q(horizon), Af(horizon),
- (2.0 * Q_final_ * Bf(horizon)).transpose());
-}
-
-const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
-CatapultProblemGenerator::P(size_t horizon) {
- CHECK_GT(horizon, 0u);
- CHECK_LE(horizon, horizon_);
- return 2.0 * (WM_.block(0, 0, horizon, horizon).transpose() * Pi(horizon) *
- WM_.block(0, 0, horizon, horizon) +
- Bf(horizon).transpose() * Q_final_ * Bf(horizon));
-}
-
-const Eigen::Matrix<double, Eigen::Dynamic, 1> CatapultProblemGenerator::q(
- size_t horizon, Eigen::Matrix<double, 2, 1> X_initial,
- Eigen::Matrix<double, 2, 1> X_final) {
- CHECK_GT(horizon, 0u);
- CHECK_LE(horizon, horizon_);
- return 2.0 * X_initial(1, 0) * accel_q(horizon) +
- 2.0 * ((Af(horizon) * X_initial - X_final).transpose() * Q_final_ *
- Bf(horizon))
- .transpose();
-}
-
-const Eigen::Matrix<double, Eigen::Dynamic, 1>
-CatapultProblemGenerator::accel_q(size_t horizon) {
- return 2.0 * ((Wmpw_.block(0, 0, horizon, 1)).transpose() * Pi(horizon) *
- WM_.block(0, 0, horizon, horizon))
- .transpose();
-}
-
-const Eigen::Matrix<double, 2, 2> CatapultProblemGenerator::Af(size_t horizon) {
- CHECK_GT(horizon, 0u);
- CHECK_LE(horizon, horizon_);
- return As_.block<2, 2>(2 * (horizon - 1), 0);
-}
-
-const Eigen::Matrix<double, 2, Eigen::Dynamic> CatapultProblemGenerator::Bf(
- size_t horizon) {
- CHECK_GT(horizon, 0u);
- CHECK_LE(horizon, horizon_);
- return Bs_.block(2 * (horizon - 1), 0, 2, horizon);
-}
-
-const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
-CatapultProblemGenerator::Pi(size_t horizon) {
- CHECK_GT(horizon, 0u);
- CHECK_LE(horizon, horizon_);
- return Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>(Pi_).block(
- horizon_ - horizon, horizon_ - horizon, horizon, horizon);
-}
-
-Eigen::Matrix<double, Eigen::Dynamic, 2> CatapultProblemGenerator::MakeAs() {
- Eigen::Matrix<double, Eigen::Dynamic, 2> As =
- Eigen::Matrix<double, Eigen::Dynamic, 2>::Zero(horizon_ * 2, 2);
- for (size_t i = 0; i < horizon_; ++i) {
- if (i == 0) {
- As.block<2, 2>(0, 0) = plant_.A();
- } else {
- As.block<2, 2>(i * 2, 0) = plant_.A() * As.block<2, 2>((i - 1) * 2, 0);
- }
- }
- return As;
-}
-
-Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
-CatapultProblemGenerator::MakeBs() {
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Bs =
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(horizon_ * 2,
- horizon_);
- for (size_t i = 0; i < horizon_; ++i) {
- for (size_t j = 0; j < i + 1; ++j) {
- if (i == j) {
- Bs.block<2, 1>(i * 2, j) = plant_.B();
- } else {
- Bs.block<2, 1>(i * 2, j) =
- As_.block<2, 2>((i - j - 1) * 2, 0) * plant_.B();
- }
- }
- }
- return Bs;
-}
-
-Eigen::Matrix<double, Eigen::Dynamic, 1> CatapultProblemGenerator::Makem() {
- Eigen::Matrix<double, Eigen::Dynamic, 1> m =
- Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(horizon_, 1);
- for (size_t i = 0; i < horizon_; ++i) {
- m(i, 0) = As_(1 + 2 * i, 1);
- }
- return m;
-}
-
-Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
-CatapultProblemGenerator::MakeM() {
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> M =
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(horizon_,
- horizon_);
- for (size_t i = 0; i < horizon_; ++i) {
- for (size_t j = 0; j < horizon_; ++j) {
- M(i, j) = Bs_(2 * i + 1, j);
- }
- }
- return M;
-}
-
-Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
-CatapultProblemGenerator::MakeW() {
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> W =
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Identity(horizon_,
- horizon_);
- for (size_t i = 0; i < horizon_ - 1; ++i) {
- W(i + 1, i) = -1.0;
- }
- W /= std::chrono::duration<double>(plant_.dt()).count();
- return W;
-}
-
-Eigen::Matrix<double, Eigen::Dynamic, 1> CatapultProblemGenerator::Makew() {
- Eigen::Matrix<double, Eigen::Dynamic, 1> w =
- Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(horizon_, 1);
- w(0, 0) = -1.0 / std::chrono::duration<double>(plant_.dt()).count();
- return w;
-}
-
-Eigen::DiagonalMatrix<double, Eigen::Dynamic>
-CatapultProblemGenerator::MakePi() {
- Eigen::DiagonalMatrix<double, Eigen::Dynamic> Pi(horizon_);
- for (size_t i = 0; i < horizon_; ++i) {
- Pi.diagonal()(i) =
- std::pow(0.01, 2.0) +
- std::pow(0.02 * std::max(0.0, (20 - ((int)horizon_ - (int)i)) / 20.),
- 2.0);
- }
- return Pi;
-}
-
-Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
-CatapultProblemGenerator::MakeP() {
- return 2.0 * (M_.transpose() * W_.transpose() * Pi_ * W_ * M_ +
- Bf(horizon_).transpose() * Q_final_ * Bf(horizon_));
-}
-
-CatapultController::CatapultController(size_t horizon) : generator_(horizon) {
- problems_.reserve(generator_.horizon());
- for (size_t i = generator_.horizon(); i > 0; --i) {
- problems_.emplace_back(generator_.MakeProblem(i));
- }
-
- Reset();
-}
-
-void CatapultController::Reset() {
- current_controller_ = 0;
- solve_time_ = 0.0;
-}
-
-void CatapultController::SetState(Eigen::Matrix<double, 2, 1> X_initial,
- Eigen::Matrix<double, 2, 1> X_final) {
- if (current_controller_ >= problems_.size()) {
- return;
- }
- problems_[current_controller_]->SetState(X_initial, X_final);
-}
-
-bool CatapultController::Solve() {
- if (current_controller_ >= problems_.size()) {
- return true;
- }
- const bool result = problems_[current_controller_]->Solve();
- solve_time_ = problems_[current_controller_]->solve_time();
- return result;
-}
-
-std::optional<double> CatapultController::Next() {
- if (current_controller_ >= problems_.size()) {
- return std::nullopt;
- }
-
- double u;
- size_t solution_number = 0;
- if (current_controller_ == 0u) {
- while (solution_number < problems_[current_controller_]->horizon() &&
- problems_[current_controller_]->U(solution_number) < 0.01) {
- u = problems_[current_controller_]->U(solution_number);
- ++solution_number;
- }
- }
- u = problems_[current_controller_]->U(solution_number);
-
- if (current_controller_ + 1u + solution_number < problems_.size()) {
- problems_[current_controller_ + solution_number + 1]->WarmStart(
- *problems_[current_controller_]);
- }
- current_controller_ += 1u + solution_number;
- return u;
-}
-
-const flatbuffers::Offset<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
-Catapult::Iterate(const CatapultGoal *catapult_goal, const Position *position,
- double battery_voltage, double *catapult_voltage, bool fire,
- flatbuffers::FlatBufferBuilder *fbb) {
- const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
- *return_goal =
- catapult_goal != nullptr && catapult_goal->has_return_position()
- ? catapult_goal->return_position()
- : nullptr;
-
- const bool catapult_disabled = catapult_.Correct(
- return_goal, position->catapult(), catapult_voltage == nullptr);
-
- if (catapult_disabled) {
- catapult_state_ = CatapultState::PROFILE;
- } else if (catapult_.running() && catapult_goal != nullptr && fire &&
- !last_firing_) {
- catapult_state_ = CatapultState::FIRING;
- latched_shot_position = catapult_goal->shot_position();
- latched_shot_velocity = catapult_goal->shot_velocity();
- }
-
- // Don't update last_firing_ if the catapult is disabled, so that we actually
- // end up firing once it's enabled
- if (catapult_.running() && !catapult_disabled) {
- last_firing_ = fire;
- }
-
- use_profile_ = true;
-
- switch (catapult_state_) {
- case CatapultState::FIRING: {
- // Select the ball controller. We should only be firing if we have a
- // ball, or at least should only care about the shot accuracy.
- catapult_.set_controller_index(0);
- // Ok, so we've now corrected. Next step is to run the MPC.
- //
- // Since there is a unit delay between when we ask for a U and the
- // hardware applies it, we need to run the optimizer for the position at
- // the *next* control loop cycle.
-
- Eigen::Vector3d next_X = catapult_.estimated_state();
- for (int i = catapult_.controller().plant().coefficients().delayed_u;
- i > 1; --i) {
- next_X = catapult_.controller().plant().A() * next_X +
- catapult_.controller().plant().B() *
- catapult_.controller().observer().last_U(i - 1);
- }
-
- catapult_mpc_.SetState(
- next_X.block<2, 1>(0, 0),
- Eigen::Vector2d(latched_shot_position, latched_shot_velocity));
-
- const bool solved = catapult_mpc_.Solve();
- current_horizon_ = catapult_mpc_.current_horizon();
- const bool started = catapult_mpc_.started();
- if (solved || started) {
- std::optional<double> solution = catapult_mpc_.Next();
-
- if (!solution.has_value()) {
- CHECK_NOTNULL(catapult_voltage);
- *catapult_voltage = 0.0;
- if (catapult_mpc_.started()) {
- ++shot_count_;
- // Finished the catapult, time to fire.
- catapult_state_ = CatapultState::RESETTING;
- }
- } else {
- // TODO(austin): Voltage error?
- CHECK_NOTNULL(catapult_voltage);
- if (current_horizon_ == 1) {
- battery_voltage = 12.0;
- }
- *catapult_voltage = std::max(
- 0.0, std::min(12.0, (*solution - 0.0 * next_X(2, 0)) * 12.0 /
- std::max(battery_voltage, 8.0)));
- use_profile_ = false;
- }
- } else {
- if (!fire) {
- // Eh, didn't manage to solve before it was time to fire. Give up.
- catapult_state_ = CatapultState::PROFILE;
- }
- }
-
- if (!use_profile_) {
- catapult_.ForceGoal(catapult_.estimated_position(),
- catapult_.estimated_velocity());
- }
- }
- if (catapult_state_ != CatapultState::RESETTING) {
- break;
- } else {
- [[fallthrough]];
- }
-
- case CatapultState::RESETTING:
- if (catapult_.controller().R(1, 0) > 7.0) {
- catapult_.AdjustProfile(7.0, 2000.0);
- } else if (catapult_.controller().R(1, 0) > 0.0) {
- catapult_.AdjustProfile(7.0, 1000.0);
- } else {
- catapult_state_ = CatapultState::PROFILE;
- }
- [[fallthrough]];
-
- case CatapultState::PROFILE:
- break;
- }
-
- if (use_profile_) {
- if (catapult_state_ != CatapultState::FIRING) {
- catapult_mpc_.Reset();
- }
- // Select the controller designed for when we have no ball.
- catapult_.set_controller_index(1);
-
- current_horizon_ = 0u;
- const double output_voltage = catapult_.UpdateController(catapult_disabled);
- if (catapult_voltage != nullptr) {
- *catapult_voltage = output_voltage;
- }
- }
-
- catapult_.UpdateObserver(catapult_voltage != nullptr
- ? (*catapult_voltage * battery_voltage / 12.0)
- : 0.0);
-
- return catapult_.MakeStatus(fbb);
-}
-
-} // namespace y2022::control_loops::superstructure::catapult
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
deleted file mode 100644
index d30e8f5..0000000
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ /dev/null
@@ -1,254 +0,0 @@
-#ifndef Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_CATAPULT_CATAPULT_H_
-#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_CATAPULT_CATAPULT_H_
-
-#include "Eigen/Dense"
-#include "glog/logging.h"
-
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/zeroing/pot_and_absolute_encoder.h"
-#include "osqp++.h"
-#include "y2022/constants.h"
-#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2022/control_loops/superstructure/superstructure_position_generated.h"
-#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2022 {
-namespace control_loops {
-namespace superstructure {
-namespace catapult {
-
-// MPC problem for a specified horizon. This contains all the state for the
-// solver, setters to modify the current and target state, and a way to fetch
-// the solution.
-class MPCProblem {
- public:
- MPCProblem(size_t horizon,
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P,
- Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q,
- Eigen::Matrix<double, 2, 2> Af,
- Eigen::Matrix<double, Eigen::Dynamic, 2> final_q);
-
- MPCProblem(MPCProblem const &) = delete;
- void operator=(MPCProblem const &x) = delete;
-
- // Sets the current and final state. This keeps the problem in tact and
- // doesn't recreate it, so it will be fast.
- void SetState(Eigen::Matrix<double, 2, 1> X_initial,
- Eigen::Matrix<double, 2, 1> X_final);
-
- // Solves our problem.
- bool Solve();
-
- double solve_time() const { return solve_time_; }
-
- // Returns the solution that the solver found when Solve was last called.
- double U(size_t i) const { return solver_.primal_solution()(i); }
-
- // Returns the number of U's to be solved.
- size_t horizon() const { return horizon_; }
-
- // Warm starts the optimizer with the provided solution to make it solve
- // faster.
- void WarmStart(const MPCProblem &p);
-
- private:
- // The number of u's to solve for.
- const size_t horizon_;
-
- // The problem statement variables needed by SetState to update q.
- const Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q_;
- const Eigen::Matrix<double, 2, 2> Af_;
- const Eigen::Matrix<double, Eigen::Dynamic, 2> final_q_;
-
- Eigen::Matrix<double, 2, 1> X_initial_;
- Eigen::Matrix<double, 2, 1> X_final_;
-
- Eigen::Matrix<double, Eigen::Dynamic, 1> objective_vector_;
-
- // Solver state.
- osqp::OsqpInstance instance_;
- osqp::OsqpSolver solver_;
- osqp::OsqpSettings settings_;
-
- double solve_time_ = 0;
-};
-
-// Decently efficient problem generator for multiple horizons given a max
-// horizon to solve for.
-//
-// The math is documented in mpc.tex
-class CatapultProblemGenerator {
- public:
- // Builds a problem generator for the specified max horizon and caches a lot
- // of the state.
- CatapultProblemGenerator(size_t horizon);
-
- // Returns the maximum horizon.
- size_t horizon() const { return horizon_; }
-
- // Makes a problem for the specificed horizon.
- std::unique_ptr<MPCProblem> MakeProblem(size_t horizon);
-
- // Returns the P and Q matrices for the problem statement.
- // cost = 0.5 X.T P X + q.T X
- const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P(size_t horizon);
- const Eigen::Matrix<double, Eigen::Dynamic, 1> q(
- size_t horizon, Eigen::Matrix<double, 2, 1> X_initial,
- Eigen::Matrix<double, 2, 1> X_final);
-
- private:
- const Eigen::Matrix<double, Eigen::Dynamic, 1> accel_q(size_t horizon);
-
- const Eigen::Matrix<double, 2, 2> Af(size_t horizon);
- const Eigen::Matrix<double, 2, Eigen::Dynamic> Bf(size_t horizon);
- const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Pi(
- size_t horizon);
-
- // These functions are used in the constructor to build up the matrices below.
- Eigen::Matrix<double, Eigen::Dynamic, 2> MakeAs();
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeBs();
- Eigen::Matrix<double, Eigen::Dynamic, 1> Makem();
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeM();
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeW();
- Eigen::Matrix<double, Eigen::Dynamic, 1> Makew();
- Eigen::DiagonalMatrix<double, Eigen::Dynamic> MakePi();
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MakeP();
-
- const StateFeedbackPlant<2, 1, 1> plant_;
- const size_t horizon_;
-
- const Eigen::DiagonalMatrix<double, 2> Q_final_;
-
- const Eigen::Matrix<double, Eigen::Dynamic, 2> As_;
- const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Bs_;
- const Eigen::Matrix<double, Eigen::Dynamic, 1> m_;
- const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> M_;
-
- const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> W_;
- const Eigen::Matrix<double, Eigen::Dynamic, 1> w_;
- const Eigen::DiagonalMatrix<double, Eigen::Dynamic> Pi_;
-
- const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> WM_;
- const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Wmpw_;
-};
-
-// A class to hold all the state needed to manage the catapult MPC solvers for
-// repeated shots.
-//
-// The solver may take a couple of cycles to get everything converged and ready.
-// The flow is as follows:
-// 1) Reset() the state for the new problem.
-// 2) Update to the current state with SetState()
-// 3) Call Solve(). This will return true if it is ready to be executed, false
-// if it needs more iterations to fully converge.
-// 4) Next() returns the current optimal control output and advances the
-// pointers to the next problem.
-// 5) Go back to 2 for the next cycle.
-class CatapultController {
- public:
- CatapultController(size_t horizon);
-
- // Starts back over at the first controller.
- void Reset();
-
- // Updates our current and final states for the current controller.
- void SetState(Eigen::Matrix<double, 2, 1> X_initial,
- Eigen::Matrix<double, 2, 1> X_final);
-
- // Solves! Returns true if the solution converged and osqp was happy.
- bool Solve();
-
- // Returns the time in seconds it last took Solve to run.
- double solve_time() const { return solve_time_; }
-
- // Returns the horizon of the current controller.
- size_t current_horizon() const {
- if (current_controller_ >= problems_.size()) {
- return 0u;
- } else {
- return problems_[current_controller_]->horizon();
- }
- }
-
- // Returns the controller value if there is a controller to run, or nullopt if
- // we finished the last controller. Advances the controller pointer to the
- // next controller and warms up the next controller.
- std::optional<double> Next();
-
- // Returns true if Next has been called and a controller has been used. Reset
- // starts over.
- bool started() const { return current_controller_ != 0u; }
-
- private:
- CatapultProblemGenerator generator_;
-
- std::vector<std::unique_ptr<MPCProblem>> problems_;
-
- size_t current_controller_ = 0;
- double solve_time_ = 0.0;
-};
-
-// Class to handle transitioning between both the profiled subsystem and the MPC
-// for shooting.
-class Catapult {
- public:
- Catapult(const constants::Values &values)
- : catapult_(values.catapult.subsystem_params), catapult_mpc_(30) {}
-
- using PotAndAbsoluteEncoderSubsystem =
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
- ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
- ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
-
- // Resets all state for when WPILib restarts.
- void Reset() { catapult_.Reset(); }
-
- void Estop() { catapult_.Estop(); }
-
- bool zeroed() const { return catapult_.zeroed(); }
- bool estopped() const { return catapult_.estopped(); }
- double solve_time() const { return catapult_mpc_.solve_time(); }
-
- uint8_t mpc_horizon() const { return current_horizon_; }
-
- bool mpc_active() const { return !use_profile_; }
-
- // Returns the number of shots taken.
- int shot_count() const { return shot_count_; }
-
- // Returns the estimated position
- double estimated_position() const { return catapult_.estimated_position(); }
-
- // Runs either the MPC or the profiled subsystem depending on if we are
- // shooting or not. Returns the status.
- const flatbuffers::Offset<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
- Iterate(const CatapultGoal *unsafe_goal, const Position *position,
- double battery_voltage, double *catapult_voltage, bool fire,
- flatbuffers::FlatBufferBuilder *fbb);
-
- private:
- PotAndAbsoluteEncoderSubsystem catapult_;
-
- catapult::CatapultController catapult_mpc_;
-
- enum CatapultState { PROFILE, FIRING, RESETTING };
-
- CatapultState catapult_state_ = CatapultState::PROFILE;
-
- double latched_shot_position = 0.0;
- double latched_shot_velocity = 0.0;
-
- bool last_firing_ = false;
- bool use_profile_ = true;
-
- int shot_count_ = 0;
- uint8_t current_horizon_ = 0u;
-};
-
-} // namespace catapult
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2022
-
-#endif // Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_CATAPULT_CATAPULT_H_
diff --git a/y2022/control_loops/superstructure/catapult/catapult_main.cc b/y2022/control_loops/superstructure/catapult/catapult_main.cc
index e3f897c..5124745 100644
--- a/y2022/control_loops/superstructure/catapult/catapult_main.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult_main.cc
@@ -1,26 +1,29 @@
#include "aos/init.h"
#include "aos/realtime.h"
#include "aos/time/time.h"
-#include "y2022/control_loops/superstructure/catapult/catapult.h"
+#include "frc971/control_loops/catapult/catapult.h"
#include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
namespace y2022::control_loops::superstructure::catapult {
namespace chrono = std::chrono;
+using CatapultProblemGenerator =
+ frc971::control_loops::catapult::CatapultProblemGenerator;
+using MPCProblem = frc971::control_loops::catapult::MPCProblem;
void OSQPSolve() {
Eigen::Matrix<double, 2, 1> X_initial(0.0, 0.0);
Eigen::Matrix<double, 2, 1> X_final(2.0, 25.0);
LOG(INFO) << "Starting a dynamic OSQP solve";
- CatapultProblemGenerator g(35);
const StateFeedbackPlant<2, 1, 1> plant = MakeCatapultPlant();
+ CatapultProblemGenerator g(MakeCatapultPlant(), 35);
constexpr int kHorizon = 35;
// TODO(austin): This is a good unit test! Make sure computing the problem
// different ways comes out the same.
{
- CatapultProblemGenerator g2(10);
+ CatapultProblemGenerator g2(MakeCatapultPlant(), 10);
constexpr int kTestHorizon = 10;
CHECK(g2.P(kTestHorizon) == g.P(kTestHorizon))
<< g2.P(kTestHorizon) - g.P(kTestHorizon);
diff --git a/y2022/control_loops/superstructure/catapult/catapult_test.cc b/y2022/control_loops/superstructure/catapult/catapult_test.cc
index b2cd180..0077633 100644
--- a/y2022/control_loops/superstructure/catapult/catapult_test.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult_test.cc
@@ -1,4 +1,4 @@
-#include "y2022/control_loops/superstructure/catapult/catapult.h"
+#include "frc971/control_loops/catapult/catapult.h"
#include "glog/logging.h"
#include "gtest/gtest.h"
@@ -6,15 +6,17 @@
#include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
namespace y2022::control_loops::superstructure::catapult::testing {
-
+using CatapultProblemGenerator =
+ frc971::control_loops::catapult::CatapultProblemGenerator;
+using MPCProblem = frc971::control_loops::catapult::MPCProblem;
// Tests that computing P and q with 2 different horizons comes out the same.
TEST(MPCTest, HorizonTest) {
Eigen::Matrix<double, 2, 1> X_initial(0.0, 0.0);
Eigen::Matrix<double, 2, 1> X_final(2.0, 25.0);
- CatapultProblemGenerator g(35);
+ CatapultProblemGenerator g(MakeCatapultPlant(), 35);
- CatapultProblemGenerator g2(10);
+ CatapultProblemGenerator g2(MakeCatapultPlant(), 10);
constexpr int kTestHorizon = 10;
EXPECT_TRUE(g2.P(kTestHorizon) == g.P(kTestHorizon))
<< g2.P(kTestHorizon) - g.P(kTestHorizon);
@@ -29,8 +31,8 @@
Eigen::Matrix<double, 2, 1> X_initial(0.0, 0.0);
Eigen::Matrix<double, 2, 1> X_final(2.0, 25.0);
- CatapultProblemGenerator g(35);
const StateFeedbackPlant<2, 1, 1> plant = MakeCatapultPlant();
+ CatapultProblemGenerator g(MakeCatapultPlant(), 35);
std::vector<std::unique_ptr<MPCProblem>> problems;
for (size_t i = g.horizon(); i > 0; --i) {
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index fc6aeee..dca0a96 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -25,7 +25,8 @@
intake_front_(values_->intake_front.subsystem_params),
intake_back_(values_->intake_back.subsystem_params),
turret_(values_->turret.subsystem_params),
- catapult_(*values_),
+ catapult_(values_->catapult.subsystem_params,
+ catapult::MakeCatapultPlant()),
drivetrain_status_fetcher_(
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
@@ -57,8 +58,11 @@
aos::FlatbufferFixedAllocatorArray<
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
turret_loading_goal_buffer;
- aos::FlatbufferFixedAllocatorArray<CatapultGoal, 512> catapult_goal_buffer;
- aos::FlatbufferFixedAllocatorArray<CatapultGoal, 512>
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::catapult::CatapultGoal, 512>
+ catapult_goal_buffer;
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::catapult::CatapultGoal, 512>
catapult_discarding_goal_buffer;
const aos::monotonic_clock::time_point timestamp =
@@ -107,7 +111,7 @@
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*turret_goal = nullptr;
- const CatapultGoal *catapult_goal = nullptr;
+ const frc971::control_loops::catapult::CatapultGoal *catapult_goal = nullptr;
double roller_speed_compensated_front = 0.0;
double roller_speed_compensated_back = 0.0;
double transfer_roller_speed = 0.0;
@@ -149,7 +153,8 @@
return_position_offset = {aos::CopyFlatBuffer(
unsafe_goal->catapult()->return_position(), catapult_goal_fbb)};
}
- CatapultGoal::Builder builder(*catapult_goal_fbb);
+ frc971::control_loops::catapult::CatapultGoal::Builder builder(
+ *catapult_goal_fbb);
builder.add_shot_position(shot_params.shot_angle);
builder.add_shot_velocity(shot_params.shot_velocity);
if (return_position_offset.has_value()) {
@@ -170,7 +175,8 @@
return_position_offset = {aos::CopyFlatBuffer(
unsafe_goal->catapult()->return_position(), catapult_goal_fbb)};
}
- CatapultGoal::Builder builder(*catapult_goal_fbb);
+ frc971::control_loops::catapult::CatapultGoal::Builder builder(
+ *catapult_goal_fbb);
builder.add_shot_position(kDiscardingPosition);
builder.add_shot_velocity(kDiscardingVelocity);
if (return_position_offset.has_value()) {
@@ -538,7 +544,7 @@
// flippers
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
catapult_status_offset = catapult_.Iterate(
- catapult_goal, position, robot_state().voltage_battery(),
+ catapult_goal, position->catapult(), robot_state().voltage_battery(),
output != nullptr && !catapult_.estopped()
? &(output_struct.catapult_voltage)
: nullptr,
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 36269e9..3e10956 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -2,11 +2,13 @@
#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
#include "aos/events/event_loop.h"
+#include "frc971/control_loops/catapult/catapult.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022/constants.h"
-#include "y2022/control_loops/superstructure/catapult/catapult.h"
+#include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
+#include "y2022/control_loops/superstructure/catapult/integral_catapult_plant.h"
#include "y2022/control_loops/superstructure/collision_avoidance.h"
#include "y2022/control_loops/superstructure/superstructure_can_position_generated.h"
#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
@@ -70,7 +72,7 @@
PotAndAbsoluteEncoderSubsystem intake_front_;
PotAndAbsoluteEncoderSubsystem intake_back_;
PotAndAbsoluteEncoderSubsystem turret_;
- catapult::Catapult catapult_;
+ ::frc971::control_loops::catapult::Catapult catapult_;
CollisionAvoidance collision_avoidance_;
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 4d4a81f..0c57362 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -1,4 +1,5 @@
include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/control_loops/catapult/catapult_goal.fbs";
namespace y2022.control_loops.superstructure;
@@ -8,20 +9,6 @@
kBack = 1,
}
-table CatapultGoal {
- // Old fire flag, only kept for backwards-compatability with logs.
- // Use the fire flag in the root Goal instead
- fire:bool (id: 0, deprecated);
-
- // The target shot position and velocity. If these are provided before fire
- // is called, the optimizer can pre-compute the trajectory.
- shot_position:double (id: 1);
- shot_velocity:double (id: 2);
-
- // The target position to return the catapult to when not shooting.
- return_position:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);
-}
-
table Goal {
// Height of the climber above rest point
climber:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 0);
@@ -52,7 +39,7 @@
turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 7);
// Catapult goal state.
- catapult:CatapultGoal (id: 8);
+ catapult:frc971.control_loops.catapult.CatapultGoal (id: 8);
// If true, fire! The robot will only fire when ready.
fire:bool (id: 9);
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index cbe3c97..eb33f29 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -691,7 +691,8 @@
const auto catapult_return_offset =
CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), kCatapultReturnPosition);
- auto catapult_builder = builder.MakeBuilder<CatapultGoal>();
+ auto catapult_builder =
+ builder.MakeBuilder<frc971::control_loops::catapult::CatapultGoal>();
catapult_builder.add_return_position(catapult_return_offset);
const auto catapult_offset = catapult_builder.Finish();
@@ -853,7 +854,8 @@
const auto catapult_return_offset =
CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), kCatapultReturnPosition);
- auto catapult_builder = builder.MakeBuilder<CatapultGoal>();
+ auto catapult_builder =
+ builder.MakeBuilder<frc971::control_loops::catapult::CatapultGoal>();
catapult_builder.add_shot_position(0.3);
catapult_builder.add_shot_velocity(15.0);
catapult_builder.add_return_position(catapult_return_offset);
@@ -1040,14 +1042,16 @@
*builder.fbb(), constants::Values::kCatapultRange().lower,
CreateProfileParameters(*builder.fbb(), 4.0, 20.0));
- CatapultGoal::Builder catapult_goal_builder =
- builder.MakeBuilder<CatapultGoal>();
+ frc971::control_loops::catapult::CatapultGoal::Builder
+ catapult_goal_builder =
+ builder
+ .MakeBuilder<frc971::control_loops::catapult::CatapultGoal>();
catapult_goal_builder.add_shot_position(0.3);
catapult_goal_builder.add_shot_velocity(15.0);
catapult_goal_builder.add_return_position(catapult_return_position_offset);
- flatbuffers::Offset<CatapultGoal> catapult_goal_offset =
- catapult_goal_builder.Finish();
+ flatbuffers::Offset<frc971::control_loops::catapult::CatapultGoal>
+ catapult_goal_offset = catapult_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_fire(false);
@@ -1075,14 +1079,16 @@
*builder.fbb(), constants::Values::kCatapultRange().lower,
CreateProfileParameters(*builder.fbb(), 4.0, 20.0));
- CatapultGoal::Builder catapult_goal_builder =
- builder.MakeBuilder<CatapultGoal>();
+ frc971::control_loops::catapult::CatapultGoal::Builder
+ catapult_goal_builder =
+ builder
+ .MakeBuilder<frc971::control_loops::catapult::CatapultGoal>();
catapult_goal_builder.add_shot_position(0.5);
catapult_goal_builder.add_shot_velocity(20.0);
catapult_goal_builder.add_return_position(catapult_return_position_offset);
- flatbuffers::Offset<CatapultGoal> catapult_goal_offset =
- catapult_goal_builder.Finish();
+ flatbuffers::Offset<frc971::control_loops::catapult::CatapultGoal>
+ catapult_goal_offset = catapult_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 9993e8f..dda5b9c 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -26,6 +26,7 @@
using frc971::CreateProfileParameters;
using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::catapult::CatapultGoal;
using frc971::input::driver_station::ButtonLocation;
using frc971::input::driver_station::ControlBit;
using frc971::input::driver_station::JoystickAxis;
@@ -320,12 +321,12 @@
*builder.fbb(), climber_position,
frc971::CreateProfileParameters(*builder.fbb(), 1.0, 1.0));
- superstructure::CatapultGoal::Builder catapult_builder =
- builder.MakeBuilder<superstructure::CatapultGoal>();
+ CatapultGoal::Builder catapult_builder =
+ builder.MakeBuilder<CatapultGoal>();
catapult_builder.add_return_position(catapult_return_offset);
catapult_builder.add_shot_position(catapult_pos);
catapult_builder.add_shot_velocity(catapult_speed);
- flatbuffers::Offset<superstructure::CatapultGoal> catapult_offset =
+ flatbuffers::Offset<CatapultGoal> catapult_offset =
catapult_builder.Finish();
superstructure::Goal::Builder superstructure_goal_builder =
diff --git a/y2024/control_loops/superstructure/BUILD b/y2024/control_loops/superstructure/BUILD
index 8ef0717..28e4d00 100644
--- a/y2024/control_loops/superstructure/BUILD
+++ b/y2024/control_loops/superstructure/BUILD
@@ -12,6 +12,7 @@
deps = [
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/catapult:catapult_goal_fbs",
],
)
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index c00c70e..63cbcf0 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -1,4 +1,5 @@
include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/control_loops/catapult/catapult_goal.fbs";
namespace y2024.control_loops.superstructure;
@@ -15,21 +16,11 @@
RETRACTED = 1,
}
-table CatapultGoal {
- // The target shot position and velocity. If these are provided before fire
- // is called, the optimizer can pre-compute the trajectory.
- shot_position: double (id: 0);
- shot_velocity: double (id: 1);
-
- // Return position of the catapult
- return_position: double (id: 2);
-}
-
table Goal {
intake_roller_goal:IntakeRollerGoal (id: 0);
intake_pivot_goal:IntakePivotGoal (id: 1);
- catapult_goal:CatapultGoal (id: 2);
+ catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 2);
}
root_type Goal;