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