<NME> Thelen2003Muscle.cpp
<BEF> /* -------------------------------------------------------------------------- *
 *                       OpenSim:  Thelen2003Muscle.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-2017 Stanford University and the Authors                *
 * Author(s): Matthew Millard                                                 *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */
//=============================================================================
// INCLUDES
//=============================================================================
#include <fstream>
#include <OpenSim/Simulation/Model/Model.h>
#include "Thelen2003Muscle.h"

//=============================================================================
// STATICS
//=============================================================================
using namespace std;
using namespace OpenSim;
using namespace SimTK;

//=============================================================================
// CONSTRUCTORS
//=============================================================================
// Uses default (compiler-generated) destructor, copy constructor, copy 
// assignment operator.


//_____________________________________________________________________________
/**
 * Default constructor.
 */
Thelen2003Muscle::Thelen2003Muscle()          
{    
    setNull();
    constructProperties();
}

//_____________________________________________________________________________
/**
 * Constructor.
 */
Thelen2003Muscle::
Thelen2003Muscle(const std::string& aName,  double aMaxIsometricForce,
                  double aOptimalFiberLength,double aTendonSlackLength,
                  double aPennationAngle)
{
    setNull();
    constructProperties();
    setName(aName);
    
    setMaxIsometricForce(aMaxIsometricForce);
    setOptimalFiberLength(aOptimalFiberLength);
    setTendonSlackLength(aTendonSlackLength);
    setPennationAngleAtOptimalFiberLength(aPennationAngle);
}

//==============================================================================
// Component Interface
//==============================================================================
void Thelen2003Muscle::extendFinalizeFromProperties()
{
    Super::extendFinalizeFromProperties();

    SimTK_ERRCHK1_ALWAYS(get_FmaxTendonStrain() > 0,
        "Thelen2003Muscle::extendFinalizeFromProperties",
        "%s: FmaxTendonStrain must be greater than zero", getName().c_str());
    SimTK_ERRCHK1_ALWAYS(get_FmaxMuscleStrain() > 0,
        "Thelen2003Muscle::extendFinalizeFromProperties",
        "%s: FmaxMuscleStrain must be greater than zero", getName().c_str());
    SimTK_ERRCHK1_ALWAYS(get_KshapeActive() > 0,
        "Thelen2003Muscle::extendFinalizeFromProperties",
        "%s: KshapeActive must be greater than zero", getName().c_str());
    SimTK_ERRCHK1_ALWAYS(get_KshapePassive() > 0,
        "Thelen2003Muscle::extendFinalizeFromProperties",
        "%s: KshapePassive must be greater than zero", getName().c_str());
    SimTK_ERRCHK1_ALWAYS(get_Af() > 0,
        "Thelen2003Muscle::extendFinalizeFromProperties",
        "%s: Af must be greater than zero", getName().c_str());
    SimTK_ERRCHK1_ALWAYS(get_Flen() > 1.0,
        "Thelen2003Muscle::extendFinalizeFromProperties",
        "%s: Flen must be greater than 1.0", getName().c_str());
    SimTK_ERRCHK1_ALWAYS(get_fv_linear_extrap_threshold() > 1.0/get_Flen(),
        "Thelen2003::extendFinalizeFromProperties",
        "%s: F-v extrapolation threshold must be greater than 1.0/Flen",
        getName().c_str());


    OPENSIM_THROW_IF_FRMOBJ(get_minimum_activation() < 0.01,
        InvalidPropertyValue, getProperty_minimum_activation().getName(),
        "Minimum activation cannot be less than 0.01");

    OPENSIM_THROW_IF_FRMOBJ(getMinControl() < get_minimum_activation(),
        InvalidPropertyValue, getProperty_min_control().getName(),
        "Minimum control cannot be less than minimum activation");

    // Propagate properties down to pennation model subcomponent. If any of the
    // new property values are invalid, restore the subcomponent's current
    // property values (to avoid throwing again when the subcomponent's
    // extendFinalizeFromProperties() method is called directly) and then
    // re-throw the exception thrown by the subcomponent.
    auto& pennMdl =
        updMemberSubcomponent<MuscleFixedWidthPennationModel>(pennMdlIdx);
    MuscleFixedWidthPennationModel pennMdlCopy(pennMdl);
    pennMdl.set_optimal_fiber_length(getOptimalFiberLength());
    pennMdl.set_pennation_angle_at_optimal(
        getPennationAngleAtOptimalFiberLength());
    pennMdl.set_maximum_pennation_angle(get_maximum_pennation_angle());
    try {
        pennMdl.finalizeFromProperties();
    } catch (const InvalidPropertyValue&) {
        pennMdl = pennMdlCopy;
        throw;
    }

    // Propagate properties down to activation dynamics model subcomponent.
    // Handle invalid properties as above for pennation model.
    auto& actMdl =
        updMemberSubcomponent<MuscleFirstOrderActivationDynamicModel>(actMdlIdx);
    MuscleFirstOrderActivationDynamicModel actMdlCopy(actMdl);
    actMdl.set_activation_time_constant(get_activation_time_constant());
    actMdl.set_deactivation_time_constant(get_deactivation_time_constant());
    actMdl.set_minimum_activation(get_minimum_activation());
    try {
        actMdl.finalizeFromProperties();
    } catch (const InvalidPropertyValue&) {
        actMdl = actMdlCopy;
        throw;
    }
}

//====================================================================
// Model Component Interface
//====================================================================
void Thelen2003Muscle::extendConnectToModel(Model& aModel)
{
    Super::extendConnectToModel(aModel);
}

    constructProperty_MuscleFixedWidthPennationModel(
        MuscleFixedWidthPennationModel());
    setMaximumPennationAngle(acos(0.1));

    constructProperty_FmaxTendonStrain(0.04); // was 0.033
}

//_____________________________________________________________________________
// Set the data members of this muscle to their null values.
void Thelen2003Muscle::setNull()
{
    // no data members
    setAuthors("Matthew Millard");
}

//_____________________________________________________________________________
/**
 * Populate this object's properties
 */
void Thelen2003Muscle::constructProperties()
{
    constructProperty_FmaxTendonStrain(0.04); // was 0.033
    constructProperty_FmaxMuscleStrain(0.6);
    constructProperty_KshapeActive(0.45);   
    constructProperty_KshapePassive(5.0);   
    constructProperty_Af(0.25); 
    constructProperty_Flen(1.4);    //was 1.8, 
    constructProperty_fv_linear_extrap_threshold(0.95);
    //acos(0.1) = 84.26 degrees
    constructProperty_maximum_pennation_angle(acos(0.1));
    constructProperty_activation_time_constant(0.015);
    constructProperty_deactivation_time_constant(0.050);
    constructProperty_minimum_activation(0.01);

    setMinControl(get_minimum_activation());
}

//=============================================================================
// GET
//=============================================================================
double Thelen2003Muscle::getActivationTimeConstant() const
{   return get_activation_time_constant(); }

double Thelen2003Muscle::getDeactivationTimeConstant() const
{   return get_deactivation_time_constant(); }

double Thelen2003Muscle::getMinimumActivation() const
{   return get_minimum_activation(); }

const MuscleFirstOrderActivationDynamicModel& Thelen2003Muscle::
getActivationModel() const
{
    return getMemberSubcomponent<MuscleFirstOrderActivationDynamicModel>(
           actMdlIdx);
}

const MuscleFixedWidthPennationModel& Thelen2003Muscle::
getPennationModel() const
{   return getMemberSubcomponent<MuscleFixedWidthPennationModel>(pennMdlIdx); }

double Thelen2003Muscle::getMaximumPennationAngle() const
{   return get_maximum_pennation_angle(); }

//=============================================================================
// SET
//=============================================================================
void Thelen2003Muscle::setActivationTimeConstant(double actTimeConstant)
{   set_activation_time_constant(actTimeConstant); }

void Thelen2003Muscle::setDeactivationTimeConstant(double deactTimeConstant)
{   set_deactivation_time_constant(deactTimeConstant); }

void Thelen2003Muscle::setMinimumActivation(double minimumActivation)
{   set_minimum_activation(minimumActivation); }

void Thelen2003Muscle::setMaximumPennationAngle(double maximumPennationAngle)
{   set_maximum_pennation_angle(maximumPennationAngle); }

//==============================================================================
//                             START OF DEPRECATED
//==============================================================================
double Thelen2003Muscle::
calcInextensibleTendonActiveFiberForce(SimTK::State& s, 
                                       double aActivation) const
{      
    double inextensibleTendonActiveFiberForce = 0;

    double muscleLength = getLength(s);
    double muscleVelocity = getLengtheningSpeed(s);
    double tendonSlackLength = getTendonSlackLength();
    double tendonVelocity = 0.0; //Inextensible tendon;

    double fiberLength  = getPennationModel().calcFiberLength(muscleLength,
                                            tendonSlackLength);
        
    if(fiberLength > getPennationModel().getMinimumFiberLength()) {
        double phi = getPennationModel().calcPennationAngle(fiberLength);
        
        double fiberVelocity = getPennationModel().calcFiberVelocity(
                                    cos(phi),muscleVelocity,tendonVelocity);

        inextensibleTendonActiveFiberForce = 
            calcActiveFiberForceAlongTendon(    aActivation,
                                                fiberLength,
                                                fiberVelocity);
    }

    return inextensibleTendonActiveFiberForce;
}

double Thelen2003Muscle::
            calcActiveFiberForceAlongTendon(double activation, 
                                            double fiberLength, 
                                            double fiberVelocity) const
{
    double activeFiberForce = 0;    
    double clampedFiberLength = getPennationModel()
                                .clampFiberLength(fiberLength);

    //If the fiber is in a legal range, compute the force its generating
    if(fiberLength > getPennationModel().getMinimumFiberLength())
    {
        //Clamp activation to a legal range
        double clampedActivation = getActivationModel()
                                   .clampActivation(activation);

        //Normalize fiber length and velocity
        double normFiberLength    = clampedFiberLength/getOptimalFiberLength();
        double normFiberVelocity  = fiberVelocity / 
                        (getOptimalFiberLength() * getMaxContractionVelocity());


        //Evaluate the force active length and force velocity multipliers
        double fal  = calcfal(normFiberLength);

        double fv = SimTK::NaN;
        double fvTol = 1e-6;
        int fvMaxIter= 100;        
        fv = calcfvInv( clampedActivation,
                                            fal,
                                            normFiberVelocity,
                                            fvTol,
                                            fvMaxIter);

        double fiso = getMaxIsometricForce();

        //Evaluate the pennation angle
        double phi = getPennationModel().calcPennationAngle(fiberLength);

        //Compute the active fiber force 
        activeFiberForce = fiso * clampedActivation * fal * fv * cos(phi);
    }
    //Compute the active fiber force
    

    return activeFiberForce;
}

//==============================================================================
//                              END OF DEPRECATED
//==============================================================================

//==============================================================================
// Muscle.h Interface
//==============================================================================

double  Thelen2003Muscle::computeActuation(const SimTK::State& s) const
{
    const MuscleDynamicsInfo& mdi = getMuscleDynamicsInfo(s);
    setActuation(s, mdi.tendonForce);
    return( mdi.tendonForce );
}

void Thelen2003Muscle::computeInitialFiberEquilibrium(SimTK::State& s) const
{
    //Initial activation and fiber length from input State, s.
    _model->getMultibodySystem().realize(s, SimTK::Stage::Velocity);
    double activation = getActivation(s);

    //Tolerance, in Newtons, of the desired equilibrium
    const double tol = max( 1e-8*getMaxIsometricForce(), 
                            SimTK::SignificantReal * 10 );

    int maxIter = 20;  //Should this be user settable?

    std::pair<StatusFromInitMuscleState, ValuesFromInitMuscleState> result;

    try {
         result = initMuscleState(s, activation, tol, maxIter);
    }
    catch (const std::exception& x) {
        OPENSIM_THROW_FRMOBJ(MuscleCannotEquilibrate, x.what());
    }

    switch(result.first) {

    case StatusFromInitMuscleState::Success_Converged:
        setActuation(s, result.second["tendon_force"]);
        setFiberLength(s, result.second["fiber_length"]);
        break;

    case StatusFromInitMuscleState::Warning_FiberAtLowerBound:
        log_warn("Thelen2003Muscle initialization: '{}' is at its minimum "
                 "fiber length of {}.", getName(),
                result.second["fiber_length"]);
        setActuation(s, result.second["tendon_force"]);
        setFiberLength(s, result.second["fiber_length"]);
        break;

    case StatusFromInitMuscleState::Failure_MaxIterationsReached:
        // Report internal variables and throw exception.
        std::ostringstream ss;
        ss << "\n  Solution error " << abs(result.second["solution_error"])
           << " exceeds tolerance of " << tol << "\n"
           << "  Newton iterations reached limit of " << maxIter << "\n"
           << "  Activation is " << activation << "\n"
           << "  Fiber length is " << result.second["fiber_length"] << "\n";
        OPENSIM_THROW_FRMOBJ(MuscleCannotEquilibrate, ss.str());
        break;
    }
}

void Thelen2003Muscle::calcMuscleLengthInfo(const SimTK::State& s,
                                            MuscleLengthInfo& mli) const
{    
    try{
        double optFiberLength   = getOptimalFiberLength();
        double mclLength        = getLength(s);
        double tendonSlackLen   = getTendonSlackLength();

        //Clamp the minimum fiber length to its minimum physical value.
        mli.fiberLength  = getPennationModel().clampFiberLength(
                                getStateVariableValue(s, STATE_FIBER_LENGTH_PATH));

        mli.normFiberLength = mli.fiberLength/optFiberLength;       
        mli.pennationAngle  = getPennationModel()
                              .calcPennationAngle(mli.fiberLength);

        mli.cosPennationAngle = cos(mli.pennationAngle);
        mli.sinPennationAngle = sin(mli.pennationAngle);

        mli.fiberLengthAlongTendon = mli.fiberLength*mli.cosPennationAngle;
    
        mli.tendonLength      = getPennationModel().calcTendonLength(
                                    mli.cosPennationAngle,
                                    mli.fiberLength,mclLength );
        mli.normTendonLength  = mli.tendonLength / tendonSlackLen;
        mli.tendonStrain      = mli.normTendonLength -  1.0;
        

    
        mli.fiberPassiveForceLengthMultiplier= calcfpe(mli.normFiberLength);
        mli.fiberActiveForceLengthMultiplier = calcfal(mli.normFiberLength);
    }catch(const std::exception &x){
        std::string msg = "Exception caught in Thelen2003Muscle::" 
                          "calcMuscleLengthInfo\n"                 
                           "of " + getName()  + "\n"                            
                           + x.what();
        throw OpenSim::Exception(msg);
    }
}

void Thelen2003Muscle::calcMusclePotentialEnergyInfo(const SimTK::State& s,
        MusclePotentialEnergyInfo& mpei) const
{
    try {
        // Get the quantities that we've already computed.
        const MuscleLengthInfo &mli = getMuscleLengthInfo(s);
        mpei.fiberPotentialEnergy = calcfpefisoPE(mli.fiberLength);
        mpei.tendonPotentialEnergy= calcfsefisoPE(mli.tendonStrain);
        mpei.musclePotentialEnergy=  mpei.fiberPotentialEnergy 
                                    + mpei.tendonPotentialEnergy;
    }
    catch(const std::exception &x){
        std::string msg = "Exception caught in Thelen2003Muscle::" 
                          "calcMusclePotentialEnergyInfo\n"                 
                           "of " + getName()  + "\n"                            
                           + x.what();
        throw OpenSim::Exception(msg);
    }
}


