Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Cleanup: Muscle state estimate return type in Millard2012QuilibriumMuscle #3500

Merged
merged 6 commits into from
Jul 19, 2023
Merged

Cleanup: Muscle state estimate return type in Millard2012QuilibriumMuscle #3500

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
2f0d239
adds MuscleStateEstimatorResult struct to replace std::map<...>
pepbos Jun 27, 2023
a8396a6
renaming of variables and member fields in Millard2012EquilibriumMuscle.
pepbos Jun 27, 2023
bc8d62a
improve throwing message in Millard2012EquilibriumMuscle
pepbos Jun 28, 2023
f3590fd
Re-adds comments in Millard2012EquilibriumMuscle for readability
pepbos Jun 28, 2023
10d7ee1
prefer `enum class` over `enum`. So this changes that.
pepbos Jun 28, 2023
838db49
fmt
pepbos Jul 3, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
139 changes: 59 additions & 80 deletions OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -556,43 +556,38 @@ computeFiberEquilibrium(SimTK::State& s, bool solveForVelocity) const
double pathSpeed = solveForVelocity ? getLengtheningSpeed(s) : 0;
double activation = getActivation(s);

try {
std::pair<StatusFromEstimateMuscleFiberState,
ValuesFromEstimateMuscleFiberState> 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 MuscleStateEstimate result =
estimateMuscleFiberState(
activation,
pathLength,
pathSpeed,
tol,
maxIter,
solveForVelocity);

if (result.status ==
MuscleStateEstimate::Status::Failure_MaxIterationsReached) {
std::ostringstream oss;
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"
<< " Activation is " << activation << "\n"
<< " Fiber length is " << result.fiberLength << "\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 ==
MuscleStateEstimate::Status::Warning_FiberAtLowerBound) {
log_warn(
"Millard2012EquilibriumMuscle static solution: '{}' is at its"
"minimum fiber length of {}.",
getName(),
result.fiberLength);
}

setActuation(s, result.tendonForce);
adamkewley marked this conversation as resolved.
Show resolved Hide resolved
setFiberLength(s, result.fiberLength);
}

//==============================================================================
Expand Down Expand Up @@ -1356,15 +1351,14 @@ double Millard2012EquilibriumMuscle::clampFiberLength(double lce) const
return max(lce, getMinimumFiberLength());
}

std::pair<Millard2012EquilibriumMuscle::StatusFromEstimateMuscleFiberState,
Millard2012EquilibriumMuscle::ValuesFromEstimateMuscleFiberState>
Millard2012EquilibriumMuscle::estimateMuscleFiberState(
const double aActivation,
const double pathLength,
const double pathLengtheningSpeed,
const double aSolTolerance,
const int aMaxIterations,
bool staticSolution) const
Millard2012EquilibriumMuscle::MuscleStateEstimate
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
Expand Down Expand Up @@ -1601,59 +1595,44 @@ Millard2012EquilibriumMuscle::estimateMuscleFiberState(
iter++;
}

// Populate the result map.
ValuesFromEstimateMuscleFiberState resultValues;
MuscleStateEstimate result;
result.solutionError = ferr;

if(abs(ferr) < aSolTolerance) { // The solution converged.
// Check if solution converged.
if(abs(ferr) < aSolTolerance) {
adamkewley marked this conversation as resolved.
Show resolved Hide resolved
result.status = MuscleStateEstimate::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.fiberLength = clampFiberLength(lce);
result.tendonVelocity = dlce;
result.tendonForce = fse * fiso;

return std::pair<StatusFromEstimateMuscleFiberState,
ValuesFromEstimateMuscleFiberState>
(StatusFromEstimateMuscleFiberState::Success_Converged, resultValues);
return result;
}

// Fiber length is at or exceeds its lower bound.
// Check if fiberlength is at or exceeds lower bound.
if (lce <= getMinimumFiberLength()) {
result.status = MuscleStateEstimate::Status::
Warning_FiberAtLowerBound;

lce = getMinimumFiberLength();
lce = getMinimumFiberLength();
phi = getPennationModel().calcPennationAngle(lce);
cosphi = cos(phi);
tl = getPennationModel().calcTendonLength(cosphi,lce,ml);
lceN = lce/ofl;
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.fiberLength = lce;
result.tendonVelocity = 0;
result.tendonForce = fse * fiso;

return std::pair<StatusFromEstimateMuscleFiberState,
ValuesFromEstimateMuscleFiberState>
(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,
ValuesFromEstimateMuscleFiberState>
(StatusFromEstimateMuscleFiberState::Failure_MaxIterationsReached,
resultValues);
// Solution failed to converge.
result.status = MuscleStateEstimate::Status::
Failure_MaxIterationsReached;
return result;
}

//==============================================================================
Expand Down
25 changes: 12 additions & 13 deletions OpenSim/Actuators/Millard2012EquilibriumMuscle.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 MuscleStateEstimate {
double solutionError = SimTK::NaN;
double fiberLength = SimTK::NaN;
double tendonVelocity = SimTK::NaN;
double tendonForce = SimTK::NaN;

enum class Status {
Success_Converged,
Warning_FiberAtLowerBound,
Failure_MaxIterationsReached
} status = Status::Failure_MaxIterationsReached;
};

// Associative array of values returned by estimateMuscleFiberState():
// solution_error, iterations, fiber_length, fiber_velocity, and
// tendon_force.
typedef std::map<std::string, double> 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.
Expand All @@ -760,9 +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<StatusFromEstimateMuscleFiberState,
ValuesFromEstimateMuscleFiberState>
estimateMuscleFiberState(const double aActivation,
MuscleStateEstimate estimateMuscleFiberState(const double aActivation,
const double pathLength,
const double pathLengtheningSpeed,
const double aSolTolerance,
Expand Down