Finalize test improvements; add warning for large slack variables

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"),

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<std::string> 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);
+    }
+} 

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

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();

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<MocoCasADiSolver>(true, 
-                    dynamics_mode, "Bordalba2023", scheme);
+                    dynamics_mode, "Bordalba2023", scheme, num_mesh_intervals);
         }
         SECTION("PrescribedMotion") {
             testDoublePendulumPrescribedMotion<MocoCasADiSolver>(true, 
-                    dynamics_mode, "Bordalba2023", scheme);
+                    dynamics_mode, "Bordalba2023", scheme, num_mesh_intervals);
         }
         SECTION("PointOnLine") {
             testDoublePendulumPointOnLine<MocoCasADiSolver>(
-                    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<std::string>{},
-//             "trapezoidal", "hermite-simpson", "legendre-gauss-3", 
-//             "legendre-gauss-radau-3");
-
-//     int num_mesh_intervals = 25;
-//     DYNAMIC_SECTION("scheme: " << scheme) {        
-//         SECTION("CoordinateCouplerConstraint") {
-//             testDoublePendulumCoordinateCoupler<MocoCasADiSolver>(
-//                     true, "implicit", "Bordalba2023", scheme,
-//                     num_mesh_intervals);
-//         }
-//         SECTION("PrescribedMotion") {
-//             testDoublePendulumPrescribedMotion<MocoCasADiSolver>(
-//                     true, "implicit", "Bordalba2023", scheme,
-//                     num_mesh_intervals);
-//         }
-//         SECTION("PointOnLine") {
-//             testDoublePendulumPointOnLine<MocoCasADiSolver>(
-//                     true, "implicit", "Bordalba2023", scheme,
-//                     num_mesh_intervals);
-//         }
-//     }
-// }
-
 TEST_CASE("Bad configurations with kinematic constraints") {
     MocoStudy study;
     study.setName("double_pendulum_coordinate_coupler");

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;
 }