void Thelen2003Muscle::calcFiberVelocityInfo(const SimTK::State& s, 
                                               FiberVelocityInfo& fvi) const
{
    try{
        //Get the quantities that we've already computed
            const MuscleLengthInfo &mli = getMuscleLengthInfo(s);

        //Get the static properties of this muscle
            // double mclLength      = getLength(s);
            double tendonSlackLen = getTendonSlackLength();
            double optFiberLen    = getOptimalFiberLength();
        //=========================================================================
        // Compute fv by inverting the force-velocity relationship in the 
        // equilibrium equations
        //=========================================================================

        //1. Get fiber/tendon kinematic information

        //clamp activation to a legal range
        double a = getActivationModel().clampActivation(getStateVariableValue(s,
                                          STATE_ACTIVATION_PATH));
   

        double lce  = mli.fiberLength;   
        double phi  = mli.pennationAngle;
        double cosphi=mli.cosPennationAngle;
        double sinphi = mli.sinPennationAngle;

        //2. compute the tendon length ... because we can with this kind of model
        double tl = mli.tendonLength;

        
        //3. Compute force multipliers and the fiber velocity
    
            //This exception was causing headaches, so we're handling the special
            //case of a fiber that is at its minimum length instead
                //SimTK_ERRCHK1_ALWAYS(cosphi > SimTK::Eps, fcnName.c_str(),
                //    "%s: Pennation angle is 90 degrees, and is causing a singularity", 
                //    muscleName.c_str());
    

            //6. Invert the force velocity curve to get dlce. Check for singularities 
            //   first

            //These exceptions were causing headaches, so we're handling the special
            //case of a fiber that is at its minimum length instead
                //SimTK_ERRCHK1_ALWAYS(a > SimTK::Eps, fcnName.c_str(),
                //    "%s: Activation is 0, and is causing a singularity", 
                //    muscleName.c_str());
                //SimTK_ERRCHK1_ALWAYS(fal > SimTK::Eps, fcnName.c_str(),
                //    "%s: The active force length curve value is 0,"
                //    " and is causing a singularity", 
                //    muscleName.c_str());
    
            //Check for singularity conditions, and clamp output appropriately

        double dmcldt = getLengtheningSpeed(s);

        //default values that are appropriate when fiber length has been clamped
        //to its minimum allowable value.

        double fse  = calcfse(tl/tendonSlackLen);    
        double fal  = mli.fiberActiveForceLengthMultiplier;
        double fpe  = mli.fiberPassiveForceLengthMultiplier;

        double afalfv = ((fse/cosphi)-fpe); //we can do this without fear of
                                              //a singularity because fiber length
                                              //is clamped
        double fv     = afalfv/(a*fal);
        double dlceN  = calcdlceN(a,fal,afalfv);
        double dlce   = dlceN*getMaxContractionVelocity()*optFiberLen;
        double tanPhi = tan(phi);
        double dphidt = getPennationModel().calcPennationAngularVelocity(
                                            tanPhi,lce,dlce);
        double dlceAT = getPennationModel().calcFiberVelocityAlongTendon(
                            lce, dlce, sinphi, cosphi, dphidt);
        double dtl    = getPennationModel().calcTendonVelocity(
                            cosphi, sinphi, dphidt, lce, dlce, dmcldt);
    
    
        //Switching condition: if the fiber is clamped and the tendon and the 
        //                   : fiber are out of equilibrium
        double fiberStateClamped = 0.0;
        if(isFiberStateClamped(s,dlceN)){
             dlce = 0;
             dlceAT = 0;
             dlceN = 0;
             dphidt = 0;
             dtl = dmcldt;
             fv = 1.0;
             fiberStateClamped = 1.0;
        }

        //Populate the struct;
        fvi.fiberVelocity               = dlce;
        fvi.fiberVelocityAlongTendon    = dlceAT;
        fvi.normFiberVelocity           = dlceN;

        fvi.pennationAngularVelocity    = dphidt;

        fvi.tendonVelocity              = dtl;
        fvi.normTendonVelocity          = dtl/getTendonSlackLength();

        fvi.fiberForceVelocityMultiplier = fv;

        fvi.userDefinedVelocityExtras.resize(2);
        fvi.userDefinedVelocityExtras[0]=fse;
        fvi.userDefinedVelocityExtras[1]=fiberStateClamped;
    }
    catch(const std::exception &x){
        std::string msg = "Exception caught in Thelen2003Muscle::" 
                            "calcFiberVelocityInfo\n"                 
                            "of " + getName()  + "\n"                            
                            + x.what();
        throw OpenSim::Exception(msg);
    }
}


//=======================================
// computeFiberVelocityInfo helper functions
//=======================================

void Thelen2003Muscle::calcMuscleDynamicsInfo(const SimTK::State& s, 
                                               MuscleDynamicsInfo& mdi) const
{
    try {
        //Get the quantities that we've already computed
        const MuscleLengthInfo &mli = getMuscleLengthInfo(s);
        const FiberVelocityInfo &mvi = getFiberVelocityInfo(s);
        //Get the static properties of this muscle
        // double mclLength      = getLength(s);
        double tendonSlackLen = getTendonSlackLength();
        double optFiberLen    = getOptimalFiberLength();
        double fiso           = getMaxIsometricForce();
        double penHeight      = getPennationModel().getParallelogramHeight();

        //=========================================================================
        // Compute required quantities
        //=========================================================================
        //1. Get fiber/tendon kinematic information
        double a = getActivationModel().clampActivation(
                       getStateVariableValue(s, STATE_ACTIVATION_PATH) );

        double lce      = mli.fiberLength;
        double fiberStateClamped = mvi.userDefinedVelocityExtras[1];
        double dlce     = mvi.fiberVelocity;
        double phi      = mli.pennationAngle;
        double cosphi   = mli.cosPennationAngle;
        // double sinphi   = mli.sinPennationAngle;

        double tl   = mli.tendonLength; 
        double dtl  = mvi.tendonVelocity;
        // double tlN  = mli.normTendonLength;

        //Default values appropriate when the fiber is clamped to its minimum length
        //and is generating no force

        //These quantities should already be set to legal values from
        //calcFiberVelocityInfo
        double fal  = mli.fiberActiveForceLengthMultiplier;
        double fpe  = mli.fiberPassiveForceLengthMultiplier;
        double fv   = mvi.fiberForceVelocityMultiplier;
        double fse  = mvi.userDefinedVelocityExtras[0];
    
        double aFm          = 0; //active fiber force
        double Fm           = 0;
        double dFm_dlce     = 0;
        double dFmAT_dlce   = 0;
        double dFmAT_dlceAT = 0;    
        double dFt_dtl      = 0; 
        double Ke           = 0;

        if(fiberStateClamped < 0.5){        
            aFm          = calcActiveFm(a,fal,fv,fiso);
            Fm           = calcFm(a,fal,fv,fpe,fiso);
            dFm_dlce     = calcDFmDlce(lce,a,fv,fiso,optFiberLen);
            dFmAT_dlce   = calcDFmATDlce(lce,phi,cosphi,Fm,dFm_dlce,penHeight);

            //The expression below is correct only because we are using a pennation
            //model that has a parallelogram of constant height.
            dFmAT_dlceAT= dFmAT_dlce*cosphi;

            dFt_dtl = calcDFseDtl(tl, fiso, tendonSlackLen);

            //Compute the stiffness of the whole muscle/tendon complex
            Ke = (dFmAT_dlceAT*dFt_dtl)/(dFmAT_dlceAT+dFt_dtl);
        }
    
        mdi.activation                   = a;
        mdi.fiberForce                   = Fm; 
        mdi.fiberForceAlongTendon        = Fm*cosphi;
        mdi.normFiberForce               = Fm/fiso;
        mdi.activeFiberForce             = aFm;
        mdi.passiveFiberForce            = fpe*fiso;
                                     
        mdi.tendonForce                  = fse*fiso;
        mdi.normTendonForce              = fse;
                                     
        mdi.fiberStiffness               = dFm_dlce;
        mdi.fiberStiffnessAlongTendon    = dFmAT_dlceAT;
        mdi.tendonStiffness              = dFt_dtl;
        mdi.muscleStiffness              = Ke;
                                     
    

        //Check that the derivative of system energy less work is zero within
        //a reasonable numerical tolerance. Throw an exception if this is not true    
        double dFibPEdt     = fpe*fiso*dlce;
        double dTdnPEdt     = fse*fiso*dtl;
        double dFibWdt      = -mdi.activeFiberForce*mvi.fiberVelocity;
        double dmcldt       = getLengtheningSpeed(s);
        double dBoundaryWdt = mdi.tendonForce * dmcldt;
        double ddt_KEPEmW   = dFibPEdt+dTdnPEdt-dFibWdt-dBoundaryWdt;
        SimTK::Vector userVec(1);
        userVec(0) = ddt_KEPEmW;  
        mdi.userDefinedDynamicsExtras = userVec;

        /////////////////////////////
        //Populate the power entries
        /////////////////////////////
        mdi.fiberActivePower             = dFibWdt;
        mdi.fiberPassivePower            = -dFibPEdt;
        mdi.tendonPower                  = -dTdnPEdt;       
        mdi.musclePower                  = -dBoundaryWdt;

    }
    catch(const std::exception &x) {
        std::string msg = "Exception caught in Thelen2003Muscle::" 
                            "calcMuscleDynamicsInfo\n"                 
                            "of " + getName()  + "\n"                            
                            + x.what();
        throw OpenSim::Exception(msg);
    }
   
}

double Thelen2003Muscle::getMinimumFiberLength() const
{
    return getPennationModel().getMinimumFiberLength();
}


bool Thelen2003Muscle::
    isFiberStateClamped(const SimTK::State& s, double dlceN) const
{
    bool clamped = false;

    //Is the fiber length  clamped and it is shortening, then the fiber length
    //not valid
    if( (getStateVariableValue(s, STATE_FIBER_LENGTH_PATH)
            <= getMinimumFiberLength())
        && dlceN <= 0){
        clamped = true;
    }

    return clamped;     
}

//==============================================================================
// ActivationFiberLength.h Interface
//==============================================================================


/** Get the rate change of activation */
double Thelen2003Muscle::calcActivationRate(const SimTK::State& s) const 
{    
    double excitation = getExcitation(s);
    double activation = getActivation(s);
    double dadt = getActivationModel().calcDerivative(activation,excitation);
    return dadt;
}  


//==============================================================================
// Numerical Guts: Initialization
//==============================================================================
std::pair<Thelen2003Muscle::StatusFromInitMuscleState,
          Thelen2003Muscle::ValuesFromInitMuscleState>
Thelen2003Muscle::initMuscleState(const SimTK::State& s,
                                  const double aActivation,
                                  const double aSolTolerance,
                                  const int aMaxIterations) const
{
    // Using short variable names to facilitate writing out long equations
    const double ma = aActivation;
    const double ml = getLength(s);
    const double dml= getLengtheningSpeed(s);

    //Shorter version of the constants
    const double tsl = getTendonSlackLength();
    const double ofl = getOptimalFiberLength();
    const double ophi= getPennationAngleAtOptimalFiberLength();
    const double vol = ofl * sin(ophi);
    const double fiso= getMaxIsometricForce();
    const double vmax = getMaxContractionVelocity();

    //Shorter version of normalized muscle multipliers
    double fse = 0; //Normalized tendon (series element) force
    double fal = 0; //Normalized active force length multiplier
    double fpe = 0; //Normalized parallel element force
    double fv  = 0; //Normalized force-velocity multiplier

    //*******************************
    //Position level
    double tl  = getTendonSlackLength()*1.01;
    double lce = getPennationModel().calcFiberLength(ml, tl);
    
    double phi    = 0.0; 
    double cosphi = 1.0; 

    //Normalized quantities
    double tlN  = tl/tsl;
    double lceN = lce/ofl;
    //Velocity level
    double dtl      = 0;
    double dlce = 0; // (dml - dtl) * cos(phi);
    double dlceN = 0; // dlce / (vmax*ofl);
    double dphi = 0; // -(dlce / lce)*tan(phi);
    // double dlceAT   = dlce*cosphi -vol*dphi;

    //*******************************
    //Internal variables for the loop
    double Fm = 0;          // Muscle force
    double FmAT=0;          // Muscle force along tendon
    double Ft = 0;          // Tendon force
    double ferr = SimTK::MostPositiveReal;        // Solution error
    
    double dFm_dlce     = 0;  // Partial derivative of muscle force w.r.t. lce
    double dFmAT_dlce   = 0;  // Partial derivative of muscle force along 
                               // tendon w.r.t. lce
    double dFmAT_dlceAT = 0;  // Partial derivative of muscle force along
                               // tendon w.r.t. lce along the tendon.

    double dFt_d_lce   = 0;  // Partial derivative of tendon force w.r.t. lce
    double dFt_d_tl   = 0;   // Partial derivative of tendon force w.r.t. tl

    double dferr_d_lce = 0;  // Partial derivative of the solution error w.r.t
                             // lce
    double delta_lce   = 0;  // Chance in lce

    double Ke          = 0;  // Linearized local stiffness of the muscle

    //*******************************
    // Helper functions
    //Update position level quantities, only if they won't go singular
    auto positionFunc = [&] {
        phi = getPennationModel().calcPennationAngle(lce);
        cosphi = cos(phi);
        tl = ml - lce*cosphi;
        lceN = lce / ofl;
        tlN = tl / tsl;
    };

    // Functional to update the force multipliers 
    auto multipliersFunc = [&] {
        fse = calcfse(tlN);
        fal = calcfal(lceN);
        fpe = calcfpe(lceN);
    };

    // Functional to compute the equilibrium force error
    auto ferrFunc = [&] {
        Fm = (ma*fal*fv + fpe)*fiso;
        FmAT = Fm*cosphi;
        Ft = fse*fiso;
        ferr = FmAT - Ft;
    };

    // Functional to compute the partial derivative of muscle force w.r.t. lce
    auto partialsFunc = [&] {
        dFm_dlce = calcDFmDlce(lce, ma, fv, fiso, ofl);
        dFmAT_dlce = calcDFmATDlce(lce, phi, cosphi, Fm, dFm_dlce, vol);
        dFmAT_dlceAT = dFmAT_dlce*cosphi;

        dFt_d_tl = calcDFseDtl(tl, fiso, tsl);
        dFt_d_lce = calcDFseDlce(tl, lce, phi, cosphi, fiso, tsl, vol);
    };

    // Functional to estimate fiber velocity and force-velocity multiplier
    // from the relative fiber and tendon stiffnesses from above partials 
    auto velocityFunc = [&] {
        //Update velocity level quantities: share the muscle velocity 
        //between the tendon and the fiber according to their relative 
        //stiffness:
        //
        //Fm = Ft at equilibrium
        //Fm = Km*lceAT
        //Ft = Kt*xt
        //dFm_d_xm = Km*dlceAT + dKm_d_t*lceAT (assume dKm_d_t = 0)
        //dFt_d_xt = Kt*dtl + dKt_d_t*dtl (assume dKt_d_t = 0)
        //
        //This is a heuristic. The above assumptions are necessary as 
        //computing the partial derivatives of Km or Kt w.r.t. requires 
        //acceleration level knowledge, which is not available in 
        //general.

        //Stiffness of the muscle is the stiffness of the tendon and the 
        //fiber (along the tendon) in series

        //the if statement here is to handle the special case when the
        //negative stiffness of the fiber (which could happen in this
        // model) is equal to the positive stiffness of the tendon.
        if (abs(dFmAT_dlceAT + dFt_d_tl) > SimTK::SignificantReal
            && tl > getTendonSlackLength()) {

            //Ke = (dFmAT_dlceAT*dFt_d_tl) / (dFmAT_dlceAT + dFt_d_tl);
            // resultant stiffness = k1/(k1+k2)
            dtl = (dFmAT_dlceAT / (dFmAT_dlceAT + dFt_d_tl)) * dml;
        }
        else {
            dtl = dml;
        }

        // Update fiber velocity
        dlce = getPennationModel().calcFiberVelocity(cosphi, dml, dtl);
        dlceN = dlce / (vmax*ofl);
        // Update the force-velocity multiplier
        fv = calcfvInv(ma, fal, dlceN, aSolTolerance, 100);
    };

    //*******************************
    //Initialize the loop
    int iter = 0;

    // Estimate the position level quantities (lengths, angles) of the muscle
    positionFunc();

    // Multipliers based on initial fiber-length estimate
    multipliersFunc();

    // Starting guess at the force-velocity multiplier
    fv = 1.0;

    // Estimate partial derivatives of muscle forces
    partialsFunc();

    // Update the velocity (and multiplier) estimate from partials
    velocityFunc();

    // Compute the equilibrium error with velocity estimate
    ferrFunc();

    // Update the partial derivatives of the force error w.r.t. lce with 
    // newly estimated fv
    partialsFunc();

    double ferrPrev = ferr;
    double lcePrev = lce;

    double h = 1.0;
    while( (abs(ferr) > aSolTolerance)  && (iter < aMaxIterations)) {
        // Compute the search direction
        dferr_d_lce = dFmAT_dlce - dFt_d_lce;
        h = 1.0;

        while (abs(ferr) >= abs(ferrPrev)) {
             // Compute the Newton step
            delta_lce = -h*ferrPrev / dferr_d_lce;
            // Take a Newton Step if the step is nonzero
            if (abs(delta_lce) > SimTK::SignificantReal)
                lce = lcePrev + delta_lce;
            else {
                // We've stagnated or hit a limit; assume we are hitting local
                // minimum and attempt to approach from the other direction.
                lce = lcePrev - sign(delta_lce)*SimTK::SqrtEps;
                // Force a break, which will update the derivatives of
                // the muscle force and estimate of the fiber-velocity 
                h = 0;
            }

            if (lce < getMinimumFiberLength()) {
                lce = getMinimumFiberLength();
            }

            // Update the muscles's position level quantities (lengths, angles)
            positionFunc();

            // Update the muscle force multipliers
            multipliersFunc();

            // Compute the force error assuming fiber-velocity is unchanged
            ferrFunc();

            if (h <= SimTK::SqrtEps ) {
                break;
            }
            else
                h = 0.5*h;
        }

        ferrPrev = ferr;
        lcePrev = lce;

        // Update the partial derivative of the force error w.r.t. lce
        partialsFunc();
        // Update velocity estimate and velocity multiplier
        velocityFunc();

        iter++;
    }

    // Populate the result map.
    ValuesFromInitMuscleState resultValues;

    if (abs(ferr) < aSolTolerance) {  // The solution converged.

        resultValues["solution_error"] = ferr;
        resultValues["iterations"]     = (double)iter;
        resultValues["fiber_length"]   = lce;
        resultValues["passive_force"]  = fpe*fiso;
        resultValues["tendon_force"]   = fse*fiso;

        return std::pair<StatusFromInitMuscleState, ValuesFromInitMuscleState>
            (StatusFromInitMuscleState::Success_Converged, resultValues);
    }

    // Fiber length is at or exceeds its lower bound.
    if (lce <= getMinimumFiberLength()) {

        lce = getMinimumFiberLength();
        phi = getPennationModel().calcPennationAngle(lce);
        cosphi = cos(phi);
        tl  = getPennationModel().calcTendonLength(cosphi,lce,ml);
        lceN = lce/ofl;
        tlN  = tl/tsl;
        fse = calcfse(tlN);
        fpe = calcfpe(lceN);

        resultValues["solution_error"] = ferr;
        resultValues["iterations"]     = (double)iter;
        resultValues["fiber_length"]   = lce;
        resultValues["passive_force"]  = fpe*fiso;
        resultValues["tendon_force"]   = fse*fiso;

        return std::pair<StatusFromInitMuscleState, ValuesFromInitMuscleState>
           (StatusFromInitMuscleState::Warning_FiberAtLowerBound, resultValues);
    }

    resultValues["solution_error"] = ferr;
    resultValues["iterations"]     = (double)iter;
    resultValues["fiber_length"]   = SimTK::NaN;
    resultValues["passive_force"]  = SimTK::NaN;
    resultValues["tendon_force"]   = SimTK::NaN;

    return std::pair<StatusFromInitMuscleState, ValuesFromInitMuscleState>
        (StatusFromInitMuscleState::Failure_MaxIterationsReached, resultValues);
}

