From 2f0d23947aba112fd1bf6db4e3a668bf8124eef3 Mon Sep 17 00:00:00 2001 From: pep Date: Tue, 27 Jun 2023 14:06:50 +0200 Subject: [PATCH 1/6] adds MuscleStateEstimatorResult struct to replace std::map<...> Replaces the std::map<...>, which was the return type of `Millard2012EquilibriumMuscle::estimateMuscleFiberState` --- .../Millard2012EquilibriumMuscle.cpp | 134 +++++++----------- .../Actuators/Millard2012EquilibriumMuscle.h | 24 ++-- 2 files changed, 67 insertions(+), 91 deletions(-) diff --git a/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp b/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp index 397a1a6642..a926e5c9a8 100644 --- a/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp +++ b/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp @@ -556,43 +556,38 @@ computeFiberEquilibrium(SimTK::State& s, bool solveForVelocity) const double pathSpeed = solveForVelocity ? getLengtheningSpeed(s) : 0; double activation = getActivation(s); - try { - std::pair result = - estimateMuscleFiberState(activation, pathLength, pathSpeed, - tol, maxIter, solveForVelocity); - - switch(result.first) { - - case StatusFromEstimateMuscleFiberState::Success_Converged: - setActuation(s, result.second["tendon_force"]); - setFiberLength(s, result.second["fiber_length"]); - break; - - case StatusFromEstimateMuscleFiberState::Warning_FiberAtLowerBound: - log_warn("Millard2012EquilibriumMuscle static solution: '{}' is " - "at its minimum fiber length of {}.", - getName(), result.second["fiber_length"]); - setActuation(s, result.second["tendon_force"]); - setFiberLength(s, result.second["fiber_length"]); - break; - - case StatusFromEstimateMuscleFiberState::Failure_MaxIterationsReached: - // Report internal variables and throw exception. - std::ostringstream ss; - ss << "\n Solution error " << abs(result.second["solution_error"]) - << " exceeds tolerance of " << tol << "\n" - << " Newton iterations reached limit of " << maxIter << "\n" - << " Activation is " << activation << "\n" - << " Fiber length is " << result.second["fiber_length"] << "\n"; - OPENSIM_THROW_FRMOBJ(MuscleCannotEquilibrate, ss.str()); - break; - } + const MuscleStateEstimatorResult result = + estimateMuscleFiberState( + activation, + pathLength, + pathSpeed, + tol, + maxIter, + solveForVelocity); + + if (result.status == + MuscleStateEstimatorResult::Status::Failure_MaxIterationsReached) { + std::ostringstream oss; + oss << "Internal exception encountered:\n" + << " Solution error " << std::abs(result.solution_error) + << " exceeds tolerance of " << tol << "\n" + << " Newton iterations reached limit of " << maxIter << "\n" + << " Activation is " << activation << "\n" + << " Fiber length is " << result.fiber_length << "\n"; + OPENSIM_THROW_FRMOBJ(MuscleCannotEquilibrate, oss.str()); + } - } catch (const std::exception& x) { - OPENSIM_THROW_FRMOBJ(MuscleCannotEquilibrate, - "Internal exception encountered.\n" + std::string{x.what()}); + if (result.status == + MuscleStateEstimatorResult::Status::Warning_FiberAtLowerBound) { + log_warn( + "Millard2012EquilibriumMuscle static solution: '{}' is at its" + "minimum fiber length of {}.", + getName(), + result.fiber_length); } + + setActuation(s, result.tendon_force); + setFiberLength(s, result.fiber_length); } //============================================================================== @@ -1356,15 +1351,14 @@ double Millard2012EquilibriumMuscle::clampFiberLength(double lce) const return max(lce, getMinimumFiberLength()); } -std::pair -Millard2012EquilibriumMuscle::estimateMuscleFiberState( - const double aActivation, - const double pathLength, - const double pathLengtheningSpeed, - const double aSolTolerance, - const int aMaxIterations, - bool staticSolution) const +Millard2012EquilibriumMuscle::MuscleStateEstimatorResult + Millard2012EquilibriumMuscle::estimateMuscleFiberState( + const double aActivation, + const double pathLength, + const double pathLengtheningSpeed, + const double aSolTolerance, + const int aMaxIterations, + bool staticSolution) const { // If seeking a static solution, set velocities to zero and avoid the // velocity-sharing algorithm below, as it can produce nonzero fiber and @@ -1601,28 +1595,22 @@ Millard2012EquilibriumMuscle::estimateMuscleFiberState( iter++; } - // Populate the result map. - ValuesFromEstimateMuscleFiberState resultValues; + MuscleStateEstimatorResult result; + result.solution_error = ferr; - if(abs(ferr) < aSolTolerance) { // The solution converged. + if(abs(ferr) < aSolTolerance) { + result.status = MuscleStateEstimatorResult::Status::Success_Converged; - if (isFiberStateClamped(lce, dlceN)) { - lce = getMinimumFiberLength(); - } - - resultValues["solution_error"] = ferr; - resultValues["iterations"] = (double)iter; - resultValues["fiber_length"] = lce; - resultValues["fiber_velocity"] = dlce; - resultValues["tendon_force"] = fse*fiso; + result.fiber_length = clampFiberLength(lce); + result.fiber_velocity = dlce; + result.tendon_force = fse*fiso; - return std::pair - (StatusFromEstimateMuscleFiberState::Success_Converged, resultValues); + return result; } - // Fiber length is at or exceeds its lower bound. if (lce <= getMinimumFiberLength()) { + result.status = MuscleStateEstimatorResult::Status:: + Warning_FiberAtLowerBound; lce = getMinimumFiberLength(); phi = getPennationModel().calcPennationAngle(lce); @@ -1632,28 +1620,16 @@ Millard2012EquilibriumMuscle::estimateMuscleFiberState( tlN = tl/tsl; fse = fseCurve.calcValue(tlN); - resultValues["solution_error"] = ferr; - resultValues["iterations"] = (double)iter; - resultValues["fiber_length"] = lce; - resultValues["fiber_velocity"] = 0; - resultValues["tendon_force"] = fse*fiso; + result.fiber_length = lce; + result.fiber_velocity = 0; + result.tendon_force = fse*fiso; - return std::pair - (StatusFromEstimateMuscleFiberState::Warning_FiberAtLowerBound, - resultValues); + return result; } - resultValues["solution_error"] = ferr; - resultValues["iterations"] = (double)iter; - resultValues["fiber_length"] = SimTK::NaN; - resultValues["fiber_velocity"] = SimTK::NaN; - resultValues["tendon_force"] = SimTK::NaN; - - return std::pair - (StatusFromEstimateMuscleFiberState::Failure_MaxIterationsReached, - resultValues); + result.status = MuscleStateEstimatorResult::Status:: + Failure_MaxIterationsReached; + return result; } //============================================================================== diff --git a/OpenSim/Actuators/Millard2012EquilibriumMuscle.h b/OpenSim/Actuators/Millard2012EquilibriumMuscle.h index 40b66095e5..63d3c2a233 100644 --- a/OpenSim/Actuators/Millard2012EquilibriumMuscle.h +++ b/OpenSim/Actuators/Millard2012EquilibriumMuscle.h @@ -734,18 +734,19 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Millard2012EquilibriumMuscle, Muscle); // length. double clampFiberLength(double lce) const; - // Status flag returned by estimateMuscleFiberState(). - enum StatusFromEstimateMuscleFiberState { - Success_Converged, - Warning_FiberAtLowerBound, - Failure_MaxIterationsReached + struct MuscleStateEstimatorResult { + double solution_error = SimTK::NaN; + double fiber_length = SimTK::NaN; + double fiber_velocity = SimTK::NaN; + double tendon_force = SimTK::NaN; + + enum Status { + Success_Converged, + Warning_FiberAtLowerBound, + Failure_MaxIterationsReached + } status = Failure_MaxIterationsReached; }; - // Associative array of values returned by estimateMuscleFiberState(): - // solution_error, iterations, fiber_length, fiber_velocity, and - // tendon_force. - typedef std::map ValuesFromEstimateMuscleFiberState; - /* Solves fiber length and velocity to satisfy the equilibrium equations. The velocity of the entire musculotendon actuator is shared between the tendon and the fiber based on their relative mechanical stiffnesses. @@ -760,8 +761,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Millard2012EquilibriumMuscle, Muscle); @param staticSolution set to true to calculate the static equilibrium solution, setting fiber and tendon velocities to zero */ - std::pair + MuscleStateEstimatorResult estimateMuscleFiberState(const double aActivation, const double pathLength, const double pathLengtheningSpeed, From a8396a682ffeec77005d005fcf97a42edb6d8c0e Mon Sep 17 00:00:00 2001 From: pep Date: Tue, 27 Jun 2023 19:42:46 +0200 Subject: [PATCH 2/6] renaming of variables and member fields in Millard2012EquilibriumMuscle. MuscleStateEstimatorResult to MuscleStateEstimate and member fields in OpenSim style. --- .../Millard2012EquilibriumMuscle.cpp | 44 +++++++++---------- .../Actuators/Millard2012EquilibriumMuscle.h | 12 ++--- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp b/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp index a926e5c9a8..bdc8695bed 100644 --- a/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp +++ b/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp @@ -556,7 +556,7 @@ computeFiberEquilibrium(SimTK::State& s, bool solveForVelocity) const double pathSpeed = solveForVelocity ? getLengtheningSpeed(s) : 0; double activation = getActivation(s); - const MuscleStateEstimatorResult result = + const MuscleStateEstimate result = estimateMuscleFiberState( activation, pathLength, @@ -566,28 +566,28 @@ computeFiberEquilibrium(SimTK::State& s, bool solveForVelocity) const solveForVelocity); if (result.status == - MuscleStateEstimatorResult::Status::Failure_MaxIterationsReached) { + MuscleStateEstimate::Status::Failure_MaxIterationsReached) { std::ostringstream oss; oss << "Internal exception encountered:\n" - << " Solution error " << std::abs(result.solution_error) - << " exceeds tolerance of " << tol << "\n" + << " Solution error " << std::abs(result.solutionError) + << " exceeds tolerance of " << tol << "\n" << " Newton iterations reached limit of " << maxIter << "\n" << " Activation is " << activation << "\n" - << " Fiber length is " << result.fiber_length << "\n"; + << " Fiber length is " << result.fiberLength << "\n"; OPENSIM_THROW_FRMOBJ(MuscleCannotEquilibrate, oss.str()); } if (result.status == - MuscleStateEstimatorResult::Status::Warning_FiberAtLowerBound) { + MuscleStateEstimate::Status::Warning_FiberAtLowerBound) { log_warn( "Millard2012EquilibriumMuscle static solution: '{}' is at its" "minimum fiber length of {}.", getName(), - result.fiber_length); + result.fiberLength); } - setActuation(s, result.tendon_force); - setFiberLength(s, result.fiber_length); + setActuation(s, result.tendonForce); + setFiberLength(s, result.fiberLength); } //============================================================================== @@ -1351,7 +1351,7 @@ double Millard2012EquilibriumMuscle::clampFiberLength(double lce) const return max(lce, getMinimumFiberLength()); } -Millard2012EquilibriumMuscle::MuscleStateEstimatorResult +Millard2012EquilibriumMuscle::MuscleStateEstimate Millard2012EquilibriumMuscle::estimateMuscleFiberState( const double aActivation, const double pathLength, @@ -1595,24 +1595,24 @@ Millard2012EquilibriumMuscle::MuscleStateEstimatorResult iter++; } - MuscleStateEstimatorResult result; - result.solution_error = ferr; + MuscleStateEstimate result; + result.solutionError = ferr; if(abs(ferr) < aSolTolerance) { - result.status = MuscleStateEstimatorResult::Status::Success_Converged; + result.status = MuscleStateEstimate::Status::Success_Converged; - result.fiber_length = clampFiberLength(lce); - result.fiber_velocity = dlce; - result.tendon_force = fse*fiso; + result.fiberLength = clampFiberLength(lce); + result.tendonVelocity = dlce; + result.tendonForce = fse * fiso; return result; } if (lce <= getMinimumFiberLength()) { - result.status = MuscleStateEstimatorResult::Status:: + result.status = MuscleStateEstimate::Status:: Warning_FiberAtLowerBound; - lce = getMinimumFiberLength(); + lce = getMinimumFiberLength(); phi = getPennationModel().calcPennationAngle(lce); cosphi = cos(phi); tl = getPennationModel().calcTendonLength(cosphi,lce,ml); @@ -1620,14 +1620,14 @@ Millard2012EquilibriumMuscle::MuscleStateEstimatorResult tlN = tl/tsl; fse = fseCurve.calcValue(tlN); - result.fiber_length = lce; - result.fiber_velocity = 0; - result.tendon_force = fse*fiso; + result.fiberLength = lce; + result.tendonVelocity = 0; + result.tendonForce = fse * fiso; return result; } - result.status = MuscleStateEstimatorResult::Status:: + result.status = MuscleStateEstimate::Status:: Failure_MaxIterationsReached; return result; } diff --git a/OpenSim/Actuators/Millard2012EquilibriumMuscle.h b/OpenSim/Actuators/Millard2012EquilibriumMuscle.h index 63d3c2a233..52e275dfca 100644 --- a/OpenSim/Actuators/Millard2012EquilibriumMuscle.h +++ b/OpenSim/Actuators/Millard2012EquilibriumMuscle.h @@ -734,11 +734,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Millard2012EquilibriumMuscle, Muscle); // length. double clampFiberLength(double lce) const; - struct MuscleStateEstimatorResult { - double solution_error = SimTK::NaN; - double fiber_length = SimTK::NaN; - double fiber_velocity = SimTK::NaN; - double tendon_force = SimTK::NaN; + struct MuscleStateEstimate { + double solutionError = SimTK::NaN; + double fiberLength = SimTK::NaN; + double tendonVelocity = SimTK::NaN; + double tendonForce = SimTK::NaN; enum Status { Success_Converged, @@ -761,7 +761,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Millard2012EquilibriumMuscle, Muscle); @param staticSolution set to true to calculate the static equilibrium solution, setting fiber and tendon velocities to zero */ - MuscleStateEstimatorResult + MuscleStateEstimate estimateMuscleFiberState(const double aActivation, const double pathLength, const double pathLengtheningSpeed, From bc8d62a49d096d23407dd72e9024a6af617da859 Mon Sep 17 00:00:00 2001 From: pep Date: Wed, 28 Jun 2023 17:26:46 +0200 Subject: [PATCH 3/6] improve throwing message in Millard2012EquilibriumMuscle --- OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp b/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp index bdc8695bed..1e5cf06e25 100644 --- a/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp +++ b/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp @@ -568,7 +568,7 @@ computeFiberEquilibrium(SimTK::State& s, bool solveForVelocity) const if (result.status == MuscleStateEstimate::Status::Failure_MaxIterationsReached) { std::ostringstream oss; - oss << "Internal exception encountered:\n" + oss << "Failed to compute muscle equilibrium state:\n" << " Solution error " << std::abs(result.solutionError) << " exceeds tolerance of " << tol << "\n" << " Newton iterations reached limit of " << maxIter << "\n" From f3590fd3728ea2697b526725157611cc1445ad92 Mon Sep 17 00:00:00 2001 From: pep Date: Wed, 28 Jun 2023 17:27:49 +0200 Subject: [PATCH 4/6] Re-adds comments in Millard2012EquilibriumMuscle for readability --- OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp b/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp index 1e5cf06e25..c54a6fc5d5 100644 --- a/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp +++ b/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp @@ -1598,6 +1598,7 @@ Millard2012EquilibriumMuscle::MuscleStateEstimate MuscleStateEstimate result; result.solutionError = ferr; + // Check if solution converged. if(abs(ferr) < aSolTolerance) { result.status = MuscleStateEstimate::Status::Success_Converged; @@ -1608,6 +1609,7 @@ Millard2012EquilibriumMuscle::MuscleStateEstimate return result; } + // Check if fiberlength is at or exceeds lower bound. if (lce <= getMinimumFiberLength()) { result.status = MuscleStateEstimate::Status:: Warning_FiberAtLowerBound; @@ -1627,6 +1629,7 @@ Millard2012EquilibriumMuscle::MuscleStateEstimate return result; } + // Solution failed to converge. result.status = MuscleStateEstimate::Status:: Failure_MaxIterationsReached; return result; From 10d7ee16180ea76968b4ae834df4c040a8fec342 Mon Sep 17 00:00:00 2001 From: pep Date: Wed, 28 Jun 2023 17:29:28 +0200 Subject: [PATCH 5/6] prefer `enum class` over `enum`. So this changes that. --- OpenSim/Actuators/Millard2012EquilibriumMuscle.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Actuators/Millard2012EquilibriumMuscle.h b/OpenSim/Actuators/Millard2012EquilibriumMuscle.h index 52e275dfca..f617576897 100644 --- a/OpenSim/Actuators/Millard2012EquilibriumMuscle.h +++ b/OpenSim/Actuators/Millard2012EquilibriumMuscle.h @@ -740,11 +740,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Millard2012EquilibriumMuscle, Muscle); double tendonVelocity = SimTK::NaN; double tendonForce = SimTK::NaN; - enum Status { + enum class Status { Success_Converged, Warning_FiberAtLowerBound, Failure_MaxIterationsReached - } status = Failure_MaxIterationsReached; + } status = Status::Failure_MaxIterationsReached; }; /* Solves fiber length and velocity to satisfy the equilibrium equations. From 838db498248cabe7428447c3cd257457f964ae64 Mon Sep 17 00:00:00 2001 From: pep Date: Mon, 3 Jul 2023 11:36:47 +0200 Subject: [PATCH 6/6] fmt --- OpenSim/Actuators/Millard2012EquilibriumMuscle.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/OpenSim/Actuators/Millard2012EquilibriumMuscle.h b/OpenSim/Actuators/Millard2012EquilibriumMuscle.h index f617576897..a02808c183 100644 --- a/OpenSim/Actuators/Millard2012EquilibriumMuscle.h +++ b/OpenSim/Actuators/Millard2012EquilibriumMuscle.h @@ -761,8 +761,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Millard2012EquilibriumMuscle, Muscle); @param staticSolution set to true to calculate the static equilibrium solution, setting fiber and tendon velocities to zero */ - MuscleStateEstimate - estimateMuscleFiberState(const double aActivation, + MuscleStateEstimate estimateMuscleFiberState(const double aActivation, const double pathLength, const double pathLengtheningSpeed, const double aSolTolerance,