Remove memory allocation in y2022 catapult
Change-Id: Ibce7aebe5181f07b46edf4a0b8ce7f686ad05fb2
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index 612a17a..cdffecd 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -67,7 +67,8 @@
Eigen::Matrix<double, 2, 1> X_final) {
X_initial_ = X_initial;
X_final_ = X_final;
- objective_vector_ =
+ // 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_);