diff --git a/Applications/IK/ik.cpp b/Applications/IK/ik.cpp index 2b1684e23d..e32e356e9f 100644 --- a/Applications/IK/ik.cpp +++ b/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(); diff --git a/OpenSim/Simulation/AssemblySolver.cpp b/OpenSim/Simulation/AssemblySolver.cpp index 327da77c9a..c3b7d8e2e9 100644 --- a/OpenSim/Simulation/AssemblySolver.cpp +++ b/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())); } } diff --git a/OpenSim/Tools/InverseKinematicsTool.cpp b/OpenSim/Tools/InverseKinematicsTool.cpp index f43f543424..88ca416f82 100644 --- a/OpenSim/Tools/InverseKinematicsTool.cpp +++ b/OpenSim/Tools/InverseKinematicsTool.cpp @@ -169,7 +169,10 @@ bool InverseKinematicsTool::run() // create the solver given the input data InverseKinematicsSolver ikSolver(*_model, make_shared(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); diff --git a/OpenSim/Tools/InverseKinematicsToolBase.h b/OpenSim/Tools/InverseKinematicsToolBase.h index 9f369a9c09..707271c524 100644 --- a/OpenSim/Tools/InverseKinematicsToolBase.h +++ b/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 range{SimTK::Infinity, 2}; // Make range -Infinity to Infinity unless limited by data range[0] = -SimTK::Infinity;