From 2063d6f2d13177c829a9b7e39e662c0599d275dc Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Fri, 5 Jul 2024 02:24:19 -0500 Subject: [PATCH 01/25] MeyerFregly2016Muscle --- Bindings/OpenSimHeaders_actuators.h | 1 + Bindings/actuators.i | 1 + OpenSim/Actuators/MeyerFregly2016Muscle.cpp | 1053 +++++++++++++++++ OpenSim/Actuators/MeyerFregly2016Muscle.h | 959 +++++++++++++++ .../Actuators/RegisterTypes_osimActuators.cpp | 1 + OpenSim/Actuators/osimActuators.h | 1 + 6 files changed, 2016 insertions(+) create mode 100644 OpenSim/Actuators/MeyerFregly2016Muscle.cpp create mode 100644 OpenSim/Actuators/MeyerFregly2016Muscle.h 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" From 0a5110e212c71e616f169cd8e8420e28a0949f1e Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Fri, 5 Jul 2024 02:24:26 -0500 Subject: [PATCH 02/25] Contact element --- .../Components/StationPlaneContactForce.h | 42 +++++++++++++------ 1 file changed, 29 insertions(+), 13 deletions(-) diff --git a/OpenSim/Moco/Components/StationPlaneContactForce.h b/OpenSim/Moco/Components/StationPlaneContactForce.h index d1a16642d1..f6d2dde73c 100644 --- a/OpenSim/Moco/Components/StationPlaneContactForce.h +++ b/OpenSim/Moco/Components/StationPlaneContactForce.h @@ -5,7 +5,7 @@ * -------------------------------------------------------------------------- * * Copyright (c) 2017 Stanford University and the Authors * * * - * Author(s): Nicholas Bianco, Chris Dembia * + * Author(s): Nicholas Bianco, Chris Dembia, Spencer Williams * * * * 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 * @@ -155,8 +155,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, "Spring stiffness in N/m (default: 1e4)."); OpenSim_DECLARE_PROPERTY(dissipation, double, "Dissipation coefficient in s/m (default: 0.01)."); - OpenSim_DECLARE_PROPERTY(tscale, double, - "TODO"); + OpenSim_DECLARE_PROPERTY(dynamic_friction, double, + "Dynamic friction coefficient (default: 0)."); + OpenSim_DECLARE_PROPERTY(viscous_friction, double, + "Viscous friction coefficient (default: 5)."); + OpenSim_DECLARE_PROPERTY(latch_velocity, double, + "Latching velocity in m/s (default: 0.05)."); MeyerFregly2016Force() { constructProperties(); @@ -172,14 +176,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, const auto& vel = pt.getVelocityInGround(s); const SimTK::Real y = pos[1]; const SimTK::Real velNormal = vel[1]; - // TODO should project vel into ground. - const SimTK::Real velSliding = vel[0]; - // const SimTK::Real depth = 0 - y; + const SimTK::Real velSliding = sqrt(vel[0] * vel[0] + vel[2] * vel[2]); const SimTK::Real depthRate = 0 - velNormal; const SimTK::Real Kval = get_stiffness(); const SimTK::Real Cval = get_dissipation(); - const SimTK::Real tscale = get_tscale(); - const SimTK::Real klow = 1e-1 / (tscale * tscale); + const SimTK::Real klow = 1e-1; const SimTK::Real h = 1e-3; const SimTK::Real c = 5e-4; const SimTK::Real ymax = 1e-2; @@ -202,11 +203,24 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, force[1] = Fy; /// Friction force. - const SimTK::Real mu_d = 1; - const SimTK::Real latchvel = 0.05; // m/s + const SimTK::Real mu_d = get_dynamic_friction(); + const SimTK::Real mu_v = get_viscous_friction(); + const SimTK::Real latchvel = get_latch_velocity(); - const SimTK::Real mu = mu_d * tanh(velSliding / latchvel / 2); - force[0] = -force[1] * mu; + if (velSliding < 1e-10) { + velSliding = 0; + } + const SimTK::Real horizontalForce = force[1] * ( + mu_d * tanh(velSliding / latchvel) + mu_v * velSliding + ); + if (SimTK::isNaN(horizontalForce) || SimTK::isInf(horizontalForce)) { + horizontalForce = 0; + } + + const SimTK::Real slipOffset = 1e-4; + + force[0] = -vel[0] / (velSliding + slipOffset) * horizontalForce; + force[2] = -vel[2] / (velSliding + slipOffset) * horizontalForce; return force; } @@ -215,7 +229,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, void constructProperties() { constructProperty_stiffness(1e4); constructProperty_dissipation(1e-2); - constructProperty_tscale(1.0); + constructProperty_dynamic_friction(0); + constructProperty_viscous_friction(5); + constructProperty_latch_velocity(0.05); } }; From ce52f67d1f7cb6d6f3f3ea9978945334a9bbc62c Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Fri, 5 Jul 2024 02:24:32 -0500 Subject: [PATCH 03/25] Actuator test --- OpenSim/Moco/Test/testMocoActuators.cpp | 52 +++++++++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/OpenSim/Moco/Test/testMocoActuators.cpp b/OpenSim/Moco/Test/testMocoActuators.cpp index 190533574b..735f1d594c 100644 --- a/OpenSim/Moco/Test/testMocoActuators.cpp +++ b/OpenSim/Moco/Test/testMocoActuators.cpp @@ -76,6 +76,54 @@ namespace { return model; } + + Model createHangingMuscleModelMeyerFregly(double optimalFiberLength, + double tendonSlackLength, bool ignoreActivationDynamics, + bool ignoreTendonCompliance, bool isTendonDynamicsExplicit) { + Model model; + model.setName("isometric_muscle"); + model.set_gravity(SimTK::Vec3(9.81, 0, 0)); + auto* body = new Body("body", 0.5, SimTK::Vec3(0), SimTK::Inertia(0)); + model.addComponent(body); + + // Allows translation along x. + auto* joint = new SliderJoint("joint", model.getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("height"); + model.addComponent(joint); + + auto* actu = new MeyerFregly2016Muscle(); + actu->setName("muscle"); + actu->set_max_isometric_force(10.0); + actu->set_optimal_fiber_length(optimalFiberLength); + actu->set_tendon_slack_length(tendonSlackLength); + actu->set_tendon_strain_at_one_norm_force(0.10); + actu->set_ignore_activation_dynamics(ignoreActivationDynamics); + actu->set_ignore_tendon_compliance(ignoreTendonCompliance); + actu->set_fiber_damping(0.01); + if (!isTendonDynamicsExplicit) { + actu->set_tendon_compliance_dynamics_mode("implicit"); + } + actu->set_max_contraction_velocity(10); + actu->set_pennation_angle_at_optimal(0); + actu->addNewPathPoint("origin", model.updGround(), SimTK::Vec3(0)); + actu->addNewPathPoint("insertion", *body, SimTK::Vec3(0)); + model.addForce(actu); + + body->attachGeometry(new Sphere(0.05)); + + CMC_TaskSet tasks; + CMC_Joint task; + task.setName(coord.getName()); + task.setCoordinateName(coord.getName()); + task.setKP(100, 1, 1); + task.setKV(20, 1, 1); + task.setActive(true, false, false); + tasks.cloneAndAppend(task); + tasks.print("hanging_muscle_cmc_tasks.xml"); + + return model; + } } TEMPLATE_TEST_CASE( @@ -104,6 +152,10 @@ TEMPLATE_TEST_CASE( MocoBounds actuBounds(0.1, 1); MocoBounds normTendonBounds(0.1, 2); + Model model = createHangingMuscleModelMeyerFregly(optimalFiberLength, + tendonSlackLength, ignoreActivationDynamics, true, + isTendonDynamicsExplicit); + Model model = createHangingMuscleModel(optimalFiberLength, tendonSlackLength, ignoreActivationDynamics, ignoreTendonCompliance, isTendonDynamicsExplicit); From bac8757eb2872c01dc5fa98bf47e93d5b335ea3d Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Fri, 5 Jul 2024 12:07:20 -0500 Subject: [PATCH 04/25] Update StationPlaneContactForce.h --- OpenSim/Moco/Components/StationPlaneContactForce.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Moco/Components/StationPlaneContactForce.h b/OpenSim/Moco/Components/StationPlaneContactForce.h index f6d2dde73c..1ecf62ad8b 100644 --- a/OpenSim/Moco/Components/StationPlaneContactForce.h +++ b/OpenSim/Moco/Components/StationPlaneContactForce.h @@ -176,7 +176,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, const auto& vel = pt.getVelocityInGround(s); const SimTK::Real y = pos[1]; const SimTK::Real velNormal = vel[1]; - const SimTK::Real velSliding = sqrt(vel[0] * vel[0] + vel[2] * vel[2]); + SimTK::Real velSliding = sqrt(vel[0] * vel[0] + vel[2] * vel[2]); const SimTK::Real depthRate = 0 - velNormal; const SimTK::Real Kval = get_stiffness(); const SimTK::Real Cval = get_dissipation(); @@ -210,7 +210,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, if (velSliding < 1e-10) { velSliding = 0; } - const SimTK::Real horizontalForce = force[1] * ( + SimTK::Real horizontalForce = force[1] * ( mu_d * tanh(velSliding / latchvel) + mu_v * velSliding ); if (SimTK::isNaN(horizontalForce) || SimTK::isInf(horizontalForce)) { From 78dea72f8d4e5c3d44c5ee3cf2098ac875859a66 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Fri, 5 Jul 2024 14:52:05 -0500 Subject: [PATCH 05/25] Simplify actuator test --- OpenSim/Moco/Test/testMocoActuators.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/OpenSim/Moco/Test/testMocoActuators.cpp b/OpenSim/Moco/Test/testMocoActuators.cpp index 735f1d594c..df2963f79b 100644 --- a/OpenSim/Moco/Test/testMocoActuators.cpp +++ b/OpenSim/Moco/Test/testMocoActuators.cpp @@ -153,10 +153,6 @@ TEMPLATE_TEST_CASE( MocoBounds normTendonBounds(0.1, 2); Model model = createHangingMuscleModelMeyerFregly(optimalFiberLength, - tendonSlackLength, ignoreActivationDynamics, true, - isTendonDynamicsExplicit); - - Model model = createHangingMuscleModel(optimalFiberLength, tendonSlackLength, ignoreActivationDynamics, ignoreTendonCompliance, isTendonDynamicsExplicit); From 355066c7db07f3fbdaf29365a078cd21effe224b Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Thu, 15 Aug 2024 23:38:16 -0500 Subject: [PATCH 06/25] Clean comments and integral function --- OpenSim/Actuators/MeyerFregly2016Muscle.h | 47 +++++-------------- .../Components/StationPlaneContactForce.h | 3 +- 2 files changed, 13 insertions(+), 37 deletions(-) diff --git a/OpenSim/Actuators/MeyerFregly2016Muscle.h b/OpenSim/Actuators/MeyerFregly2016Muscle.h index c9e2f20067..2393df0f05 100644 --- a/OpenSim/Actuators/MeyerFregly2016Muscle.h +++ b/OpenSim/Actuators/MeyerFregly2016Muscle.h @@ -10,7 +10,7 @@ * through the Warrior Web program. * * * * Copyright (c) 2005-2020 Stanford University and the Authors * - * Author(s): Spencer Williams, Christopher Dembia, Nicholas Bianco * + * Author(s): Christopher Dembia, Nicholas Bianco, Spencer Williams * * * * 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 * @@ -35,17 +35,10 @@ namespace OpenSim { // might be slow. // TODO prohibit fiber length from going below 0.2. -/** This muscle model was published in De Groote et al. 2016. +/** This muscle model was published in Meyer 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 muscle implementation is based on the previously implemented +DeGrooteFregly2016Muscle. This implementation introduces the property 'active_force_width_scale' as an addition to the original model, which allows users to effectively make @@ -97,10 +90,10 @@ 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 */ +Meyer A. J., Eskinazi, I., Jackson, J. N., Rao, A. V., Patten, C., & Fregly, +B. J. (2016). Muscle Synergies Facilitate Computational Prediction of +Subject-Specific Walking Motions. Frontiers in Bioengineering and +Biotechnology, 4, 1055–27. http://doi.org/10.3389/fbioe.2016.00077 */ class OSIMACTUATORS_API MeyerFregly2016Muscle : public Muscle { OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Muscle, Muscle); @@ -428,15 +421,6 @@ class OSIMACTUATORS_API MeyerFregly2016Muscle : public Muscle { /// 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; @@ -458,19 +442,12 @@ class OSIMACTUATORS_API MeyerFregly2016Muscle : public Muscle { /// the normalized fiber length over the domain /// [minNormFiberLength normFiberLength], where minNormFiberLength is the /// value return by getMinNormalizedFiberLength(). + /// + /// This placeholder implementation returns zero. + 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); + return 0; } /// The normalized tendon force as a function of normalized tendon length. diff --git a/OpenSim/Moco/Components/StationPlaneContactForce.h b/OpenSim/Moco/Components/StationPlaneContactForce.h index 1ecf62ad8b..a10073f9b2 100644 --- a/OpenSim/Moco/Components/StationPlaneContactForce.h +++ b/OpenSim/Moco/Components/StationPlaneContactForce.h @@ -144,8 +144,7 @@ Meyer A. J., Eskinazi, I., Jackson, J. N., Rao, A. V., Patten, C., & Fregly, B. J. (2016). Muscle Synergies Facilitate Computational Prediction of Subject-Specific Walking Motions. Frontiers in Bioengineering and Biotechnology, 4, 1055–27. http://doi.org/10.3389/fbioe.2016.00077 - -@underdevelopment */ + */ class OSIMMOCO_API MeyerFregly2016Force : public StationPlaneContactForce { OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, From 83edd247f8573626b967abe9c9c8a0f6c24eabed Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Thu, 15 Aug 2024 23:38:27 -0500 Subject: [PATCH 07/25] Revert "Simplify actuator test" This reverts commit 78dea72f8d4e5c3d44c5ee3cf2098ac875859a66. --- OpenSim/Moco/Test/testMocoActuators.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/OpenSim/Moco/Test/testMocoActuators.cpp b/OpenSim/Moco/Test/testMocoActuators.cpp index df2963f79b..735f1d594c 100644 --- a/OpenSim/Moco/Test/testMocoActuators.cpp +++ b/OpenSim/Moco/Test/testMocoActuators.cpp @@ -153,6 +153,10 @@ TEMPLATE_TEST_CASE( MocoBounds normTendonBounds(0.1, 2); Model model = createHangingMuscleModelMeyerFregly(optimalFiberLength, + tendonSlackLength, ignoreActivationDynamics, true, + isTendonDynamicsExplicit); + + Model model = createHangingMuscleModel(optimalFiberLength, tendonSlackLength, ignoreActivationDynamics, ignoreTendonCompliance, isTendonDynamicsExplicit); From 5b997647bd4636462a06dec6ed2e8085c5811b0b Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Thu, 15 Aug 2024 23:39:00 -0500 Subject: [PATCH 08/25] Revert "Actuator test" This reverts commit ce52f67d1f7cb6d6f3f3ea9978945334a9bbc62c. --- OpenSim/Moco/Test/testMocoActuators.cpp | 52 ------------------------- 1 file changed, 52 deletions(-) diff --git a/OpenSim/Moco/Test/testMocoActuators.cpp b/OpenSim/Moco/Test/testMocoActuators.cpp index 735f1d594c..190533574b 100644 --- a/OpenSim/Moco/Test/testMocoActuators.cpp +++ b/OpenSim/Moco/Test/testMocoActuators.cpp @@ -76,54 +76,6 @@ namespace { return model; } - - Model createHangingMuscleModelMeyerFregly(double optimalFiberLength, - double tendonSlackLength, bool ignoreActivationDynamics, - bool ignoreTendonCompliance, bool isTendonDynamicsExplicit) { - Model model; - model.setName("isometric_muscle"); - model.set_gravity(SimTK::Vec3(9.81, 0, 0)); - auto* body = new Body("body", 0.5, SimTK::Vec3(0), SimTK::Inertia(0)); - model.addComponent(body); - - // Allows translation along x. - auto* joint = new SliderJoint("joint", model.getGround(), *body); - auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); - coord.setName("height"); - model.addComponent(joint); - - auto* actu = new MeyerFregly2016Muscle(); - actu->setName("muscle"); - actu->set_max_isometric_force(10.0); - actu->set_optimal_fiber_length(optimalFiberLength); - actu->set_tendon_slack_length(tendonSlackLength); - actu->set_tendon_strain_at_one_norm_force(0.10); - actu->set_ignore_activation_dynamics(ignoreActivationDynamics); - actu->set_ignore_tendon_compliance(ignoreTendonCompliance); - actu->set_fiber_damping(0.01); - if (!isTendonDynamicsExplicit) { - actu->set_tendon_compliance_dynamics_mode("implicit"); - } - actu->set_max_contraction_velocity(10); - actu->set_pennation_angle_at_optimal(0); - actu->addNewPathPoint("origin", model.updGround(), SimTK::Vec3(0)); - actu->addNewPathPoint("insertion", *body, SimTK::Vec3(0)); - model.addForce(actu); - - body->attachGeometry(new Sphere(0.05)); - - CMC_TaskSet tasks; - CMC_Joint task; - task.setName(coord.getName()); - task.setCoordinateName(coord.getName()); - task.setKP(100, 1, 1); - task.setKV(20, 1, 1); - task.setActive(true, false, false); - tasks.cloneAndAppend(task); - tasks.print("hanging_muscle_cmc_tasks.xml"); - - return model; - } } TEMPLATE_TEST_CASE( @@ -152,10 +104,6 @@ TEMPLATE_TEST_CASE( MocoBounds actuBounds(0.1, 1); MocoBounds normTendonBounds(0.1, 2); - Model model = createHangingMuscleModelMeyerFregly(optimalFiberLength, - tendonSlackLength, ignoreActivationDynamics, true, - isTendonDynamicsExplicit); - Model model = createHangingMuscleModel(optimalFiberLength, tendonSlackLength, ignoreActivationDynamics, ignoreTendonCompliance, isTendonDynamicsExplicit); From 75c00e7c0d55b1e6d8d86420be31e2ed0ff8b5e2 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Fri, 16 Aug 2024 13:02:32 -0500 Subject: [PATCH 09/25] Update CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 307b9686fe..c1d8519ba9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -60,6 +60,7 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781). - Upgrade Python and NumPy versions to 3.10 and 1.25, repectively, in ci workflow (#3794). - Fixed bug in `report.py` preventing plotting multiple MocoParameter values. (#3808) - Added SynergyController, a controller that computes controls for a model based on a linear combination of a set of Input control signals and a set of synergy vectors. (#3796) +- Added `MeyerFregly2016Muscle` and completed the implementation of the `MeyerFregly2016Force` included in the `StationPlaneContactForce` class to support NMSM Pipeline-equivalent muscle and contant models in Moco. v4.5 From 2c49c57ed488afab1fdfd9b9c522640925841e9d Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Fri, 6 Sep 2024 00:22:05 -0500 Subject: [PATCH 10/25] Force documentation and suggestions --- .../Components/StationPlaneContactForce.h | 57 ++++++++++++++++--- 1 file changed, 48 insertions(+), 9 deletions(-) diff --git a/OpenSim/Moco/Components/StationPlaneContactForce.h b/OpenSim/Moco/Components/StationPlaneContactForce.h index a10073f9b2..9a796a9a2c 100644 --- a/OpenSim/Moco/Components/StationPlaneContactForce.h +++ b/OpenSim/Moco/Components/StationPlaneContactForce.h @@ -3,7 +3,7 @@ /* -------------------------------------------------------------------------- * * OpenSim: StationPlaneContactForce.h * * -------------------------------------------------------------------------- * - * Copyright (c) 2017 Stanford University and the Authors * + * Copyright (c) 2024 Stanford University and the Authors * * * * Author(s): Nicholas Bianco, Chris Dembia, Spencer Williams * * * @@ -144,6 +144,51 @@ Meyer A. J., Eskinazi, I., Jackson, J. N., Rao, A. V., Patten, C., & Fregly, B. J. (2016). Muscle Synergies Facilitate Computational Prediction of Subject-Specific Walking Motions. Frontiers in Bioengineering and Biotechnology, 4, 1055–27. http://doi.org/10.3389/fbioe.2016.00077 + +Following OpenSim convention, this contact element assumes that the y direction +is vertical. Vertical contact force is calculated based on vertical position +and velocity relative to a floor at y=0 using these equations: + +\f[ +v = \frac{k_{val} + k_{low}}{k_{val} - k_{low}} +\f] +\f[ +s = \frac{k_{val} - k_{low}}{2} +\f] +\f[ +R = -s * (v * y_{max} - c * log(\frac{cosh(y_{max} + h)}{c}) +\f] +\f[ +F = -s * (v * y - c * log(\frac{cosh(y + h)}{c}) - R +\f] + +With the following values: +- \f$ k_{val} \f$: stiffness coefficient of contact element +- \f$ k_{low} \f$: small out-of-contact stiffness to assist optimizations +- \f$ y_{max} \f$: y value were out-of-contact force becomes zero +- \f$ c \f$: transition curvature of transition between linear regions +- \f$ h \f$: horizontal offset defining slope transition location +- \f$ y \f$: current y (vertical) position of contact element + +Velocity is then used to incorporate non-linear damping: + +\f[ +F_{damped} = F * (1 + C_{val} * y_{vel}) +\f] + +With the following values: +- \f$ C_{val} \f$: damping coefficient of contact element +- \f$ y_{vel} \f$: current y (vertical) velocity of contact element, negated + +The force equation produces a force-penetration curve similar to a leaky +rectified linear function with a smooth transition region near zero. This +produces a smooth curve with a slight out-of-contact slope to assist +gradient-based optimizations when an element is inappropriately out of contact. + +Horizontal forces are then calculated based on this vertical force and the +horizontal velocity components of the contact element. Both dynamic (modeled +with a tanh function) and viscous (modeled with a linear function) friction +models may be used. */ class OSIMMOCO_API MeyerFregly2016Force : public StationPlaneContactForce { @@ -183,6 +228,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, const SimTK::Real h = 1e-3; const SimTK::Real c = 5e-4; const SimTK::Real ymax = 1e-2; + // Prevents dividing by zero when sliding velocity is zero. + const SimTK::Real slipOffset = 1e-4; /// Normal force. const SimTK::Real vp = (Kval + klow) / (Kval - klow); @@ -193,9 +240,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, SimTK::Real Fspring = -sp * (vp * y - c * log(cosh((y + h) / c))) - constant; - if (SimTK::isNaN(Fspring) || SimTK::isInf(Fspring)) { - Fspring = 0; - } const SimTK::Real Fy = Fspring * (1 + Cval * depthRate); @@ -212,11 +256,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, SimTK::Real horizontalForce = force[1] * ( mu_d * tanh(velSliding / latchvel) + mu_v * velSliding ); - if (SimTK::isNaN(horizontalForce) || SimTK::isInf(horizontalForce)) { - horizontalForce = 0; - } - - const SimTK::Real slipOffset = 1e-4; force[0] = -vel[0] / (velSliding + slipOffset) * horizontalForce; force[2] = -vel[2] / (velSliding + slipOffset) * horizontalForce; From 86ec6cd88220e5ef30a60fd513bc6046e2a4af52 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Sun, 15 Sep 2024 23:45:52 -0500 Subject: [PATCH 11/25] Remove MeyerFregly2016Muscle from branch --- Bindings/OpenSimHeaders_actuators.h | 1 - Bindings/actuators.i | 1 - OpenSim/Actuators/MeyerFregly2016Muscle.cpp | 1053 ----------------- OpenSim/Actuators/MeyerFregly2016Muscle.h | 936 --------------- .../Actuators/RegisterTypes_osimActuators.cpp | 1 - OpenSim/Actuators/osimActuators.h | 1 - 6 files changed, 1993 deletions(-) delete mode 100644 OpenSim/Actuators/MeyerFregly2016Muscle.cpp delete mode 100644 OpenSim/Actuators/MeyerFregly2016Muscle.h diff --git a/Bindings/OpenSimHeaders_actuators.h b/Bindings/OpenSimHeaders_actuators.h index 9e643e616f..c891314945 100644 --- a/Bindings/OpenSimHeaders_actuators.h +++ b/Bindings/OpenSimHeaders_actuators.h @@ -16,7 +16,6 @@ #include #include #include -#include #include #include diff --git a/Bindings/actuators.i b/Bindings/actuators.i index 1f99941298..608ff189dc 100644 --- a/Bindings/actuators.i +++ b/Bindings/actuators.i @@ -12,7 +12,6 @@ %include %include %include -%include %template (SetFunctionBasedPaths) OpenSim::Set; %include diff --git a/OpenSim/Actuators/MeyerFregly2016Muscle.cpp b/OpenSim/Actuators/MeyerFregly2016Muscle.cpp deleted file mode 100644 index 7d7e471289..0000000000 --- a/OpenSim/Actuators/MeyerFregly2016Muscle.cpp +++ /dev/null @@ -1,1053 +0,0 @@ -/* -------------------------------------------------------------------------- * - * 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 deleted file mode 100644 index 2393df0f05..0000000000 --- a/OpenSim/Actuators/MeyerFregly2016Muscle.h +++ /dev/null @@ -1,936 +0,0 @@ -#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): Christopher Dembia, Nicholas Bianco, Spencer Williams * - * * - * 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 Meyer et al. 2016. - -This muscle implementation is based on the previously implemented -DeGrooteFregly2016Muscle. - -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. - -Meyer A. J., Eskinazi, I., Jackson, J. N., Rao, A. V., Patten, C., & Fregly, -B. J. (2016). Muscle Synergies Facilitate Computational Prediction of -Subject-Specific Walking Motions. Frontiers in Bioengineering and -Biotechnology, 4, 1055–27. http://doi.org/10.3389/fbioe.2016.00077 */ -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. - 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(). - /// - /// This placeholder implementation returns zero. - - SimTK::Real calcPassiveForceMultiplierIntegral( - const SimTK::Real& normFiberLength) const { - return 0; - } - - /// 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 30f922b60b..f7c3458214 100644 --- a/OpenSim/Actuators/RegisterTypes_osimActuators.cpp +++ b/OpenSim/Actuators/RegisterTypes_osimActuators.cpp @@ -79,7 +79,6 @@ 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 7084fe0cf2..b9bda3cf10 100644 --- a/OpenSim/Actuators/osimActuators.h +++ b/OpenSim/Actuators/osimActuators.h @@ -40,7 +40,6 @@ #include "Millard2012EquilibriumMuscle.h" #include "Millard2012AccelerationMuscle.h" #include "DeGrooteFregly2016Muscle.h" -#include "MeyerFregly2016Muscle.h" #include "McKibbenActuator.h" From 6f165d1b0192e53518b5b29db9b14d81a923fb41 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Sun, 15 Sep 2024 23:46:09 -0500 Subject: [PATCH 12/25] Remove potential discontinuity --- OpenSim/Moco/Components/StationPlaneContactForce.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/OpenSim/Moco/Components/StationPlaneContactForce.h b/OpenSim/Moco/Components/StationPlaneContactForce.h index 85b6a4d753..5883bca83b 100644 --- a/OpenSim/Moco/Components/StationPlaneContactForce.h +++ b/OpenSim/Moco/Components/StationPlaneContactForce.h @@ -255,9 +255,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, const SimTK::Real mu_v = get_viscous_friction(); const SimTK::Real latchvel = get_latch_velocity(); - if (velSliding < 1e-10) { - velSliding = 0; - } SimTK::Real horizontalForce = force[1] * ( mu_d * tanh(velSliding / latchvel) + mu_v * velSliding ); From 51a4827bc2dbf697f947a9000667568b986ec4d7 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Sun, 15 Sep 2024 23:46:12 -0500 Subject: [PATCH 13/25] Update CHANGELOG.md --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 35fcd46f6f..da5609f099 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -23,7 +23,7 @@ v4.6 components. The `ForceProducer` API was also rolled out to a variety of existing `Force` components, which means that API users can now now ask many `Force` components what forces they produce (see #3891 for a comprehensive overview). -- Added `MeyerFregly2016Muscle` and completed the implementation of the `MeyerFregly2016Force` included in the `StationPlaneContactForce` class to support NMSM Pipeline-equivalent muscle and contant models in Moco. (#3877) +- Completed the implementation of the `MeyerFregly2016Force` included in the `StationPlaneContactForce` class to support NMSM Pipeline-equivalent contant models in Moco. (#3877) v4.5.1 ====== From e4cc0263e5ca99c6ec60365eca617860f4f72bfc Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Sun, 22 Sep 2024 21:34:26 -0500 Subject: [PATCH 14/25] Fix comments --- CHANGELOG.md | 2 +- OpenSim/Moco/Components/StationPlaneContactForce.h | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index da5609f099..9c0d4c3f29 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -23,7 +23,7 @@ v4.6 components. The `ForceProducer` API was also rolled out to a variety of existing `Force` components, which means that API users can now now ask many `Force` components what forces they produce (see #3891 for a comprehensive overview). -- Completed the implementation of the `MeyerFregly2016Force` included in the `StationPlaneContactForce` class to support NMSM Pipeline-equivalent contant models in Moco. (#3877) +- Completed the implementation of the `MeyerFregly2016Force` included in the `StationPlaneContactForce` class to support NMSM Pipeline-equivalent contact models in Moco. (#3877) v4.5.1 ====== diff --git a/OpenSim/Moco/Components/StationPlaneContactForce.h b/OpenSim/Moco/Components/StationPlaneContactForce.h index 5883bca83b..1f649e311d 100644 --- a/OpenSim/Moco/Components/StationPlaneContactForce.h +++ b/OpenSim/Moco/Components/StationPlaneContactForce.h @@ -150,7 +150,7 @@ B. J. (2016). Muscle Synergies Facilitate Computational Prediction of Subject-Specific Walking Motions. Frontiers in Bioengineering and Biotechnology, 4, 1055–27. http://doi.org/10.3389/fbioe.2016.00077 -Following OpenSim convention, this contact element assumes that the y direction +Following OpenSim convention, this contact element assumes that the y-direction is vertical. Vertical contact force is calculated based on vertical position and velocity relative to a floor at y=0 using these equations: @@ -193,8 +193,7 @@ gradient-based optimizations when an element is inappropriately out of contact. Horizontal forces are then calculated based on this vertical force and the horizontal velocity components of the contact element. Both dynamic (modeled with a tanh function) and viscous (modeled with a linear function) friction -models may be used. - */ +models may be used. */ class OSIMMOCO_API MeyerFregly2016Force : public StationPlaneContactForce { OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, From 5a18a15f27b571a333d835a97323e17aea858ac8 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Sun, 22 Sep 2024 21:45:38 -0500 Subject: [PATCH 15/25] Update contact force documentation --- .../Components/StationPlaneContactForce.h | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/OpenSim/Moco/Components/StationPlaneContactForce.h b/OpenSim/Moco/Components/StationPlaneContactForce.h index 1f649e311d..479939c94f 100644 --- a/OpenSim/Moco/Components/StationPlaneContactForce.h +++ b/OpenSim/Moco/Components/StationPlaneContactForce.h @@ -27,12 +27,18 @@ namespace OpenSim { /** This class models compliant point contact with a ground plane y=0. +Vertical force is calculated using stiffness and dissipation coefficients, +while horizontal force is calculated using a friction model. This class +includes multiple contact models, though currently only MeyerFregly2016Force +has a complete implementation. Details for how this model calculates forces +are included in its documentation below. + @underdevelopment */ class OSIMMOCO_API StationPlaneContactForce : public ForceProducer { OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer); public: OpenSim_DECLARE_OUTPUT(force_on_station, SimTK::Vec3, - calcContactForceOnStation, SimTK::Stage::Velocity); + computeContactForceOnStation, SimTK::Stage::Velocity); OpenSim_DECLARE_SOCKET(station, Station, "The body-fixed point that can contact the plane."); @@ -49,7 +55,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer); const override { OpenSim::Array values; // TODO cache. - const SimTK::Vec3 force = calcContactForceOnStation(s); + const SimTK::Vec3 force = computeContactForceOnStation(s); values.append(force[0]); values.append(force[1]); values.append(force[2]); @@ -59,8 +65,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer); const SimTK::State& s, SimTK::Array_& geoms) const override; - // TODO rename to computeContactForceOnStation - virtual SimTK::Vec3 calcContactForceOnStation( + virtual SimTK::Vec3 computeContactForceOnStation( const SimTK::State& s) const = 0; private: @@ -68,7 +73,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer); const SimTK::State& s, ForceConsumer& forceConsumer) const override { - const SimTK::Vec3 force = calcContactForceOnStation(s); + const SimTK::Vec3 force = computeContactForceOnStation(s); const auto& pt = getConnectee("station"); const auto& pos = pt.getLocationInGround(s); const auto& frame = pt.getParentFrame(); @@ -100,7 +105,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(AckermannVanDenBogert2010Force, /// Compute the force applied to body to which the station is attached, at /// the station, expressed in ground. - SimTK::Vec3 calcContactForceOnStation(const SimTK::State& s) + SimTK::Vec3 computeContactForceOnStation(const SimTK::State& s) const override { SimTK::Vec3 force(0); const auto& pt = getConnectee("station"); @@ -150,7 +155,7 @@ B. J. (2016). Muscle Synergies Facilitate Computational Prediction of Subject-Specific Walking Motions. Frontiers in Bioengineering and Biotechnology, 4, 1055–27. http://doi.org/10.3389/fbioe.2016.00077 -Following OpenSim convention, this contact element assumes that the y-direction +Following OpenSim convention, this contact element assumes that the y-direction is vertical. Vertical contact force is calculated based on vertical position and velocity relative to a floor at y=0 using these equations: @@ -216,7 +221,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, /// Compute the force applied to body to which the station is attached, at /// the station, expressed in ground. - SimTK::Vec3 calcContactForceOnStation(const SimTK::State& s) + SimTK::Vec3 computeContactForceOnStation(const SimTK::State& s) const override { SimTK::Vec3 force(0); const auto& pt = getConnectee("station"); @@ -316,7 +321,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(EspositoMiller2018Force, /// Compute the force applied to body to which the station is attached, at /// the station, expressed in ground. - SimTK::Vec3 calcContactForceOnStation(const SimTK::State& s) + SimTK::Vec3 computeContactForceOnStation(const SimTK::State& s) const override { using SimTK::square; SimTK::Vec3 force(0); From 869ada9b0b932ff2b48a7afd0bc7ba39defa5ad4 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Sun, 22 Sep 2024 21:47:59 -0500 Subject: [PATCH 16/25] Move StationPlaneContactForce --- .../Model}/StationPlaneContactForce.cpp | 0 .../Model}/StationPlaneContactForce.h | 8 ++++---- 2 files changed, 4 insertions(+), 4 deletions(-) rename OpenSim/{Moco/Components => Simulation/Model}/StationPlaneContactForce.cpp (100%) rename OpenSim/{Moco/Components => Simulation/Model}/StationPlaneContactForce.h (98%) diff --git a/OpenSim/Moco/Components/StationPlaneContactForce.cpp b/OpenSim/Simulation/Model/StationPlaneContactForce.cpp similarity index 100% rename from OpenSim/Moco/Components/StationPlaneContactForce.cpp rename to OpenSim/Simulation/Model/StationPlaneContactForce.cpp diff --git a/OpenSim/Moco/Components/StationPlaneContactForce.h b/OpenSim/Simulation/Model/StationPlaneContactForce.h similarity index 98% rename from OpenSim/Moco/Components/StationPlaneContactForce.h rename to OpenSim/Simulation/Model/StationPlaneContactForce.h index 479939c94f..0fb92c4bdc 100644 --- a/OpenSim/Moco/Components/StationPlaneContactForce.h +++ b/OpenSim/Simulation/Model/StationPlaneContactForce.h @@ -34,7 +34,7 @@ has a complete implementation. Details for how this model calculates forces are included in its documentation below. @underdevelopment */ -class OSIMMOCO_API StationPlaneContactForce : public ForceProducer { +class OSIMSIMULATION_API StationPlaneContactForce : public ForceProducer { OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer); public: OpenSim_DECLARE_OUTPUT(force_on_station, SimTK::Vec3, @@ -84,7 +84,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer); }; /** This class is still under development. */ -class OSIMMOCO_API AckermannVanDenBogert2010Force +class OSIMSIMULATION_API AckermannVanDenBogert2010Force : public StationPlaneContactForce { OpenSim_DECLARE_CONCRETE_OBJECT(AckermannVanDenBogert2010Force, StationPlaneContactForce); @@ -199,7 +199,7 @@ Horizontal forces are then calculated based on this vertical force and the horizontal velocity components of the contact element. Both dynamic (modeled with a tanh function) and viscous (modeled with a linear function) friction models may be used. */ -class OSIMMOCO_API MeyerFregly2016Force +class OSIMSIMULATION_API MeyerFregly2016Force : public StationPlaneContactForce { OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, StationPlaneContactForce); @@ -294,7 +294,7 @@ retains a normal metabolic cost in simulated walking after transtibial limb loss. PLoS ONE, 13(1), e0191310. http://doi.org/10.1371/journal.pone.0191310 @underdevelopment */ -class OSIMMOCO_API EspositoMiller2018Force +class OSIMSIMULATION_API EspositoMiller2018Force : public StationPlaneContactForce { OpenSim_DECLARE_CONCRETE_OBJECT(EspositoMiller2018Force, StationPlaneContactForce); From 8251a914da662bf0cfb49c4f76e862eee31773ef Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Sun, 22 Sep 2024 22:22:30 -0500 Subject: [PATCH 17/25] Move AckermannVanDenBogert2010Force --- .../Model/StationPlaneContactForce.h | 130 +++++++++--------- 1 file changed, 64 insertions(+), 66 deletions(-) diff --git a/OpenSim/Simulation/Model/StationPlaneContactForce.h b/OpenSim/Simulation/Model/StationPlaneContactForce.h index 0fb92c4bdc..60754d7c86 100644 --- a/OpenSim/Simulation/Model/StationPlaneContactForce.h +++ b/OpenSim/Simulation/Model/StationPlaneContactForce.h @@ -83,72 +83,6 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer); } }; -/** This class is still under development. */ -class OSIMSIMULATION_API AckermannVanDenBogert2010Force - : public StationPlaneContactForce { -OpenSim_DECLARE_CONCRETE_OBJECT(AckermannVanDenBogert2010Force, - StationPlaneContactForce); -public: - OpenSim_DECLARE_PROPERTY(stiffness, double, - "Spring stiffness in N/m^3 (default: 5e7)."); - OpenSim_DECLARE_PROPERTY(dissipation, double, - "Dissipation coefficient in s/m (default: 1.0)."); - OpenSim_DECLARE_PROPERTY(friction_coefficient, double, - "Friction coefficient"); - // TODO rename to transition_velocity - OpenSim_DECLARE_PROPERTY(tangent_velocity_scaling_factor, double, - "Governs how rapidly friction develops (default: 0.05)."); - - AckermannVanDenBogert2010Force() { - constructProperties(); - } - - /// Compute the force applied to body to which the station is attached, at - /// the station, expressed in ground. - SimTK::Vec3 computeContactForceOnStation(const SimTK::State& s) - const override { - SimTK::Vec3 force(0); - const auto& pt = getConnectee("station"); - const auto& pos = pt.getLocationInGround(s); - const auto& vel = pt.getVelocityInGround(s); - const SimTK::Real y = pos[1]; - const SimTK::Real velNormal = vel[1]; - // TODO should project vel into ground. - const SimTK::Real velSliding = vel[0]; - const SimTK::Real depth = 0 - y; - const SimTK::Real depthRate = 0 - velNormal; - const SimTK::Real& a = get_stiffness(); - const SimTK::Real& b = get_dissipation(); - if (depth > 0) { - force[1] = fmax(0, a * pow(depth, 3) * (1 + b * depthRate)); - } - const SimTK::Real voidStiffness = 1.0; // N/m - force[1] += voidStiffness * depth; - - const SimTK::Real velSlidingScaling = - get_tangent_velocity_scaling_factor(); - // The paper used (1 - exp(-x)) / (1 + exp(-x)) = tanh(2x). - // tanh() has a wider domain than using exp(). - const SimTK::Real transition = tanh(velSliding / velSlidingScaling / 2); - - const SimTK::Real frictionForce = - -transition * get_friction_coefficient() * force[1]; - - force[0] = frictionForce; - return force; - } - -private: - void constructProperties() { - constructProperty_friction_coefficient(1.0); - constructProperty_stiffness(5e7); - constructProperty_dissipation(1.0); - constructProperty_tangent_velocity_scaling_factor(0.05); - } -}; - - - /** This contact model is from the following paper: Meyer A. J., Eskinazi, I., Jackson, J. N., Rao, A. V., Patten, C., & Fregly, B. J. (2016). Muscle Synergies Facilitate Computational Prediction of @@ -280,6 +214,70 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, }; +/** This class is still under development. */ +class OSIMSIMULATION_API AckermannVanDenBogert2010Force + : public StationPlaneContactForce { +OpenSim_DECLARE_CONCRETE_OBJECT(AckermannVanDenBogert2010Force, + StationPlaneContactForce); +public: + OpenSim_DECLARE_PROPERTY(stiffness, double, + "Spring stiffness in N/m^3 (default: 5e7)."); + OpenSim_DECLARE_PROPERTY(dissipation, double, + "Dissipation coefficient in s/m (default: 1.0)."); + OpenSim_DECLARE_PROPERTY(friction_coefficient, double, + "Friction coefficient"); + // TODO rename to transition_velocity + OpenSim_DECLARE_PROPERTY(tangent_velocity_scaling_factor, double, + "Governs how rapidly friction develops (default: 0.05)."); + + AckermannVanDenBogert2010Force() { + constructProperties(); + } + + /// Compute the force applied to body to which the station is attached, at + /// the station, expressed in ground. + SimTK::Vec3 computeContactForceOnStation(const SimTK::State& s) + const override { + SimTK::Vec3 force(0); + const auto& pt = getConnectee("station"); + const auto& pos = pt.getLocationInGround(s); + const auto& vel = pt.getVelocityInGround(s); + const SimTK::Real y = pos[1]; + const SimTK::Real velNormal = vel[1]; + // TODO should project vel into ground. + const SimTK::Real velSliding = vel[0]; + const SimTK::Real depth = 0 - y; + const SimTK::Real depthRate = 0 - velNormal; + const SimTK::Real& a = get_stiffness(); + const SimTK::Real& b = get_dissipation(); + if (depth > 0) { + force[1] = fmax(0, a * pow(depth, 3) * (1 + b * depthRate)); + } + const SimTK::Real voidStiffness = 1.0; // N/m + force[1] += voidStiffness * depth; + + const SimTK::Real velSlidingScaling = + get_tangent_velocity_scaling_factor(); + // The paper used (1 - exp(-x)) / (1 + exp(-x)) = tanh(2x). + // tanh() has a wider domain than using exp(). + const SimTK::Real transition = tanh(velSliding / velSlidingScaling / 2); + + const SimTK::Real frictionForce = + -transition * get_friction_coefficient() * force[1]; + + force[0] = frictionForce; + return force; + } + +private: + void constructProperties() { + constructProperty_friction_coefficient(1.0); + constructProperty_stiffness(5e7); + constructProperty_dissipation(1.0); + constructProperty_tangent_velocity_scaling_factor(0.05); + } +}; + /** This contact model uses a continuous equation to transition between in and out of contact. The equation for the smooth transition was published in the following two papers: From 7c80a3f3d4c19731b04f8626f8fde9a63b484e49 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Wed, 25 Sep 2024 22:07:27 -0500 Subject: [PATCH 18/25] Update class documentation --- OpenSim/Simulation/Model/StationPlaneContactForce.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/OpenSim/Simulation/Model/StationPlaneContactForce.h b/OpenSim/Simulation/Model/StationPlaneContactForce.h index 60754d7c86..1c54b88d58 100644 --- a/OpenSim/Simulation/Model/StationPlaneContactForce.h +++ b/OpenSim/Simulation/Model/StationPlaneContactForce.h @@ -28,10 +28,9 @@ namespace OpenSim { /** This class models compliant point contact with a ground plane y=0. Vertical force is calculated using stiffness and dissipation coefficients, -while horizontal force is calculated using a friction model. This class -includes multiple contact models, though currently only MeyerFregly2016Force -has a complete implementation. Details for how this model calculates forces -are included in its documentation below. +while horizontal force is calculated using a friction model. Multiple concrete +implementation of this class are available, but currently MeyerFregly2016Force +is the only complete and well-tested implementation and therefore recommended. @underdevelopment */ class OSIMSIMULATION_API StationPlaneContactForce : public ForceProducer { From 72d39e09175971a1b2c4c8de72577f3d98164c8e Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Wed, 25 Sep 2024 22:08:02 -0500 Subject: [PATCH 19/25] Enable MeyerFregly2016Force in template --- OpenSim/Moco/Test/testMocoContact.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Moco/Test/testMocoContact.cpp b/OpenSim/Moco/Test/testMocoContact.cpp index f694da1601..e1c5d0f5bc 100644 --- a/OpenSim/Moco/Test/testMocoContact.cpp +++ b/OpenSim/Moco/Test/testMocoContact.cpp @@ -514,8 +514,8 @@ void testSmoothSphereHalfSpaceForce_FrictionForce( } TEMPLATE_TEST_CASE("testStationPlaneContactForce", "[tropter]", - AckermannVanDenBogert2010Force, EspositoMiller2018Force - /* TODO MeyerFregly2016Force */) { + /*AckermannVanDenBogert2010Force, EspositoMiller2018Force,*/ + MeyerFregly2016Force) { testStationPlaneContactForce(); } From 8dcb99967684a0411dbcbef379703aea3437dfe4 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Wed, 25 Sep 2024 22:31:15 -0500 Subject: [PATCH 20/25] Add spring resting length --- OpenSim/Simulation/Model/StationPlaneContactForce.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/OpenSim/Simulation/Model/StationPlaneContactForce.h b/OpenSim/Simulation/Model/StationPlaneContactForce.h index 1c54b88d58..4434de37ee 100644 --- a/OpenSim/Simulation/Model/StationPlaneContactForce.h +++ b/OpenSim/Simulation/Model/StationPlaneContactForce.h @@ -141,6 +141,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, "Spring stiffness in N/m (default: 1e4)."); OpenSim_DECLARE_PROPERTY(dissipation, double, "Dissipation coefficient in s/m (default: 0.01)."); + OpenSim_DECLARE_PROPERTY(spring_resting_length, double, + "Spring resting length in m (default: 0)."); OpenSim_DECLARE_PROPERTY(dynamic_friction, double, "Dynamic friction coefficient (default: 0)."); OpenSim_DECLARE_PROPERTY(viscous_friction, double, @@ -160,6 +162,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, const auto& pt = getConnectee("station"); const auto& pos = pt.getLocationInGround(s); const auto& vel = pt.getVelocityInGround(s); + const SimTK::Real resting_length = get_spring_resting_length(); const SimTK::Real y = pos[1]; const SimTK::Real velNormal = vel[1]; SimTK::Real velSliding = sqrt(vel[0] * vel[0] + vel[2] * vel[2]); @@ -174,6 +177,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, const SimTK::Real slipOffset = 1e-4; /// Normal force. + y = y - resting_length; const SimTK::Real vp = (Kval + klow) / (Kval - klow); const SimTK::Real sp = (Kval - klow) / 2; From cd46b4d75117b28de7ecee1e470e127448bec498 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Wed, 25 Sep 2024 22:43:21 -0500 Subject: [PATCH 21/25] Resting length in properties --- OpenSim/Simulation/Model/StationPlaneContactForce.h | 1 + 1 file changed, 1 insertion(+) diff --git a/OpenSim/Simulation/Model/StationPlaneContactForce.h b/OpenSim/Simulation/Model/StationPlaneContactForce.h index 4434de37ee..ccc214c58e 100644 --- a/OpenSim/Simulation/Model/StationPlaneContactForce.h +++ b/OpenSim/Simulation/Model/StationPlaneContactForce.h @@ -210,6 +210,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force, void constructProperties() { constructProperty_stiffness(1e4); constructProperty_dissipation(1e-2); + constructProperty_spring_resting_length(0); constructProperty_dynamic_friction(0); constructProperty_viscous_friction(5); constructProperty_latch_velocity(0.05); From a07c257078776843a4bea60ab69db94b9cc3c1da Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Wed, 25 Sep 2024 22:43:29 -0500 Subject: [PATCH 22/25] Known kinematics test --- OpenSim/Moco/Test/testMocoContact.cpp | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/OpenSim/Moco/Test/testMocoContact.cpp b/OpenSim/Moco/Test/testMocoContact.cpp index e1c5d0f5bc..cf214979dc 100644 --- a/OpenSim/Moco/Test/testMocoContact.cpp +++ b/OpenSim/Moco/Test/testMocoContact.cpp @@ -255,6 +255,30 @@ void testFrictionForce(const SimTK::Real& equilibriumHeight) { } } +// Test that the contact model produces the expected force output for a given +// set of input kinematics and default parameters. +template +void testKnownKinematics() { + Model modelTemp = create2DPointMassModel(); + modelTemp.finalizeConnections(); + Model model(modelTemp); + model.finalizeConnections(); + + SimTK::State state = model.initSystem(); + model.setStateVariableValue(state, "ty/ty/value", -0.005); + model.setStateVariableValue(state, "ty/ty/speed", -0.01); + model.setStateVariableValue(state, "tx/tx/speed", 0.03); + model.setStateVariableValue(state, "tz/tz/speed", 0.02); + + auto& contact = model.template getComponent("contact"); + model.realizeVelocity(state); + const Vec3 contactForce = contact.calcContactForceOnStation(state); + + CHECK(contactForce[0] == Approx(-5.9842).margin(1e-3)); + CHECK(contactForce[1] == Approx(40.0051).margin(1e-3)); + CHECK(contactForce[2] == Approx(-3.9894).margin(1e-3)); +} + template void testStationPlaneContactForce() { const SimTK::Real equilibriumHeight = testNormalForce(); From 5dd1ae5eaa919755bc4a008a1ad0d8d7a729b22a Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Thu, 26 Sep 2024 19:34:14 -0500 Subject: [PATCH 23/25] Fix typo --- OpenSim/Simulation/Model/StationPlaneContactForce.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Simulation/Model/StationPlaneContactForce.h b/OpenSim/Simulation/Model/StationPlaneContactForce.h index ccc214c58e..fcf3499b60 100644 --- a/OpenSim/Simulation/Model/StationPlaneContactForce.h +++ b/OpenSim/Simulation/Model/StationPlaneContactForce.h @@ -29,7 +29,7 @@ namespace OpenSim { Vertical force is calculated using stiffness and dissipation coefficients, while horizontal force is calculated using a friction model. Multiple concrete -implementation of this class are available, but currently MeyerFregly2016Force +implementations of this class are available, but currently MeyerFregly2016Force is the only complete and well-tested implementation and therefore recommended. @underdevelopment */ From 09e11f97e0a1653fe6e067d4c18d3ea72f256472 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Thu, 26 Sep 2024 19:41:36 -0500 Subject: [PATCH 24/25] Add kinematics test case for MeyerFregly2016Force --- OpenSim/Moco/Test/testMocoContact.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/OpenSim/Moco/Test/testMocoContact.cpp b/OpenSim/Moco/Test/testMocoContact.cpp index cf214979dc..168ba56fd2 100644 --- a/OpenSim/Moco/Test/testMocoContact.cpp +++ b/OpenSim/Moco/Test/testMocoContact.cpp @@ -257,9 +257,8 @@ void testFrictionForce(const SimTK::Real& equilibriumHeight) { // Test that the contact model produces the expected force output for a given // set of input kinematics and default parameters. -template void testKnownKinematics() { - Model modelTemp = create2DPointMassModel(); + Model modelTemp = create2DPointMassModel(); modelTemp.finalizeConnections(); Model model(modelTemp); model.finalizeConnections(); @@ -538,7 +537,7 @@ void testSmoothSphereHalfSpaceForce_FrictionForce( } TEMPLATE_TEST_CASE("testStationPlaneContactForce", "[tropter]", - /*AckermannVanDenBogert2010Force, EspositoMiller2018Force,*/ + AckermannVanDenBogert2010Force, EspositoMiller2018Force, MeyerFregly2016Force) { testStationPlaneContactForce(); } @@ -640,3 +639,7 @@ TEST_CASE("MocoContactTrackingGoal", "[casadi]") { externalLoadsTimeStepping, "ground_force_r_vy", 0.5); } + +TEST_CASE("testMeyerFregly2016ForceValues", "[casadi]") { + testKnownKinematics(); +} From f30a8c9569fd6c8d667374c880fccb04aca48781 Mon Sep 17 00:00:00 2001 From: stwilliams333 Date: Wed, 9 Oct 2024 16:25:41 -0500 Subject: [PATCH 25/25] Update CMakeLists.txt --- OpenSim/Moco/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/OpenSim/Moco/CMakeLists.txt b/OpenSim/Moco/CMakeLists.txt index f618f15df8..fc0a8afd2f 100644 --- a/OpenSim/Moco/CMakeLists.txt +++ b/OpenSim/Moco/CMakeLists.txt @@ -86,8 +86,6 @@ set(MOCO_SOURCES MocoFrameDistanceConstraint.cpp MocoOutputConstraint.h MocoOutputConstraint.cpp - Components/StationPlaneContactForce.h - Components/StationPlaneContactForce.cpp Components/DiscreteForces.h Components/DiscreteForces.cpp Components/AccelerationMotion.h