Replace objective with cost in OCPs

SleipnirGroup · Jul 21, 2024 · 3915518 · 3915518
1 parent 4d06d00
commit 3915518
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 14 deletions.
diff --git 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
@@ -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
@@ -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});