Add flywheel OCP example (#217)

Fixes #97.
SleipnirGroup · Dec 9, 2023 · 1fe1b66 · 1fe1b66
1 parent 228b124
commit 1fe1b66
Show file tree

Hide file tree

Showing 4 changed files with 71 additions and 6 deletions.
diff --git a/examples/FlywheelOCP/src/Main.cpp b/examples/FlywheelOCP/src/Main.cpp
@@ -0,0 +1,58 @@
+// Copyright (c) Sleipnir contributors
+
+#include <chrono>
+#include <cmath>
+
+#include <Eigen/Core>
+#include <fmt/core.h>
+#include <sleipnir/control/OCPSolver.hpp>
+#include <sleipnir/optimization/OptimizationProblem.hpp>
+#include <units/time.h>
+
+#ifndef RUNNING_TESTS
+int main() {
+  constexpr auto T = 5_s;
+  constexpr units::second_t dt = 5_ms;
+  constexpr int N = T / dt;
+
+  // Flywheel model:
+  // States: [velocity]
+  // Inputs: [voltage]
+  Eigen::Matrix<double, 1, 1> A{-1.0};
+  Eigen::Matrix<double, 1, 1> B{1.0};
+
+  Eigen::Matrix<double, 1, 1> A_discrete{std::exp(A(0) * dt.value())};
+  Eigen::Matrix<double, 1, 1> B_discrete{(1.0 - A_discrete(0)) * B(0)};
+
+  auto f_discrete = [=](sleipnir::Variable t, sleipnir::VariableMatrix x,
+                        sleipnir::VariableMatrix u, sleipnir::Variable dt) {
+    return A_discrete * x + B_discrete * u;
+  };
+
+  Eigen::Matrix<double, 1, 1> r{10.0};
+
+  sleipnir::OCPSolver solver(
+      1, 1, std::chrono::duration<double>{dt.value()}, N, f_discrete,
+      sleipnir::DynamicsType::kDiscrete, sleipnir::TimestepMethod::kFixed,
+      sleipnir::TranscriptionMethod::kDirectTranscription);
+  solver.ConstrainInitialState(0.0);
+  solver.SetUpperInputBound(12);
+  solver.SetLowerInputBound(-12);
+
+  // Set up objective
+  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);
+
+  solver.Solve();
+
+  // The first state
+  fmt::print("x₀ = {}\n", solver.X().Value(0, 0));
+
+  // The first input
+  fmt::print("u₀ = {}\n", solver.U().Value(0, 0));
+}
+#endif
diff --git a/test/src/control/OCPCartPoleTest.cpp b/test/src/control/OCPCartPoleTest.cpp
@@ -206,7 +206,7 @@ TEST(OCPSolverTest, DISABLED_CartPoleProblem) {
   };
 
   sleipnir::OCPSolver problem(
-      4, 1, std::chrono::duration<double>(dt.value()), N, dynamicsFunction,
+      4, 1, std::chrono::duration<double>{dt.value()}, N, dynamicsFunction,
       sleipnir::DynamicsType::kExplicitODE,
       sleipnir::TimestepMethod::kVariableSingle,
       sleipnir::TranscriptionMethod::kDirectCollocation);
@@ -217,6 +217,7 @@ TEST(OCPSolverTest, DISABLED_CartPoleProblem) {
       Eigen::Matrix<double, 4, 1>{1.0, std::numbers::pi, 0.0, 0.0});
   problem.SetLowerInputBound(-u_max.value());
   problem.SetUpperInputBound(u_max.value());
+
   // x = [q, q̇]ᵀ = [x, θ, ẋ, θ̇]ᵀ
   auto X = problem.X();
 