//==============================================================================
//
// STIFFNESS RELATED FUNCTIONS
//
//==============================================================================
double Thelen2003Muscle::calcFm(double ma, double fal, double fv, double fpe,
                                 double fiso) const
{
    double Fm = (ma*fal*fv + fpe)*fiso;
    return Fm;
}

double Thelen2003Muscle::calcActiveFm(double ma, double fal, double fv,
                                 double fiso) const
{
    double aFm = (ma*fal*fv)*fiso;
    return aFm;
}

double Thelen2003Muscle::calcDFmDlce(double lce, double ma, double fv, 
                                      double fiso, double ofl) const
{

            double lceN = lce/ofl;
            double dfal_d_lceN = calcDfalDlceN(lceN);
            double dfpe_d_lceN = calcDfpeDlceN(lceN);

            double dFm_d_lce = ((ma*fv)*dfal_d_lceN + dfpe_d_lceN)*fiso
                              *(1/ofl);                   
            return dFm_d_lce;
}

double Thelen2003Muscle::calcDFmATDlce(double lce, double phi, double cosphi, 
    double Fm, double d_Fm_d_lce, double penHeight) const
{
    //SINGULARITY: when vol*vol/(lce*lce) = 1,same as phi=pi/2
    double tmp1 = penHeight*penHeight;
    double tmp2 = lce*lce;
    double tmp3 = tmp2*lce;
    double dcosphi_d_lce = (tmp1 /(tmp3*pow((1-(tmp1/tmp2)),0.5 ) ));


    double d_FmAT_d_lce = d_Fm_d_lce*cosphi + Fm*dcosphi_d_lce;
    return d_FmAT_d_lce;
}

double Thelen2003Muscle::calcDFseDlce(double tl, double lce, double phi, double cosphi,
                                      double fiso, double tsl, double vol) const
{
    double tlN = tl/tsl;
        //SINGULARITY: When lce = 0
    double tmp1 = vol/lce;        

    //SINGULARITY: when vol/lce = 1 - equivalent to when phi = pi/2
    double dphi_d_lce = -vol / ( lce*lce * pow( (1-(tmp1)*(tmp1)),0.5)); 
    double dtl_d_lce  = -cos(phi) + lce*sin(phi)*dphi_d_lce;

    double dfse_d_tlN  = calcDfseDtlN(tlN); 
            tmp1 = (fiso/tsl);
    double dFt_d_lce = dfse_d_tlN*dtl_d_lce*tmp1;  
    return dFt_d_lce;
}

double Thelen2003Muscle::calcDFseDtl(double tl, double fiso, double tsl) const
{
    double dfse_d_tlN  = calcDfseDtlN(tl/tsl);
    double tmp1 = (fiso/tsl);
    double dFt_d_tl= dfse_d_tlN*tmp1;
    return dFt_d_tl;
}


//==============================================================================
//
// Convenience Method
//
//==============================================================================
void Thelen2003Muscle::printCurveToCSVFile(const CurveType ctype, 
                                           const std::string&path) const
{
    std::string fname = getName();

    switch(ctype){
        case FiberActiveForceLength:{
            
            fname.append("_FiberActiveForceLength.csv");
            
            SimTK::Matrix data(100,3);
            SimTK::Array_<std::string> colNames(data.ncol());
            colNames[0] = "lceN";
            colNames[1] = "fal";
            colNames[2] = "DfalDlceN";

            double delta = (2.0-0.0)/(data.nrow()-1.0);
            double lceN = 0;
            double lceN0 = 0;
            for(int i=0; i<data.nrow(); i++){
                lceN = lceN0 + i*delta;
                data(i,0) = lceN;
                data(i,1) = calcfal(lceN);
                data(i,2) = calcDfalDlceN(lceN);
            }

            printMatrixToFile(data,colNames,path, fname);

        }break;
        case FiberPassiveForceLength:{
            fname.append("_FiberPassiveForceLength.csv");
            
            SimTK::Matrix data(100,3);
            SimTK::Array_<std::string> colNames(data.ncol());
            colNames[0] = "lceN";
            colNames[1] = "fpe";
            colNames[2] = "DfpeDlceN";

            double lceNMax = get_FmaxMuscleStrain()+1.0;
            double lceNMin = 1.0;
            double lceNExtra = 0.1*(lceNMax-lceNMin);

            double delta = ((lceNMax + lceNExtra)-(lceNMin-lceNExtra))
                           /(data.nrow()-1.0);
            double lceN = 0;
            double lceN0 = lceNMin-lceNExtra;

            for(int i=0; i<data.nrow(); i++){
                lceN = lceN0 + i*delta;
                data(i,0) = lceN;
                data(i,1) = calcfpe(lceN);
                data(i,2) = calcDfpeDlceN(lceN);
            }

            printMatrixToFile(data,colNames,path, fname);

        }break;
        case FiberForceVelocity:{

            fname.append("_FiberForceVelocity.csv");
            
            SimTK::Matrix data(1000,5);
            SimTK::Array_<std::string> colNames(data.ncol());
            colNames[0] = "a";
            colNames[1] = "fal";
            colNames[2] = "dlceN";
            colNames[3] = "fv";
            colNames[4] = "DfvDdlceN";

            //Thelen's fv varies with activation
            double a = 0;
            double a0 = 0.1;
            double da = 0.1;

            //Thelen's fv varies with fiber velocity
            double dlceNMax = 1.0;
            double dlceNMin = -1.0;
            double dlceNExtra = 0.1*(dlceNMax-dlceNMin);

            double delta = ((dlceNMax + dlceNExtra)-(dlceNMin-dlceNExtra))
                           /(100.0-1.0);
            double dlceN = 0;
            double dlceN0 = dlceNMin-dlceNExtra;

            //Thelen's fv also varies with the value of the active force 
            //length curve. Most experiments that establish fv do so at 
            //a normalized fiber length of 1.0, where fal is 1. We will
            //also make that assumption here

            double fvInv;
            double DdlceDaFalFv = 0;
            double DfvDdlceN = 0;

            const double fal = 1.0;

            int idx = 0;
            for(int i=0; i<=9; i++){
                a = a0 + da*i;

                for(int j=0; j<100; j++){
                    dlceN = dlceN0 + j*delta;
                    fvInv = calcfvInv(a, fal, dlceN, 1e-6,100);
                    DdlceDaFalFv = calcDdlceDaFalFv(a,fal,a*fal*fvInv);
                    DfvDdlceN = (1/(DdlceDaFalFv*a*fal));

                    data(idx,0) = a;
                    data(idx,1) = fal;
                    data(idx,2) = dlceN;
                    data(idx,3) = fvInv;
                    data(idx,4) = DfvDdlceN;
                    
                    idx++;
                }
            }
            printMatrixToFile(data,colNames,path, fname);

        }break;
        case TendonForceLength:{
            fname.append("_TendonForceLength.csv");
            
            SimTK::Matrix data(100,3);
            SimTK::Array_<std::string> colNames(data.ncol());
            colNames[0] = "ltN";
            colNames[1] = "fse";
            colNames[2] = "DfseDtlN";

            double ltNMax = get_FmaxTendonStrain()+1.0;
            double ltNMin = 1.0;
            double ltNExtra = 0.1*(ltNMax-ltNMin);

            double delta = ((ltNMax + ltNExtra)-(ltNMin-ltNExtra))
                           /(data.nrow()-1.0);
            double ltN = 0;
            double ltN0 = ltNMin-ltNExtra;

            for(int i=0; i<data.nrow(); i++){
                ltN = ltN0 + i*delta;
                data(i,0) = ltN;
                data(i,1) = calcfse(ltN);
                data(i,2) = calcDfseDtlN(ltN);
            }

            printMatrixToFile(data,colNames,path, fname);

        }break;
        default:{
            std::string msg = "Thelen2003Muscle::printCurveToCSVFile ";
            msg.append(getName());
            msg.append(" invalid curve type");
            SimTK_ASSERT(false,msg.c_str());
        }
    }

}

/*
This function will print cvs file of the column vector col0 and the matrix data

@params data: A matrix of data
@params filename: The name of the file to print
*/
void Thelen2003Muscle::
    printMatrixToFile(SimTK::Matrix& data, SimTK::Array_<std::string>& colNames,
    const std::string& path, const std::string& filename) const
{
    
    ofstream datafile;
    std::string fullpath = path;
    
    if(fullpath.length() > 0)
        fullpath.append("/");
    
    fullpath.append(filename);

    datafile.open(fullpath.c_str(),std::ios::out);

    if(!datafile){
        datafile.close();
        string name = getName();
        SimTK_ERRCHK2_ALWAYS( false, 
                "Thelen2003Muscle::printMatrixToFile",
                "%s: Failed to open the file path: %s", 
                name.c_str(),
                fullpath.c_str());
    }


    for(int i = 0; i < (signed)colNames.size(); i++){
        if(i < (signed)colNames.size()-1)
            datafile << colNames[i] << ",";
        else
            datafile << colNames[i] << "\n";
    }

    for(int i = 0; i < data.nrow(); i++){       
        for(int j = 0; j < data.ncol(); j++){
            if(j<data.ncol()-1)
                datafile << data(i,j) << ",";
            else
                datafile << data(i,j) << "\n";
        }   
    }
    datafile.close();
} 

//==============================================================================
//
// TENDON RELATED FUNCTIONS
//
//==============================================================================

double Thelen2003Muscle::calcfse(const double tlN) const 
{
    double x = tlN-1;
    double e0 = get_FmaxTendonStrain();
    
    /*The paper reports etoe = 0.609e0, however, this is a severely rounded off
        The exact answer, to SimTK::Eps is   
        etoe =  99*e0*e^3 / ( 166*e^3 - 67)
        klin =  67 /( 100*(e0 - (99*e0*e^3)/(166*e^3-67)) )
        See thelenINIT_20120127.mw for details
    */    
    double kToe = 3.0;
    double Ftoe = 33.0/100.0;
    double t1   = exp(0.3e1);
    double eToe = (0.99e2*e0*t1) / (0.166e3*t1 - 0.67e2);
    t1 = exp(0.3e1);
    double klin = (0.67e2/0.100e3) 
                * 1.0/(e0 - (0.99e2*e0*t1) / (0.166e3*t1 - 0.67e2));

    //Compute tendon force
    double fse = 0;
    if (x > eToe){
        fse = klin*(x-eToe)+Ftoe;
    }else if (x>0.0){ 
        fse =(Ftoe/(exp(kToe)-1.0))*(exp(kToe*x/eToe)-1.0);
    }else{
        fse=0.;}

    return fse;
}


double Thelen2003Muscle::calcDfseDtlN(const double tlN) const {
    double x = tlN-1;
    double e0 = get_FmaxTendonStrain();
    
    /*The paper reports etoe = 0.609e0, however, this is a severely rounded off
    result of the exact answer:    
        etoe =  99*e0*e^3 / ( 166*e^3 - 67)
        See thelenINIT_20120127.mw for details
    */
    double kToe = 3.0;
    double Ftoe = 33.0/100.0;

    double t1   = exp(0.3e1);
    double eToe = (0.99e2*e0*t1) / (0.166e3*t1 - 0.67e2);

    /*The paper reports etoe = 0.609e0, however, this is a severely rounded off
    result of the exact answer:    
        klin =  67 /( 100*(e0 - (99*e0*e^3)/(166*e^3-67)) )
        See thelenINIT_20120127.mw for details
    */
    t1 = exp(0.3e1);
    double klin = (0.67e2/0.100e3) 
                * 1.0/(e0 - (0.99e2*e0*t1) / (0.166e3*t1 - 0.67e2));

    //Compute tendon force
    double dfse_d_dtlN = 0;
    if (x > eToe){
        dfse_d_dtlN = klin;
    }else if (x>0.0){ 
        dfse_d_dtlN =(Ftoe/(exp(kToe)-1.0)) * (kToe/eToe) * (exp(kToe*x/eToe));
    }else{
        dfse_d_dtlN=0.;}

    return dfse_d_dtlN;
}

