From 63a35cfa11fd975ee214911a792588fb33a9e6c8 Mon Sep 17 00:00:00 2001 From: Anton Sobinov Date: Mon, 27 Dec 2021 17:22:11 -0600 Subject: [PATCH 1/3] Adaptive accuracy option --- Applications/IK/ik.cpp | 8 +++ OpenSim/Tools/InverseKinematicsTool.cpp | 65 +++++++++++++++++++++-- OpenSim/Tools/InverseKinematicsToolBase.h | 23 +++++++- 3 files changed, 92 insertions(+), 4 deletions(-) diff --git a/Applications/IK/ik.cpp b/Applications/IK/ik.cpp index 2b1684e23d..50d39e98fd 100644 --- a/Applications/IK/ik.cpp +++ b/Applications/IK/ik.cpp @@ -141,6 +141,14 @@ int main(int argc,char **argv) log_info("Constructing tool from setup file {}.", setupFileName); InverseKinematicsTool ik(setupFileName); + // testprint + 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."); + // start timing std::clock_t startTime = std::clock(); diff --git a/OpenSim/Tools/InverseKinematicsTool.cpp b/OpenSim/Tools/InverseKinematicsTool.cpp index f43f543424..5c818d708a 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,62 @@ bool InverseKinematicsTool::run() for (int i = start_ix; i <= final_ix; ++i) { s.updTime() = times[i]; - ikSolver.track(s); + + // if the accuracy was changed on the previous step, change it back + if (get_adaptiveAccuracy() && currentAccuracy != default_accuracy) { + log_info("Resetting accuracy to {}.", default_accuracy); + + currentAccuracy = default_accuracy; + set_accuracy(currentAccuracy); + ikSolver.setAccuracy(currentAccuracy); + + runAssemble = true; + } + + // Try to run track until it does not throw an exception with decreasing + // accuracy threshold if adaptiveAccuracy is on + bool ongoingAdaptingAccuracy = true; + while (ongoingAdaptingAccuracy) { + try { + if (runAssemble) { + log_info("Starting asseble at time {}", + times[i]); + ikSolver.assemble(s); + runAssemble = false; + } + log_info("Starting track at time {} with accuracy {}.", + times[i], currentAccuracy); + ikSolver.track(s); + ongoingAdaptingAccuracy = false; + } + catch (const std::exception& ex) { + if (get_adaptiveAccuracy()) { + // try again with lower accuracy + currentAccuracy *= get_adaptiveAccuracyStep(); + if (currentAccuracy <= + get_adaptiveAccuracyThreshold()) { + log_info("Caught exception by adaptive accuracy: {}", ex.what()); + log_info("Updating accuracy to {}.", + currentAccuracy); + + set_accuracy(currentAccuracy); + ikSolver.setAccuracy(currentAccuracy); + + runAssemble = true; + } else { // if it cannot be solved with desired + // accuracy, rethrow + log_info("Adaptive accuracy failed, rethrowing the " + "Assembler error."); + throw(ex); + } + + } else { // default behavior + throw(ex); + } + } + }; + + // 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); @@ -398,7 +456,8 @@ void InverseKinematicsTool::updateFromXMLNode(SimTK::Xml::Element& aNode, int ve Object::updateFromXMLNode(aNode, versionNumber); } -void InverseKinematicsTool::populateReferences(MarkersReference& markersReference, +void InverseKinematicsTool::populateReferences( + MarkersReference& markersReference, SimTK::Array_&coordinateReferences) const { FunctionSet *coordFunctions = NULL; diff --git a/OpenSim/Tools/InverseKinematicsToolBase.h b/OpenSim/Tools/InverseKinematicsToolBase.h index 9f369a9c09..4276419897 100644 --- a/OpenSim/Tools/InverseKinematicsToolBase.h +++ b/OpenSim/Tools/InverseKinematicsToolBase.h @@ -68,7 +68,25 @@ 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_LIST_PROPERTY_SIZE( time_range, double, 2, "The time range for the study."); @@ -115,6 +133,9 @@ 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); Array range{SimTK::Infinity, 2}; // Make range -Infinity to Infinity unless limited by data range[0] = -SimTK::Infinity; From 93ad8828a4503678ad0caeceb07d382011a9fcbf Mon Sep 17 00:00:00 2001 From: Anton Sobinov Date: Tue, 28 Dec 2021 16:28:11 -0600 Subject: [PATCH 2/3] Cleaner version with an additional flag of ingoring thrown errors --- Applications/IK/ik.cpp | 8 ++- OpenSim/Simulation/AssemblySolver.cpp | 2 +- OpenSim/Tools/InverseKinematicsTool.cpp | 82 +++++++++++++++-------- OpenSim/Tools/InverseKinematicsToolBase.h | 8 ++- 4 files changed, 70 insertions(+), 30 deletions(-) diff --git a/Applications/IK/ik.cpp b/Applications/IK/ik.cpp index 50d39e98fd..e32e356e9f 100644 --- a/Applications/IK/ik.cpp +++ b/Applications/IK/ik.cpp @@ -141,7 +141,7 @@ int main(int argc,char **argv) log_info("Constructing tool from setup file {}.", setupFileName); InverseKinematicsTool ik(setupFileName); - // testprint + // Adaptive accuracy announcement if (ik.get_adaptiveAccuracy()) log_info("Adaptive accuracy enabled with step {} and threshold {}.", std::to_string(ik.get_adaptiveAccuracyStep()), @@ -149,6 +149,12 @@ int main(int argc,char **argv) 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 5c818d708a..8f61867c20 100644 --- a/OpenSim/Tools/InverseKinematicsTool.cpp +++ b/OpenSim/Tools/InverseKinematicsTool.cpp @@ -196,9 +196,13 @@ bool InverseKinematicsTool::run() for (int i = start_ix; i <= final_ix; ++i) { s.updTime() = times[i]; - // if the accuracy was changed on the previous step, change it back + // 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) { - log_info("Resetting accuracy to {}.", default_accuracy); + if (get_report_errors()) + log_info("Adaptive accuracy. Resetting accuracy to {}.", + default_accuracy); currentAccuracy = default_accuracy; set_accuracy(currentAccuracy); @@ -207,50 +211,75 @@ bool InverseKinematicsTool::run() runAssemble = true; } - // Try to run track until it does not throw an exception with decreasing - // accuracy threshold if adaptiveAccuracy is on + // 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) { - log_info("Starting asseble at time {}", - times[i]); + if (get_report_errors()) + log_info("Adaptive accuracy. Starting asseble at " + "time {}", + times[i]); ikSolver.assemble(s); runAssemble = false; } - log_info("Starting track at time {} with accuracy {}.", - times[i], currentAccuracy); ikSolver.track(s); + // this will become false only if track does not throw an + // exception ongoingAdaptingAccuracy = false; - } - catch (const std::exception& ex) { - if (get_adaptiveAccuracy()) { - // try again with lower accuracy + } 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()) { - log_info("Caught exception by adaptive accuracy: {}", ex.what()); - log_info("Updating accuracy to {}.", - currentAccuracy); - + 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; - } else { // if it cannot be solved with desired - // accuracy, rethrow - log_info("Adaptive accuracy failed, rethrowing the " - "Assembler error."); - throw(ex); + 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; } - - } else { // default behavior - throw(ex); } + // 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); @@ -456,8 +485,7 @@ void InverseKinematicsTool::updateFromXMLNode(SimTK::Xml::Element& aNode, int ve Object::updateFromXMLNode(aNode, versionNumber); } -void InverseKinematicsTool::populateReferences( - MarkersReference& markersReference, +void InverseKinematicsTool::populateReferences(MarkersReference& markersReference, SimTK::Array_&coordinateReferences) const { FunctionSet *coordFunctions = NULL; diff --git a/OpenSim/Tools/InverseKinematicsToolBase.h b/OpenSim/Tools/InverseKinematicsToolBase.h index 4276419897..707271c524 100644 --- a/OpenSim/Tools/InverseKinematicsToolBase.h +++ b/OpenSim/Tools/InverseKinematicsToolBase.h @@ -72,7 +72,7 @@ class OSIMTOOLS_API InverseKinematicsToolBase : public Tool { "1e-5."); OpenSim_DECLARE_PROPERTY(adaptiveAccuracy, bool, - "If true, Whenever a timepoint cannot be resolved with the desired " + "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."); @@ -88,6 +88,11 @@ class OSIMTOOLS_API InverseKinematicsToolBase : public Tool { "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."); @@ -136,6 +141,7 @@ class OSIMTOOLS_API InverseKinematicsToolBase : public Tool { 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; From 58b5e3d932a850f6769589a2437f4a25886739ac Mon Sep 17 00:00:00 2001 From: Anton Sobinov Date: Tue, 22 Feb 2022 17:09:21 -0600 Subject: [PATCH 3/3] typo --- OpenSim/Tools/InverseKinematicsTool.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Tools/InverseKinematicsTool.cpp b/OpenSim/Tools/InverseKinematicsTool.cpp index 8f61867c20..88ca416f82 100644 --- a/OpenSim/Tools/InverseKinematicsTool.cpp +++ b/OpenSim/Tools/InverseKinematicsTool.cpp @@ -219,7 +219,7 @@ bool InverseKinematicsTool::run() // if the accuracy was changed, reassemble if (runAssemble) { if (get_report_errors()) - log_info("Adaptive accuracy. Starting asseble at " + log_info("Adaptive accuracy. Starting assemble at " "time {}", times[i]); ikSolver.assemble(s);