From 3915518a21e8918d59e78ddf91e6d439853bbee4 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 20 Jul 2024 17:31:26 -0700
Subject: [PATCH] Replace objective with cost in OCPs

---
 examples/FlywheelOCP/src/Main.cpp                    | 11 ++++-------
 test/src/control/OCPSolverTest_DifferentialDrive.cpp |  2 +-
 test/src/control/OCPSolverTest_Flywheel.cpp          | 10 ++++------
 3 files changed, 9 insertions(+), 14 deletions(-)

diff --git a/examples/FlywheelOCP/src/Main.cpp b/examples/FlywheelOCP/src/Main.cpp
index f6fc2bc2..f3ded3c7 100644
--- a/examples/FlywheelOCP/src/Main.cpp
+++ b/examples/FlywheelOCP/src/Main.cpp
@@ -30,7 +30,7 @@ int main() {
     return A_discrete * x + B_discrete * u;
   };
 
-  Eigen::Matrix<double, 1, 1> r{10.0};
+  constexpr double r = 10.0;
 
   sleipnir::OCPSolver solver(
       1, 1, dt, N, f_discrete, sleipnir::DynamicsType::kDiscrete,
@@ -40,13 +40,10 @@ int main() {
   solver.SetUpperInputBound(12);
   solver.SetLowerInputBound(-12);
 
-  // Set up objective
+  // Set up cost
   Eigen::Matrix<double, 1, N + 1> r_mat =
-      r * Eigen::Matrix<double, 1, N + 1>::Ones();
-  sleipnir::VariableMatrix r_mat_vmat{r_mat};
-  sleipnir::VariableMatrix objective =
-      (r_mat_vmat - solver.X()) * (r_mat_vmat - solver.X()).T();
-  solver.Minimize(objective);
+      Eigen::Matrix<double, 1, N + 1>::Constant(r);
+  solver.Minimize((r_mat - solver.X()) * (r_mat - solver.X()).T());
 
   solver.Solve();
 
diff --git a/test/src/control/OCPSolverTest_DifferentialDrive.cpp b/test/src/control/OCPSolverTest_DifferentialDrive.cpp
index 9c16a4bb..670ac689 100644
--- a/test/src/control/OCPSolverTest_DifferentialDrive.cpp
+++ b/test/src/control/OCPSolverTest_DifferentialDrive.cpp
@@ -79,7 +79,7 @@ TEST_CASE("OCPSolver - Differential drive", "[OCPSolver]") {
   problem.SetMinTimestep(minTimestep);
   problem.SetMaxTimestep(3s);
 
-  // Set up objective
+  // Set up cost
   problem.Minimize(problem.DT() * Eigen::Matrix<double, N + 1, 1>::Ones());
 
   auto status = problem.Solve({.maxIterations = 1000, .diagnostics = true});
diff --git a/test/src/control/OCPSolverTest_Flywheel.cpp b/test/src/control/OCPSolverTest_Flywheel.cpp
index 23245075..c63232cd 100644
--- a/test/src/control/OCPSolverTest_Flywheel.cpp
+++ b/test/src/control/OCPSolverTest_Flywheel.cpp
@@ -40,6 +40,7 @@ void TestFlywheel(std::string testName, double A, double B,
   // Inputs: [voltage]
   double A_discrete = std::exp(A * dt.count());
   double B_discrete = (1.0 - A_discrete) * B;
+
   constexpr double r = 10.0;
 
   sleipnir::OCPSolver solver(1, 1, dt, N, F, dynamicsType,
@@ -48,13 +49,10 @@ void TestFlywheel(std::string testName, double A, double B,
   solver.SetUpperInputBound(12);
   solver.SetLowerInputBound(-12);
 
-  // Set up objective
+  // Set up cost
   Eigen::Matrix<double, 1, N + 1> r_mat =
-      r * Eigen::Matrix<double, 1, N + 1>::Ones();
-  sleipnir::VariableMatrix r_mat_vmat{r_mat};
-  sleipnir::VariableMatrix objective =
-      (r_mat_vmat - solver.X()) * (r_mat_vmat - solver.X()).T();
-  solver.Minimize(objective);
+      Eigen::Matrix<double, 1, N + 1>::Constant(r);
+  solver.Minimize((r_mat - solver.X()) * (r_mat - solver.X()).T());
 
   auto status = solver.Solve({.diagnostics = true});