Make exit condition checks for failing tests more specific (#282)

SleipnirGroup · Dec 23, 2023 · 40ba228 · 40ba228
1 parent 1e16997
commit 40ba228
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 32 deletions.
diff --git a/jormungandr/test/optimization/optimization_problem_arm_on_elevator_test.py b/jormungandr/test/optimization/optimization_problem_arm_on_elevator_test.py
@@ -92,4 +92,7 @@ def test_optimization_problem_arm_on_elevator():
     assert status.equality_constraint_type == ExpressionType.LINEAR
     assert status.inequality_constraint_type == ExpressionType.NONLINEAR
     # FIXME: Fails with "bad search direction"
-    # assert status.exit_condition == SolverExitCondition.SUCCESS
+    assert (
+        status.exit_condition == SolverExitCondition.SUCCESS
+        or status.exit_condition == SolverExitCondition.BAD_SEARCH_DIRECTION
+    )
diff --git a/test/src/control/OCPSolverTest_CartPole.cpp b/test/src/control/OCPSolverTest_CartPole.cpp
@@ -88,38 +88,40 @@ TEST(OCPSolverTest, CartPole) {
             status.equalityConstraintType);
   EXPECT_EQ(sleipnir::ExpressionType::kLinear, status.inequalityConstraintType);
   // FIXME: Fails with "bad search direction"
-  // EXPECT_EQ(sleipnir::SolverExitCondition::kSuccess, status.exitCondition);
-
-#if 0
-  // Verify initial state
-  EXPECT_NEAR(0.0, X.Value(0, 0), 1e-2);
-  EXPECT_NEAR(0.0, X.Value(1, 0), 1e-2);
-  EXPECT_NEAR(0.0, X.Value(2, 0), 1e-2);
-  EXPECT_NEAR(0.0, X.Value(3, 0), 1e-2);
-
-  // Verify solution
-  Eigen::Matrix<double, 4, 1> x{0.0, 0.0, 0.0, 0.0};
-  Eigen::Matrix<double, 1, 1> u{0.0};
-  for (int k = 0; k < N; ++k) {
-    u = problem.U().Col(k).Value();
-
-    // Verify state
-    EXPECT_NEAR(x(0), X.Value(0, k), 1e-2) << fmt::format("  k = {}", k);
-    EXPECT_NEAR(x(1), X.Value(1, k), 1e-2) << fmt::format("  k = {}", k);
-    EXPECT_NEAR(x(2), X.Value(2, k), 1e-2) << fmt::format("  k = {}", k);
-    EXPECT_NEAR(x(3), X.Value(3, k), 1e-2) << fmt::format("  k = {}", k);
+  EXPECT_TRUE(status.exitCondition == sleipnir::SolverExitCondition::kSuccess ||
+              status.exitCondition ==
+                  sleipnir::SolverExitCondition::kBadSearchDirection);
+
+  if (status.exitCondition == sleipnir::SolverExitCondition::kSuccess) {
+    // Verify initial state
+    EXPECT_NEAR(0.0, X.Value(0, 0), 1e-2);
+    EXPECT_NEAR(0.0, X.Value(1, 0), 1e-2);
+    EXPECT_NEAR(0.0, X.Value(2, 0), 1e-2);
+    EXPECT_NEAR(0.0, X.Value(3, 0), 1e-2);
+
+    // Verify solution
+    Eigen::Matrix<double, 4, 1> x{0.0, 0.0, 0.0, 0.0};
+    Eigen::Matrix<double, 1, 1> u{0.0};
+    for (int k = 0; k < N; ++k) {
+      u = problem.U().Col(k).Value();
+
+      // Verify state
+      EXPECT_NEAR(x(0), X.Value(0, k), 1e-2) << fmt::format("  k = {}", k);
+      EXPECT_NEAR(x(1), X.Value(1, k), 1e-2) << fmt::format("  k = {}", k);
+      EXPECT_NEAR(x(2), X.Value(2, k), 1e-2) << fmt::format("  k = {}", k);
+      EXPECT_NEAR(x(3), X.Value(3, k), 1e-2) << fmt::format("  k = {}", k);
+
+      // Project state forward
+      x = RK4(CartPoleDynamicsDouble, x, u, dt);
+    }
 
-    // Project state forward
-    x = RK4(CartPoleDynamicsDouble, x, u, dt);
+    // Verify final state
+    EXPECT_NEAR(1.0, X.Value(0, N - 1), 1e-2);
+    EXPECT_NEAR(std::numbers::pi, X.Value(1, N - 1), 1e-2);
+    EXPECT_NEAR(0.0, X.Value(2, N - 1), 1e-2);
+    EXPECT_NEAR(0.0, X.Value(3, N - 1), 1e-2);
   }
 
-  // Verify final state
-  EXPECT_NEAR(1.0, X.Value(0, N - 1), 1e-2);
-  EXPECT_NEAR(std::numbers::pi, X.Value(1, N - 1), 1e-2);
-  EXPECT_NEAR(0.0, X.Value(2, N - 1), 1e-2);
-  EXPECT_NEAR(0.0, X.Value(3, N - 1), 1e-2);
-#endif
-
   // Log states for offline viewing
   std::ofstream states{"OCPSolver Cart-pole states.csv"};
   if (states.is_open()) {

diff --git a/test/src/control/OCPSolverTest_Unicycle.cpp b/test/src/control/OCPSolverTest_Unicycle.cpp
@@ -83,7 +83,9 @@ TEST(OCPSolverTest, Unicycle) {
             status.equalityConstraintType);
   EXPECT_EQ(sleipnir::ExpressionType::kLinear, status.inequalityConstraintType);
   // FIXME: Fails with "bad search direction"
-  // EXPECT_EQ(sleipnir::SolverExitCondition::kSuccess, status.exitCondition);
+  EXPECT_TRUE(status.exitCondition == sleipnir::SolverExitCondition::kSuccess ||
+              status.exitCondition ==
+                  sleipnir::SolverExitCondition::kBadSearchDirection);
 
   // Log states for offline viewing
   std::ofstream states{"OCPSolver Unicycle states.csv"};

diff --git a/test/src/optimization/OptimizationProblemTest_ArmOnElevator.cpp b/test/src/optimization/OptimizationProblemTest_ArmOnElevator.cpp
@@ -102,5 +102,7 @@ TEST(OptimizationProblemTest, ArmOnElevator) {
   EXPECT_EQ(sleipnir::ExpressionType::kNonlinear,
             status.inequalityConstraintType);
   // FIXME: Fails with "bad search direction"
-  // EXPECT_EQ(sleipnir::SolverExitCondition::kSuccess, status.exitCondition);
+  EXPECT_TRUE(status.exitCondition == sleipnir::SolverExitCondition::kSuccess ||
+              status.exitCondition ==
+                  sleipnir::SolverExitCondition::kBadSearchDirection);
 }