Continuing inverse kinematics after IpOpt fails to achieve desired accuracy

Applications/IK/ik.cpp

@@ -141,6 +141,20 @@ int main(int argc,char **argv)
     log_info("Constructing tool from setup file {}.", setupFileName);
     InverseKinematicsTool ik(setupFileName);
 
+    // Adaptive accuracy announcement
+    if (ik.get_adaptiveAccuracy())
+        log_info("Adaptive accuracy enabled with step {} and threshold {}.",
+                std::to_string(ik.get_adaptiveAccuracyStep()),
+                std::to_string(ik.get_adaptiveAccuracyThreshold()));
+    else
+        log_info("Adaptive accuracy not enabled.");
+
+    // Ignoring convergence errors announcement
+    if (ik.get_ignoreConvergenceErrors())
+        log_info("Ignoring convergence errors.");
+    else
+        log_info("Ignoring convergence not enabled.");
+
     // start timing
     std::clock_t startTime = std::clock();
 

OpenSim/Simulation/AssemblySolver.cpp

@@ -282,7 +282,7 @@ void AssemblySolver::track(SimTK::State &s)
     catch (const std::exception& ex)
     {
         log_info( "AssemblySolver::track() attempt Failed: {}", ex.what());
-        throw Exception("AssemblySolver::track() attempt failed.");
+        throw Exception(fmt::format("AssemblySolver::track() attempt failed because:\n{}", ex.what()));
     }
 }
 

OpenSim/Tools/InverseKinematicsTool.cpp

@@ -169,7 +169,10 @@ bool InverseKinematicsTool::run()
         // create the solver given the input data
         InverseKinematicsSolver ikSolver(*_model, make_shared<MarkersReference>(markersReference),
             coordinateReferences, get_constraint_weight());
-        ikSolver.setAccuracy(get_accuracy());
+        double default_accuracy = get_accuracy();
+        double currentAccuracy = default_accuracy;
+        bool runAssemble = false;
+        ikSolver.setAccuracy(default_accuracy);
         s.updTime() = times[start_ix];
         ikSolver.assemble(s);
         kinematicsReporter->begin(s);