double Thelen2003Muscle::calcfsefisoPE(double tendonStrain) const
{

    double tendon_strain =  tendonStrain;
    double fmaxTendonStrain = get_FmaxTendonStrain();       

    //Future optimization opportunity: precompute kToe, fToe, eToe and klin
    //when the muscle is initialized. Store these values rather than 
    //computing them every time.

    double kToe = 3.0;
    double Ftoe = 33.0/100.0;

    double t1   = exp(0.3e1);
    double eToe = (0.99e2*fmaxTendonStrain*t1) / (0.166e3*t1 - 0.67e2);

    t1 = exp(0.3e1);
    double klin = (0.67e2/0.100e3) 
                * 1.0/(fmaxTendonStrain - (0.99e2*fmaxTendonStrain*t1) 
                / (0.166e3*t1 - 0.67e2));

    //Compute the energy stored in the tendon. 
    //Integrals computed symbolically in muscle_kepew_20111021.mw just to check
    double tendonPE = 0.0;
    double lenR        = getTendonSlackLength();
    double lenTdn    = (tendon_strain+1)*lenR;
    double lenToe    = (eToe+1.0)*lenR;    
    double fiso        = getMaxIsometricForce();

    if (tendon_strain>eToe){
       //compute the energy stored in the toe portion of the tendon strain curve
        double len = lenToe;
        double toePE_len = (fiso*Ftoe/(exp(kToe)-1.0))
                            *((lenR*eToe/kToe)
                            *exp(kToe*(len-lenR)/(lenR*eToe)) - len);
        len =  lenR;
        double toePE_0    =  (fiso*Ftoe/(exp(kToe)-1.0))
                            *((lenR*eToe/kToe)
                            *exp(kToe*(len-lenR)/(lenR*eToe)) - len);
        // double toePEtest = toePE_len-toePE_0;

        //compute the energy stored in the linear section of the 
        //tendon strain curve from ..... 0 to len
        len = lenTdn;
        double linPE_len = (1.0/2.0)*(fiso*klin*(len*len)/lenR) 
                           + fiso*len*(klin*(-1.0-eToe)+Ftoe);
        //ditto from 0 .... eToe
        len = lenToe;
        double linPE_eToe= (1.0/2.0)*(fiso*klin*(len*len)/lenR) 
                            + fiso*len*(klin*(-1.0-eToe)+Ftoe);       
        
        //compute the total potential energy stored in the tendon
         tendonPE =(toePE_len-toePE_0) + (linPE_len-linPE_eToe);
    }else if (tendon_strain>0.0){ 
        //PE from 0 .... len
        double len = lenTdn;
        double toePE_len = (fiso*Ftoe/(exp(kToe)-1.0)) * ((lenR*eToe/kToe) 
            * exp(kToe*(len-lenR)/(lenR*eToe)) - len);
        //PE from 0 .... eToe
        len = lenR;
        double toePE_0    =  (fiso*Ftoe/(exp(kToe)-1.0)) * ((lenR*eToe/kToe) 
            * exp(kToe*(len-lenR)/(lenR*eToe)) - len);

        //Compute the total PE stored in the tendon
        tendonPE = toePE_len-toePE_0;
    }else{
        tendonPE = 0.0;
    }
    
    
    return tendonPE;
}

//==============================================================================
//
// ACTIVE FORCE LENGTH FUNCTIONS
//
//==============================================================================
double Thelen2003Muscle::calcfal(const double lceN) const{       
    double kShapeActive = get_KshapeActive();   
    double x=(lceN-1.)*(lceN-1.);
    double fal = exp(-x/kShapeActive);
    return fal;
}
double Thelen2003Muscle::calcDfalDlceN(const double lceN) const {
    double kShapeActive = get_KshapeActive();   
    double t1 = lceN - 0.10e1;
    double t2 = 0.1e1 / kShapeActive;
    double t4 = t1 * t1;
    double t6 = exp(-t4 * t2);
    double dfal_d_lceN = -0.2e1 * t1 * t2 * t6;
    return dfal_d_lceN;
}

//=============================================================================
//
// FIBER PARALLEL ELEMENT HELPER FUNCTIONS
//
//=============================================================================
double Thelen2003Muscle::calcfpe(const double lceN) const {
    double fpe = 0;
    double e0 = get_FmaxMuscleStrain();
    double kpe = get_KshapePassive();

    //Compute the passive force developed by the muscle
    if(lceN > 1.0){
        double t5 = exp(kpe * (lceN - 0.10e1) / e0);
        double t7 = exp(kpe);
        fpe = (t5 - 0.10e1) / (t7 - 0.10e1);
    }
    return fpe;
}

double Thelen2003Muscle::calcDfpeDlceN(const double lceN) const {
    double dfpe_d_lceN = 0;
    double e0 = get_FmaxMuscleStrain();
    double kpe = get_KshapePassive();

    if(lceN > 1.0){
        double t1 = 0.1e1 / e0;
        double t6 = exp(kpe * (lceN - 0.10e1) * t1);
        double t7 = exp(kpe);
        dfpe_d_lceN = kpe * t1 * t6 / (t7 - 0.10e1);
    }
    return dfpe_d_lceN;
}

double Thelen2003Muscle::calcfpefisoPE(double lceN) const
{
    double fmaxMuscleStrain = get_FmaxMuscleStrain();
    double kShapePassive = get_KshapePassive();

    double musclePE = 0.0;
    //Compute the potential energy stored in the muscle
    if(lceN > 1.0){
        //Shorter variable names to make the equations readable.
        double lenR = getOptimalFiberLength();
        double fiso = getMaxIsometricForce();
        double len = lceN*lenR;        
        double kpe = kShapePassive;
        double e0 = fmaxMuscleStrain;


        //PE storage at current stretch
        double fpePE_len = (fiso/(exp(kpe)-1))
            *( (lenR*e0/kpe)*exp( (kpe/e0)*( (len/lenR)-1)) - len); 
        
        //PE stored between 0 and 1 for the exponential function that is 
        //used to represent fpe for normalized fiber lengths > 1.
        len = lenR;
        double fpePE_0 = (fiso/(exp(kpe)-1))
            *( (lenR*e0/kpe)*exp( (kpe/e0)*( (len/lenR)-1)) - len); 

        musclePE = fpePE_len - fpePE_0;

    }else{
        musclePE = 0.0;
    }
    return musclePE;
}


//=============================================================================
//
// FIBER FORCE - VELOCITY CURVE FUNCTIONS
//
//=============================================================================

double Thelen2003Muscle::calcdlceN(double act,double fal,double actFalFv) const
{
    //The variable names have all been switched to closely match 
    //with the notation in Thelen 2003.
    double dlceN = 0.0;      //contractile element velocity    
    double af   = get_Af();

    double a    = act;
    double afl  = a*fal; //afl = a*fl
    double Fm   = actFalFv;     //Fm = a*fl*fv    
    double flen = get_Flen();
    // double Fmlen_afl = flen*afl;

    double dlcedFm = 0.0; //partial derivative of contractile element
                          // velocity w.r.t. Fm

    double b = 0;
    double db= 0;

    double Fm_asyC = 0;           //Concentric contraction asymptote
    double Fm_asyE = afl*flen;    
                                //Eccentric contraction asymptote
    double asyE_thresh = get_fv_linear_extrap_threshold();

    //If fv is in the appropriate region, use 
    //Thelen 2003 Eqns 6 & 7 to compute dlceN
    if (Fm > Fm_asyC && Fm < Fm_asyE*asyE_thresh){

        if( Fm <= afl ){        //Muscle is concentrically contracting
            b = afl + Fm/af;
            db= 1/af;
        }else{                    //Muscle is eccentrically contracting
            b = ((2+2/af)*(afl*flen-Fm))/(flen-1); 
            db= ((2+2/af)*(-1))/(flen-1); 
        }

        dlceN = (0.25 + 0.75*a)*(Fm-afl)/b; 
        //Scaling by VMAX is left out, and is post multiplied outside 
        //of the function


    }else{  //Linear extrapolation
            double Fm0 = 0.0; //Last Fm value from the Thelen curve

            //Compute d and db/dFm from Eqn 7. of Thelen2003
            //for the last
            if(Fm <= Fm_asyC){ //Concentrically contracting
                Fm0 = Fm_asyC;
                b = afl + Fm0/af;
                db= 1/af;               
            }else{             //Eccentrically contracting
                Fm0 = asyE_thresh*Fm_asyE;
                b = ((2+2/af)*(afl*flen-Fm0))/(flen-1); 
                db= ((2+2/af)*(-1))/(flen-1); 
            }

            //Compute the last dlceN value that falls in the region where
            //Thelen 2003 Eqn. 6 is valid
            double dlce0 = (0.25 + 0.75*a)*(Fm0-afl)/b;

            //Compute the dlceN/dfm of Eqn. 6 of Thelen 2003 at the last
            //valid point
            dlcedFm = (0.25 + 0.75*a)*(1)/b 
                    - ((0.25 + 0.75*a)*(Fm0-afl)/(b*b))*db;

            //Linearly extrapolate Eqn. 6 from Thelen 2003 to compute
            //the new value for dlceN/dFm
            dlceN = dlce0 + dlcedFm*(Fm-Fm0);            
        }
            
        return dlceN;
}

double Thelen2003Muscle::
    calcfv(double aFse,     double aFpe, double aFal, 
           double aCosPhi,  double aAct) const
{
    //This only works for an equilibrium model, but its a lot less 
    //computationally expensive (and error prone) than trying to invert the 
    //weird function that defines the fv curve in the Thelen model
    double fv = ((aFse/aCosPhi) - aFpe)/(aAct*aFal);
    return fv;
}

double Thelen2003Muscle::calcDdlceDaFalFv(double aAct, 
                                          double aFal, double aFalFv) const
{
    //The variable names have all been switched to closely match with 
    //the notation in Thelen 2003.
    // double dlceN = 0.0;      //contractile element velocity    
    double af   = get_Af();

    double a    = aAct;
    double afl  = aAct*aFal;  //afl = a*fl
    double Fm   = aFalFv;    //Fm = a*fl*fv    
    double flen = get_Flen();
    // double Fmlen_afl = flen*aAct*aFal;

    double dlcedFm = 0.0; //partial derivative of contractile element 
                          //velocity w.r.t. Fm

    double b = 0;
    double db= 0;

    double Fm_asyC = 0;           //Concentric contraction asymptote
    double Fm_asyE = aAct*aFal*flen;    
                                //Eccentric contraction asymptote
    double asyE_thresh = get_fv_linear_extrap_threshold();

    //If fv is in the appropriate region, use 
    //Thelen 2003 Eqns 6 & 7 to compute dlceN
    if (Fm > Fm_asyC && Fm < Fm_asyE*asyE_thresh){

        if( Fm <= afl ){        //Muscle is concentrically contracting
            b = afl + Fm/af;
            db= 1/af;
        }else{                    //Muscle is eccentrically contracting
            b = ((2+2/af)*(afl*flen-Fm))/(flen-1); 
            db= ((2+2/af)*(-1))/(flen-1); 
        }

        //This variable may have future use outside this function
        dlcedFm = (0.25 + 0.75*a)*(1)/b - ((0.25 + 0.75*a)*(Fm-afl)/(b*b))*db;            

    }else{  //Linear extrapolation
            double Fm0 = 0.0; //Last Fm value from the Thelen curve

            //Compute d and db/dFm from Eqn 7. of Thelen2003
            //for the last
            if(Fm <= Fm_asyC){ //Concentrically contracting
                Fm0 = Fm_asyC;
                b = afl + Fm0/af;
                db= 1/af;               
            }else{             //Eccentrically contracting
                Fm0 = asyE_thresh*Fm_asyE;
                b = ((2+2/af)*(afl*flen-Fm0))/(flen-1); 
                db= ((2+2/af)*(-1))/(flen-1); 
            }

            
            //Compute the dlceN/dfm of Eqn. 6 of Thelen 2003 at the last
            //valid point
            dlcedFm = (0.25 + 0.75*a)*(1)/b 
                - ((0.25 + 0.75*a)*(Fm0-afl)/(b*b))*db;
          
        }
            
        return dlcedFm;
}

// Compute the force-velocity multiplier by inverting Thelen 2003' f-v
// equations for fiber-velocity given the active fiber force (see calcdlceN()).
// This is here because it is non-trivial to correctly invert the piece-wise
// continuous force velocity curve specified by the modified Thelen2003Muscle
// force velocity curve. This converges quickly and is well tested.
double Thelen2003Muscle::
        calcfvInv(double aAct,double aFal,double dlceN,
                  double tolerance, int maxIterations) const
{
    double result = SimTK::NaN;
    double ferr=1;
    double iter= 0;

    double dlceN1 = 0;
    double dlceN1_d_Fm = 0;
    double fv = 1;
    double aFalFv = fv*aAct*aFal;
    double delta_aFalFv = 0;

    while(abs(ferr) >= tolerance && iter < maxIterations)
    {
        dlceN1 = calcdlceN(aAct,aFal, aFalFv);
        ferr   = dlceN1-dlceN;
        dlceN1_d_Fm = calcDdlceDaFalFv(aAct,aFal,aFalFv);


        if(abs(dlceN1_d_Fm) > SimTK::SignificantReal){
           delta_aFalFv = -ferr/(dlceN1_d_Fm);
           aFalFv = aFalFv + delta_aFalFv;
        }
        iter = iter+1;
    }

    if(abs(ferr) < tolerance){
        result = max(0.0, aFalFv/(aAct*aFal));
        return result;
    }

    OPENSIM_THROW_FRMOBJ(Exception, 
        "Solver for force-velocity multiplier failed to converge.");
}

<MSG> Added missing initialization to constructProperties().

<DFF> @@ -154,6 +154,10 @@ void Thelen2003Muscle::constructProperties()
 
     constructProperty_MuscleFixedWidthPennationModel(
         MuscleFixedWidthPennationModel());
+    upd_MuscleFixedWidthPennationModel()
+        .set_optimal_fiber_length(getOptimalFiberLength());
+    upd_MuscleFixedWidthPennationModel()
+        .set_pennation_angle_at_optimal(getPennationAngleAtOptimalFiberLength());
     setMaximumPennationAngle(acos(0.1));
 
     constructProperty_FmaxTendonStrain(0.04); // was 0.033

