From 40ba2285b8abbc625353e016c67c517ca37a8679 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 22 Dec 2023 17:02:43 -0800 Subject: [PATCH] Make exit condition checks for failing tests more specific (#282) --- ...timization_problem_arm_on_elevator_test.py | 5 +- test/src/control/OCPSolverTest_CartPole.cpp | 60 ++++++++++--------- test/src/control/OCPSolverTest_Unicycle.cpp | 4 +- .../OptimizationProblemTest_ArmOnElevator.cpp | 4 +- 4 files changed, 41 insertions(+), 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 index da833129..87422371 100644 --- 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 index c3e877d8..17e22538 100644 --- 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 x{0.0, 0.0, 0.0, 0.0}; - Eigen::Matrix 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 x{0.0, 0.0, 0.0, 0.0}; + Eigen::Matrix 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 index 3bdf5a18..91a1e69a 100644 --- 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 index d4db41a6..13a03170 100644 --- 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); }