@@ -192,7 +195,91 @@ bool InverseKinematicsTool::run()
 
         for (int i = start_ix; i <= final_ix; ++i) {
             s.updTime() = times[i];
-            ikSolver.track(s);
+
+            // if running with adaptive accurcay and the accuracy was changed on
+            // the previous step, change it back. Do not reassemble here,
+            // because it can also throw optimizer error
+            if (get_adaptiveAccuracy() && currentAccuracy != default_accuracy) {
+                if (get_report_errors())
+                    log_info("Adaptive accuracy. Resetting accuracy to {}.",
+                            default_accuracy);
+
+                currentAccuracy = default_accuracy;
+                set_accuracy(currentAccuracy);
+                ikSolver.setAccuracy(currentAccuracy);
+
+                runAssemble = true;
+            }
+
+            // If adaptiveAccuracy is on, try to run track with decreasing
+            // accuracy until it is able to converge
+            bool ongoingAdaptingAccuracy = true;
+            while (ongoingAdaptingAccuracy) {
+                try {
+                    // if the accuracy was changed, reassemble
+                    if (runAssemble) {
+                        if (get_report_errors())
+                            log_info("Adaptive accuracy. Starting assemble at "
+                                     "time {}",
+                                    times[i]);
+                        ikSolver.assemble(s);
+                        runAssemble = false;
+                    }
+                    ikSolver.track(s);
+                    // this will become false only if track does not throw an
+                    // exception
+                    ongoingAdaptingAccuracy = false;
+                } catch (const Exception& ex) {
+                    // process exceptions that were thrown because IpOpt failed
+                    if (get_adaptiveAccuracy() &&
+                            std::string(ex.what()).find(
+                                    "Ipopt: Maximum iterations exceeded") !=
+                                    std::string::npos) {
+                        // lower accuracy
+                        currentAccuracy *= get_adaptiveAccuracyStep();
+                        if (currentAccuracy <=
+                                get_adaptiveAccuracyThreshold()) {
+                            if (get_report_errors()) {
+                                log_info("Adaptive accuracy caught exception: "
+                                         "{}",
+                                        ex.what());
+                                log_info("Updating accuracy to {}.",
+                                        currentAccuracy);
+                            }
+                            set_accuracy(currentAccuracy);
+                            ikSolver.setAccuracy(currentAccuracy);
+
+                            runAssemble = true;
+                            continue;
+                        } else {
+                            // if it cannot be solved with desired accuracy,
+                            // restore default accuracy and rethrow
+                            if (get_report_errors())
+                                log_info("Adaptive accuracy failed to "
+                                         "converge, rethrowing the error.");
+                            // to work together with ignoring convergence errors
+                            runAssemble = true;
+                        }
+                    }
+                    // if AA was used, reset the accuracy
+                    if (get_adaptiveAccuracy()) {
+                        currentAccuracy = default_accuracy;
+                        set_accuracy(currentAccuracy);
+                        ikSolver.setAccuracy(currentAccuracy);
+                    }
+                    // default behavior or AA failed
+                    if (get_ignoreConvergenceErrors() &&
+                            std::string(ex.what()).find(
+                                    "Ipopt: Maximum iterations exceeded") !=
+                                    std::string::npos) {
+                        if (get_report_errors())
+                            log_info("Ignoring convergence error.");
+                        ongoingAdaptingAccuracy = false;
+                    } else
+                        throw;
+                }
+            };
+
             // show progress line every 1000 frames so users see progress
             if (std::remainder(i - start_ix, 1000) == 0 && i != start_ix)
                 log_info("Solved {} frame(s)...", i - start_ix);

OpenSim/Tools/InverseKinematicsToolBase.h

@@ -68,7 +68,30 @@ class OSIMTOOLS_API InverseKinematicsToolBase : public Tool {
 
     OpenSim_DECLARE_PROPERTY(accuracy, double,
             "The accuracy of the solution in absolute terms, i.e. the number of "
-            "significant digits to which the solution can be trusted. Default 1e-5.");
+            "significant digits to which the solution can be trusted. Default "
+            "1e-5.");
+
+    OpenSim_DECLARE_PROPERTY(adaptiveAccuracy, bool,
+            "If true, whenever a timepoint cannot be resolved with the desired "
+            "threshold, a lower accuracy will be used for that point instead "
+            "of throwing exception. Default false.");
+
+    OpenSim_DECLARE_PROPERTY(adaptiveAccuracyStep, double,
+            "A parameter of adaptive accuracy behavior. Increase the threshold "
+            "by this value every time the IK solver fails to reach the desired "
+            "accuracy threshold and throws an exception. For the next step, "
+            "accuracy threshold is reset to this->get_accuracy(). Has to be > 1."
+            "Default 10.");
+
+    OpenSim_DECLARE_PROPERTY(adaptiveAccuracyThreshold, double,
+            "A parameter of adaptive accuracy behavior. The program will stop "
+            "raising the threshold when the threshold will be greater than "
+            "this value and throw the error. Default: 1e-3.");
+
+    OpenSim_DECLARE_PROPERTY(ignoreConvergenceErrors, bool,
+            "Make the inverse kinematics proceed to the next frame instead of "
+            "breaking whenever the optimizer cannot converge on a frame. "
+            "Default false.");
 
     OpenSim_DECLARE_LIST_PROPERTY_SIZE(
             time_range, double, 2, "The time range for the study.");
@@ -115,6 +138,10 @@ class OSIMTOOLS_API InverseKinematicsToolBase : public Tool {
         constructProperty_model_file("");
         constructProperty_constraint_weight(SimTK::Infinity);
         constructProperty_accuracy(1e-5);
+        constructProperty_adaptiveAccuracy(false);
+        constructProperty_adaptiveAccuracyStep(10);
+        constructProperty_adaptiveAccuracyThreshold(1e-3);
+        constructProperty_ignoreConvergenceErrors(false);
         Array<double> range{SimTK::Infinity, 2};
         // Make range -Infinity to Infinity unless limited by data
         range[0] = -SimTK::Infinity;