{'commit': 'diff --git a/OpenSim/Actuators/Thelen2003Muscle.cpp b/OpenSim/Actuators/Thelen2003Muscle.cpp\nindex b735076ab..2221320a4 100644\n--- a/OpenSim/Actuators/Thelen2003Muscle.cpp\n+++ b/OpenSim/Actuators/Thelen2003Muscle.cpp\n@@ -154,6 +154,10 @@ void Thelen2003Muscle::constructProperties()\n \n     constructProperty_MuscleFixedWidthPennationModel(\n         MuscleFixedWidthPennationModel());\n+    upd_MuscleFixedWidthPennationModel()\n+        .set_optimal_fiber_length(getOptimalFiberLength());\n+    upd_MuscleFixedWidthPennationModel()\n+        .set_pennation_angle_at_optimal(getPennationAngleAtOptimalFiberLength());\n     setMaximumPennationAngle(acos(0.1));\n \n     constructProperty_FmaxTendonStrain(0.04); // was 0.033\n', 'commit_subject': 'Added missing initialization to constructProperties().', 'repo_name': 'opensim-org/opensim-core', 'committer': {'name': 'Tom Uchida', 'email': 'e5a9f087e6112d34b8b71a2bd0996bfe1bbd2e15@gmail.com', 'time_sec': '1425345821', 'tz_offset': '-480', 'date': {'seconds': '1425345821'}}, 'commit_tree': '5acf40bceebc830fb39fc01fa31c32ed49ae4163', 'commit_message': 'Added missing initialization to constructProperties().\n', 'language_name': 'C++', 'license': 'apache-2.0', 'hunks': '@@ -154,6 +154,10 @@ void Thelen2003Muscle::constructProperties()\n \n     constructProperty_MuscleFixedWidthPennationModel(\n         MuscleFixedWidthPennationModel());\n+    upd_MuscleFixedWidthPennationModel()\n+        .set_optimal_fiber_length(getOptimalFiberLength());\n+    upd_MuscleFixedWidthPennationModel()\n+        .set_pennation_angle_at_optimal(getPennationAngleAtOptimalFiberLength());\n     setMaximumPennationAngle(acos(0.1));\n \n     constructProperty_FmaxTendonStrain(0.04); // was 0.033\n', 'addition_count': 4, 'deletion_count': 0, 'src_file': 'a/OpenSim/Actuators/Thelen2003Muscle.cpp', 'tgt_file': 'b/OpenSim/Actuators/Thelen2003Muscle.cpp', 'file_extension': '.cpp', 'before_file': '/* -------------------------------------------------------------------------- *\n *                       OpenSim:  Thelen2003Muscle.cpp                       *\n * -------------------------------------------------------------------------- *\n * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *\n * See http://opensim.stanford.edu and the NOTICE file for more information.  *\n * OpenSim is developed at Stanford University and supported by the US        *\n * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *\n * through the Warrior Web program.                                           *\n *                                                                            *\n * Copyright (c) 2005-2017 Stanford University and the Authors                *\n * Author(s): Matthew Millard                                                 *\n *                                                                            *\n * Licensed under the Apache License, Version 2.0 (the "License"); you may    *\n * not use this file except in compliance with the License. You may obtain a  *\n * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *\n *                                                                            *\n * Unless required by applicable law or agreed to in writing, software        *\n * distributed under the License is distributed on an "AS IS" BASIS,          *\n * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *\n * See the License for the specific language governing permissions and        *\n * limitations under the License.                                             *\n * -------------------------------------------------------------------------- */\n//=============================================================================\n// INCLUDES\n//=============================================================================\n#include <fstream>\n#include <OpenSim/Simulation/Model/Model.h>\n#include "Thelen2003Muscle.h"\n\n//=============================================================================\n// STATICS\n//=============================================================================\nusing namespace std;\nusing namespace OpenSim;\nusing namespace SimTK;\n\n//=============================================================================\n// CONSTRUCTORS\n//=============================================================================\n// Uses default (compiler-generated) destructor, copy constructor, copy \n// assignment operator.\n\n\n//_____________________________________________________________________________\n/**\n * Default constructor.\n */\nThelen2003Muscle::Thelen2003Muscle()          \n{    \n    setNull();\n    constructProperties();\n}\n\n//_____________________________________________________________________________\n/**\n * Constructor.\n */\nThelen2003Muscle::\nThelen2003Muscle(const std::string& aName,  double aMaxIsometricForce,\n                  double aOptimalFiberLength,double aTendonSlackLength,\n                  double aPennationAngle)\n{\n    setNull();\n    constructProperties();\n    setName(aName);\n    \n    setMaxIsometricForce(aMaxIsometricForce);\n    setOptimalFiberLength(aOptimalFiberLength);\n    setTendonSlackLength(aTendonSlackLength);\n    setPennationAngleAtOptimalFiberLength(aPennationAngle);\n}\n\n//==============================================================================\n// Component Interface\n//==============================================================================\nvoid Thelen2003Muscle::extendFinalizeFromProperties()\n{\n    Super::extendFinalizeFromProperties();\n\n    SimTK_ERRCHK1_ALWAYS(get_FmaxTendonStrain() > 0,\n        "Thelen2003Muscle::extendFinalizeFromProperties",\n        "%s: FmaxTendonStrain must be greater than zero", getName().c_str());\n    SimTK_ERRCHK1_ALWAYS(get_FmaxMuscleStrain() > 0,\n        "Thelen2003Muscle::extendFinalizeFromProperties",\n        "%s: FmaxMuscleStrain must be greater than zero", getName().c_str());\n    SimTK_ERRCHK1_ALWAYS(get_KshapeActive() > 0,\n        "Thelen2003Muscle::extendFinalizeFromProperties",\n        "%s: KshapeActive must be greater than zero", getName().c_str());\n    SimTK_ERRCHK1_ALWAYS(get_KshapePassive() > 0,\n        "Thelen2003Muscle::extendFinalizeFromProperties",\n        "%s: KshapePassive must be greater than zero", getName().c_str());\n    SimTK_ERRCHK1_ALWAYS(get_Af() > 0,\n        "Thelen2003Muscle::extendFinalizeFromProperties",\n        "%s: Af must be greater than zero", getName().c_str());\n    SimTK_ERRCHK1_ALWAYS(get_Flen() > 1.0,\n        "Thelen2003Muscle::extendFinalizeFromProperties",\n        "%s: Flen must be greater than 1.0", getName().c_str());\n    SimTK_ERRCHK1_ALWAYS(get_fv_linear_extrap_threshold() > 1.0/get_Flen(),\n        "Thelen2003::extendFinalizeFromProperties",\n        "%s: F-v extrapolation threshold must be greater than 1.0/Flen",\n        getName().c_str());\n\n\n    OPENSIM_THROW_IF_FRMOBJ(get_minimum_activation() < 0.01,\n        InvalidPropertyValue, getProperty_minimum_activation().getName(),\n        "Minimum activation cannot be less than 0.01");\n\n    OPENSIM_THROW_IF_FRMOBJ(getMinControl() < get_minimum_activation(),\n        InvalidPropertyValue, getProperty_min_control().getName(),\n        "Minimum control cannot be less than minimum activation");\n\n    // Propagate properties down to pennation model subcomponent. If any of the\n    // new property values are invalid, restore the subcomponent\'s current\n    // property values (to avoid throwing again when the subcomponent\'s\n    // extendFinalizeFromProperties() method is called directly) and then\n    // re-throw the exception thrown by the subcomponent.\n    auto& pennMdl =\n        updMemberSubcomponent<MuscleFixedWidthPennationModel>(pennMdlIdx);\n    MuscleFixedWidthPennationModel pennMdlCopy(pennMdl);\n    pennMdl.set_optimal_fiber_length(getOptimalFiberLength());\n    pennMdl.set_pennation_angle_at_optimal(\n        getPennationAngleAtOptimalFiberLength());\n    pennMdl.set_maximum_pennation_angle(get_maximum_pennation_angle());\n    try {\n        pennMdl.finalizeFromProperties();\n    } catch (const InvalidPropertyValue&) {\n        pennMdl = pennMdlCopy;\n        throw;\n    }\n\n    // Propagate properties down to activation dynamics model subcomponent.\n    // Handle invalid properties as above for pennation model.\n    auto& actMdl =\n        updMemberSubcomponent<MuscleFirstOrderActivationDynamicModel>(actMdlIdx);\n    MuscleFirstOrderActivationDynamicModel actMdlCopy(actMdl);\n    actMdl.set_activation_time_constant(get_activation_time_constant());\n    actMdl.set_deactivation_time_constant(get_deactivation_time_constant());\n    actMdl.set_minimum_activation(get_minimum_activation());\n    try {\n        actMdl.finalizeFromProperties();\n    } catch (const InvalidPropertyValue&) {\n        actMdl = actMdlCopy;\n        throw;\n    }\n}\n\n//====================================================================\n// Model Component Interface\n//====================================================================\nvoid Thelen2003Muscle::extendConnectToModel(Model& aModel)\n{\n    Super::extendConnectToModel(aModel);\n}\n\n    constructProperty_MuscleFixedWidthPennationModel(\n        MuscleFixedWidthPennationModel());\n    setMaximumPennationAngle(acos(0.1));\n\n    constructProperty_FmaxTendonStrain(0.04); // was 0.033\n}\n\n//_____________________________________________________________________________\n// Set the data members of this muscle to their null values.\nvoid Thelen2003Muscle::setNull()\n{\n    // no data members\n    setAuthors("Matthew Millard");\n}\n\n//_____________________________________________________________________________\n/**\n * Populate this object\'s properties\n */\nvoid Thelen2003Muscle::constructProperties()\n{\n    constructProperty_FmaxTendonStrain(0.04); // was 0.033\n    constructProperty_FmaxMuscleStrain(0.6);\n    constructProperty_KshapeActive(0.45);   \n    constructProperty_KshapePassive(5.0);   \n    constructProperty_Af(0.25); \n    constructProperty_Flen(1.4);    //was 1.8, \n    constructProperty_fv_linear_extrap_threshold(0.95);\n    //acos(0.1) = 84.26 degrees\n    constructProperty_maximum_pennation_angle(acos(0.1));\n    constructProperty_activation_time_constant(0.015);\n    constructProperty_deactivation_time_constant(0.050);\n    constructProperty_minimum_activation(0.01);\n\n    setMinControl(get_minimum_activation());\n}\n\n//=============================================================================\n// GET\n//=============================================================================\ndouble Thelen2003Muscle::getActivationTimeConstant() const\n{   return get_activation_time_constant(); }\n\ndouble Thelen2003Muscle::getDeactivationTimeConstant() const\n{   return get_deactivation_time_constant(); }\n\ndouble Thelen2003Muscle::getMinimumActivation() const\n{   return get_minimum_activation(); }\n\nconst MuscleFirstOrderActivationDynamicModel& Thelen2003Muscle::\ngetActivationModel() const\n{\n    return getMemberSubcomponent<MuscleFirstOrderActivationDynamicModel>(\n           actMdlIdx);\n}\n\nconst MuscleFixedWidthPennationModel& Thelen2003Muscle::\ngetPennationModel() const\n{   return getMemberSubcomponent<MuscleFixedWidthPennationModel>(pennMdlIdx); }\n\ndouble Thelen2003Muscle::getMaximumPennationAngle() const\n{   return get_maximum_pennation_angle(); }\n\n//=============================================================================\n// SET\n//=============================================================================\nvoid Thelen2003Muscle::setActivationTimeConstant(double actTimeConstant)\n{   set_activation_time_constant(actTimeConstant); }\n\nvoid Thelen2003Muscle::setDeactivationTimeConstant(double deactTimeConstant)\n{   set_deactivation_time_constant(deactTimeConstant); }\n\nvoid Thelen2003Muscle::setMinimumActivation(double minimumActivation)\n{   set_minimum_activation(minimumActivation); }\n\nvoid Thelen2003Muscle::setMaximumPennationAngle(double maximumPennationAngle)\n{   set_maximum_pennation_angle(maximumPennationAngle); }\n\n//==============================================================================\n//                             START OF DEPRECATED\n//==============================================================================\ndouble Thelen2003Muscle::\ncalcInextensibleTendonActiveFiberForce(SimTK::State& s, \n                                       double aActivation) const\n{      \n    double inextensibleTendonActiveFiberForce = 0;\n\n    double muscleLength = getLength(s);\n    double muscleVelocity = getLengtheningSpeed(s);\n    double tendonSlackLength = getTendonSlackLength();\n    double tendonVelocity = 0.0; //Inextensible tendon;\n\n    double fiberLength  = getPennationModel().calcFiberLength(muscleLength,\n                                            tendonSlackLength);\n        \n    if(fiberLength > getPennationModel().getMinimumFiberLength()) {\n        double phi = getPennationModel().calcPennationAngle(fiberLength);\n        \n        double fiberVelocity = getPennationModel().calcFiberVelocity(\n                                    cos(phi),muscleVelocity,tendonVelocity);\n\n        inextensibleTendonActiveFiberForce = \n            calcActiveFiberForceAlongTendon(    aActivation,\n                                                fiberLength,\n                                                fiberVelocity);\n    }\n\n    return inextensibleTendonActiveFiberForce;\n}\n\ndouble Thelen2003Muscle::\n            calcActiveFiberForceAlongTendon(double activation, \n                                            double fiberLength, \n                                            double fiberVelocity) const\n{\n    double activeFiberForce = 0;    \n    double clampedFiberLength = getPennationModel()\n                                .clampFiberLength(fiberLength);\n\n    //If the fiber is in a legal range, compute the force its generating\n    if(fiberLength > getPennationModel().getMinimumFiberLength())\n    {\n        //Clamp activation to a legal range\n        double clampedActivation = getActivationModel()\n                                   .clampActivation(activation);\n\n        //Normalize fiber length and velocity\n        double normFiberLength    = clampedFiberLength/getOptimalFiberLength();\n        double normFiberVelocity  = fiberVelocity / \n                        (getOptimalFiberLength() * getMaxContractionVelocity());\n\n\n        //Evaluate the force active length and force velocity multipliers\n        double fal  = calcfal(normFiberLength);\n\n        double fv = SimTK::NaN;\n        double fvTol = 1e-6;\n        int fvMaxIter= 100;        \n        fv = calcfvInv( clampedActivation,\n                                            fal,\n                                            normFiberVelocity,\n                                            fvTol,\n                                            fvMaxIter);\n\n        double fiso = getMaxIsometricForce();\n\n        //Evaluate the pennation angle\n        double phi = getPennationModel().calcPennationAngle(fiberLength);\n\n        //Compute the active fiber force \n        activeFiberForce = fiso * clampedActivation * fal * fv * cos(phi);\n    }\n    //Compute the active fiber force\n    \n\n    return activeFiberForce;\n}\n\n//==============================================================================\n//                              END OF DEPRECATED\n//==============================================================================\n\n//==============================================================================\n// Muscle.h Interface\n//==============================================================================\n\ndouble  Thelen2003Muscle::computeActuation(const SimTK::State& s) const\n{\n    const MuscleDynamicsInfo& mdi = getMuscleDynamicsInfo(s);\n    setActuation(s, mdi.tendonForce);\n    return( mdi.tendonForce );\n}\n\nvoid Thelen2003Muscle::computeInitialFiberEquilibrium(SimTK::State& s) const\n{\n    //Initial activation and fiber length from input State, s.\n    _model->getMultibodySystem().realize(s, SimTK::Stage::Velocity);\n    double activation = getActivation(s);\n\n    //Tolerance, in Newtons, of the desired equilibrium\n    const double tol = max( 1e-8*getMaxIsometricForce(), \n                            SimTK::SignificantReal * 10 );\n\n    int maxIter = 20;  //Should this be user settable?\n\n    std::pair<StatusFromInitMuscleState, ValuesFromInitMuscleState> result;\n\n    try {\n         result = initMuscleState(s, activation, tol, maxIter);\n    }\n    catch (const std::exception& x) {\n        OPENSIM_THROW_FRMOBJ(MuscleCannotEquilibrate, x.what());\n    }\n\n    switch(result.first) {\n\n    case StatusFromInitMuscleState::Success_Converged:\n        setActuation(s, result.second["tendon_force"]);\n        setFiberLength(s, result.second["fiber_length"]);\n        break;\n\n    case StatusFromInitMuscleState::Warning_FiberAtLowerBound:\n        log_warn("Thelen2003Muscle initialization: \'{}\' is at its minimum "\n                 "fiber length of {}.", getName(),\n                result.second["fiber_length"]);\n        setActuation(s, result.second["tendon_force"]);\n        setFiberLength(s, result.second["fiber_length"]);\n        break;\n\n    case StatusFromInitMuscleState::Failure_MaxIterationsReached:\n        // Report internal variables and throw exception.\n        std::ostringstream ss;\n        ss << "\\n  Solution error " << abs(result.second["solution_error"])\n           << " exceeds tolerance of " << tol << "\\n"\n           << "  Newton iterations reached limit of " << maxIter << "\\n"\n           << "  Activation is " << activation << "\\n"\n           << "  Fiber length is " << result.second["fiber_length"] << "\\n";\n        OPENSIM_THROW_FRMOBJ(MuscleCannotEquilibrate, ss.str());\n        break;\n    }\n}\n\nvoid Thelen2003Muscle::calcMuscleLengthInfo(const SimTK::State& s,\n                                            MuscleLengthInfo& mli) const\n{    \n    try{\n        double optFiberLength   = getOptimalFiberLength();\n        double mclLength        = getLength(s);\n        double tendonSlackLen   = getTendonSlackLength();\n\n        //Clamp the minimum fiber length to its minimum physical value.\n        mli.fiberLength  = getPennationModel().clampFiberLength(\n                                getStateVariableValue(s, STATE_FIBER_LENGTH_PATH));\n\n        mli.normFiberLength = mli.fiberLength/optFiberLength;       \n        mli.pennationAngle  = getPennationModel()\n                              .calcPennationAngle(mli.fiberLength);\n\n        mli.cosPennationAngle = cos(mli.pennationAngle);\n        mli.sinPennationAngle = sin(mli.pennationAngle);\n\n        mli.fiberLengthAlongTendon = mli.fiberLength*mli.cosPennationAngle;\n    \n        mli.tendonLength      = getPennationModel().calcTendonLength(\n                                    mli.cosPennationAngle,\n                                    mli.fiberLength,mclLength );\n        mli.normTendonLength  = mli.tendonLength / tendonSlackLen;\n        mli.tendonStrain      = mli.normTendonLength -  1.0;\n        \n\n    \n        mli.fiberPassiveForceLengthMultiplier= calcfpe(mli.normFiberLength);\n        mli.fiberActiveForceLengthMultiplier = calcfal(mli.normFiberLength);\n    }catch(const std::exception &x){\n        std::string msg = "Exception caught in Thelen2003Muscle::" \n                          "calcMuscleLengthInfo\\n"                 \n                           "of " + getName()  + "\\n"                            \n                           + x.what();\n        throw OpenSim::Exception(msg);\n    }\n}\n\nvoid Thelen2003Muscle::calcMusclePotentialEnergyInfo(const SimTK::State& s,\n        MusclePotentialEnergyInfo& mpei) const\n{\n    try {\n        // Get the quantities that we\'ve already computed.\n        const MuscleLengthInfo &mli = getMuscleLengthInfo(s);\n        mpei.fiberPotentialEnergy = calcfpefisoPE(mli.fiberLength);\n        mpei.tendonPotentialEnergy= calcfsefisoPE(mli.tendonStrain);\n        mpei.musclePotentialEnergy=  mpei.fiberPotentialEnergy \n                                    + mpei.tendonPotentialEnergy;\n    }\n    catch(const std::exception &x){\n        std::string msg = "Exception caught in Thelen2003Muscle::" \n                          "calcMusclePotentialEnergyInfo\\n"                 \n                           "of " + getName()  + "\\n"                            \n                           + x.what();\n        throw OpenSim::Exception(msg);\n    }\n}\n\n\nvoid Thelen2003Muscle::calcFiberVelocityInfo(const SimTK::State& s, \n                                               FiberVelocityInfo& fvi) const\n{\n    try{\n        //Get the quantities that we\'ve already computed\n            const MuscleLengthInfo &mli = getMuscleLengthInfo(s);\n\n        //Get the static properties of this muscle\n            // double mclLength      = getLength(s);\n            double tendonSlackLen = getTendonSlackLength();\n            double optFiberLen    = getOptimalFiberLength();\n        //=========================================================================\n        // Compute fv by inverting the force-velocity relationship in the \n        // equilibrium equations\n        //=========================================================================\n\n        //1. Get fiber/tendon kinematic information\n\n        //clamp activation to a legal range\n        double a = getActivationModel().clampActivation(getStateVariableValue(s,\n                                          STATE_ACTIVATION_PATH));\n   \n\n        double lce  = mli.fiberLength;   \n        double phi  = mli.pennationAngle;\n        double cosphi=mli.cosPennationAngle;\n        double sinphi = mli.sinPennationAngle;\n\n        //2. compute the tendon length ... because we can with this kind of model\n        double tl = mli.tendonLength;\n\n        \n        //3. Compute force multipliers and the fiber velocity\n    \n            //This exception was causing headaches, so we\'re handling the special\n            //case of a fiber that is at its minimum length instead\n                //SimTK_ERRCHK1_ALWAYS(cosphi > SimTK::Eps, fcnName.c_str(),\n                //    "%s: Pennation angle is 90 degrees, and is causing a singularity", \n                //    muscleName.c_str());\n    \n\n            //6. Invert the force velocity curve to get dlce. Check for singularities \n            //   first\n\n            //These exceptions were causing headaches, so we\'re handling the special\n            //case of a fiber that is at its minimum length instead\n                //SimTK_ERRCHK1_ALWAYS(a > SimTK::Eps, fcnName.c_str(),\n                //    "%s: Activation is 0, and is causing a singularity", \n                //    muscleName.c_str());\n                //SimTK_ERRCHK1_ALWAYS(fal > SimTK::Eps, fcnName.c_str(),\n                //    "%s: The active force length curve value is 0,"\n                //    " and is causing a singularity", \n                //    muscleName.c_str());\n    \n            //Check for singularity conditions, and clamp output appropriately\n\n        double dmcldt = getLengtheningSpeed(s);\n\n        //default values that are appropriate when fiber length has been clamped\n        //to its minimum allowable value.\n\n        double fse  = calcfse(tl/tendonSlackLen);    \n        double fal  = mli.fiberActiveForceLengthMultiplier;\n        double fpe  = mli.fiberPassiveForceLengthMultiplier;\n\n        double afalfv = ((fse/cosphi)-fpe); //we can do this without fear of\n                                              //a singularity because fiber length\n                                              //is clamped\n        double fv     = afalfv/(a*fal);\n        double dlceN  = calcdlceN(a,fal,afalfv);\n        double dlce   = dlceN*getMaxContractionVelocity()*optFiberLen;\n        double tanPhi = tan(phi);\n        double dphidt = getPennationModel().calcPennationAngularVelocity(\n                                            tanPhi,lce,dlce);\n        double dlceAT = getPennationModel().calcFiberVelocityAlongTendon(\n                            lce, dlce, sinphi, cosphi, dphidt);\n        double dtl    = getPennationModel().calcTendonVelocity(\n                            cosphi, sinphi, dphidt, lce, dlce, dmcldt);\n    \n    \n        //Switching condition: if the fiber is clamped and the tendon and the \n        //                   : fiber are out of equilibrium\n        double fiberStateClamped = 0.0;\n        if(isFiberStateClamped(s,dlceN)){\n             dlce = 0;\n             dlceAT = 0;\n             dlceN = 0;\n             dphidt = 0;\n             dtl = dmcldt;\n             fv = 1.0;\n             fiberStateClamped = 1.0;\n        }\n\n        //Populate the struct;\n        fvi.fiberVelocity               = dlce;\n        fvi.fiberVelocityAlongTendon    = dlceAT;\n        fvi.normFiberVelocity           = dlceN;\n\n        fvi.pennationAngularVelocity    = dphidt;\n\n        fvi.tendonVelocity              = dtl;\n        fvi.normTendonVelocity          = dtl/getTendonSlackLength();\n\n        fvi.fiberForceVelocityMultiplier = fv;\n\n        fvi.userDefinedVelocityExtras.resize(2);\n        fvi.userDefinedVelocityExtras[0]=fse;\n        fvi.userDefinedVelocityExtras[1]=fiberStateClamped;\n    }\n    catch(const std::exception &x){\n        std::string msg = "Exception caught in Thelen2003Muscle::" \n                            "calcFiberVelocityInfo\\n"                 \n                            "of " + getName()  + "\\n"                            \n                            + x.what();\n        throw OpenSim::Exception(msg);\n    }\n}\n\n\n//=======================================\n// computeFiberVelocityInfo helper functions\n//=======================================\n\nvoid Thelen2003Muscle::calcMuscleDynamicsInfo(const SimTK::State& s, \n                                               MuscleDynamicsInfo& mdi) const\n{\n    try {\n        //Get the quantities that we\'ve already computed\n        const MuscleLengthInfo &mli = getMuscleLengthInfo(s);\n        const FiberVelocityInfo &mvi = getFiberVelocityInfo(s);\n        //Get the static properties of this muscle\n        // double mclLength      = getLength(s);\n        double tendonSlackLen = getTendonSlackLength();\n        double optFiberLen    = getOptimalFiberLength();\n        double fiso           = getMaxIsometricForce();\n        double penHeight      = getPennationModel().getParallelogramHeight();\n\n        //=========================================================================\n        // Compute required quantities\n        //=========================================================================\n        //1. Get fiber/tendon kinematic information\n        double a = getActivationModel().clampActivation(\n                       getStateVariableValue(s, STATE_ACTIVATION_PATH) );\n\n        double lce      = mli.fiberLength;\n        double fiberStateClamped = mvi.userDefinedVelocityExtras[1];\n        double dlce     = mvi.fiberVelocity;\n        double phi      = mli.pennationAngle;\n        double cosphi   = mli.cosPennationAngle;\n        // double sinphi   = mli.sinPennationAngle;\n\n        double tl   = mli.tendonLength; \n        double dtl  = mvi.tendonVelocity;\n        // double tlN  = mli.normTendonLength;\n\n        //Default values appropriate when the fiber is clamped to its minimum length\n        //and is generating no force\n\n        //These quantities should already be set to legal values from\n        //calcFiberVelocityInfo\n        double fal  = mli.fiberActiveForceLengthMultiplier;\n        double fpe  = mli.fiberPassiveForceLengthMultiplier;\n        double fv   = mvi.fiberForceVelocityMultiplier;\n        double fse  = mvi.userDefinedVelocityExtras[0];\n    \n        double aFm          = 0; //active fiber force\n        double Fm           = 0;\n        double dFm_dlce     = 0;\n        double dFmAT_dlce   = 0;\n        double dFmAT_dlceAT = 0;    \n        double dFt_dtl      = 0; \n        double Ke           = 0;\n\n        if(fiberStateClamped < 0.5){        \n            aFm          = calcActiveFm(a,fal,fv,fiso);\n            Fm           = calcFm(a,fal,fv,fpe,fiso);\n            dFm_dlce     = calcDFmDlce(lce,a,fv,fiso,optFiberLen);\n            dFmAT_dlce   = calcDFmATDlce(lce,phi,cosphi,Fm,dFm_dlce,penHeight);\n\n            //The expression below is correct only because we are using a pennation\n            //model that has a parallelogram of constant height.\n            dFmAT_dlceAT= dFmAT_dlce*cosphi;\n\n            dFt_dtl = calcDFseDtl(tl, fiso, tendonSlackLen);\n\n            //Compute the stiffness of the whole muscle/tendon complex\n            Ke = (dFmAT_dlceAT*dFt_dtl)/(dFmAT_dlceAT+dFt_dtl);\n        }\n    \n        mdi.activation                   = a;\n        mdi.fiberForce                   = Fm; \n        mdi.fiberForceAlongTendon        = Fm*cosphi;\n        mdi.normFiberForce               = Fm/fiso;\n        mdi.activeFiberForce             = aFm;\n        mdi.passiveFiberForce            = fpe*fiso;\n                                     \n        mdi.tendonForce                  = fse*fiso;\n        mdi.normTendonForce              = fse;\n                                     \n        mdi.fiberStiffness               = dFm_dlce;\n        mdi.fiberStiffnessAlongTendon    = dFmAT_dlceAT;\n        mdi.tendonStiffness              = dFt_dtl;\n        mdi.muscleStiffness              = Ke;\n                                     \n    \n\n        //Check that the derivative of system energy less work is zero within\n        //a reasonable numerical tolerance. Throw an exception if this is not true    \n        double dFibPEdt     = fpe*fiso*dlce;\n        double dTdnPEdt     = fse*fiso*dtl;\n        double dFibWdt      = -mdi.activeFiberForce*mvi.fiberVelocity;\n        double dmcldt       = getLengtheningSpeed(s);\n        double dBoundaryWdt = mdi.tendonForce * dmcldt;\n        double ddt_KEPEmW   = dFibPEdt+dTdnPEdt-dFibWdt-dBoundaryWdt;\n        SimTK::Vector userVec(1);\n        userVec(0) = ddt_KEPEmW;  \n        mdi.userDefinedDynamicsExtras = userVec;\n\n        /////////////////////////////\n        //Populate the power entries\n        /////////////////////////////\n        mdi.fiberActivePower             = dFibWdt;\n        mdi.fiberPassivePower            = -dFibPEdt;\n        mdi.tendonPower                  = -dTdnPEdt;       \n        mdi.musclePower                  = -dBoundaryWdt;\n\n    }\n    catch(const std::exception &x) {\n        std::string msg = "Exception caught in Thelen2003Muscle::" \n                            "calcMuscleDynamicsInfo\\n"                 \n                            "of " + getName()  + "\\n"                            \n                            + x.what();\n        throw OpenSim::Exception(msg);\n    }\n   \n}\n\ndouble Thelen2003Muscle::getMinimumFiberLength() const\n{\n    return getPennationModel().getMinimumFiberLength();\n}\n\n\nbool Thelen2003Muscle::\n    isFiberStateClamped(const SimTK::State& s, double dlceN) const\n{\n    bool clamped = false;\n\n    //Is the fiber length  clamped and it is shortening, then the fiber length\n    //not valid\n    if( (getStateVariableValue(s, STATE_FIBER_LENGTH_PATH)\n            <= getMinimumFiberLength())\n        && dlceN <= 0){\n        clamped = true;\n    }\n\n    return clamped;     \n}\n\n//==============================================================================\n// ActivationFiberLength.h Interface\n//==============================================================================\n\n\n/** Get the rate change of activation */\ndouble Thelen2003Muscle::calcActivationRate(const SimTK::State& s) const \n{    \n    double excitation = getExcitation(s);\n    double activation = getActivation(s);\n    double dadt = getActivationModel().calcDerivative(activation,excitation);\n    return dadt;\n}  \n\n\n//==============================================================================\n// Numerical Guts: Initialization\n//==============================================================================\nstd::pair<Thelen2003Muscle::StatusFromInitMuscleState,\n          Thelen2003Muscle::ValuesFromInitMuscleState>\nThelen2003Muscle::initMuscleState(const SimTK::State& s,\n                                  const double aActivation,\n                                  const double aSolTolerance,\n                                  const int aMaxIterations) const\n{\n    // Using short variable names to facilitate writing out long equations\n    const double ma = aActivation;\n    const double ml = getLength(s);\n    const double dml= getLengtheningSpeed(s);\n\n    //Shorter version of the constants\n    const double tsl = getTendonSlackLength();\n    const double ofl = getOptimalFiberLength();\n    const double ophi= getPennationAngleAtOptimalFiberLength();\n    const double vol = ofl * sin(ophi);\n    const double fiso= getMaxIsometricForce();\n    const double vmax = getMaxContractionVelocity();\n\n    //Shorter version of normalized muscle multipliers\n    double fse = 0; //Normalized tendon (series element) force\n    double fal = 0; //Normalized active force length multiplier\n    double fpe = 0; //Normalized parallel element force\n    double fv  = 0; //Normalized force-velocity multiplier\n\n    //*******************************\n    //Position level\n    double tl  = getTendonSlackLength()*1.01;\n    double lce = getPennationModel().calcFiberLength(ml, tl);\n    \n    double phi    = 0.0; \n    double cosphi = 1.0; \n\n    //Normalized quantities\n    double tlN  = tl/tsl;\n    double lceN = lce/ofl;\n    //Velocity level\n    double dtl      = 0;\n    double dlce = 0; // (dml - dtl) * cos(phi);\n    double dlceN = 0; // dlce / (vmax*ofl);\n    double dphi = 0; // -(dlce / lce)*tan(phi);\n    // double dlceAT   = dlce*cosphi -vol*dphi;\n\n    //*******************************\n    //Internal variables for the loop\n    double Fm = 0;          // Muscle force\n    double FmAT=0;          // Muscle force along tendon\n    double Ft = 0;          // Tendon force\n    double ferr = SimTK::MostPositiveReal;        // Solution error\n    \n    double dFm_dlce     = 0;  // Partial derivative of muscle force w.r.t. lce\n    double dFmAT_dlce   = 0;  // Partial derivative of muscle force along \n                               // tendon w.r.t. lce\n    double dFmAT_dlceAT = 0;  // Partial derivative of muscle force along\n                               // tendon w.r.t. lce along the tendon.\n\n    double dFt_d_lce   = 0;  // Partial derivative of tendon force w.r.t. lce\n    double dFt_d_tl   = 0;   // Partial derivative of tendon force w.r.t. tl\n\n    double dferr_d_lce = 0;  // Partial derivative of the solution error w.r.t\n                             // lce\n    double delta_lce   = 0;  // Chance in lce\n\n    double Ke          = 0;  // Linearized local stiffness of the muscle\n\n    //*******************************\n    // Helper functions\n    //Update position level quantities, only if they won\'t go singular\n    auto positionFunc = [&] {\n        phi = getPennationModel().calcPennationAngle(lce);\n        cosphi = cos(phi);\n        tl = ml - lce*cosphi;\n        lceN = lce / ofl;\n        tlN = tl / tsl;\n    };\n\n    // Functional to update the force multipliers \n    auto multipliersFunc = [&] {\n        fse = calcfse(tlN);\n        fal = calcfal(lceN);\n        fpe = calcfpe(lceN);\n    };\n\n    // Functional to compute the equilibrium force error\n    auto ferrFunc = [&] {\n        Fm = (ma*fal*fv + fpe)*fiso;\n        FmAT = Fm*cosphi;\n        Ft = fse*fiso;\n        ferr = FmAT - Ft;\n    };\n\n    // Functional to compute the partial derivative of muscle force w.r.t. lce\n    auto partialsFunc = [&] {\n        dFm_dlce = calcDFmDlce(lce, ma, fv, fiso, ofl);\n        dFmAT_dlce = calcDFmATDlce(lce, phi, cosphi, Fm, dFm_dlce, vol);\n        dFmAT_dlceAT = dFmAT_dlce*cosphi;\n\n        dFt_d_tl = calcDFseDtl(tl, fiso, tsl);\n        dFt_d_lce = calcDFseDlce(tl, lce, phi, cosphi, fiso, tsl, vol);\n    };\n\n    // Functional to estimate fiber velocity and force-velocity multiplier\n    // from the relative fiber and tendon stiffnesses from above partials \n    auto velocityFunc = [&] {\n        //Update velocity level quantities: share the muscle velocity \n        //between the tendon and the fiber according to their relative \n        //stiffness:\n        //\n        //Fm = Ft at equilibrium\n        //Fm = Km*lceAT\n        //Ft = Kt*xt\n        //dFm_d_xm = Km*dlceAT + dKm_d_t*lceAT (assume dKm_d_t = 0)\n        //dFt_d_xt = Kt*dtl + dKt_d_t*dtl (assume dKt_d_t = 0)\n        //\n        //This is a heuristic. The above assumptions are necessary as \n        //computing the partial derivatives of Km or Kt w.r.t. requires \n        //acceleration level knowledge, which is not available in \n        //general.\n\n        //Stiffness of the muscle is the stiffness of the tendon and the \n        //fiber (along the tendon) in series\n\n        //the if statement here is to handle the special case when the\n        //negative stiffness of the fiber (which could happen in this\n        // model) is equal to the positive stiffness of the tendon.\n        if (abs(dFmAT_dlceAT + dFt_d_tl) > SimTK::SignificantReal\n            && tl > getTendonSlackLength()) {\n\n            //Ke = (dFmAT_dlceAT*dFt_d_tl) / (dFmAT_dlceAT + dFt_d_tl);\n            // resultant stiffness = k1/(k1+k2)\n            dtl = (dFmAT_dlceAT / (dFmAT_dlceAT + dFt_d_tl)) * dml;\n        }\n        else {\n            dtl = dml;\n        }\n\n        // Update fiber velocity\n        dlce = getPennationModel().calcFiberVelocity(cosphi, dml, dtl);\n        dlceN = dlce / (vmax*ofl);\n        // Update the force-velocity multiplier\n        fv = calcfvInv(ma, fal, dlceN, aSolTolerance, 100);\n    };\n\n    //*******************************\n    //Initialize the loop\n    int iter = 0;\n\n    // Estimate the position level quantities (lengths, angles) of the muscle\n    positionFunc();\n\n    // Multipliers based on initial fiber-length estimate\n    multipliersFunc();\n\n    // Starting guess at the force-velocity multiplier\n    fv = 1.0;\n\n    // Estimate partial derivatives of muscle forces\n    partialsFunc();\n\n    // Update the velocity (and multiplier) estimate from partials\n    velocityFunc();\n\n    // Compute the equilibrium error with velocity estimate\n    ferrFunc();\n\n    // Update the partial derivatives of the force error w.r.t. lce with \n    // newly estimated fv\n    partialsFunc();\n\n    double ferrPrev = ferr;\n    double lcePrev = lce;\n\n    double h = 1.0;\n    while( (abs(ferr) > aSolTolerance)  && (iter < aMaxIterations)) {\n        // Compute the search direction\n        dferr_d_lce = dFmAT_dlce - dFt_d_lce;\n        h = 1.0;\n\n        while (abs(ferr) >= abs(ferrPrev)) {\n             // Compute the Newton step\n            delta_lce = -h*ferrPrev / dferr_d_lce;\n            // Take a Newton Step if the step is nonzero\n            if (abs(delta_lce) > SimTK::SignificantReal)\n                lce = lcePrev + delta_lce;\n            else {\n                // We\'ve stagnated or hit a limit; assume we are hitting local\n                // minimum and attempt to approach from the other direction.\n                lce = lcePrev - sign(delta_lce)*SimTK::SqrtEps;\n                // Force a break, which will update the derivatives of\n                // the muscle force and estimate of the fiber-velocity \n                h = 0;\n            }\n\n            if (lce < getMinimumFiberLength()) {\n                lce = getMinimumFiberLength();\n            }\n\n            // Update the muscles\'s position level quantities (lengths, angles)\n            positionFunc();\n\n            // Update the muscle force multipliers\n            multipliersFunc();\n\n            // Compute the force error assuming fiber-velocity is unchanged\n            ferrFunc();\n\n            if (h <= SimTK::SqrtEps ) {\n                break;\n            }\n            else\n                h = 0.5*h;\n        }\n\n        ferrPrev = ferr;\n        lcePrev = lce;\n\n        // Update the partial derivative of the force error w.r.t. lce\n        partialsFunc();\n        // Update velocity estimate and velocity multiplier\n        velocityFunc();\n\n        iter++;\n    }\n\n    // Populate the result map.\n    ValuesFromInitMuscleState resultValues;\n\n    if (abs(ferr) < aSolTolerance) {  // The solution converged.\n\n        resultValues["solution_error"] = ferr;\n        resultValues["iterations"]     = (double)iter;\n        resultValues["fiber_length"]   = lce;\n        resultValues["passive_force"]  = fpe*fiso;\n        resultValues["tendon_force"]   = fse*fiso;\n\n        return std::pair<StatusFromInitMuscleState, ValuesFromInitMuscleState>\n            (StatusFromInitMuscleState::Success_Converged, resultValues);\n    }\n\n    // Fiber length is at or exceeds its lower bound.\n    if (lce <= getMinimumFiberLength()) {\n\n        lce = getMinimumFiberLength();\n        phi = getPennationModel().calcPennationAngle(lce);\n        cosphi = cos(phi);\n        tl  = getPennationModel().calcTendonLength(cosphi,lce,ml);\n        lceN = lce/ofl;\n        tlN  = tl/tsl;\n        fse = calcfse(tlN);\n        fpe = calcfpe(lceN);\n\n        resultValues["solution_error"] = ferr;\n        resultValues["iterations"]     = (double)iter;\n        resultValues["fiber_length"]   = lce;\n        resultValues["passive_force"]  = fpe*fiso;\n        resultValues["tendon_force"]   = fse*fiso;\n\n        return std::pair<StatusFromInitMuscleState, ValuesFromInitMuscleState>\n           (StatusFromInitMuscleState::Warning_FiberAtLowerBound, resultValues);\n    }\n\n    resultValues["solution_error"] = ferr;\n    resultValues["iterations"]     = (double)iter;\n    resultValues["fiber_length"]   = SimTK::NaN;\n    resultValues["passive_force"]  = SimTK::NaN;\n    resultValues["tendon_force"]   = SimTK::NaN;\n\n    return std::pair<StatusFromInitMuscleState, ValuesFromInitMuscleState>\n        (StatusFromInitMuscleState::Failure_MaxIterationsReached, resultValues);\n}\n\n//==============================================================================\n//\n// STIFFNESS RELATED FUNCTIONS\n//\n//==============================================================================\ndouble Thelen2003Muscle::calcFm(double ma, double fal, double fv, double fpe,\n                                 double fiso) const\n{\n    double Fm = (ma*fal*fv + fpe)*fiso;\n    return Fm;\n}\n\ndouble Thelen2003Muscle::calcActiveFm(double ma, double fal, double fv,\n                                 double fiso) const\n{\n    double aFm = (ma*fal*fv)*fiso;\n    return aFm;\n}\n\ndouble Thelen2003Muscle::calcDFmDlce(double lce, double ma, double fv, \n                                      double fiso, double ofl) const\n{\n\n            double lceN = lce/ofl;\n            double dfal_d_lceN = calcDfalDlceN(lceN);\n            double dfpe_d_lceN = calcDfpeDlceN(lceN);\n\n            double dFm_d_lce = ((ma*fv)*dfal_d_lceN + dfpe_d_lceN)*fiso\n                              *(1/ofl);                   \n            return dFm_d_lce;\n}\n\ndouble Thelen2003Muscle::calcDFmATDlce(double lce, double phi, double cosphi, \n    double Fm, double d_Fm_d_lce, double penHeight) const\n{\n    //SINGULARITY: when vol*vol/(lce*lce) = 1,same as phi=pi/2\n    double tmp1 = penHeight*penHeight;\n    double tmp2 = lce*lce;\n    double tmp3 = tmp2*lce;\n    double dcosphi_d_lce = (tmp1 /(tmp3*pow((1-(tmp1/tmp2)),0.5 ) ));\n\n\n    double d_FmAT_d_lce = d_Fm_d_lce*cosphi + Fm*dcosphi_d_lce;\n    return d_FmAT_d_lce;\n}\n\ndouble Thelen2003Muscle::calcDFseDlce(double tl, double lce, double phi, double cosphi,\n                                      double fiso, double tsl, double vol) const\n{\n    double tlN = tl/tsl;\n        //SINGULARITY: When lce = 0\n    double tmp1 = vol/lce;        \n\n    //SINGULARITY: when vol/lce = 1 - equivalent to when phi = pi/2\n    double dphi_d_lce = -vol / ( lce*lce * pow( (1-(tmp1)*(tmp1)),0.5)); \n    double dtl_d_lce  = -cos(phi) + lce*sin(phi)*dphi_d_lce;\n\n    double dfse_d_tlN  = calcDfseDtlN(tlN); \n            tmp1 = (fiso/tsl);\n    double dFt_d_lce = dfse_d_tlN*dtl_d_lce*tmp1;  \n    return dFt_d_lce;\n}\n\ndouble Thelen2003Muscle::calcDFseDtl(double tl, double fiso, double tsl) const\n{\n    double dfse_d_tlN  = calcDfseDtlN(tl/tsl);\n    double tmp1 = (fiso/tsl);\n    double dFt_d_tl= dfse_d_tlN*tmp1;\n    return dFt_d_tl;\n}\n\n\n//==============================================================================\n//\n// Convenience Method\n//\n//==============================================================================\nvoid Thelen2003Muscle::printCurveToCSVFile(const CurveType ctype, \n                                           const std::string&path) const\n{\n    std::string fname = getName();\n\n    switch(ctype){\n        case FiberActiveForceLength:{\n            \n            fname.append("_FiberActiveForceLength.csv");\n            \n            SimTK::Matrix data(100,3);\n            SimTK::Array_<std::string> colNames(data.ncol());\n            colNames[0] = "lceN";\n            colNames[1] = "fal";\n            colNames[2] = "DfalDlceN";\n\n            double delta = (2.0-0.0)/(data.nrow()-1.0);\n            double lceN = 0;\n            double lceN0 = 0;\n            for(int i=0; i<data.nrow(); i++){\n                lceN = lceN0 + i*delta;\n                data(i,0) = lceN;\n                data(i,1) = calcfal(lceN);\n                data(i,2) = calcDfalDlceN(lceN);\n            }\n\n            printMatrixToFile(data,colNames,path, fname);\n\n        }break;\n        case FiberPassiveForceLength:{\n            fname.append("_FiberPassiveForceLength.csv");\n            \n            SimTK::Matrix data(100,3);\n            SimTK::Array_<std::string> colNames(data.ncol());\n            colNames[0] = "lceN";\n            colNames[1] = "fpe";\n            colNames[2] = "DfpeDlceN";\n\n            double lceNMax = get_FmaxMuscleStrain()+1.0;\n            double lceNMin = 1.0;\n            double lceNExtra = 0.1*(lceNMax-lceNMin);\n\n            double delta = ((lceNMax + lceNExtra)-(lceNMin-lceNExtra))\n                           /(data.nrow()-1.0);\n            double lceN = 0;\n            double lceN0 = lceNMin-lceNExtra;\n\n            for(int i=0; i<data.nrow(); i++){\n                lceN = lceN0 + i*delta;\n                data(i,0) = lceN;\n                data(i,1) = calcfpe(lceN);\n                data(i,2) = calcDfpeDlceN(lceN);\n            }\n\n            printMatrixToFile(data,colNames,path, fname);\n\n        }break;\n        case FiberForceVelocity:{\n\n            fname.append("_FiberForceVelocity.csv");\n            \n            SimTK::Matrix data(1000,5);\n            SimTK::Array_<std::string> colNames(data.ncol());\n            colNames[0] = "a";\n            colNames[1] = "fal";\n            colNames[2] = "dlceN";\n            colNames[3] = "fv";\n            colNames[4] = "DfvDdlceN";\n\n            //Thelen\'s fv varies with activation\n            double a = 0;\n            double a0 = 0.1;\n            double da = 0.1;\n\n            //Thelen\'s fv varies with fiber velocity\n            double dlceNMax = 1.0;\n            double dlceNMin = -1.0;\n            double dlceNExtra = 0.1*(dlceNMax-dlceNMin);\n\n            double delta = ((dlceNMax + dlceNExtra)-(dlceNMin-dlceNExtra))\n                           /(100.0-1.0);\n            double dlceN = 0;\n            double dlceN0 = dlceNMin-dlceNExtra;\n\n            //Thelen\'s fv also varies with the value of the active force \n            //length curve. Most experiments that establish fv do so at \n            //a normalized fiber length of 1.0, where fal is 1. We will\n            //also make that assumption here\n\n            double fvInv;\n            double DdlceDaFalFv = 0;\n            double DfvDdlceN = 0;\n\n            const double fal = 1.0;\n\n            int idx = 0;\n            for(int i=0; i<=9; i++){\n                a = a0 + da*i;\n\n                for(int j=0; j<100; j++){\n                    dlceN = dlceN0 + j*delta;\n                    fvInv = calcfvInv(a, fal, dlceN, 1e-6,100);\n                    DdlceDaFalFv = calcDdlceDaFalFv(a,fal,a*fal*fvInv);\n                    DfvDdlceN = (1/(DdlceDaFalFv*a*fal));\n\n                    data(idx,0) = a;\n                    data(idx,1) = fal;\n                    data(idx,2) = dlceN;\n                    data(idx,3) = fvInv;\n                    data(idx,4) = DfvDdlceN;\n                    \n                    idx++;\n                }\n            }\n            printMatrixToFile(data,colNames,path, fname);\n\n        }break;\n        case TendonForceLength:{\n            fname.append("_TendonForceLength.csv");\n            \n            SimTK::Matrix data(100,3);\n            SimTK::Array_<std::string> colNames(data.ncol());\n            colNames[0] = "ltN";\n            colNames[1] = "fse";\n            colNames[2] = "DfseDtlN";\n\n            double ltNMax = get_FmaxTendonStrain()+1.0;\n            double ltNMin = 1.0;\n            double ltNExtra = 0.1*(ltNMax-ltNMin);\n\n            double delta = ((ltNMax + ltNExtra)-(ltNMin-ltNExtra))\n                           /(data.nrow()-1.0);\n            double ltN = 0;\n            double ltN0 = ltNMin-ltNExtra;\n\n            for(int i=0; i<data.nrow(); i++){\n                ltN = ltN0 + i*delta;\n                data(i,0) = ltN;\n                data(i,1) = calcfse(ltN);\n                data(i,2) = calcDfseDtlN(ltN);\n            }\n\n            printMatrixToFile(data,colNames,path, fname);\n\n        }break;\n        default:{\n            std::string msg = "Thelen2003Muscle::printCurveToCSVFile ";\n            msg.append(getName());\n            msg.append(" invalid curve type");\n            SimTK_ASSERT(false,msg.c_str());\n        }\n    }\n\n}\n\n/*\nThis function will print cvs file of the column vector col0 and the matrix data\n\n@params data: A matrix of data\n@params filename: The name of the file to print\n*/\nvoid Thelen2003Muscle::\n    printMatrixToFile(SimTK::Matrix& data, SimTK::Array_<std::string>& colNames,\n    const std::string& path, const std::string& filename) const\n{\n    \n    ofstream datafile;\n    std::string fullpath = path;\n    \n    if(fullpath.length() > 0)\n        fullpath.append("/");\n    \n    fullpath.append(filename);\n\n    datafile.open(fullpath.c_str(),std::ios::out);\n\n    if(!datafile){\n        datafile.close();\n        string name = getName();\n        SimTK_ERRCHK2_ALWAYS( false, \n                "Thelen2003Muscle::printMatrixToFile",\n                "%s: Failed to open the file path: %s", \n                name.c_str(),\n                fullpath.c_str());\n    }\n\n\n    for(int i = 0; i < (signed)colNames.size(); i++){\n        if(i < (signed)colNames.size()-1)\n            datafile << colNames[i] << ",";\n        else\n            datafile << colNames[i] << "\\n";\n    }\n\n    for(int i = 0; i < data.nrow(); i++){       \n        for(int j = 0; j < data.ncol(); j++){\n            if(j<data.ncol()-1)\n                datafile << data(i,j) << ",";\n            else\n                datafile << data(i,j) << "\\n";\n        }   \n    }\n    datafile.close();\n} \n\n//==============================================================================\n//\n// TENDON RELATED FUNCTIONS\n//\n//==============================================================================\n\ndouble Thelen2003Muscle::calcfse(const double tlN) const \n{\n    double x = tlN-1;\n    double e0 = get_FmaxTendonStrain();\n    \n    /*The paper reports etoe = 0.609e0, however, this is a severely rounded off\n        The exact answer, to SimTK::Eps is   \n        etoe =  99*e0*e^3 / ( 166*e^3 - 67)\n        klin =  67 /( 100*(e0 - (99*e0*e^3)/(166*e^3-67)) )\n        See thelenINIT_20120127.mw for details\n    */    \n    double kToe = 3.0;\n    double Ftoe = 33.0/100.0;\n    double t1   = exp(0.3e1);\n    double eToe = (0.99e2*e0*t1) / (0.166e3*t1 - 0.67e2);\n    t1 = exp(0.3e1);\n    double klin = (0.67e2/0.100e3) \n                * 1.0/(e0 - (0.99e2*e0*t1) / (0.166e3*t1 - 0.67e2));\n\n    //Compute tendon force\n    double fse = 0;\n    if (x > eToe){\n        fse = klin*(x-eToe)+Ftoe;\n    }else if (x>0.0){ \n        fse =(Ftoe/(exp(kToe)-1.0))*(exp(kToe*x/eToe)-1.0);\n    }else{\n        fse=0.;}\n\n    return fse;\n}\n\n\ndouble Thelen2003Muscle::calcDfseDtlN(const double tlN) const {\n    double x = tlN-1;\n    double e0 = get_FmaxTendonStrain();\n    \n    /*The paper reports etoe = 0.609e0, however, this is a severely rounded off\n    result of the exact answer:    \n        etoe =  99*e0*e^3 / ( 166*e^3 - 67)\n        See thelenINIT_20120127.mw for details\n    */\n    double kToe = 3.0;\n    double Ftoe = 33.0/100.0;\n\n    double t1   = exp(0.3e1);\n    double eToe = (0.99e2*e0*t1) / (0.166e3*t1 - 0.67e2);\n\n    /*The paper reports etoe = 0.609e0, however, this is a severely rounded off\n    result of the exact answer:    \n        klin =  67 /( 100*(e0 - (99*e0*e^3)/(166*e^3-67)) )\n        See thelenINIT_20120127.mw for details\n    */\n    t1 = exp(0.3e1);\n    double klin = (0.67e2/0.100e3) \n                * 1.0/(e0 - (0.99e2*e0*t1) / (0.166e3*t1 - 0.67e2));\n\n    //Compute tendon force\n    double dfse_d_dtlN = 0;\n    if (x > eToe){\n        dfse_d_dtlN = klin;\n    }else if (x>0.0){ \n        dfse_d_dtlN =(Ftoe/(exp(kToe)-1.0)) * (kToe/eToe) * (exp(kToe*x/eToe));\n    }else{\n        dfse_d_dtlN=0.;}\n\n    return dfse_d_dtlN;\n}\n\ndouble Thelen2003Muscle::calcfsefisoPE(double tendonStrain) const\n{\n\n    double tendon_strain =  tendonStrain;\n    double fmaxTendonStrain = get_FmaxTendonStrain();       \n\n    //Future optimization opportunity: precompute kToe, fToe, eToe and klin\n    //when the muscle is initialized. Store these values rather than \n    //computing them every time.\n\n    double kToe = 3.0;\n    double Ftoe = 33.0/100.0;\n\n    double t1   = exp(0.3e1);\n    double eToe = (0.99e2*fmaxTendonStrain*t1) / (0.166e3*t1 - 0.67e2);\n\n    t1 = exp(0.3e1);\n    double klin = (0.67e2/0.100e3) \n                * 1.0/(fmaxTendonStrain - (0.99e2*fmaxTendonStrain*t1) \n                / (0.166e3*t1 - 0.67e2));\n\n    //Compute the energy stored in the tendon. \n    //Integrals computed symbolically in muscle_kepew_20111021.mw just to check\n    double tendonPE = 0.0;\n    double lenR        = getTendonSlackLength();\n    double lenTdn    = (tendon_strain+1)*lenR;\n    double lenToe    = (eToe+1.0)*lenR;    \n    double fiso        = getMaxIsometricForce();\n\n    if (tendon_strain>eToe){\n       //compute the energy stored in the toe portion of the tendon strain curve\n        double len = lenToe;\n        double toePE_len = (fiso*Ftoe/(exp(kToe)-1.0))\n                            *((lenR*eToe/kToe)\n                            *exp(kToe*(len-lenR)/(lenR*eToe)) - len);\n        len =  lenR;\n        double toePE_0    =  (fiso*Ftoe/(exp(kToe)-1.0))\n                            *((lenR*eToe/kToe)\n                            *exp(kToe*(len-lenR)/(lenR*eToe)) - len);\n        // double toePEtest = toePE_len-toePE_0;\n\n        //compute the energy stored in the linear section of the \n        //tendon strain curve from ..... 0 to len\n        len = lenTdn;\n        double linPE_len = (1.0/2.0)*(fiso*klin*(len*len)/lenR) \n                           + fiso*len*(klin*(-1.0-eToe)+Ftoe);\n        //ditto from 0 .... eToe\n        len = lenToe;\n        double linPE_eToe= (1.0/2.0)*(fiso*klin*(len*len)/lenR) \n                            + fiso*len*(klin*(-1.0-eToe)+Ftoe);       \n        \n        //compute the total potential energy stored in the tendon\n         tendonPE =(toePE_len-toePE_0) + (linPE_len-linPE_eToe);\n    }else if (tendon_strain>0.0){ \n        //PE from 0 .... len\n        double len = lenTdn;\n        double toePE_len = (fiso*Ftoe/(exp(kToe)-1.0)) * ((lenR*eToe/kToe) \n            * exp(kToe*(len-lenR)/(lenR*eToe)) - len);\n        //PE from 0 .... eToe\n        len = lenR;\n        double toePE_0    =  (fiso*Ftoe/(exp(kToe)-1.0)) * ((lenR*eToe/kToe) \n            * exp(kToe*(len-lenR)/(lenR*eToe)) - len);\n\n        //Compute the total PE stored in the tendon\n        tendonPE = toePE_len-toePE_0;\n    }else{\n        tendonPE = 0.0;\n    }\n    \n    \n    return tendonPE;\n}\n\n//==============================================================================\n//\n// ACTIVE FORCE LENGTH FUNCTIONS\n//\n//==============================================================================\ndouble Thelen2003Muscle::calcfal(const double lceN) const{       \n    double kShapeActive = get_KshapeActive();   \n    double x=(lceN-1.)*(lceN-1.);\n    double fal = exp(-x/kShapeActive);\n    return fal;\n}\ndouble Thelen2003Muscle::calcDfalDlceN(const double lceN) const {\n    double kShapeActive = get_KshapeActive();   \n    double t1 = lceN - 0.10e1;\n    double t2 = 0.1e1 / kShapeActive;\n    double t4 = t1 * t1;\n    double t6 = exp(-t4 * t2);\n    double dfal_d_lceN = -0.2e1 * t1 * t2 * t6;\n    return dfal_d_lceN;\n}\n\n//=============================================================================\n//\n// FIBER PARALLEL ELEMENT HELPER FUNCTIONS\n//\n//=============================================================================\ndouble Thelen2003Muscle::calcfpe(const double lceN) const {\n    double fpe = 0;\n    double e0 = get_FmaxMuscleStrain();\n    double kpe = get_KshapePassive();\n\n    //Compute the passive force developed by the muscle\n    if(lceN > 1.0){\n        double t5 = exp(kpe * (lceN - 0.10e1) / e0);\n        double t7 = exp(kpe);\n        fpe = (t5 - 0.10e1) / (t7 - 0.10e1);\n    }\n    return fpe;\n}\n\ndouble Thelen2003Muscle::calcDfpeDlceN(const double lceN) const {\n    double dfpe_d_lceN = 0;\n    double e0 = get_FmaxMuscleStrain();\n    double kpe = get_KshapePassive();\n\n    if(lceN > 1.0){\n        double t1 = 0.1e1 / e0;\n        double t6 = exp(kpe * (lceN - 0.10e1) * t1);\n        double t7 = exp(kpe);\n        dfpe_d_lceN = kpe * t1 * t6 / (t7 - 0.10e1);\n    }\n    return dfpe_d_lceN;\n}\n\ndouble Thelen2003Muscle::calcfpefisoPE(double lceN) const\n{\n    double fmaxMuscleStrain = get_FmaxMuscleStrain();\n    double kShapePassive = get_KshapePassive();\n\n    double musclePE = 0.0;\n    //Compute the potential energy stored in the muscle\n    if(lceN > 1.0){\n        //Shorter variable names to make the equations readable.\n        double lenR = getOptimalFiberLength();\n        double fiso = getMaxIsometricForce();\n        double len = lceN*lenR;        \n        double kpe = kShapePassive;\n        double e0 = fmaxMuscleStrain;\n\n\n        //PE storage at current stretch\n        double fpePE_len = (fiso/(exp(kpe)-1))\n            *( (lenR*e0/kpe)*exp( (kpe/e0)*( (len/lenR)-1)) - len); \n        \n        //PE stored between 0 and 1 for the exponential function that is \n        //used to represent fpe for normalized fiber lengths > 1.\n        len = lenR;\n        double fpePE_0 = (fiso/(exp(kpe)-1))\n            *( (lenR*e0/kpe)*exp( (kpe/e0)*( (len/lenR)-1)) - len); \n\n        musclePE = fpePE_len - fpePE_0;\n\n    }else{\n        musclePE = 0.0;\n    }\n    return musclePE;\n}\n\n\n//=============================================================================\n//\n// FIBER FORCE - VELOCITY CURVE FUNCTIONS\n//\n//=============================================================================\n\ndouble Thelen2003Muscle::calcdlceN(double act,double fal,double actFalFv) const\n{\n    //The variable names have all been switched to closely match \n    //with the notation in Thelen 2003.\n    double dlceN = 0.0;      //contractile element velocity    \n    double af   = get_Af();\n\n    double a    = act;\n    double afl  = a*fal; //afl = a*fl\n    double Fm   = actFalFv;     //Fm = a*fl*fv    \n    double flen = get_Flen();\n    // double Fmlen_afl = flen*afl;\n\n    double dlcedFm = 0.0; //partial derivative of contractile element\n                          // velocity w.r.t. Fm\n\n    double b = 0;\n    double db= 0;\n\n    double Fm_asyC = 0;           //Concentric contraction asymptote\n    double Fm_asyE = afl*flen;    \n                                //Eccentric contraction asymptote\n    double asyE_thresh = get_fv_linear_extrap_threshold();\n\n    //If fv is in the appropriate region, use \n    //Thelen 2003 Eqns 6 & 7 to compute dlceN\n    if (Fm > Fm_asyC && Fm < Fm_asyE*asyE_thresh){\n\n        if( Fm <= afl ){        //Muscle is concentrically contracting\n            b = afl + Fm/af;\n            db= 1/af;\n        }else{                    //Muscle is eccentrically contracting\n            b = ((2+2/af)*(afl*flen-Fm))/(flen-1); \n            db= ((2+2/af)*(-1))/(flen-1); \n        }\n\n        dlceN = (0.25 + 0.75*a)*(Fm-afl)/b; \n        //Scaling by VMAX is left out, and is post multiplied outside \n        //of the function\n\n\n    }else{  //Linear extrapolation\n            double Fm0 = 0.0; //Last Fm value from the Thelen curve\n\n            //Compute d and db/dFm from Eqn 7. of Thelen2003\n            //for the last\n            if(Fm <= Fm_asyC){ //Concentrically contracting\n                Fm0 = Fm_asyC;\n                b = afl + Fm0/af;\n                db= 1/af;               \n            }else{             //Eccentrically contracting\n                Fm0 = asyE_thresh*Fm_asyE;\n                b = ((2+2/af)*(afl*flen-Fm0))/(flen-1); \n                db= ((2+2/af)*(-1))/(flen-1); \n            }\n\n            //Compute the last dlceN value that falls in the region where\n            //Thelen 2003 Eqn. 6 is valid\n            double dlce0 = (0.25 + 0.75*a)*(Fm0-afl)/b;\n\n            //Compute the dlceN/dfm of Eqn. 6 of Thelen 2003 at the last\n            //valid point\n            dlcedFm = (0.25 + 0.75*a)*(1)/b \n                    - ((0.25 + 0.75*a)*(Fm0-afl)/(b*b))*db;\n\n            //Linearly extrapolate Eqn. 6 from Thelen 2003 to compute\n            //the new value for dlceN/dFm\n            dlceN = dlce0 + dlcedFm*(Fm-Fm0);            \n        }\n            \n        return dlceN;\n}\n\ndouble Thelen2003Muscle::\n    calcfv(double aFse,     double aFpe, double aFal, \n           double aCosPhi,  double aAct) const\n{\n    //This only works for an equilibrium model, but its a lot less \n    //computationally expensive (and error prone) than trying to invert the \n    //weird function that defines the fv curve in the Thelen model\n    double fv = ((aFse/aCosPhi) - aFpe)/(aAct*aFal);\n    return fv;\n}\n\ndouble Thelen2003Muscle::calcDdlceDaFalFv(double aAct, \n                                          double aFal, double aFalFv) const\n{\n    //The variable names have all been switched to closely match with \n    //the notation in Thelen 2003.\n    // double dlceN = 0.0;      //contractile element velocity    \n    double af   = get_Af();\n\n    double a    = aAct;\n    double afl  = aAct*aFal;  //afl = a*fl\n    double Fm   = aFalFv;    //Fm = a*fl*fv    \n    double flen = get_Flen();\n    // double Fmlen_afl = flen*aAct*aFal;\n\n    double dlcedFm = 0.0; //partial derivative of contractile element \n                          //velocity w.r.t. Fm\n\n    double b = 0;\n    double db= 0;\n\n    double Fm_asyC = 0;           //Concentric contraction asymptote\n    double Fm_asyE = aAct*aFal*flen;    \n                                //Eccentric contraction asymptote\n    double asyE_thresh = get_fv_linear_extrap_threshold();\n\n    //If fv is in the appropriate region, use \n    //Thelen 2003 Eqns 6 & 7 to compute dlceN\n    if (Fm > Fm_asyC && Fm < Fm_asyE*asyE_thresh){\n\n        if( Fm <= afl ){        //Muscle is concentrically contracting\n            b = afl + Fm/af;\n            db= 1/af;\n        }else{                    //Muscle is eccentrically contracting\n            b = ((2+2/af)*(afl*flen-Fm))/(flen-1); \n            db= ((2+2/af)*(-1))/(flen-1); \n        }\n\n        //This variable may have future use outside this function\n        dlcedFm = (0.25 + 0.75*a)*(1)/b - ((0.25 + 0.75*a)*(Fm-afl)/(b*b))*db;            \n\n    }else{  //Linear extrapolation\n            double Fm0 = 0.0; //Last Fm value from the Thelen curve\n\n            //Compute d and db/dFm from Eqn 7. of Thelen2003\n            //for the last\n            if(Fm <= Fm_asyC){ //Concentrically contracting\n                Fm0 = Fm_asyC;\n                b = afl + Fm0/af;\n                db= 1/af;               \n            }else{             //Eccentrically contracting\n                Fm0 = asyE_thresh*Fm_asyE;\n                b = ((2+2/af)*(afl*flen-Fm0))/(flen-1); \n                db= ((2+2/af)*(-1))/(flen-1); \n            }\n\n            \n            //Compute the dlceN/dfm of Eqn. 6 of Thelen 2003 at the last\n            //valid point\n            dlcedFm = (0.25 + 0.75*a)*(1)/b \n                - ((0.25 + 0.75*a)*(Fm0-afl)/(b*b))*db;\n          \n        }\n            \n        return dlcedFm;\n}\n\n// Compute the force-velocity multiplier by inverting Thelen 2003\' f-v\n// equations for fiber-velocity given the active fiber force (see calcdlceN()).\n// This is here because it is non-trivial to correctly invert the piece-wise\n// continuous force velocity curve specified by the modified Thelen2003Muscle\n// force velocity curve. This converges quickly and is well tested.\ndouble Thelen2003Muscle::\n        calcfvInv(double aAct,double aFal,double dlceN,\n                  double tolerance, int maxIterations) const\n{\n    double result = SimTK::NaN;\n    double ferr=1;\n    double iter= 0;\n\n    double dlceN1 = 0;\n    double dlceN1_d_Fm = 0;\n    double fv = 1;\n    double aFalFv = fv*aAct*aFal;\n    double delta_aFalFv = 0;\n\n    while(abs(ferr) >= tolerance && iter < maxIterations)\n    {\n        dlceN1 = calcdlceN(aAct,aFal, aFalFv);\n        ferr   = dlceN1-dlceN;\n        dlceN1_d_Fm = calcDdlceDaFalFv(aAct,aFal,aFalFv);\n\n\n        if(abs(dlceN1_d_Fm) > SimTK::SignificantReal){\n           delta_aFalFv = -ferr/(dlceN1_d_Fm);\n           aFalFv = aFalFv + delta_aFalFv;\n        }\n        iter = iter+1;\n    }\n\n    if(abs(ferr) < tolerance){\n        result = max(0.0, aFalFv/(aAct*aFal));\n        return result;\n    }\n\n    OPENSIM_THROW_FRMOBJ(Exception, \n        "Solver for force-velocity multiplier failed to converge.");\n}\n', 'message': 'Added missing initialization to constructProperties().\n', 'author_name': 'Tom Uchida', 'lang': 'cpp'}