diff --git a/test/src/control/OCPFlywheelTest.cpp b/test/src/control/OCPFlywheelTest.cpp
@@ -40,12 +40,13 @@ void TestFlywheel(std::string testName, Eigen::Matrix<double, 1, 1> A,
   Eigen::Matrix<double, 1, 1> B_discrete{(1.0 - A_discrete(0)) * B(0)};
   Eigen::Matrix<double, 1, 1> r{10.0};
 
-  sleipnir::OCPSolver solver(1, 1, std::chrono::duration<double>(dt.value()), N,
+  sleipnir::OCPSolver solver(1, 1, std::chrono::duration<double>{dt.value()}, N,
                              F, dynamicsType, sleipnir::TimestepMethod::kFixed,
                              method);
   solver.ConstrainInitialState(0.0);
   solver.SetUpperInputBound(12);
   solver.SetLowerInputBound(-12);
+
   // Set up objective
   Eigen::Matrix<double, 1, N + 1> r_mat =
       r * Eigen::Matrix<double, 1, N + 1>::Ones();
@@ -152,9 +153,11 @@ void TestFlywheel(std::string testName, Eigen::Matrix<double, 1, 1> A,
 TEST(OCPSolverTest, FlywheelExplicit) {
   Eigen::Matrix<double, 1, 1> A{-1.0};
   Eigen::Matrix<double, 1, 1> B{1.0};
+
   auto f_ode = [=](sleipnir::Variable t, sleipnir::VariableMatrix x,
                    sleipnir::VariableMatrix u,
                    sleipnir::Variable dt) { return A * x + B * u; };
+
   TestFlywheel("Explicit Collocation", A, B, f_ode,
                sleipnir::DynamicsType::kExplicitODE,
                sleipnir::TranscriptionMethod::kDirectCollocation);
@@ -169,13 +172,16 @@ TEST(OCPSolverTest, FlywheelExplicit) {
 TEST(OCPSolverTest, FlywheelDiscrete) {
   Eigen::Matrix<double, 1, 1> A{-1.0};
   Eigen::Matrix<double, 1, 1> B{1.0};
-  units::second_t dt = 5_ms;
+  constexpr units::second_t dt = 5_ms;
+
   Eigen::Matrix<double, 1, 1> A_discrete{std::exp(A(0) * dt.value())};
   Eigen::Matrix<double, 1, 1> B_discrete{(1.0 - A_discrete(0)) * B(0)};
+
   auto f_discrete = [=](sleipnir::Variable t, sleipnir::VariableMatrix x,
                         sleipnir::VariableMatrix u, sleipnir::Variable dt) {
     return A_discrete * x + B_discrete * u;
   };
+
   TestFlywheel("Discrete Transcription", A, B, f_discrete,
                sleipnir::DynamicsType::kDiscrete,
                sleipnir::TranscriptionMethod::kDirectTranscription);

diff --git a/test/src/control/OCPRobotTest.cpp b/test/src/control/OCPRobotTest.cpp
@@ -37,7 +37,7 @@ TEST(OCPSolverTest, Robot) {
   constexpr Eigen::Matrix<double, 2, 1> inputMin{{0.0, 0.0}};
 
   sleipnir::OCPSolver solverMinTime(
-      3, 2, std::chrono::duration<double>(minTimestep.value()), N,
+      3, 2, std::chrono::duration<double>{minTimestep.value()}, N,
       dynamicsFunction, sleipnir::DynamicsType::kExplicitODE,
       sleipnir::TimestepMethod::kVariableSingle,
       sleipnir::TranscriptionMethod::kDirectTranscription);
@@ -57,8 +57,8 @@ TEST(OCPSolverTest, Robot) {
   // TODO: Solver is unhappy when more than one minimum timestep is constrained.
   // Detect this in either OptimizationProblem or OCPSolver.
   solverMinTime.SetMinTimestep(
-      std::chrono::duration<double>(minTimestep.value()));
-  solverMinTime.SetMaxTimestep(std::chrono::duration<double>(3.0));
+      std::chrono::duration<double>{minTimestep.value()});
+  solverMinTime.SetMaxTimestep(std::chrono::duration<double>{3.0});
 
   // Set up objective
   solverMinTime.Minimize(solverMinTime.DT() *