diff --git a/Bindings/OpenSimHeaders_actuators.h b/Bindings/OpenSimHeaders_actuators.h index c891314945..9e643e616f 100644 --- a/Bindings/OpenSimHeaders_actuators.h +++ b/Bindings/OpenSimHeaders_actuators.h @@ -16,6 +16,7 @@ #include #include #include +#include #include #include diff --git a/Bindings/actuators.i b/Bindings/actuators.i index 608ff189dc..1f99941298 100644 --- a/Bindings/actuators.i +++ b/Bindings/actuators.i @@ -12,6 +12,7 @@ %include %include %include +%include %template (SetFunctionBasedPaths) OpenSim::Set; %include diff --git a/OpenSim/Actuators/MeyerFregly2016Muscle.cpp b/OpenSim/Actuators/MeyerFregly2016Muscle.cpp new file mode 100644 index 0000000000..7d7e471289 --- /dev/null +++ b/OpenSim/Actuators/MeyerFregly2016Muscle.cpp @@ -0,0 +1,1053 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: MeyerFregly2016Muscle.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2020 Stanford University and the Authors * + * Author(s): Spencer Williams, Christopher Dembia, Nicholas Bianco * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "MeyerFregly2016Muscle.h" + +#include +#include +#include +#include +#include "OpenSim/Common/STOFileAdapter.h" + +using namespace OpenSim; + +const std::string MeyerFregly2016Muscle::STATE_ACTIVATION_NAME("activation"); +const std::string MeyerFregly2016Muscle::STATE_NORMALIZED_TENDON_FORCE_NAME( + "normalized_tendon_force"); +const std::string + MeyerFregly2016Muscle::DERIVATIVE_NORMALIZED_TENDON_FORCE_NAME( + "implicitderiv_normalized_tendon_force"); +const std::string + MeyerFregly2016Muscle::RESIDUAL_NORMALIZED_TENDON_FORCE_NAME( + "implicitresidual_normalized_tendon_force"); + +// We must define these variables in some compilation unit (pre-C++17). +// https://stackoverflow.com/questions/40690260/undefined-reference-error-for-static-constexpr-member?noredirect=1&lq=1 +constexpr double MeyerFregly2016Muscle::b11; +constexpr double MeyerFregly2016Muscle::b21; +constexpr double MeyerFregly2016Muscle::b31; +constexpr double MeyerFregly2016Muscle::b41; +constexpr double MeyerFregly2016Muscle::b12; +constexpr double MeyerFregly2016Muscle::b22; +constexpr double MeyerFregly2016Muscle::b32; +constexpr double MeyerFregly2016Muscle::b42; +constexpr double MeyerFregly2016Muscle::b13; +constexpr double MeyerFregly2016Muscle::b23; +constexpr double MeyerFregly2016Muscle::b33; +constexpr double MeyerFregly2016Muscle::b43; +constexpr double MeyerFregly2016Muscle::m_minNormFiberLength; +constexpr double MeyerFregly2016Muscle::m_maxNormFiberLength; +constexpr double MeyerFregly2016Muscle::m_minNormTendonForce; +constexpr double MeyerFregly2016Muscle::m_maxNormTendonForce; +constexpr int MeyerFregly2016Muscle::m_mdi_passiveFiberElasticForce; +constexpr int MeyerFregly2016Muscle::m_mdi_passiveFiberDampingForce; +constexpr int + MeyerFregly2016Muscle::m_mdi_partialPennationAnglePartialFiberLength; +constexpr int MeyerFregly2016Muscle:: + m_mdi_partialFiberForceAlongTendonPartialFiberLength; +constexpr int + MeyerFregly2016Muscle::m_mdi_partialTendonForcePartialFiberLength; + +void MeyerFregly2016Muscle::constructProperties() { + constructProperty_activation_time_constant(0.015); + constructProperty_deactivation_time_constant(0.060); + constructProperty_default_activation(0.5); + constructProperty_default_normalized_tendon_force(0.5); + constructProperty_active_force_width_scale(1.0); + constructProperty_fiber_damping(0.0); + constructProperty_passive_fiber_strain_at_one_norm_force(0.6); + constructProperty_tendon_strain_at_one_norm_force(0.049); + constructProperty_ignore_passive_fiber_force(false); + constructProperty_tendon_compliance_dynamics_mode("explicit"); +} + +void MeyerFregly2016Muscle::extendFinalizeFromProperties() { + Super::extendFinalizeFromProperties(); + OPENSIM_THROW_IF_FRMOBJ(!getProperty_optimal_force().getValueIsDefault(), + Exception, + "The optimal_force property is ignored for this Force; " + "use max_isometric_force instead."); + + SimTK_ERRCHK2_ALWAYS(get_activation_time_constant() > 0, + "MeyerFregly2016Muscle::extendFinalizeFromProperties", + "%s: activation_time_constant must be greater than zero, " + "but it is %g.", + getName().c_str(), get_activation_time_constant()); + + SimTK_ERRCHK2_ALWAYS(get_deactivation_time_constant() > 0, + "MeyerFregly2016Muscle::extendFinalizeFromProperties", + "%s: deactivation_time_constant must be greater than zero, " + "but it is %g.", + getName().c_str(), get_deactivation_time_constant()); + + SimTK_ERRCHK2_ALWAYS(get_default_activation() > 0, + "MeyerFregly2016Muscle::extendFinalizeFromProperties", + "%s: default_activation must be greater than zero, " + "but it is %g.", + getName().c_str(), get_default_activation()); + + SimTK_ERRCHK2_ALWAYS(get_default_normalized_tendon_force() >= 0, + "MeyerFregly2016Muscle::extendFinalizeFromProperties", + "%s: default_normalized_tendon_force must be greater than or equal " + "to zero, but it is %g.", + getName().c_str(), get_default_normalized_tendon_force()); + + SimTK_ERRCHK2_ALWAYS(get_default_normalized_tendon_force() <= 5, + "MeyerFregly2016Muscle::extendFinalizeFromProperties", + "%s: default_normalized_tendon_force must be less than or equal to " + "5.0, but it is %g.", + getName().c_str(), get_default_normalized_tendon_force()); + + SimTK_ERRCHK2_ALWAYS(get_active_force_width_scale() >= 1, + "MeyerFregly2016Muscle::extendFinalizeFromProperties", + "%s: active_force_width_scale must be greater than or equal to " + "1.0, " + "but it is %g.", + getName().c_str(), get_active_force_width_scale()); + + SimTK_ERRCHK2_ALWAYS(get_fiber_damping() >= 0, + "MeyerFregly2016Muscle::extendFinalizeFromProperties", + "%s: fiber_damping must be greater than or equal to zero, " + "but it is %g.", + getName().c_str(), get_fiber_damping()); + + SimTK_ERRCHK2_ALWAYS(get_passive_fiber_strain_at_one_norm_force() > 0, + "MeyerFregly2016Muscle::extendFinalizeFromProperties", + "%s: passive_fiber_strain_at_one_norm_force must be greater " + "than zero, but it is %g.", + getName().c_str(), get_passive_fiber_strain_at_one_norm_force()); + + SimTK_ERRCHK2_ALWAYS(get_tendon_strain_at_one_norm_force() > 0, + "MeyerFregly2016Muscle::extendFinalizeFromProperties", + "%s: tendon_strain_at_one_norm_force must be greater than zero, " + "but it is %g.", + getName().c_str(), get_tendon_strain_at_one_norm_force()); + + OPENSIM_THROW_IF_FRMOBJ( + get_pennation_angle_at_optimal() < 0 || + get_pennation_angle_at_optimal() > + SimTK::Pi / 2.0 - SimTK::SignificantReal, + InvalidPropertyValue, + getProperty_pennation_angle_at_optimal().getName(), + "Pennation angle at optimal fiber length must be in the range [0, " + "Pi/2)."); + + using SimTK::square; + const auto normFiberWidth = sin(get_pennation_angle_at_optimal()); + m_fiberWidth = get_optimal_fiber_length() * normFiberWidth; + m_squareFiberWidth = square(m_fiberWidth); + m_maxContractionVelocityInMetersPerSecond = + get_max_contraction_velocity() * get_optimal_fiber_length(); + m_kT = log((1.0 + c3) / c1) / + (1.0 + get_tendon_strain_at_one_norm_force() - c2); + m_isTendonDynamicsExplicit = + get_tendon_compliance_dynamics_mode() == "explicit"; +} + +void MeyerFregly2016Muscle::extendAddToSystem( + SimTK::MultibodySystem& system) const { + Super::extendAddToSystem(system); + if (!get_ignore_activation_dynamics()) { + addStateVariable(STATE_ACTIVATION_NAME, SimTK::Stage::Dynamics); + } + if (!get_ignore_tendon_compliance()) { + addStateVariable( + STATE_NORMALIZED_TENDON_FORCE_NAME, SimTK::Stage::Dynamics); + if (!m_isTendonDynamicsExplicit) { + addDiscreteVariable(DERIVATIVE_NORMALIZED_TENDON_FORCE_NAME, + SimTK::Stage::Dynamics); + addCacheVariable(RESIDUAL_NORMALIZED_TENDON_FORCE_NAME, double(0), + SimTK::Stage::Dynamics); + } + } +} + +void MeyerFregly2016Muscle::extendInitStateFromProperties( + SimTK::State& s) const { + Super::extendInitStateFromProperties(s); + if (!get_ignore_activation_dynamics()) { + setActivation(s, get_default_activation()); + } + if (!get_ignore_tendon_compliance()) { + setNormalizedTendonForce(s, get_default_normalized_tendon_force()); + } +} + +void MeyerFregly2016Muscle::extendSetPropertiesFromState( + const SimTK::State& s) { + Super::extendSetPropertiesFromState(s); + if (!get_ignore_activation_dynamics()) { + set_default_activation(getActivation(s)); + } + if (!get_ignore_tendon_compliance()) { + set_default_normalized_tendon_force(getNormalizedTendonForce(s)); + } +} + +void MeyerFregly2016Muscle::computeStateVariableDerivatives( + const SimTK::State& s) const { + + // Activation dynamics. + // -------------------- + if (!get_ignore_activation_dynamics()) { + const auto& activation = getActivation(s); + const auto& excitation = getControl(s); + static const double actTimeConst = get_activation_time_constant(); + static const double deactTimeConst = get_deactivation_time_constant(); + static const double tanhSteepness = 0.1; + // f = 0.5 tanh(b(e - a)) + // z = 0.5 + 1.5a + // da/dt = [(f + 0.5)/(tau_a * z) + (-f + 0.5)*z/tau_d] * (e - a) + const SimTK::Real timeConstFactor = 0.5 + 1.5 * activation; + const SimTK::Real tempAct = 1.0 / (actTimeConst * timeConstFactor); + const SimTK::Real tempDeact = timeConstFactor / deactTimeConst; + const SimTK::Real f = + 0.5 * tanh(tanhSteepness * (excitation - activation)); + const SimTK::Real timeConst = + tempAct * (f + 0.5) + tempDeact * (-f + 0.5); + const SimTK::Real derivative = timeConst * (excitation - activation); + setStateVariableDerivativeValue(s, STATE_ACTIVATION_NAME, derivative); + } + + // Tendon compliance dynamics. + // --------------------------- + if (!get_ignore_tendon_compliance()) { + double normTendonForceDerivative; + if (m_isTendonDynamicsExplicit) { + const auto& mli = getMuscleLengthInfo(s); + const auto& fvi = getFiberVelocityInfo(s); + // calcTendonForceMultiplerDerivative() is with respect to + // normalized tendon length, so using the chain rule, to get + // normalized tendon force derivative with respect to time, we + // multiply by normalized fiber velocity. + normTendonForceDerivative = + fvi.normTendonVelocity * + calcTendonForceMultiplierDerivative(mli.normTendonLength); + } else { + normTendonForceDerivative = getDiscreteVariableValue( + s, DERIVATIVE_NORMALIZED_TENDON_FORCE_NAME); + } + + setStateVariableDerivativeValue(s, STATE_NORMALIZED_TENDON_FORCE_NAME, + normTendonForceDerivative); + } +} + +double MeyerFregly2016Muscle::computeActuation(const SimTK::State& s) const { + const auto& mdi = getMuscleDynamicsInfo(s); + setActuation(s, mdi.tendonForce); + return mdi.tendonForce; +} + +void MeyerFregly2016Muscle::calcMuscleLengthInfoHelper( + const SimTK::Real& muscleTendonLength, + const bool& ignoreTendonCompliance, MuscleLengthInfo& mli, + const SimTK::Real& normTendonForce) const { + + // Tendon. + // ------- + if (ignoreTendonCompliance) { + mli.normTendonLength = 1.0; + } else { + mli.normTendonLength = + calcTendonForceLengthInverseCurve(normTendonForce); + } + mli.tendonStrain = mli.normTendonLength - 1.0; + mli.tendonLength = get_tendon_slack_length() * mli.normTendonLength; + + // Fiber. + // ------ + mli.fiberLengthAlongTendon = muscleTendonLength - mli.tendonLength; + mli.fiberLength = sqrt( + SimTK::square(mli.fiberLengthAlongTendon) + m_squareFiberWidth); + mli.normFiberLength = mli.fiberLength / get_optimal_fiber_length(); + + // Pennation. + // ---------- + mli.cosPennationAngle = mli.fiberLengthAlongTendon / mli.fiberLength; + mli.sinPennationAngle = m_fiberWidth / mli.fiberLength; + mli.pennationAngle = asin(mli.sinPennationAngle); + + // Multipliers. + // ------------ + mli.fiberPassiveForceLengthMultiplier = + calcPassiveForceMultiplier(mli.normFiberLength); + mli.fiberActiveForceLengthMultiplier = + calcActiveForceLengthMultiplier(mli.normFiberLength); +} + +void MeyerFregly2016Muscle::calcFiberVelocityInfoHelper( + const SimTK::Real& muscleTendonVelocity, const SimTK::Real& activation, + const bool& ignoreTendonCompliance, + const bool& isTendonDynamicsExplicit, const MuscleLengthInfo& mli, + FiberVelocityInfo& fvi, const SimTK::Real& normTendonForce, + const SimTK::Real& normTendonForceDerivative) const { + + if (isTendonDynamicsExplicit && !ignoreTendonCompliance) { + const auto& normFiberForce = normTendonForce / mli.cosPennationAngle; + fvi.fiberForceVelocityMultiplier = + (normFiberForce - mli.fiberPassiveForceLengthMultiplier) / + (activation * mli.fiberActiveForceLengthMultiplier); + fvi.normFiberVelocity = + calcForceVelocityInverseCurve(fvi.fiberForceVelocityMultiplier); + fvi.fiberVelocity = fvi.normFiberVelocity * + m_maxContractionVelocityInMetersPerSecond; + fvi.fiberVelocityAlongTendon = + fvi.fiberVelocity / mli.cosPennationAngle; + fvi.tendonVelocity = + muscleTendonVelocity - fvi.fiberVelocityAlongTendon; + fvi.normTendonVelocity = fvi.tendonVelocity / get_tendon_slack_length(); + } else { + if (ignoreTendonCompliance) { + fvi.normTendonVelocity = 0.0; + } else { + fvi.normTendonVelocity = + calcTendonForceLengthInverseCurveDerivative( + normTendonForceDerivative, mli.normTendonLength); + } + fvi.tendonVelocity = get_tendon_slack_length() * fvi.normTendonVelocity; + fvi.fiberVelocityAlongTendon = + muscleTendonVelocity - fvi.tendonVelocity; + fvi.fiberVelocity = + fvi.fiberVelocityAlongTendon * mli.cosPennationAngle; + fvi.normFiberVelocity = + fvi.fiberVelocity / m_maxContractionVelocityInMetersPerSecond; + fvi.fiberForceVelocityMultiplier = + calcForceVelocityMultiplier(fvi.normFiberVelocity); + } + + const SimTK::Real tanPennationAngle = + m_fiberWidth / mli.fiberLengthAlongTendon; + fvi.pennationAngularVelocity = + -fvi.fiberVelocity / mli.fiberLength * tanPennationAngle; +} + +void MeyerFregly2016Muscle::calcMuscleDynamicsInfoHelper( + const SimTK::Real& activation, const bool& ignoreTendonCompliance, + const MuscleLengthInfo& mli, const FiberVelocityInfo& fvi, + MuscleDynamicsInfo& mdi, const SimTK::Real& normTendonForce) const { + + mdi.activation = activation; + + SimTK::Real activeFiberForce; + SimTK::Real conPassiveFiberForce; + SimTK::Real nonConPassiveFiberForce; + SimTK::Real totalFiberForce; + calcFiberForce(mdi.activation, mli.fiberActiveForceLengthMultiplier, + fvi.fiberForceVelocityMultiplier, + mli.fiberPassiveForceLengthMultiplier, fvi.normFiberVelocity, + activeFiberForce, conPassiveFiberForce, nonConPassiveFiberForce, + totalFiberForce); + + SimTK::Real passiveFiberForce = + conPassiveFiberForce + nonConPassiveFiberForce; + + // TODO revisit this if compressive forces become an issue. + //// When using a rigid tendon, avoid generating compressive fiber forces by + //// saturating the damping force produced by the parallel element. + //// Based on Millard2012EquilibriumMuscle::calcMuscleDynamicsInfo(). + // if (get_ignore_tendon_compliance()) { + // if (totalFiberForce < 0) { + // totalFiberForce = 0.0; + // nonConPassiveFiberForce = -activeFiberForce - + // conPassiveFiberForce; passiveFiberForce = conPassiveFiberForce + + // nonConPassiveFiberForce; + // } + //} + + // Compute force entries. + // ---------------------- + const auto maxIsometricForce = get_max_isometric_force(); + mdi.fiberForce = totalFiberForce; + mdi.activeFiberForce = activeFiberForce; + mdi.passiveFiberForce = passiveFiberForce; + mdi.normFiberForce = mdi.fiberForce / maxIsometricForce; + mdi.fiberForceAlongTendon = mdi.fiberForce * mli.cosPennationAngle; + + if (ignoreTendonCompliance) { + mdi.normTendonForce = mdi.normFiberForce * mli.cosPennationAngle; + mdi.tendonForce = mdi.fiberForceAlongTendon; + } else { + mdi.normTendonForce = normTendonForce; + mdi.tendonForce = maxIsometricForce * mdi.normTendonForce; + } + + // Compute stiffness entries. + // -------------------------- + mdi.fiberStiffness = calcFiberStiffness(mdi.activation, mli.normFiberLength, + fvi.fiberForceVelocityMultiplier); + const auto& partialPennationAnglePartialFiberLength = + calcPartialPennationAnglePartialFiberLength(mli.fiberLength); + const auto& partialFiberForceAlongTendonPartialFiberLength = + calcPartialFiberForceAlongTendonPartialFiberLength(mdi.fiberForce, + mdi.fiberStiffness, mli.sinPennationAngle, + mli.cosPennationAngle, + partialPennationAnglePartialFiberLength); + mdi.fiberStiffnessAlongTendon = calcFiberStiffnessAlongTendon( + mli.fiberLength, partialFiberForceAlongTendonPartialFiberLength, + mli.sinPennationAngle, mli.cosPennationAngle, + partialPennationAnglePartialFiberLength); + mdi.tendonStiffness = calcTendonStiffness(mli.normTendonLength); + + const auto& partialTendonForcePartialFiberLength = + calcPartialTendonForcePartialFiberLength(mdi.tendonStiffness, + mli.fiberLength, mli.sinPennationAngle, + mli.cosPennationAngle); + + // Compute power entries. + // ---------------------- + // In order for the fiberPassivePower to be zero work, the non-conservative + // passive fiber force is lumped into active fiber power. This is based on + // the implementation in Millard2012EquilibriumMuscle (and verified over + // email with Matt Millard). + mdi.fiberActivePower = -(mdi.activeFiberForce + nonConPassiveFiberForce) * + fvi.fiberVelocity; + mdi.fiberPassivePower = -conPassiveFiberForce * fvi.fiberVelocity; + mdi.tendonPower = -mdi.tendonForce * fvi.tendonVelocity; + + mdi.userDefinedDynamicsExtras.resize(5); + mdi.userDefinedDynamicsExtras[m_mdi_passiveFiberElasticForce] = + conPassiveFiberForce; + mdi.userDefinedDynamicsExtras[m_mdi_passiveFiberDampingForce] = + nonConPassiveFiberForce; + mdi.userDefinedDynamicsExtras + [m_mdi_partialPennationAnglePartialFiberLength] = + partialPennationAnglePartialFiberLength; + mdi.userDefinedDynamicsExtras + [m_mdi_partialFiberForceAlongTendonPartialFiberLength] = + partialFiberForceAlongTendonPartialFiberLength; + mdi.userDefinedDynamicsExtras[m_mdi_partialTendonForcePartialFiberLength] = + partialTendonForcePartialFiberLength; +} + +void MeyerFregly2016Muscle::calcMusclePotentialEnergyInfoHelper( + const bool& ignoreTendonCompliance, const MuscleLengthInfo& mli, + MusclePotentialEnergyInfo& mpei) const { + + // Based on Millard2012EquilibriumMuscle::calcMusclePotentialEnergyInfo(). + + // Fiber potential energy. + // ----------------------- + mpei.fiberPotentialEnergy = + calcPassiveForceMultiplierIntegral(mli.normFiberLength) * + get_optimal_fiber_length() * get_max_isometric_force(); + + // Tendon potential energy. + // ------------------------ + mpei.tendonPotentialEnergy = 0; + if (!ignoreTendonCompliance) { + mpei.tendonPotentialEnergy = + calcTendonForceMultiplierIntegral(mli.normTendonLength) * + get_tendon_slack_length() * get_max_isometric_force(); + } + + // Total potential energy. + // ----------------------- + mpei.musclePotentialEnergy = + mpei.fiberPotentialEnergy + mpei.tendonPotentialEnergy; +} + +void MeyerFregly2016Muscle::calcMuscleLengthInfo( + const SimTK::State& s, MuscleLengthInfo& mli) const { + + const auto& muscleTendonLength = getLength(s); + SimTK::Real normTendonForce = SimTK::NaN; + if (!get_ignore_tendon_compliance()) { + normTendonForce = getNormalizedTendonForce(s); + } + calcMuscleLengthInfoHelper(muscleTendonLength, + get_ignore_tendon_compliance(), mli, normTendonForce); + + if (mli.tendonLength < get_tendon_slack_length()) { + // TODO the Millard model sets fiber velocity to zero when the + // tendon is buckling, but this may create a discontinuity. + log_info("MeyerFregly2016Muscle '{}' is buckling (length < " + "tendon_slack_length) at time {} s.", + getName(), s.getTime()); + } +} + +void MeyerFregly2016Muscle::calcFiberVelocityInfo( + const SimTK::State& s, FiberVelocityInfo& fvi) const { + + const auto& mli = getMuscleLengthInfo(s); + const auto& muscleTendonVelocity = getLengtheningSpeed(s); + const auto& activation = getActivation(s); + + SimTK::Real normTendonForce = SimTK::NaN; + SimTK::Real normTendonForceDerivative = SimTK::NaN; + if (!get_ignore_tendon_compliance()) { + if (m_isTendonDynamicsExplicit) { + normTendonForce = getNormalizedTendonForce(s); + } else { + normTendonForceDerivative = getNormalizedTendonForceDerivative(s); + } + } + + calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, + get_ignore_tendon_compliance(), m_isTendonDynamicsExplicit, mli, + fvi, normTendonForce, normTendonForceDerivative); + + if (fvi.normFiberVelocity < -1.0) { + log_info("MeyerFregly2016Muscle '{}' is exceeding maximum " + "contraction velocity at time {} s.", + getName(), s.getTime()); + } +} + +void MeyerFregly2016Muscle::calcMuscleDynamicsInfo( + const SimTK::State& s, MuscleDynamicsInfo& mdi) const { + const auto& activation = getActivation(s); + SimTK::Real normTendonForce = SimTK::NaN; + if (!get_ignore_tendon_compliance()) { + normTendonForce = getNormalizedTendonForce(s); + } + const auto& mli = getMuscleLengthInfo(s); + const auto& fvi = getFiberVelocityInfo(s); + + calcMuscleDynamicsInfoHelper(activation, get_ignore_tendon_compliance(), + mli, fvi, mdi, normTendonForce); +} + +void MeyerFregly2016Muscle::calcMusclePotentialEnergyInfo( + const SimTK::State& s, MusclePotentialEnergyInfo& mpei) const { + const MuscleLengthInfo& mli = getMuscleLengthInfo(s); + calcMusclePotentialEnergyInfoHelper( + get_ignore_tendon_compliance(), mli, mpei); +} + +double +OpenSim::MeyerFregly2016Muscle::calcInextensibleTendonActiveFiberForce( + SimTK::State& s, double activation) const { + MuscleLengthInfo mli; + FiberVelocityInfo fvi; + MuscleDynamicsInfo mdi; + const double muscleTendonLength = getLength(s); + const double muscleTendonVelocity = getLengtheningSpeed(s); + + // We set the boolean arguments to ignore tendon compliance in all three + // function calls to true since we are computing rigid tendon fiber force. + calcMuscleLengthInfoHelper(muscleTendonLength, true, mli); + // The second boolean argument is to specify how to compute velocity + // information for either explicit or implicit tendon compliance dynamics. + // However, since we are already ignoring tendon compliance, velocity + // information will be computed whether this argument is true or false. + calcFiberVelocityInfoHelper( + muscleTendonVelocity, activation, true, true, mli, fvi); + calcMuscleDynamicsInfoHelper(activation, true, mli, fvi, mdi); + + return mdi.activeFiberForce; +} + +void MeyerFregly2016Muscle::computeInitialFiberEquilibrium( + SimTK::State& s) const { + if (get_ignore_tendon_compliance()) return; + + getModel().realizeVelocity(s); + + const auto& muscleTendonLength = getLength(s); + const auto& muscleTendonVelocity = getLengtheningSpeed(s); + const auto& activation = getActivation(s); + + // We have to use the implicit form of the model since the explicit form + // will produce a zero residual for any guess of normalized tendon force. + // The implicit form requires a value for normalized tendon force + // derivative, so we'll set it to zero for simplicity. + const SimTK::Real normTendonForceDerivative = 0.0; + + // Wrap residual function so it is a function of normalized tendon force + // only. + const auto calcResidual = [this, &muscleTendonLength, &muscleTendonVelocity, + &normTendonForceDerivative, &activation]( + const SimTK::Real& normTendonForce) { + return calcEquilibriumResidual(muscleTendonLength, muscleTendonVelocity, + activation, normTendonForce, normTendonForceDerivative); + }; + + const auto equilNormTendonForce = solveBisection(calcResidual, + m_minNormTendonForce, m_maxNormTendonForce, 1e-10, 100); + + setNormalizedTendonForce(s, equilNormTendonForce); + + // TODO not working as well as bisection. + // const double tolerance = std::max( + // 1e-8 * get_max_isometric_force(), SimTK::SignificantReal * 10); + // int maxIterations = 1000; + + // try { + // auto result = estimateMuscleFiberState(activation, + // muscleTendonLength, muscleTendonVelocity, + // normTendonForceDerivative, tolerance, maxIterations); + + // switch (result.first) { + + // case Success_Converged: + // setNormalizedTendonForce(s, result.second["norm_tendon_force"]); + // setActuation(s, get_max_isometric_force() * + // result.second["norm_tendon_force"]); + // break; + + // case Warning_FiberAtLowerBound: + // printf("\n\nMeyerFregly2016Muscle static solution:" + // " %s is at its minimum fiber length of %f\n", + // getName().c_str(), result.second["fiber_length"]); + // setNormalizedTendonForce(s, result.second["norm_tendon_force"]); + // setActuation(s, get_max_isometric_force() * + // result.second["norm_tendon_force"]); + // break; + + // case Warning_FiberAtUpperBound: + // printf("\n\nMeyerFregly2016Muscle static solution:" + // " %s is at its maximum fiber length of %f\n", + // getName().c_str(), result.second["fiber_length"]); + // setNormalizedTendonForce(s, result.second["norm_tendon_force"]); + // setActuation(s, get_max_isometric_force() * + // result.second["norm_tendon_force"]); + // break; + + // case Failure_MaxIterationsReached: + // std::ostringstream ss; + // ss << "\n Solution error " << + // abs(result.second["solution_error"]) + // << " exceeds tolerance of " << tolerance << "\n" + // << " Newton iterations reached limit of " << maxIterations + // << "\n" + // << " Activation is " << activation << "\n" + // << " Fiber length is " << result.second["fiber_length"] << + // "\n"; + // OPENSIM_THROW_FRMOBJ(MuscleCannotEquilibrate, ss.str()); + // } + + //} catch (const std::exception& x) { + // OPENSIM_THROW_FRMOBJ(MuscleCannotEquilibrate, + // "Internal exception encountered.\n" + std::string{x.what()}); + //} +} + +// std::pair +// MeyerFregly2016Muscle::estimateMuscleFiberState(const double activation, +// const double muscleTendonLength, const double muscleTendonVelocity, +// const double normTendonForceDerivative, const double tolerance, +// const int maxIterations) const { +// +// MuscleLengthInfo mli; +// FiberVelocityInfo fvi; +// MuscleDynamicsInfo mdi; +// +// double normTendonForce = get_default_normalized_tendon_force(); +// calcMuscleLengthInfoHelper(muscleTendonLength, +// get_ignore_tendon_compliance(), mli, normTendonForce); +// +// double fiberLength = mli.fiberLength; +// double residual = SimTK::MostPositiveReal; +// double partialFiberForceAlongTendonPartialFiberLength = 0.0; +// double partialTendonForcePartialFiberLength = 0.0; +// double partialResidualPartialFiberLength = 0.0; +// double deltaFiberLength = 0.0; +// +// // Helper functions +// // ---------------- +// // Update position level quantities. +// auto positionFunc = [&] { +// const auto& fiberLengthAlongTendon = +// sqrt(SimTK::square(fiberLength) - m_squareFiberWidth); +// const auto& tendonLength = muscleTendonLength - +// fiberLengthAlongTendon; const auto& normTendonLength = tendonLength / +// get_tendon_slack_length(); normTendonForce = +// calcTendonForceMultiplier(normTendonLength); +// calcMuscleLengthInfoHelper(muscleTendonLength, +// get_ignore_tendon_compliance(), mli, normTendonForce); +// }; +// // Update velocity and dynamics level quantities and compute residual. +// auto dynamicsFunc = [&] { +// calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, false, +// false, mli, fvi, normTendonForce, normTendonForceDerivative); +// calcMuscleDynamicsInfoHelper(activation, muscleTendonVelocity, false, +// mli, fvi, mdi, normTendonForce); +// +// partialFiberForceAlongTendonPartialFiberLength = +// mdi.userDefinedDynamicsExtras[ +// m_mdi_partialFiberForceAlongTendonPartialFiberLength]; +// partialTendonForcePartialFiberLength = mdi.userDefinedDynamicsExtras[ +// m_mdi_partialTendonForcePartialFiberLength]; +// +// residual = calcEquilibriumResidual( +// mdi.tendonForce, mdi.fiberForceAlongTendon); +// }; +// +// // Initialize the loop. +// int iter = 0; +// positionFunc(); +// dynamicsFunc(); +// double residualPrev = residual; +// double fiberLengthPrev = fiberLength; +// double h = 1.0; +// +// while ((abs(residual) > tolerance) && (iter < maxIterations)) { +// // Compute the search direction. +// partialResidualPartialFiberLength = +// partialFiberForceAlongTendonPartialFiberLength - +// partialTendonForcePartialFiberLength; +// h = 1.0; +// +// while (abs(residual) >= abs(residualPrev)) { +// // Compute the Newton step. +// deltaFiberLength = +// -h * residualPrev / partialResidualPartialFiberLength; +// +// // Take a Newton step if the step is nonzero. +// if (abs(deltaFiberLength) > SimTK::SignificantReal) +// fiberLength = fiberLengthPrev + deltaFiberLength; +// else { +// // We've stagnated or hit a limit; assume we are hitting local +// // minimum and attempt to approach from the other direction. +// fiberLength = fiberLengthPrev - +// SimTK::sign(deltaFiberLength) * SimTK::SqrtEps; +// h = 0; +// } +// +// if (fiberLength / get_optimal_fiber_length() < +// m_minNormFiberLength) { +// fiberLength = m_minNormFiberLength * +// get_optimal_fiber_length(); +// } +// if (fiberLength / get_optimal_fiber_length() > +// m_maxNormFiberLength) { +// fiberLength = m_maxNormFiberLength * +// get_optimal_fiber_length(); +// } +// +// positionFunc(); +// dynamicsFunc(); +// +// if (h <= SimTK::SqrtEps) { break; } +// h = 0.5 * h; +// } +// +// residualPrev = residual; +// fiberLengthPrev = fiberLength; +// +// iter++; +// } +// +// // Populate the result map. +// ValuesFromEstimateMuscleFiberState resultValues; +// +// if (abs(residual) < tolerance) { // The solution converged. +// +// resultValues.iterations = iter; +// resultValues.solution_error = residual; +// resultValues.fiber_length = fiberLength; +// resultValues.fiber_velocity = fvi.fiberVelocity; +// resultValues.normalized_tendon_force = mdi.normTendonForce; +// +// return std::pair( +// Success_Converged, resultValues); +// } +// +// // Fiber length is at or exceeds its lower bound. +// if (fiberLength / get_optimal_fiber_length() <= m_minNormFiberLength) { +// +// fiberLength = m_minNormFiberLength * get_optimal_fiber_length(); +// positionFunc(); +// normTendonForce = calcTendonForceMultiplier(mli.normTendonLength); +// +// resultValues.iterations = iter; +// resultValues.solution_error = residual; +// resultValues.fiber_length = fiberLength; +// resultValues.fiber_velocity = 0; +// resultValues.normalized_tendon_force = normTendonForce; +// +// return std::pair( +// Warning_FiberAtLowerBound, resultValues); +// } +// +// // Fiber length is at or exceeds its upper bound. +// if (fiberLength / get_optimal_fiber_length() >= m_maxNormFiberLength) { +// +// fiberLength = m_maxNormFiberLength * get_optimal_fiber_length(); +// positionFunc(); +// normTendonForce = calcTendonForceMultiplier(mli.normTendonLength); +// +// resultValues.iterations = iter; +// resultValues.solution_error = residual; +// resultValues.fiber_length = fiberLength; +// resultValues.fiber_velocity = 0; +// resultValues.normalized_tendon_force = normTendonForce; +// +// return std::pair( +// Warning_FiberAtUpperBound, resultValues); +// } +// +// // Max iterations reached. +// resultValues.iterations = iter; +// resultValues.solution_error = residual; +// resultValues.fiber_length = SimTK::NaN; +// resultValues.fiber_velocity = SimTK::NaN; +// resultValues.normalized_tendon_force = SimTK::NaN; +// +// return std::pair( +// Failure_MaxIterationsReached, resultValues); +//} + +double MeyerFregly2016Muscle::getPassiveFiberElasticForce( + const SimTK::State& s) const { + return getMuscleDynamicsInfo(s) + .userDefinedDynamicsExtras[m_mdi_passiveFiberElasticForce]; +} +double MeyerFregly2016Muscle::getPassiveFiberElasticForceAlongTendon( + const SimTK::State& s) const { + return getMuscleDynamicsInfo(s) + .userDefinedDynamicsExtras[m_mdi_passiveFiberElasticForce] * + getMuscleLengthInfo(s).cosPennationAngle; +} +double MeyerFregly2016Muscle::getPassiveFiberDampingForce( + const SimTK::State& s) const { + return getMuscleDynamicsInfo(s) + .userDefinedDynamicsExtras[m_mdi_passiveFiberDampingForce]; +} +double MeyerFregly2016Muscle::getPassiveFiberDampingForceAlongTendon( + const SimTK::State& s) const { + return getMuscleDynamicsInfo(s) + .userDefinedDynamicsExtras[m_mdi_passiveFiberDampingForce] * + getMuscleLengthInfo(s).cosPennationAngle; +} + +double MeyerFregly2016Muscle::getImplicitResidualNormalizedTendonForce( + const SimTK::State& s) const { + if (get_ignore_tendon_compliance()) { return 0; } + if (m_isTendonDynamicsExplicit) { return SimTK::NaN; } + + // Recompute residual if cache is invalid. + if (!isCacheVariableValid(s, RESIDUAL_NORMALIZED_TENDON_FORCE_NAME)) { + // Compute muscle-tendon equilibrium residual value to update the + // cache variable. + setCacheVariableValue(s, RESIDUAL_NORMALIZED_TENDON_FORCE_NAME, + getEquilibriumResidual(s)); + markCacheVariableValid(s, RESIDUAL_NORMALIZED_TENDON_FORCE_NAME); + } + + return getCacheVariableValue( + s, RESIDUAL_NORMALIZED_TENDON_FORCE_NAME); +} + +DataTable MeyerFregly2016Muscle::exportFiberLengthCurvesToTable( + const SimTK::Vector& normFiberLengths) const { + SimTK::Vector def; + const SimTK::Vector* x = nullptr; + if (normFiberLengths.nrow()) { + x = &normFiberLengths; + } else { + def = createVectorLinspace( + 200, m_minNormFiberLength, m_maxNormFiberLength); + x = &def; + } + + DataTable table; + table.setColumnLabels( + {"active_force_length_multiplier", "passive_force_multiplier"}); + SimTK::RowVector row(2); + for (int irow = 0; irow < x->nrow(); ++irow) { + const auto& normFiberLength = x->get(irow); + row[0] = calcActiveForceLengthMultiplier(normFiberLength); + row[1] = calcPassiveForceMultiplier(normFiberLength); + table.appendRow(normFiberLength, row); + } + return table; +} + +DataTable MeyerFregly2016Muscle::exportTendonForceMultiplierToTable( + const SimTK::Vector& normTendonLengths) const { + SimTK::Vector def; + const SimTK::Vector* x = nullptr; + if (normTendonLengths.nrow()) { + x = &normTendonLengths; + } else { + // Evaluate the inverse of the tendon curve at y = 1. + def = createVectorLinspace( + 200, 0.95, 1.0 + get_tendon_strain_at_one_norm_force()); + x = &def; + } + + DataTable table; + table.setColumnLabels({"tendon_force_multiplier"}); + SimTK::RowVector row(1); + for (int irow = 0; irow < x->nrow(); ++irow) { + const auto& normTendonLength = x->get(irow); + row[0] = calcTendonForceMultiplier(normTendonLength); + table.appendRow(normTendonLength, row); + } + return table; +} + +DataTable MeyerFregly2016Muscle::exportFiberVelocityMultiplierToTable( + const SimTK::Vector& normFiberVelocities) const { + SimTK::Vector def; + const SimTK::Vector* x = nullptr; + if (normFiberVelocities.nrow()) { + x = &normFiberVelocities; + } else { + def = createVectorLinspace(200, -1.1, 1.1); + x = &def; + } + + DataTable table; + table.setColumnLabels({"force_velocity_multiplier"}); + SimTK::RowVector row(1); + for (int irow = 0; irow < x->nrow(); ++irow) { + const auto& normFiberVelocity = x->get(irow); + row[0] = calcForceVelocityMultiplier(normFiberVelocity); + table.appendRow(normFiberVelocity, row); + } + return table; +} + +void MeyerFregly2016Muscle::printCurvesToSTOFiles( + const std::string& directory) const { + const std::string prefix = + directory + SimTK::Pathname::getPathSeparator() + getName(); + STOFileAdapter::write(exportFiberLengthCurvesToTable(), + prefix + "_fiber_length_curves.sto"); + STOFileAdapter::write(exportFiberVelocityMultiplierToTable(), + prefix + "_fiber_velocity_multiplier.sto"); + STOFileAdapter::write(exportTendonForceMultiplierToTable(), + prefix + "_tendon_force_multiplier.sto"); +} + +void MeyerFregly2016Muscle::replaceMuscles( + Model& model, bool allowUnsupportedMuscles) { + + model.finalizeConnections(); + + // Create path actuators from muscle properties and add to the model. Save + // a list of pointers of the muscles to delete. + std::vector musclesToDelete; + auto& muscleSet = model.updMuscles(); + for (int im = 0; im < muscleSet.getSize(); ++im) { + auto& muscBase = muscleSet.get(im); + + // Pre-emptively create a default MeyerFregly2016Muscle + // (TODO: not ideal to do this). + auto actu = OpenSim::make_unique(); + + // Perform muscle-model-specific mappings or throw exception if the + // muscle is not supported. + if (auto musc = dynamic_cast( + &muscBase)) { + + actu->set_default_activation(musc->get_default_activation()); + actu->set_activation_time_constant( + musc->get_activation_time_constant()); + actu->set_deactivation_time_constant( + musc->get_deactivation_time_constant()); + actu->set_fiber_damping(musc->get_fiber_damping()); + actu->set_passive_fiber_strain_at_one_norm_force( + musc->get_FiberForceLengthCurve() + .get_strain_at_one_norm_force()); + actu->set_tendon_strain_at_one_norm_force( + musc->get_TendonForceLengthCurve() + .get_strain_at_one_norm_force()); + + } else if (auto musc = dynamic_cast(&muscBase)) { + + actu->set_default_activation(musc->getDefaultActivation()); + actu->set_activation_time_constant( + musc->get_activation_time_constant()); + actu->set_deactivation_time_constant( + musc->get_deactivation_time_constant()); + // Fiber damping needs to be hardcoded at zero since it is not a + // property of the Thelen2003 muscle. + actu->set_fiber_damping(0); + actu->set_passive_fiber_strain_at_one_norm_force( + musc->get_FmaxMuscleStrain()); + + actu->set_tendon_strain_at_one_norm_force( + musc->get_FmaxTendonStrain()); + + } else { + OPENSIM_THROW_IF(!allowUnsupportedMuscles, Exception, + "Muscle '{}' of type {} is unsupported and " + "allowUnsupportedMuscles=false.", + muscBase.getName(), muscBase.getConcreteClassName()); + continue; + } + + // Perform all the common mappings at base class level (OpenSim::Muscle) + actu->setName(muscBase.getName()); + muscBase.setName(muscBase.getName() + "_delete"); + actu->set_appliesForce(muscBase.get_appliesForce()); + actu->setMinControl(muscBase.getMinControl()); + actu->setMaxControl(muscBase.getMaxControl()); + + actu->setMaxIsometricForce(muscBase.getMaxIsometricForce()); + actu->setOptimalFiberLength(muscBase.getOptimalFiberLength()); + actu->setTendonSlackLength(muscBase.getTendonSlackLength()); + actu->setPennationAngleAtOptimalFiberLength( + muscBase.getPennationAngleAtOptimalFiberLength()); + actu->setMaxContractionVelocity(muscBase.getMaxContractionVelocity()); + actu->set_ignore_tendon_compliance( + muscBase.get_ignore_tendon_compliance()); + actu->set_ignore_activation_dynamics( + muscBase.get_ignore_activation_dynamics()); + actu->updProperty_path().assign(muscBase.getProperty_path()); + model.addForce(actu.release()); + + musclesToDelete.push_back(&muscBase); + } + + // Delete the muscles. + for (const auto* musc : musclesToDelete) { + int index = model.getForceSet().getIndex(musc, 0); + OPENSIM_THROW_IF(index == -1, Exception, + "Muscle with name {} not found in ForceSet.", musc->getName()); + bool success = model.updForceSet().remove(index); + OPENSIM_THROW_IF(!success, Exception, + "Attempt to remove muscle with " + "name {} was unsuccessful.", + musc->getName()); + } + + model.finalizeFromProperties(); + model.finalizeConnections(); +} + +void MeyerFregly2016Muscle::extendPostScale( + const SimTK::State& s, const ScaleSet& scaleSet) { + Super::extendPostScale(s, scaleSet); + + AbstractGeometryPath& path = updPath(); + if (path.getPreScaleLength(s) > 0.0) + { + double scaleFactor = path.getLength(s) / path.getPreScaleLength(s); + upd_optimal_fiber_length() *= scaleFactor; + upd_tendon_slack_length() *= scaleFactor; + + // Clear the pre-scale length that was stored in the path. + path.setPreScaleLength(s, 0.0); + } +} diff --git a/OpenSim/Actuators/MeyerFregly2016Muscle.h b/OpenSim/Actuators/MeyerFregly2016Muscle.h new file mode 100644 index 0000000000..c9e2f20067 --- /dev/null +++ b/OpenSim/Actuators/MeyerFregly2016Muscle.h @@ -0,0 +1,959 @@ +#ifndef MOCO_MEYERFREGLY2016MUSCLE_H +#define MOCO_MEYERFREGLY2016MUSCLE_H +/* -------------------------------------------------------------------------- * + * OpenSim: MeyerFregly2016Muscle.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2020 Stanford University and the Authors * + * Author(s): Spencer Williams, Christopher Dembia, Nicholas Bianco * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include + +#include +#include +#include + +namespace OpenSim { + +// TODO avoid checking ignore_tendon_compliance() in each function; +// might be slow. +// TODO prohibit fiber length from going below 0.2. + +/** This muscle model was published in De Groote et al. 2016. + +The parameters of the active force-length and force-velocity curves have +been slightly modified from what was published to ensure the curves go +through key points: +- Active force-length curve goes through (1, 1). +- Force-velocity curve goes through (-1, 0) and (0, 1). +The default tendon force curve parameters are modified from that in De +Groote et al., 2016: the curve is parameterized by the strain at 1 norm +force (rather than "kT"), and the default value for this parameter is +0.049 (same as in TendonForceLengthCurve) rather than 0.0474. + +This implementation introduces the property 'active_force_width_scale' as +an addition to the original model, which allows users to effectively make +the active force-length curve wider. This property may be useful for +improving the force-generating capacity of a muscle without increasing +maximum isometric force. This property works by scaling the normalized +fiber length when the active force-length curve is computed. For example, +a scale factor of 2 means that the fiber muscle traverses half as far +along the force-length curve in either direction. + +This implementation adds fiber damping as an addition to the original model. +Users can specify this via the 'fiber_damping' property, and damping force +along the fiber is computed by multiplying the property value by the +normalized fiber velocity and max isometric force. If using this muscle for +optimization, fiber damping is recommended as it can improve convergence. + +@note If converting from Thelen2003Muscles via replaceMuscles(), fiber + damping will be set to zero since there is no damping in that muscle + model. + +This class supports tendon compliance dynamics in both explicit and implicit +form (formulations 1 and 3 from De Groote et al. 2016). Both forms of the +dynamics use normalized tendon force as the state variable (rather than the +typical fiber length state). The explicit form is handled through the usual +Component dynamics interface. The implicit form introduces an additional +discrete state variable and cache variable in the SimTK::State for the +derivative of normalized tendon force and muscle-tendon equilibrium residual +respectively. In general, it is preferable to use the implicit form in +optimization since it can be robust to arbitrary initial guesses (see De +Groote et al. 2016). However, the implicit form is only for use with solvers +that support implicit dynamics (i.e. Moco) and cannot be used to perform a +time-stepping forward simulation with Manager; use explicit mode for +time-stepping. + +@note Normalized tendon force is bounded in the range [0, 5] in this class. + The methods getMinNormalizedTendonForce() and + getMaxNormalizedTendonForce() provide these bounds for use in custom solvers. + +@section departures Departures from the Muscle base class + +The documentation for Muscle::MuscleLengthInfo states that the +optimalFiberLength of a muscle is also its resting length, but this is not +true for this muscle: there is a non-zero passive fiber force at the +optimal fiber length. + +In the Muscle class, setIgnoreTendonCompliance() and +setIngoreActivationDynamics() control modeling options, meaning these +settings could theoretically be changed. However, for this class, the +modeling option is ignored and the values of the ignore_tendon_compliance +and ignore_activation_dynamics properties are used directly. + +De Groote, F., Kinney, A. L., Rao, A. V., & Fregly, B. J. (2016). Evaluation +of Direct Collocation Optimal Control Problem Formulations for Solving the +Muscle Redundancy Problem. Annals of Biomedical Engineering, 44(10), 1–15. +http://doi.org/10.1007/s10439-016-1591-9 */ +class OSIMACTUATORS_API MeyerFregly2016Muscle : public Muscle { + OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Muscle, Muscle); + +public: + OpenSim_DECLARE_PROPERTY(activation_time_constant, double, + "Smaller value means activation can increase more rapidly. " + "Default: 0.015 seconds."); + OpenSim_DECLARE_PROPERTY(deactivation_time_constant, double, + "Smaller value means activation can decrease more rapidly. " + "Default: 0.060 seconds."); + OpenSim_DECLARE_PROPERTY(default_activation, double, + "Value of activation in the default state returned by " + "initSystem(). Default: 0.5."); + OpenSim_DECLARE_PROPERTY(default_normalized_tendon_force, double, + "Value of normalized tendon force in the default state returned by " + "initSystem(). Default: 0.5."); + OpenSim_DECLARE_PROPERTY(active_force_width_scale, double, + "Scale factor for the width of the active force-length curve. " + "Larger values make the curve wider. Default: 1.0."); + OpenSim_DECLARE_PROPERTY(fiber_damping, double, + "Use this property to define the linear damping force that is " + "added to the total muscle fiber force. It is computed by " + "multiplying this damping parameter by the normalized fiber " + "velocity and the max isometric force. Default: 0."); + OpenSim_DECLARE_PROPERTY(ignore_passive_fiber_force, bool, + "Make the passive fiber force 0. Default: false."); + OpenSim_DECLARE_PROPERTY(passive_fiber_strain_at_one_norm_force, double, + "Fiber strain when the passive fiber force is 1 normalized force. " + "Default: 0.6."); + OpenSim_DECLARE_PROPERTY(tendon_strain_at_one_norm_force, double, + "Tendon strain at a tension of 1 normalized force. " + "Default: 0.049."); + OpenSim_DECLARE_PROPERTY(tendon_compliance_dynamics_mode, std::string, + "The dynamics method used to enforce tendon compliance dynamics. " + "Options: 'explicit' or 'implicit'. Default: 'explicit'. "); + + OpenSim_DECLARE_OUTPUT(passive_fiber_elastic_force, double, + getPassiveFiberElasticForce, SimTK::Stage::Dynamics); + OpenSim_DECLARE_OUTPUT(passive_fiber_elastic_force_along_tendon, double, + getPassiveFiberElasticForceAlongTendon, SimTK::Stage::Dynamics); + OpenSim_DECLARE_OUTPUT(passive_fiber_damping_force, double, + getPassiveFiberDampingForce, SimTK::Stage::Dynamics); + OpenSim_DECLARE_OUTPUT(passive_fiber_damping_force_along_tendon, double, + getPassiveFiberDampingForceAlongTendon, SimTK::Stage::Dynamics); + + OpenSim_DECLARE_OUTPUT(implicitresidual_normalized_tendon_force, double, + getImplicitResidualNormalizedTendonForce, SimTK::Stage::Dynamics); + OpenSim_DECLARE_OUTPUT(implicitenabled_normalized_tendon_force, bool, + getImplicitEnabledNormalizedTendonForce, SimTK::Stage::Model); + + OpenSim_DECLARE_OUTPUT(statebounds_normalized_tendon_force, SimTK::Vec2, + getBoundsNormalizedTendonForce, SimTK::Stage::Model); + + MeyerFregly2016Muscle() { constructProperties(); } + +protected: + //-------------------------------------------------------------------------- + // COMPONENT INTERFACE + //-------------------------------------------------------------------------- + /// @name Component interface + /// @{ + void extendFinalizeFromProperties() override; + void extendAddToSystem(SimTK::MultibodySystem& system) const override; + void extendInitStateFromProperties(SimTK::State& s) const override; + void extendSetPropertiesFromState(const SimTK::State& s) override; + void computeStateVariableDerivatives(const SimTK::State& s) const override; + /// @} + + //-------------------------------------------------------------------------- + // ACTUATOR INTERFACE + //-------------------------------------------------------------------------- + /// @name Actuator interface + /// @{ + double computeActuation(const SimTK::State& s) const override; + /// @} + +public: + //-------------------------------------------------------------------------- + // MUSCLE INTERFACE + //-------------------------------------------------------------------------- + /// @name Muscle interface + /// @{ + + /// If ignore_activation_dynamics is true, this gets excitation instead. + double getActivation(const SimTK::State& s) const override { + // We override the Muscle's implementation because Muscle requires + // realizing to Dynamics to access activation from MuscleDynamicsInfo, + // which is unnecessary if the activation is a state. + if (get_ignore_activation_dynamics()) { + return getControl(s); + } else { + return getStateVariableValue(s, STATE_ACTIVATION_NAME); + } + } + + /// If ignore_activation_dynamics is true, this sets excitation instead. + void setActivation(SimTK::State& s, double activation) const override { + if (get_ignore_activation_dynamics()) { + SimTK::Vector& controls(getModel().updControls(s)); + setControls(SimTK::Vector(1, activation), controls); + getModel().setControls(s, controls); + } else { + setStateVariableValue(s, STATE_ACTIVATION_NAME, activation); + } + markCacheVariableInvalid(s, "velInfo"); + markCacheVariableInvalid(s, "dynamicsInfo"); + } + +protected: + double calcInextensibleTendonActiveFiberForce( + SimTK::State&, double) const override; + void calcMuscleLengthInfo( + const SimTK::State& s, MuscleLengthInfo& mli) const override; + void calcFiberVelocityInfo( + const SimTK::State& s, FiberVelocityInfo& fvi) const override; + void calcMuscleDynamicsInfo( + const SimTK::State& s, MuscleDynamicsInfo& mdi) const override; + void calcMusclePotentialEnergyInfo(const SimTK::State& s, + MusclePotentialEnergyInfo& mpei) const override; + +public: + /// In this method, calcEquilibriumResidual() is used to find a value of the + /// normalized tendon force state variable that produces muscle-tendon + /// equilibrium. This relies on the implicit form of tendon compliance since + /// the explicit form uses the normalized tendon force state variable + /// directly to compute fiber force, which always produces a zero + /// muscle-tendon equilibrium residual. The derivative of normalized tendon + /// force is set to zero since a value is required for the implicit form of + /// the model. + void computeInitialFiberEquilibrium(SimTK::State& s) const override; + /// @} + + /// @name Get methods. + /// @{ + + /// Get the portion of the passive fiber force generated by the elastic + /// element only (N). + double getPassiveFiberElasticForce(const SimTK::State& s) const; + /// Get the portion of the passive fiber force generated by the elastic + /// element only, projected onto the tendon direction (N). + double getPassiveFiberElasticForceAlongTendon(const SimTK::State& s) const; + /// Get the portion of the passive fiber force generated by the damping + /// element only (N). + double getPassiveFiberDampingForce(const SimTK::State& s) const; + /// Get the portion of the passive fiber force generated by the damping + /// element only, projected onto the tendon direction (N). + double getPassiveFiberDampingForceAlongTendon(const SimTK::State& s) const; + + /// Get whether fiber dynamics is in implicit dynamics mode when using + /// normalized tendon force as the state. This is useful to indicate to + /// solvers to handle the normalized tendon force derivative and + /// muscle-tendon equilibrium variables, which are added to the State as + /// discrete and cache variables, respectively. + /// This function is intended primarily for the model Output + /// 'implicitenabled_normalized_tendon_force'. We don't need the state, but + /// the state parameter is a requirement of Output functions. + bool getImplicitEnabledNormalizedTendonForce(const SimTK::State&) const { + return !get_ignore_tendon_compliance() && !m_isTendonDynamicsExplicit; + } + /// Compute the muscle-tendon force equilibrium residual value when using + /// implicit contraction dynamics with normalized tendon force as the + /// state. + /// This function is intended primarily for the model Output + /// 'implicitresidual_normalized_tendon_force'. + double getImplicitResidualNormalizedTendonForce( + const SimTK::State& s) const; + + /// If ignore_tendon_compliance is true, this gets normalized fiber force + /// along the tendon instead. + double getNormalizedTendonForce(const SimTK::State& s) const { + if (get_ignore_tendon_compliance()) { + return getTendonForce(s) / get_max_isometric_force(); + } else { + return getStateVariableValue(s, STATE_NORMALIZED_TENDON_FORCE_NAME); + } + } + + /// Obtain the time derivative of the normalized tendon force. + /// - If ignore_tendon_compliance is false, this returns zero. + /// - If tendon_compliance_dynamics_mode is 'implicit', this gets the + /// discrete variable normalized tendon force derivative value. + /// - If tendon_compliance_dynamics_mode is 'explicit', this gets the value + /// returned by getStateVariableDerivativeValue() for the + /// 'normalized_tendon_force' state. + double getNormalizedTendonForceDerivative(const SimTK::State& s) const { + if (get_ignore_tendon_compliance()) { return 0.0; } + + if (m_isTendonDynamicsExplicit) { + return getStateVariableDerivativeValue( + s, STATE_NORMALIZED_TENDON_FORCE_NAME); + } else { + return getDiscreteVariableValue( + s, DERIVATIVE_NORMALIZED_TENDON_FORCE_NAME); + } + } + + /// @copydoc calcEquilibriumResidual() + /// This calls calcEquilibriumResidual() using values from the provided + /// SimTK::State as arguments. While is computed using implicit mode, the + /// value of normalized tendon force derivative used *is* consistent with + /// the property `tendon_compliance_dynamics_mode` (see + /// getNormalizedTendonForceDerivative()). + double getEquilibriumResidual(const SimTK::State& s) const { + return calcEquilibriumResidual(getLength(s), getLengtheningSpeed(s), + getActivation(s), getNormalizedTendonForce(s), + getNormalizedTendonForceDerivative(s)); + } + + /// @copydoc calcLinearizedEquilibriumResidualDerivative() + double getLinearizedEquilibriumResidualDerivative( + const SimTK::State& s) const { + return calcLinearizedEquilibriumResidualDerivative(getLength(s), + getLengtheningSpeed(s), getActivation(s), + getNormalizedTendonForce(s), + getNormalizedTendonForceDerivative(s)); + } + + static std::string getActivationStateName() { + return STATE_ACTIVATION_NAME; + } + static std::string getNormalizedTendonForceStateName() { + return STATE_NORMALIZED_TENDON_FORCE_NAME; + } + static std::string getImplicitDynamicsDerivativeName() { + return DERIVATIVE_NORMALIZED_TENDON_FORCE_NAME; + } + static std::string getImplicitDynamicsResidualName() { + return RESIDUAL_NORMALIZED_TENDON_FORCE_NAME; + } + static double getMinNormalizedTendonForce() { return m_minNormTendonForce; } + static double getMaxNormalizedTendonForce() { return m_maxNormTendonForce; } + /// The first element of the Vec2 is the lower bound, and the second is the + /// upper bound. + /// This function is intended primarily for the model Output + /// 'statebounds_normalized_tendon_force'. We don't need the state, but the + /// state parameter is a requirement of Output functions. + SimTK::Vec2 getBoundsNormalizedTendonForce(const SimTK::State&) const + { return {getMinNormalizedTendonForce(), getMaxNormalizedTendonForce()}; } + + static double getMinNormalizedFiberLength() { return m_minNormFiberLength; } + static double getMaxNormalizedFiberLength() { return m_maxNormFiberLength; } + /// The first element of the Vec2 is the lower bound, and the second is the + /// upper bound. + /// Note that since fiber length is not used as a state variable, these + /// bounds cannot be enforced directly. It is upon the user to ensure the + /// muscle fiber is operating within the specified domain. + SimTK::Vec2 getBoundsNormalizedFiberLength() const { + return {getMinNormalizedTendonForce(), getMaxNormalizedTendonForce()}; + } + /// @} + + /// @name Set methods. + /// @{ + /// If ignore_tendon_compliance is true, this sets nothing. + void setNormalizedTendonForce( + SimTK::State& s, double normTendonForce) const { + if (!get_ignore_tendon_compliance()) { + setStateVariableValue( + s, STATE_NORMALIZED_TENDON_FORCE_NAME, normTendonForce); + markCacheVariableInvalid(s, "lengthInfo"); + markCacheVariableInvalid(s, "velInfo"); + markCacheVariableInvalid(s, "dynamicsInfo"); + } + } + /// @} + + /// @name Calculation methods. + /// These functions compute the values of normalized/dimensionless curves, + /// their derivatives and integrals, and other quantities of the muscle. + /// These do not depend on a SimTK::State. + /// @{ + + /// The active force-length curve is the sum of 3 Gaussian-like curves. The + /// width of the curve can be adjusted via the 'active_force_width_scale' + /// property. + SimTK::Real calcActiveForceLengthMultiplier( + const SimTK::Real& normFiberLength) const { + const double& scale = get_active_force_width_scale(); + // Shift the curve so its peak is at the origin, scale it + // horizontally, then shift it back so its peak is still at x = 1.0. + const double x = (normFiberLength - 1.0) / scale + 1.0; + return calcGaussianLikeCurve(x, b11, b21, b31, b41) + + calcGaussianLikeCurve(x, b12, b22, b32, b42) + + calcGaussianLikeCurve(x, b13, b23, b33, b43); + } + + /// The derivative of the active force-length curve with respect to + /// normalized fiber length. This curve is based on the derivative of the + /// Gaussian-like curve used in calcActiveForceLengthMultiplier(). The + /// 'active_force_width_scale' property also affects the value of the + /// derivative curve. + SimTK::Real calcActiveForceLengthMultiplierDerivative( + const SimTK::Real& normFiberLength) const { + const double& scale = get_active_force_width_scale(); + // Shift the curve so its peak is at the origin, scale it + // horizontally, then shift it back so its peak is still at x = 1.0. + const double x = (normFiberLength - 1.0) / scale + 1.0; + return (1.0 / scale) * + (calcGaussianLikeCurveDerivative(x, b11, b21, b31, b41) + + calcGaussianLikeCurveDerivative(x, b12, b22, b32, b42) + + calcGaussianLikeCurveDerivative(x, b13, b23, b33, b43)); + } + + /// The parameters of this curve are not modifiable, so this function is + /// static. + /// @note It is upon the user to check that the muscle fiber is acting + /// within the specified domain. Force computations outside this range + /// may be incorrect. + static SimTK::Real calcForceVelocityMultiplier( + const SimTK::Real& normFiberVelocity) { + return d1 + d2 * atan(d3 + d4 * atan(d5 + d6 * normFiberVelocity)); + } + + /// This is the inverse of the force-velocity multiplier function, and + /// returns the normalized fiber velocity (in [-1, 1]) as a function of + /// the force-velocity multiplier. + static SimTK::Real calcForceVelocityInverseCurve( + const SimTK::Real& forceVelocityMult) { + // The version of this equation in the supplementary materials of De + // Groote et al., 2016 has an error (it's missing a "-d3" before + // dividing by "d2"). + + return (tan((tan((forceVelocityMult - d1) / d2) - d3) / d4) - d5) / d6; + } + + /// This is the passive force-length curve. The curve becomes negative below + /// the minNormFiberLength. + /// + /// We modified this equation from that in the supplementary materials of De + /// Groote et al., 2016, which is the same function used in + /// Thelen2003Muscle. The version in the supplementary materials passes + /// through y = 0 at x = 1.0 and allows for negative forces. We do not want + /// negative forces within the allowed range of fiber lengths, so we + /// modified the equation to pass through y = 0 at x = minNormFiberLength. + /// (This is not an issue for Thelen2003Muscle because the curve is not + /// smooth and returns 0 for lengths less than optimal fiber length.) + SimTK::Real calcPassiveForceMultiplier( + const SimTK::Real& normFiberLength) const { + if (get_ignore_passive_fiber_force()) return 0; + + return e1 * log(exp(e2 * (normFiberLength - e3)) + 1); + } + + /// This is the derivative of the passive force-length curve with respect to + /// the normalized fiber length. + SimTK::Real calcPassiveForceMultiplierDerivative( + const SimTK::Real& normFiberLength) const { + if (get_ignore_passive_fiber_force()) return 0; + + return e1 / (exp(e2 * (normFiberLength - e3)) + 1) * + exp(e2 * (normFiberLength - e3)) * e2; + } + + /// This is the integral of the passive force-length curve with respect to + /// the normalized fiber length over the domain + /// [minNormFiberLength normFiberLength], where minNormFiberLength is the + /// value return by getMinNormalizedFiberLength(). + SimTK::Real calcPassiveForceMultiplierIntegral( + const SimTK::Real& normFiberLength) const { + if (get_ignore_passive_fiber_force()) return 0; + + const double& e0 = get_passive_fiber_strain_at_one_norm_force(); + + const double temp1 = + e0 + kPE * normFiberLength - kPE * m_minNormFiberLength; + const double temp2 = exp(kPE * (normFiberLength - 1.0) / e0); + const double temp3 = exp(kPE * (m_minNormFiberLength - 1.0) / e0); + const double numer = exp(kPE) * temp1 - e0 * temp2; + const double denom = kPE * (temp3 - exp(kPE)); + return (temp1 / kPE) + (numer / denom); + } + + /// The normalized tendon force as a function of normalized tendon length. + /// Note that this curve does not go through (1, 0); when + /// normTendonLength=1, this function returns a slightly negative number. + // TODO: In explicit mode, do not allow negative tendon forces? + SimTK::Real calcTendonForceMultiplier( + const SimTK::Real& normTendonLength) const { + return c1 * exp(m_kT * (normTendonLength - c2)) - c3; + } + + /// This is the derivative of the tendon-force length curve with respect to + /// normalized tendon length. + SimTK::Real calcTendonForceMultiplierDerivative( + const SimTK::Real& normTendonLength) const { + return c1 * m_kT * exp(m_kT * (normTendonLength - c2)); + } + + /// This is the integral of the tendon-force length curve with respect to + /// normalized tendon length over the domain + /// [minNormTendonLength normTendonLength]. The lower bound on the domain + /// is computed by passing the value return by getMinNormalizedTendonForce() + /// to calcTendonForceLengthInverseCurve(). + SimTK::Real calcTendonForceMultiplierIntegral( + const SimTK::Real& normTendonLength) const { + const double minNormTendonLength = + calcTendonForceLengthInverseCurve(m_minNormTendonForce); + const double temp1 = + exp(m_kT * normTendonLength) - exp(m_kT * minNormTendonLength); + const double temp2 = c1 * exp(-c2 * m_kT) / m_kT; + const double temp3 = c3 * (normTendonLength - minNormTendonLength); + return temp1 * temp2 - temp3; + } + + /// This is the inverse of the tendon force-length curve, and returns the + /// normalized tendon length as a function of the normalized tendon force. + SimTK::Real calcTendonForceLengthInverseCurve( + const SimTK::Real& normTendonForce) const { + return log((1.0 / c1) * (normTendonForce + c3)) / m_kT + c2; + } + + /// This returns normalized tendon velocity given the derivative of + /// normalized tendon force and normalized tendon length. This is derived + /// by taking the derivative of the tendon force multiplier curve with + /// respect to time and then solving for normalized fiber velocity (see + /// supplementary information for De Groote et al. 2016). + SimTK::Real calcTendonForceLengthInverseCurveDerivative( + const SimTK::Real& derivNormTendonForce, + const SimTK::Real& normTendonLength) const { + return derivNormTendonForce / + (c1 * m_kT * exp(m_kT * (normTendonLength - c2))); + } + + /// This computes both the total fiber force and the individual components + /// of fiber force (active, conservative passive, and non-conservative + /// passive). + /// @note based on Millard2012EquilibriumMuscle::calcFiberForce(). + void calcFiberForce(const SimTK::Real& activation, + const SimTK::Real& activeForceLengthMultiplier, + const SimTK::Real& forceVelocityMultiplier, + const SimTK::Real& normPassiveFiberForce, + const SimTK::Real& normFiberVelocity, + SimTK::Real& activeFiberForce, + SimTK::Real& conPassiveFiberForce, + SimTK::Real& nonConPassiveFiberForce, + SimTK::Real& totalFiberForce) const { + const auto& maxIsometricForce = get_max_isometric_force(); + // active force + activeFiberForce = + maxIsometricForce * (activation * activeForceLengthMultiplier * + forceVelocityMultiplier); + // conservative passive force + conPassiveFiberForce = maxIsometricForce * normPassiveFiberForce; + // non-conservative passive force + nonConPassiveFiberForce = + maxIsometricForce * get_fiber_damping() * normFiberVelocity; + // total force + totalFiberForce = activeFiberForce + conPassiveFiberForce + + nonConPassiveFiberForce; + } + + /// The stiffness of the fiber in the direction of the fiber. This includes + /// both active and passive force contributions to stiffness from the muscle + /// fiber. + /// @note based on Millard2012EquilibriumMuscle::calcFiberStiffness(). + SimTK::Real calcFiberStiffness(const SimTK::Real& activation, + const SimTK::Real& normFiberLength, + const SimTK::Real& fiberVelocityMultiplier) const { + + const SimTK::Real partialNormFiberLengthPartialFiberLength = + 1.0 / get_optimal_fiber_length(); + const SimTK::Real partialNormActiveForcePartialFiberLength = + partialNormFiberLengthPartialFiberLength * + calcActiveForceLengthMultiplierDerivative(normFiberLength); + const SimTK::Real partialNormPassiveForcePartialFiberLength = + partialNormFiberLengthPartialFiberLength * + calcPassiveForceMultiplierDerivative(normFiberLength); + + // fiberStiffness = d_fiberForce / d_fiberLength + return get_max_isometric_force() * + (activation * partialNormActiveForcePartialFiberLength * + fiberVelocityMultiplier + + partialNormPassiveForcePartialFiberLength); + } + + /// The stiffness of the tendon in the direction of the tendon. + /// @note based on Millard2012EquilibriumMuscle. + SimTK::Real calcTendonStiffness(const SimTK::Real& normTendonLength) const { + + if (get_ignore_tendon_compliance()) return SimTK::Infinity; + return (get_max_isometric_force() / get_tendon_slack_length()) * + calcTendonForceMultiplierDerivative(normTendonLength); + } + + /// The stiffness of the whole musculotendon unit in the direction of the + /// tendon. + /// @note based on Millard2012EquilibriumMuscle. + SimTK::Real calcMuscleStiffness(const SimTK::Real& tendonStiffness, + const SimTK::Real& fiberStiffnessAlongTendon) const { + + if (get_ignore_tendon_compliance()) return fiberStiffnessAlongTendon; + // TODO Millard2012EquilibriumMuscle includes additional checks that + // the stiffness is non-negative and that the denominator is non-zero. + // Checks are omitted here to preserve continuity and smoothness for + // optimization (see #3685). + return (fiberStiffnessAlongTendon * tendonStiffness) / + (fiberStiffnessAlongTendon + tendonStiffness); + } + + virtual double calcMuscleStiffness(const SimTK::State& s) const override + { + const MuscleDynamicsInfo& mdi = getMuscleDynamicsInfo(s); + return calcMuscleStiffness( + mdi.tendonStiffness, + mdi.fiberStiffnessAlongTendon); + } + + /// The derivative of pennation angle with respect to fiber length. + /// @note based on + /// MuscleFixedWidthPennationModel::calc_DPennationAngle_DFiberLength(). + SimTK::Real calcPartialPennationAnglePartialFiberLength( + const SimTK::Real& fiberLength) const { + + using SimTK::square; + // pennationAngle = asin(fiberWidth/fiberLength) + // d_pennationAngle/d_fiberLength = + // d/d_fiberLength (asin(fiberWidth/fiberLength)) + return (-m_fiberWidth / square(fiberLength)) / + sqrt(1.0 - square(m_fiberWidth / fiberLength)); + } + + /// The derivative of the fiber force along the tendon with respect to fiber + /// length. + /// @note based on + /// Millard2012EquilibriumMuscle::calc_DFiberForceAT_DFiberLength(). + SimTK::Real calcPartialFiberForceAlongTendonPartialFiberLength( + const SimTK::Real& fiberForce, const SimTK::Real& fiberStiffness, + const SimTK::Real& sinPennationAngle, + const SimTK::Real& cosPennationAngle, + const SimTK::Real& partialPennationAnglePartialFiberLength) const { + + const SimTK::Real partialCosPennationAnglePartialFiberLength = + -sinPennationAngle * partialPennationAnglePartialFiberLength; + + // The stiffness of the fiber along the direction of the tendon. For + // small changes in length parallel to the fiber, this quantity is + // d_fiberForceAlongTendon / d_fiberLength = + // d/d_fiberLength(fiberForce * cosPennationAngle) + return fiberStiffness * cosPennationAngle + + fiberForce * partialCosPennationAnglePartialFiberLength; + } + + /// The derivative of the fiber force along the tendon with respect to the + /// fiber length along the tendon. + /// @note based on + /// Millard2012EquilibriumMuscle::calc_DFiberForceAT_DFiberLengthAT. + SimTK::Real calcFiberStiffnessAlongTendon(const SimTK::Real& fiberLength, + const SimTK::Real& partialFiberForceAlongTendonPartialFiberLength, + const SimTK::Real& sinPennationAngle, + const SimTK::Real& cosPennationAngle, + const SimTK::Real& partialPennationAnglePartialFiberLength) const { + + // The change in length of the fiber length along the tendon. + // fiberLengthAlongTendon = fiberLength * cosPennationAngle + const SimTK::Real partialFiberLengthAlongTendonPartialFiberLength = + cosPennationAngle - + fiberLength * sinPennationAngle * + partialPennationAnglePartialFiberLength; + + // fiberStiffnessAlongTendon + // = d_fiberForceAlongTendon / d_fiberLengthAlongTendon + // = (d_fiberForceAlongTendon / d_fiberLength) * + // (1 / (d_fiberLengthAlongTendon / d_fiberLength)) + return partialFiberForceAlongTendonPartialFiberLength * + (1.0 / partialFiberLengthAlongTendonPartialFiberLength); + } + + SimTK::Real calcPartialTendonLengthPartialFiberLength( + const SimTK::Real& fiberLength, + const SimTK::Real& sinPennationAngle, + const SimTK::Real& cosPennationAngle, + const SimTK::Real& partialPennationAnglePartialFiberLength) const { + + return fiberLength * sinPennationAngle * + partialPennationAnglePartialFiberLength - + cosPennationAngle; + } + + SimTK::Real calcPartialTendonForcePartialFiberLength( + const SimTK::Real& tendonStiffness, const SimTK::Real& fiberLength, + const SimTK::Real& sinPennationAngle, + const SimTK::Real& cosPennationAngle) const { + const SimTK::Real partialPennationAnglePartialFiberLength = + calcPartialPennationAnglePartialFiberLength(fiberLength); + + const SimTK::Real partialTendonLengthPartialFiberLength = + calcPartialTendonLengthPartialFiberLength(fiberLength, + sinPennationAngle, cosPennationAngle, + partialPennationAnglePartialFiberLength); + + return tendonStiffness * partialTendonLengthPartialFiberLength; + } + + /// The residual (i.e. error) in the muscle-tendon equilibrium equation: + /// residual = normTendonForce - normFiberForce * cosPennationAngle + /// The residual is unitless (units of normalized force). + /// This is computed using the muscle in implicit mode, since explicit mode + /// uses the normalized tendon force state variable directly + /// to compute fiber force, which always produces a zero muscle-tendon + /// equilibrium residual. + SimTK::Real calcEquilibriumResidual(const SimTK::Real& muscleTendonLength, + const SimTK::Real& muscleTendonVelocity, + const SimTK::Real& activation, const SimTK::Real& normTendonForce, + const SimTK::Real& normTendonForceDerivative) const { + + MuscleLengthInfo mli; + FiberVelocityInfo fvi; + MuscleDynamicsInfo mdi; + calcMuscleLengthInfoHelper( + muscleTendonLength, false, mli, normTendonForce); + calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, false, + false, mli, fvi, normTendonForce, normTendonForceDerivative); + calcMuscleDynamicsInfoHelper(activation, false, mli, fvi, mdi, + normTendonForce); + + return mdi.normTendonForce - + mdi.fiberForceAlongTendon / get_max_isometric_force(); + } + + /// The residual (i.e. error) in the time derivative of the linearized + /// muscle-tendon equilibrium equation (Millard et al. 2013, equation A6): + /// residual = fiberStiffnessAlongTendon * fiberVelocityAlongTendon - + /// tendonStiffness * + /// (muscleTendonVelocity - fiberVelocityAlongTendon) + /// This may be useful for finding equilibrium when there is velocity in the + /// muscle-tendon actuator. Velocity is divided between the muscle and + /// tendon based on their relative stiffnesses. + SimTK::Real calcLinearizedEquilibriumResidualDerivative( + const SimTK::Real& muscleTendonLength, + const SimTK::Real& muscleTendonVelocity, + const SimTK::Real& activation, const SimTK::Real& normTendonForce, + const SimTK::Real& normTendonForceDerivative) const { + + MuscleLengthInfo mli; + FiberVelocityInfo fvi; + MuscleDynamicsInfo mdi; + calcMuscleLengthInfoHelper( + muscleTendonLength, false, mli, normTendonForce); + calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, false, + m_isTendonDynamicsExplicit, mli, fvi, normTendonForce, + normTendonForceDerivative); + calcMuscleDynamicsInfoHelper(activation, false, mli, fvi, mdi, + normTendonForce); + + return mdi.fiberStiffnessAlongTendon * fvi.fiberVelocityAlongTendon - + mdi.tendonStiffness * + (muscleTendonVelocity - fvi.fiberVelocityAlongTendon); + } + /// @} + + /// @name Utilities + /// @{ + + /// Export the active force-length multiplier and passive force multiplier + /// curves to a DataTable. If the normFiberLengths argument is omitted, we + /// use createVectorLinspace(200, minNormFiberLength, maxNormFiberLength). + DataTable exportFiberLengthCurvesToTable( + const SimTK::Vector& normFiberLengths = SimTK::Vector()) const; + /// Export the fiber force-velocity multiplier curve to a DataTable. If + /// the normFiberVelocities argument is omitted, we use + /// createVectorLinspace(200, -1.1, 1.1). + DataTable exportFiberVelocityMultiplierToTable( + const SimTK::Vector& normFiberVelocities = SimTK::Vector()) const; + /// Export the fiber tendon force multiplier curve to a DataTable. If + /// the normFiberVelocities argument is omitted, we use + /// createVectorLinspace(200, 0.95, 1 + ) + DataTable exportTendonForceMultiplierToTable( + const SimTK::Vector& normTendonLengths = SimTK::Vector()) const; + /// Print the muscle curves to STO files. The files will be named as + /// `_.sto`. + /// + /// @param directory + /// The directory to which the data files should be written. Do NOT + /// include the filename. By default, the files are printed to the + /// current working directory. + void printCurvesToSTOFiles(const std::string& directory = ".") const; + + /// Replace muscles of other types in the model with muscles of this type. + /// Currently, only Millard2012EquilibriumMuscles and Thelen2003Muscles + /// are replaced. For these two muscle classes, we copy property values into + /// equivalent properties of the newly created DeGrooteFregly2016Muscle. + /// If the model has muscles of other types, an exception is + /// thrown unless allowUnsupportedMuscles is true, in which a + /// DeGrooteFregly2016Muscle is created using only the base Muscle class + /// property values. + /// Since the DeGrooteFregly2016Muscle implements tendon compliance dynamics + /// with normalized tendon force as the state variable, this function + /// ignores the 'default_fiber_length' property in replaced muscles. + static void replaceMuscles( + Model& model, bool allowUnsupportedMuscles = false); + /// @} + + /// @name Scaling + /// @{ + /// Adjust the properties of the muscle after the model has been scaled. The + /// optimal fiber length and tendon slack length are each multiplied by the + /// ratio of the current path length and the path length before scaling. + void extendPostScale( + const SimTK::State& s, const ScaleSet& scaleSet) override; + /// @} + +private: + void constructProperties(); + + void calcMuscleLengthInfoHelper(const SimTK::Real& muscleTendonLength, + const bool& ignoreTendonCompliance, MuscleLengthInfo& mli, + const SimTK::Real& normTendonForce = SimTK::NaN) const; + /// `normTendonForce` is required if and only if `isTendonDynamicsExplicit` + /// is true. `normTendonForceDerivative` is required if and only if + /// `isTendonDynamicsExplicit` is false. + void calcFiberVelocityInfoHelper(const SimTK::Real& muscleTendonVelocity, + const SimTK::Real& activation, const bool& ignoreTendonCompliance, + const bool& isTendonDynamicsExplicit, const MuscleLengthInfo& mli, + FiberVelocityInfo& fvi, + const SimTK::Real& normTendonForce = SimTK::NaN, + const SimTK::Real& normTendonForceDerivative = SimTK::NaN) const; + void calcMuscleDynamicsInfoHelper(const SimTK::Real& activation, + const bool& ignoreTendonCompliance, const MuscleLengthInfo& mli, + const FiberVelocityInfo& fvi, MuscleDynamicsInfo& mdi, + const SimTK::Real& normTendonForce = SimTK::NaN) const; + void calcMusclePotentialEnergyInfoHelper(const bool& ignoreTendonCompliance, + const MuscleLengthInfo& mli, MusclePotentialEnergyInfo& mpei) const; + + /// This is a Gaussian-like function used in the active force-length curve. + /// A proper Gaussian function does not have the variable in the denominator + /// of the exponent. + /// The supplement for De Groote et al., 2016 has a typo: + /// the denominator should be squared. + static SimTK::Real calcGaussianLikeCurve(const SimTK::Real& x, + const double& b1, const double& b2, const double& b3, + const double& b4) { + using SimTK::square; + return b1 * exp(-0.5 * square(x - b2) / square(b3 + b4 * x)); + } + + /// The derivative of the curve defined in calcGaussianLikeCurve() with + /// respect to 'x' (usually normalized fiber length). + static SimTK::Real calcGaussianLikeCurveDerivative(const SimTK::Real& x, + const double& b1, const double& b2, const double& b3, + const double& b4) { + using SimTK::cube; + using SimTK::square; + return (b1 * exp(-square(b2 - x) / (2 * square(b3 + b4 * x))) * + (b2 - x) * (b3 + b2 * b4)) / + cube(b3 + b4 * x); + } + + //enum StatusFromEstimateMuscleFiberState { + // Success_Converged, + // Warning_FiberAtLowerBound, + // Warning_FiberAtUpperBound, + // Failure_MaxIterationsReached + //}; + + //struct ValuesFromEstimateMuscleFiberState { + // int iterations; + // double solution_error; + // double fiber_length; + // double fiber_velocity; + // double normalized_tendon_force; + //}; + + //std::pair + //estimateMuscleFiberState(const double activation, + // const double muscleTendonLength, const double muscleTendonVelocity, + // const double normTendonForceDerivative, const double tolerance, + // const int maxIterations) const; + + // Curve parameters. + // Notation comes from De Groote et al., 2016 (supplement). + + // Parameters for the active fiber force-length curve. + // --------------------------------------------------- + // Values are taken from https://simtk.org/projects/optcntrlmuscle + // rather than the paper supplement. b11 was modified to ensure that + // f(1) = 1. + constexpr static double b11 = 0.814483478343008; + constexpr static double b21 = 1.055033428970575; + constexpr static double b31 = 0.162384573599574; + constexpr static double b41 = 0.063303448465465; + constexpr static double b12 = 0.433004984392647; + constexpr static double b22 = 0.716775413397760; + constexpr static double b32 = -0.029947116970696; + constexpr static double b42 = 0.200356847296188; + constexpr static double b13 = 0.1; + constexpr static double b23 = 1.0; + constexpr static double b33 = 0.353553390593274; // 0.5 * sqrt(0.5) + constexpr static double b43 = 0.0; + + // Parameters for the passive fiber force-length curve. + // --------------------------------------------------- + constexpr static double e1 = 0.232000797810576; + constexpr static double e2 = 12.438535493526128; + constexpr static double e3 = 1.329470475731338; + + // Exponential shape factor. + constexpr static double kPE = 4.0; + + // Parameters for the tendon force curve. + // -------------------------------------- + constexpr static double c1 = 0.200; + // Horizontal asymptote as x -> -inf is -c3. + // Normalized force at 0 strain is c1 * exp(-c2) - c3. + // This parameter is 0.995 in De Groote et al., which causes the y-value at + // 0 strain to be negative. We use 1.0 so that the y-value at 0 strain is 0 + // (since c2 == c3). + constexpr static double c2 = 1.0; + // This parameter is 0.250 in De Groote et al., which causes + // lim(x->-inf) = -0.25 instead of -0.20. + constexpr static double c3 = 0.200; + + // Parameters for the force-velocity curve. + // ---------------------------------------- + constexpr static double d1 = -12.4090551101489; + constexpr static double d2 = 9.35818070443882; + constexpr static double d3 = 7.93116233095206; + constexpr static double d4 = 2.70637350085154; + constexpr static double d5 = -0.274108493130008; + constexpr static double d6 = 8.03512618783281; + + constexpr static double m_minNormFiberLength = 0.2; + constexpr static double m_maxNormFiberLength = 1.8; + + constexpr static double m_minNormTendonForce = 0.0; + constexpr static double m_maxNormTendonForce = 5.0; + + static const std::string STATE_ACTIVATION_NAME; + static const std::string STATE_NORMALIZED_TENDON_FORCE_NAME; + static const std::string DERIVATIVE_NORMALIZED_TENDON_FORCE_NAME; + static const std::string RESIDUAL_NORMALIZED_TENDON_FORCE_NAME; + + // Computed from properties. + // ------------------------- + + // The square of (fiber_width / optimal_fiber_length). + SimTK::Real m_fiberWidth = SimTK::NaN; + SimTK::Real m_squareFiberWidth = SimTK::NaN; + SimTK::Real m_maxContractionVelocityInMetersPerSecond = SimTK::NaN; + // Tendon stiffness parameter from De Groote et al., 2016. Instead of + // kT, users specify tendon strain at 1 norm force, which is more intuitive. + SimTK::Real m_kT = SimTK::NaN; + bool m_isTendonDynamicsExplicit = true; + + // Indices for MuscleDynamicsInfo::userDefinedDynamicsExtras. + constexpr static int m_mdi_passiveFiberElasticForce = 0; + constexpr static int m_mdi_passiveFiberDampingForce = 1; + constexpr static int m_mdi_partialPennationAnglePartialFiberLength = 2; + constexpr static int m_mdi_partialFiberForceAlongTendonPartialFiberLength = + 3; + constexpr static int m_mdi_partialTendonForcePartialFiberLength = 4; +}; + +} // namespace OpenSim + +#endif // MOCO_DEGROOTEFREGLY2016MUSCLE_H diff --git a/OpenSim/Actuators/RegisterTypes_osimActuators.cpp b/OpenSim/Actuators/RegisterTypes_osimActuators.cpp index f7c3458214..30f922b60b 100644 --- a/OpenSim/Actuators/RegisterTypes_osimActuators.cpp +++ b/OpenSim/Actuators/RegisterTypes_osimActuators.cpp @@ -79,6 +79,7 @@ OSIMACTUATORS_API void RegisterTypes_osimActuators() Object::RegisterType(Millard2012EquilibriumMuscle()); Object::RegisterType(Millard2012AccelerationMuscle()); Object::RegisterType(DeGrooteFregly2016Muscle()); + Object::RegisterType(MeyerFregly2016Muscle()); Object::registerType(ModelProcessor()); Object::registerType(ModOpIgnoreActivationDynamics()); diff --git a/OpenSim/Actuators/osimActuators.h b/OpenSim/Actuators/osimActuators.h index b9bda3cf10..7084fe0cf2 100644 --- a/OpenSim/Actuators/osimActuators.h +++ b/OpenSim/Actuators/osimActuators.h @@ -40,6 +40,7 @@ #include "Millard2012EquilibriumMuscle.h" #include "Millard2012AccelerationMuscle.h" #include "DeGrooteFregly2016Muscle.h" +#include "MeyerFregly2016Muscle.h" #include "McKibbenActuator.h"