diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index 63ab79eb83..8e22dcaed9 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -420,6 +420,7 @@ MocoSolution MocoCasADiSolver::solveImpl() const { !get_minimize_lagrange_multipliers()) { checkConstraintJacobianRank(mocoSolution); } + checkSlackVariables(mocoSolution); const long long elapsed = stopwatch.getElapsedTimeInNs(); setSolutionStats(mocoSolution, casSolution.stats.at("success"), diff --git a/OpenSim/Moco/MocoDirectCollocationSolver.cpp b/OpenSim/Moco/MocoDirectCollocationSolver.cpp index 7ce9297504..5388f91243 100644 --- a/OpenSim/Moco/MocoDirectCollocationSolver.cpp +++ b/OpenSim/Moco/MocoDirectCollocationSolver.cpp @@ -87,3 +87,42 @@ void MocoDirectCollocationSolver::checkConstraintJacobianRank( log_warn(dashes); } } + +void MocoDirectCollocationSolver::checkSlackVariables( + const MocoSolution& solution) const { + const auto& slacks = solution.getSlacksTrajectory(); + const auto& slackNames = solution.getSlackNames(); + const SimTK::Real threshold = 1e-3; + bool largeSlackDetected = false; + + std::vector slackWarnings; + for (int islack = 0; islack < slacks.ncol(); ++islack) { + if (SimTK::max(SimTK::abs(slacks.col(islack))) > threshold) { + largeSlackDetected = true; + std::stringstream ss; + ss << "Slack variable '" << slackNames[islack] << "' has a maximum " + << "value of " << SimTK::max(SimTK::abs(slacks.col(islack))) + << "."; + slackWarnings.push_back(ss.str()); + } + } + + if (largeSlackDetected) { + const std::string dashes(50, '-'); + log_warn(dashes); + log_warn("Large slack variables detected."); + log_warn(dashes); + for (const auto& warning : slackWarnings) { + log_warn(warning); + } + log_warn(""); + log_warn("Slack variables with values larger than ~{} might", threshold); + log_warn("indicate that the problem is struggling to enforce"); + log_warn("kinematic constraints in the problem. Since slack variables"); + log_warn("interact with defect constraints in the direct collocation "); + log_warn("formulation, large slack values may affect the quality "); + log_warn("of the solution. Consider refining the mesh or adjusting "); + log_warn("the constraint tolerance in the MocoProblem."); + log_warn(dashes); + } +} diff --git a/OpenSim/Moco/MocoDirectCollocationSolver.h b/OpenSim/Moco/MocoDirectCollocationSolver.h index a0b9dea686..d671d17b15 100644 --- a/OpenSim/Moco/MocoDirectCollocationSolver.h +++ b/OpenSim/Moco/MocoDirectCollocationSolver.h @@ -183,8 +183,9 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "Takes precedence over uniform mesh with num_mesh_intervals."); void constructProperties(); - // Helper function for post-processing the solution. + // Helper functions for post-processing the solution. void checkConstraintJacobianRank(const MocoSolution& mocoSolution) const; + void checkSlackVariables(const MocoSolution& mocoSolution) const; }; } // namespace OpenSim diff --git a/OpenSim/Moco/MocoTropterSolver.cpp b/OpenSim/Moco/MocoTropterSolver.cpp index 4375c6b3c4..0f17c0ba58 100644 --- a/OpenSim/Moco/MocoTropterSolver.cpp +++ b/OpenSim/Moco/MocoTropterSolver.cpp @@ -372,6 +372,7 @@ MocoSolution MocoTropterSolver::solveImpl() const { !get_minimize_lagrange_multipliers()) { checkConstraintJacobianRank(mocoSolution); } + checkSlackVariables(mocoSolution); // TODO move this to convert(): const long long elapsed = stopwatch.getElapsedTimeInNs(); diff --git a/OpenSim/Moco/Test/testMocoConstraints.cpp b/OpenSim/Moco/Test/testMocoConstraints.cpp index 63e3b062ce..ce4bbaf062 100644 --- a/OpenSim/Moco/Test/testMocoConstraints.cpp +++ b/OpenSim/Moco/Test/testMocoConstraints.cpp @@ -581,7 +581,7 @@ MocoTrajectory runForwardSimulation( } // Check that the constraint errors from a MocoSolution are within a specified -// tolerance. +// tolerance. Also, check that the slack variables have a reasonable magnitude. void checkConstraintErrors(const MocoSolution& solution, const Model& model, bool enforce_constraint_derivatives, const std::string& method) { StatesTrajectory statesTraj = solution.exportToStatesTrajectory(model); @@ -613,12 +613,16 @@ void checkConstraintErrors(const MocoSolution& solution, const Model& model, } } + // If problems have converged and all kinematic constraints are satisfied, + // we expect the slack variables to be reasonably small. const auto& slacks = solution.getSlacksTrajectory(); - CAPTURE(slacks); - REQUIRE_THAT(slacks.normRMS(), Catch::Matchers::WithinAbs(0, 1e-6)); + for (int icol = 0; icol < slacks.ncol(); ++icol) { + CAPTURE(slacks.col(icol)); + REQUIRE_THAT(SimTK::max(SimTK::abs(slacks.col(icol))), + Catch::Matchers::WithinAbs(0, 1e-5)); + } } - /// Direct collocation subtests. /// ---------------------------- @@ -928,48 +932,26 @@ TEST_CASE("DoublePendulum tests, Bordalba2023 method", "[casadi]") { std::string section = fmt::format("scheme: {}, dynamics_mode: {}", scheme, dynamics_mode); + // Trapezoidal rule requires more mesh intervals to keep slack variables + // small. + int num_mesh_intervals = scheme == "trapezoidal" ? 50 : 25; DYNAMIC_SECTION(section) { SECTION("CoordinateCouplerConstraint") { testDoublePendulumCoordinateCoupler(true, - dynamics_mode, "Bordalba2023", scheme); + dynamics_mode, "Bordalba2023", scheme, num_mesh_intervals); } SECTION("PrescribedMotion") { testDoublePendulumPrescribedMotion(true, - dynamics_mode, "Bordalba2023", scheme); + dynamics_mode, "Bordalba2023", scheme, num_mesh_intervals); } SECTION("PointOnLine") { testDoublePendulumPointOnLine( - true, dynamics_mode, "Bordalba2023", scheme); + true, dynamics_mode, "Bordalba2023", scheme, + num_mesh_intervals); } } } -// TEST_CASE("DoublePendulum tests, Bordalba2023 method (implicit)", -// "[implicit]") { -// std::string scheme = GENERATE(as{}, -// "trapezoidal", "hermite-simpson", "legendre-gauss-3", -// "legendre-gauss-radau-3"); - -// int num_mesh_intervals = 25; -// DYNAMIC_SECTION("scheme: " << scheme) { -// SECTION("CoordinateCouplerConstraint") { -// testDoublePendulumCoordinateCoupler( -// true, "implicit", "Bordalba2023", scheme, -// num_mesh_intervals); -// } -// SECTION("PrescribedMotion") { -// testDoublePendulumPrescribedMotion( -// true, "implicit", "Bordalba2023", scheme, -// num_mesh_intervals); -// } -// SECTION("PointOnLine") { -// testDoublePendulumPointOnLine( -// true, "implicit", "Bordalba2023", scheme, -// num_mesh_intervals); -// } -// } -// } - TEST_CASE("Bad configurations with kinematic constraints") { MocoStudy study; study.setName("double_pendulum_coordinate_coupler"); diff --git a/OpenSim/Moco/tropter/TropterProblem.cpp b/OpenSim/Moco/tropter/TropterProblem.cpp index 3e45dd3f0f..19aeae7bc7 100644 --- a/OpenSim/Moco/tropter/TropterProblem.cpp +++ b/OpenSim/Moco/tropter/TropterProblem.cpp @@ -143,9 +143,11 @@ convertIterateTropterToMoco(const tropIterateType& tropSol, input_control_names, multiplier_names, derivative_names, parameter_names, states, controls, input_controls, multipliers, derivatives, parameters); - // Append slack variables. + + // Append slack variables. Interpolate slack variables to remove NaNs. for (int i = 0; i < numSlacks; ++i) { - mocoIter.appendSlack(slack_names[i], slacks.col(i)); + mocoIter.appendSlack(slack_names[i], + interpolate(time, slacks.col(i), time, true, true)); } return mocoIter; }