Fatigable Muscle Code

    class FatigableMuscle : public Millard2012EquilibriumMuscle {
        OpenSim_DECLARE_CONCRETE_OBJECT(FatigableMuscle, Millard2012EquilibriumMuscle);

OpenSim_DECLARE_PROPERTY(fatigue_factor, double, "percentage of active motor units that fatigue in unit time");

OpenSim_DECLARE_PROPERTY(recovery_factor, double, "percentage of fatigued motor units that recover in unit time");

OpenSim_DECLARE_PROPERTY(fatigue_factor_slow, double, "percentage of active motor units that fatigue in unit time for a slow-twitch muscle");

OpenSim_DECLARE_PROPERTY(recovery_factor_slow, double, "percentage of fatigued motor units that recover in unit time for a slow-twitch muscle");

OpenSim_DECLARE_PROPERTY(fatigue_factor_fast, double, "percentage of active motor units that fatigue in unit time for a fast-twitch muscle");

OpenSim_DECLARE_PROPERTY(recovery_factor_fast, double, "percentage of fatigued motor units that recover in unit time for a fast-twitch muscle");

OpenSim_DECLARE_PROPERTY(recruitment_from_resting_time_constant, double, "constant that scales the rate of   recruitment from resting motor units to active motor units");         

OpenSim_DECLARE_PROPERTY(default_active_motor_units, double, "default state value for the fraction of motor units that are active");

OpenSim_DECLARE_PROPERTY(default_fatigued_motor_units, double, "default state value for the fraction of motor units that are fatigued");

OpenSim_DECLARE_PROPERTY(default_resting_motor_units, double, "default state value for the fraction of motor units that are resting");
        
OpenSim_DECLARE_PROPERTY(percent_slowTwitch, double, "percent of total muscle PCSA made up of slow-twitch muscle fibers Type Ia");

OpenSim_DECLARE_PROPERTY(specificTension_slowTwitch, double, "specific tension of slow-twitch fibers in N/m^2");

OpenSim_DECLARE_PROPERTY(specificTension_fastTwitch, double, "specific tension of fast-twitch fibers in N/m^2");

OpenSim_DECLARE_PROPERTY(musclePCSA, double, "muscle PCSA = Volume / l_opt *cos alpha");
        
OpenSim_DECLARE_PROPERTY(use_orderly_recruitment_curve, bool, "boolean specificy whether to use orderly recruitment activation factor curve");

/*
 * Construct and initialize properties.
 * All properties are added to the property set. Once added, they can be
 * read in and written to files.
 */
void FatigableMuscle::constructProperties()
{
    setAuthors("Ajay Seth");
    constructProperty_fatigue_factor(0.0);
    constructProperty_recovery_factor(0.0);
    constructProperty_fatigue_factor_slow(0.0026);
    constructProperty_fatigue_factor_fast(0.0075);
    constructProperty_recovery_factor_slow(0.0125);
    constructProperty_recovery_factor_fast(0.022);
    constructProperty_recruitment_from_resting_time_constant(10.0);
    void setRecruitmentFromRestingTimeConstant(double aRecruitmentFromRestingTimeConstant);
    constructProperty_default_active_motor_units(0.0);
    constructProperty_default_fatigued_motor_units(0.0);
    constructProperty_default_resting_motor_units(1.0);
    constructProperty_percent_slowTwitch(0.5);
    constructProperty_specificTension_slowTwitch(30.0e4);
    constructProperty_specificTension_fastTwitch(30.0e4);
    constructProperty_musclePCSA(10.0e-4);
    constructProperty_use_orderly_recruitment_curve(true);
}

// Default constructor
FatigableMuscle::FatigableMuscle()
{
    constructProperties();
}

//_____________________________________________________________________________
/*
 * Constructor.
 */
FatigableMuscle::FatigableMuscle(const std::string &name, double PCSA, double percentSlow,
                                 double optimalFiberLength, double tendonSlackLength, double pennationAngle) :
Super(name, 1.0, optimalFiberLength, tendonSlackLength, pennationAngle)
{
    constructProperties();
    setMusclePCSA(PCSA);
    setPercentSlowTwitch(percentSlow);
    
    // update max isometric force based on PCSA and default values for specific tension slow/fast
    double percentFast = 1.0 - percentSlow;
    double maxIsometricForce = PCSA * (percentSlow * getSpecificTensionSlowTwitch() + percentFast * getSpecificTensionFastTwitch());
    setMaxIsometricForce(maxIsometricForce);
    
    // set fatigue and recovery factors
    double fatigueFactor = getFatigueFactorSlow() * percentSlow + getFatigueFactorFast() * percentFast;
    double recoveryFactor = getRecoveryFactorSlow() * percentSlow + getRecoveryFactorFast() * percentFast;
    setFatigueFactor(fatigueFactor);
    setRecoveryFactor(recoveryFactor);
}

//_____________________________________________________________________________
/*
 * Constructor.
 */
FatigableMuscle::FatigableMuscle(const std::string &name, double PCSA, double percentSlow,
                double optimalFiberLength, double tendonSlackLength, double pennationAngle,
                double fatigueFactor, double recoveryFactor) :
Super(name, 1.0, optimalFiberLength, tendonSlackLength, pennationAngle)
{
    constructProperties();
    setMusclePCSA(PCSA);
    setPercentSlowTwitch(percentSlow);
    setFatigueFactor(fatigueFactor);
    setRecoveryFactor(recoveryFactor);
    
    // update max isometric force based on PCSA and default values for specific tension slow/fast
    double percentFast = 1.0 - percentSlow;
    double maxIsometricForce = PCSA * (percentSlow * getSpecificTensionSlowTwitch() + percentFast * getSpecificTensionFastTwitch());
    setMaxIsometricForce(maxIsometricForce);
}

//_____________________________________________________________________________
/*
 * Constructor.
 */
FatigableMuscle::FatigableMuscle(const std::string &name, double PCSA, double percentSlow,
                                 double optimalFiberLength, double tendonSlackLength, double pennationAngle,
                                 double fatigueFactor, double recoveryFactor, double recruitmentTimeConstant) :
Super(name, 1.0, optimalFiberLength, tendonSlackLength, pennationAngle)
{
    constructProperties();
    setMusclePCSA(PCSA);
    setPercentSlowTwitch(percentSlow);
    setFatigueFactor(fatigueFactor);
    setRecoveryFactor(recoveryFactor);
    setRecruitmentFromRestingTimeConstant(recruitmentTimeConstant);
    
    // update max isometric force based on PCSA and default values for specific tension slow/fast
    double percentFast = 1.0 - percentSlow;
    double maxIsometricForce = PCSA * (percentSlow * getSpecificTensionSlowTwitch() + percentFast * getSpecificTensionFastTwitch());
    setMaxIsometricForce(maxIsometricForce);
}

// Define new states and their derivatives in the underlying system
void FatigableMuscle::addToSystem(SimTK::MultibodySystem& system) const
{
    // Allow Millard2012EquilibriumMuscle to add its states, cache, etc.
    // to the system
    Super::addToSystem(system);
    
    // Now add the states necessary to implement the fatigable behavior
    addStateVariable("target_activation");
    addStateVariable("active_motor_units");
    addStateVariable("fatigued_motor_units");
    addStateVariable("resting_motor_units");
    // and their corresponding dervivatives
    addCacheVariable("target_activation_deriv", 0.0, SimTK::Stage::Dynamics);
    addCacheVariable("active_motor_units_deriv", 0.0, SimTK::Stage::Dynamics);
    addCacheVariable("fatigued_motor_units_deriv", 0.0, SimTK::Stage::Dynamics);
    addCacheVariable("resting_motor_units_deriv", 0.0, SimTK::Stage::Dynamics);
}

//_____________________________________________________________________________
/**
 * Compute the function c(t) which represents the recruitment of resting motor units to active motor units.
 *
 * @param s  system state
 */

double FatigableMuscle::calculateRecruitmentOfResting(const SimTK::State& s) const
{
    double recruitmentOfResting;
    double excitation = getExcitation(s);
    double activation = getTargetActivation(s);
    double activeMotorUnits = getActiveMotorUnits(s);
    double restingMotorUnits = calculateRestingMotorUnits(s);
    // double restingMotorUnits = getRestingMotorUnits(s);
    if ((excitation >= activation) && ((excitation - activeMotorUnits) < restingMotorUnits))
    {
        recruitmentOfResting = excitation - activeMotorUnits;
    }
    else if ((excitation >= activation) && ((excitation - activeMotorUnits) >= restingMotorUnits))
    {
        recruitmentOfResting = restingMotorUnits;
    }
    else
    {
        recruitmentOfResting = excitation - activeMotorUnits;
    }
    return recruitmentOfResting;
}

//_____________________________________________________________________________
/**
 * Compute the derivatives of the muscle states.
 *
 * @param s  system state
 */
SimTK::Vector FatigableMuscle::
computeStateVariableDerivatives(const SimTK::State& s) const
{
    // vector of the derivatives to be returned
    SimTK::Vector derivs(getNumStateVariables(), SimTK::NaN);
    int nd = derivs.size();
    
    SimTK_ASSERT1(nd == 6, "FatigableMuscle: Expected 5 state variables"
                  " but encountered  %f.", nd);
    
    /* ----- MODIFIED SIR MODEL WITH TIME CONSTANT IN RECRUITMENT CURVE C(T)----*/
    // compute the rates at which motor units are converted to/from active
    // and fatigued states based on an SIR model
    double activeMotorUnitsDeriv = getRestingMotorUnits(s)*(calculateRecruitmentOfResting(s)/getRecruitmentFromRestingTimeConstant() - getFatigueFactor()*getActiveMotorUnits(s));
    
    double fatigueMotorUnitsDeriv = getFatigueFactor()*getActiveMotorUnits(s)*getRestingMotorUnits(s) - getRecoveryFactor()*getFatiguedMotorUnits(s);
    
    double restingMotorUnitsDeriv = getRecoveryFactor()*getFatiguedMotorUnits(s) - getRestingMotorUnits(s)*calculateRecruitmentOfResting(s)/getRecruitmentFromRestingTimeConstant();
    /*----------------------------------------------------------------------------*/
    
    
    //Compute the target activation rate based on the given activation model
    const MuscleFirstOrderActivationDynamicModel& actMdl
    = get_MuscleFirstOrderActivationDynamicModel();
    
    double excitation = getExcitation(s);
    // use the activation dynamics model to calculate the target activation
    double targetActivation = actMdl.clampActivation(getTargetActivation(s));
    double targetActivationRate = actMdl.calcDerivative(targetActivation, excitation);
    
    // specify the activation derivative based on the amount of active motor
    // units and the rate at which motor units are becoming active.
    // we assume that the actual activation = Ma*a       then,
    // activationRate = dMa/dt*a + Ma*da/dt  where a is the target activation
    double activationRate = calculateActivationFactorDeriv(s) * targetActivationRate * (1.0 - getFatiguedMotorUnits(s)) 
        - calculateActivationFactor(s)*fatigueMotorUnitsDeriv;
    
    /*
    // COMPLIANT TENDON
    // set the actual activation rate of the muscle to the fatigued one
    derivs[0] = activationRate;
    // fiber length derivative
    derivs[1] = getFiberVelocity(s);
    // the target activation derivative
    derivs[2] = targetActivationRate;
    derivs[3] = activeMotorUnitsDeriv;
    derivs[4] = fatigueMotorUnitsDeriv;
    derivs[5] = restingMotorUnitsDeriv;
     */
    
    // RIGID TENDON (fiber length is no longer a state)
    // set the actual activation rate of the muscle to the fatigued one
    derivs[0] = activationRate;
    // fiber length derivative
//    derivs[1] = getFiberVelocity(s);
    // the target activation derivative
    derivs[1] = targetActivationRate;
    derivs[2] = activeMotorUnitsDeriv;
    derivs[3] = fatigueMotorUnitsDeriv;
    derivs[4] = restingMotorUnitsDeriv;
    
    // cache the results for fast access by reporting, etc...
    setTargetActivationDeriv(s, targetActivationRate);
    setActiveMotorUnitsDeriv(s, activeMotorUnitsDeriv);
    setFatiguedMotorUnitsDeriv(s, fatigueMotorUnitsDeriv);
    setRestingMotorUnitsDeriv(s, restingMotorUnitsDeriv);
    
    return derivs;
}

//_____________________________________________________________________________
/**
 * Compute the cubic spline parameters for our activation factor.
 *
 * @param s  system state
 */

SimTK::Vector FatigableMuscle::calculateSplineParametersForActivation() const
{
    SimTK::Vector splineParameters(8, SimTK::NaN);
    double percentSlowTwitch = getPercentSlowTwitch();
    double specificTensionSlow = getSpecificTensionSlowTwitch();
    double PCSA = getMusclePCSA();
    double maxForce = getMaxIsometricForce();
    double percentMaxForceSlow = (PCSA * percentSlowTwitch * specificTensionSlow)/maxForce;
    double x = percentSlowTwitch;
    double y = percentMaxForceSlow;
    
    splineParameters[0] = 0.0;
    splineParameters[1] = 0.0;
    splineParameters[2] = 1.5*(pow(x,2.0)*(-1.0+2.0*x-pow(x,2.0)) - y*pow(x,2.0)*(-1.0+2.0*x - pow(x,2.0)) - y*(-3.0+3.0*x-(-2.0+x)*(1.0-pow(x,3.0))))/(pow(x,2.0)*(1.0+3.0*pow(x,2.0)-3.0*x-pow(x,3.0)));
    splineParameters[3] = y*(1.0+1.5*(-1.0+pow(x,2.0))*(1.0+pow(x,2.0)))/(pow(x,3.0)*(1.0+3.0*pow(x,2.0)-3.0*x-pow(x,3.0))) + (1.5+1.5*pow(x,2.0)-3.0*x-y*(-1.5+x+1.5*pow(x,2.0)))/(x*(1.0+3.0*pow(x,2.0)-3.0*x-pow(x,3.0)));
    splineParameters[4] = 0.5*(3.0*pow(x,2.0) - y - 2.0*pow(x,3.0))/(1.0+3.0*pow(x,2.0) - 3.0*x - pow(x,3.0));
    splineParameters[5] = 1.5*(y+2.0*pow(x,2.0)*(-1.5+x))/(x*(1.0+3.0*pow(x,2.0)-3.0*x-pow(x,3.0)));
    splineParameters[6] = 1.5*(x*y - 2.0*y - x*(-2.0+pow(x,2.0)))/(x*(1.0+3.0*pow(x,2.0)-3.0*x-pow(x,3.0)));
    splineParameters[7] = -(x*y - 1.5*y - 1.5*x*(-4.0/3.0+x))/(x*(1.0+3.0*pow(x,2.0)-3.0*x-pow(x,3.0)));
    return splineParameters;
}


//_____________________________________________________________________________
/**
 * Compute the function f(a) which represents activation factor for orderly recruitment.
 *
 * @param s  system state
 */
 
double FatigableMuscle::calculateActivationFactor(const SimTK::State& s) const
{
    double activation = getTargetActivation(s);
    SimTK::Vector splineParameters = calculateSplineParametersForActivation();
    double a0 = splineParameters[0];
    double a1 = splineParameters[1];
    double a2 = splineParameters[2];
    double a3 = splineParameters[3];
    double b0 = splineParameters[4];
    double b1 = splineParameters[5];
    double b2 = splineParameters[6];
    double b3 = splineParameters[7];
    
    double percentSlowTwitch = getPercentSlowTwitch();
    
    double activationFactor;

    if (activation < percentSlowTwitch)
    {
        activationFactor = a0 + a1*activation + a2*pow(activation,2.0) + a3*pow(activation,3.0);
        if (activationFactor > 1.0)
        {
            activationFactor = 1.0;
        }
        else if (activationFactor < 0.0)
        {
            activationFactor = 0.0;
        }
    }
    else
    {
        activationFactor = b0 + b1*activation + b2*pow(activation,2.0) + b3*pow(activation,3.0);
        if (activationFactor > 1.0)
        {
            activationFactor = 1.0;
        }
        else if (activationFactor < 0.0)
        {
            activationFactor = 0.0;
        }
    }
    return activationFactor;
}

/* Determine the initial state values based on initial fiber equlibrium. */
void FatigableMuscle::computeInitialFiberEquilibrium(SimTK::State& s) const
{
    // initialize th target activation to be the actual.
    setTargetActivation(s, getActivation(s));
    // assume that all motor units can be activated initially and there is
    // no appreciable fatigue
    setActiveMotorUnits(s, 0.1);
    setFatiguedMotorUnits(s, 0.0);
    setRestingMotorUnits(s, 0.9);
    
    // Compute the fiber & tendon lengths according to the parent Muscle 
    Super::computeInitialFiberEquilibrium(s);
}

    class OSIMPLUGIN_API FatigableMuscle : public Millard2012EquilibriumMuscle {
        OpenSim_DECLARE_CONCRETE_OBJECT(FatigableMuscle,
                                        Millard2012EquilibriumMuscle);

OSIMPLUGIN_API void RegisterTypes_osimPlugin()
{
    Object::RegisterType( FatigableMuscle() );
}

	int objectiveFunc(  const Vector& FRTParameters, bool new_coefficients, Real& f ) const {
        
        Set<Muscle>& muscles = osimModel.updMuscles();
        FatigableMuscle& fatigableMuscle = dynamic_cast<FatigableMuscle&>(muscles.get("fatigable"));
        fatigableMuscle.setFatigueFactor( FRTParameters[0] );
        fatigableMuscle.setRecoveryFactor( FRTParameters[1] );
        fatigableMuscle.setRecruitmentFromRestingTimeConstant( FRTParameters[2] );

	/* Calculate the scalar quantity we want to minimize or maximize.  */
        // Get the simulated storage object
	Storage simulatedForces = reporter->getForceStorage();
        
        // Load the experimental data storage object (force column is really percent max force)
	// REPLACE THIS .sto FILE NAME WITH YOUR OWN DATA FILE
        Storage experimentalForces = Storage("untrainedThumbForce.sto");
        
        // Get force data from each storage object
        Array<double> simulatedFatigableForce, experimentalFatigableForce;
        simulatedForces.getDataColumn("fatigable", simulatedFatigableForce);
        experimentalForces.getDataColumn("force", experimentalFatigableForce);
        
        // Get time data from each storage object
        Array<double> timeSimulatedForce, timeExperimentalForce;
        simulatedForces.getTimeColumn(timeSimulatedForce);
        experimentalForces.getTimeColumn(timeExperimentalForce);
        
        // normalize simulated force data to max isometric force for comparison w/experimental
        Array<double> simulatedFatigablePercentMaxIsometricForce = simulatedFatigableForce;
        double maxIsometricForce = fatigableMuscle.getMaxIsometricForce();
        
        for (int i = 0; i < timeSimulatedForce.getSize(); i++) {
            simulatedFatigablePercentMaxIsometricForce[i] = 100.0*simulatedFatigableForce[i]/maxIsometricForce;
        }
        
        // get start and end time and indices for simulated data
        double startTime = timeSimulatedForce.get(0);
        double endTime = timeSimulatedForce.getLast();
        int startIndexSim = timeSimulatedForce.findIndex(startTime);
        int endIndexSim = timeSimulatedForce.findIndex(endTime);
        
        // create a spline of the experimental data
        GCVSpline splineOfExpData(3, timeExperimentalForce.getSize(), &timeExperimentalForce[0], &experimentalFatigableForce[0]);
        
        // calculate the RMS value
        double rmsError = 0.0;
                
        for (int i = startIndexSim; i < timeSimulatedForce.getSize(); i++) {
            SimTK::Vector inputTime(1, timeSimulatedForce[i]);
            rmsError += pow( (simulatedFatigablePercentMaxIsometricForce[i] - splineOfExpData.calcValue(inputTime)), 2.0 );
        }
        
        rmsError = sqrt(rmsError/timeSimulatedForce.getSize());

    // Create two new fatiguable muscles
    double musclePCSA = 10.0e-4;
    double percentSlowTwitch = 0.20;
    double optimalFiberLength = 0.20;
    double tendonSlackLength = 0.10;
    double pennationAngle = 0.0;
    double fatigueFactor = 2.0;
    double recoveryFactor = 0.3;
    double timeConstant = 10.0;
    
    // fatigable muscle (Millard2012EquilibriumMuscle with fatigue)
    FatigableMuscle* fatigable = new FatigableMuscle("fatigable", musclePCSA, percentSlowTwitch, optimalFiberLength, tendonSlackLength, pennationAngle, fatigueFactor, recoveryFactor, timeConstant);
    
    // muscle without fatigue (fatigable muscle model with fatigue factor, recovery factor set to 0)
    FatigableMuscle* original = new FatigableMuscle("original", musclePCSA, percentSlowTwitch, optimalFiberLength, tendonSlackLength, pennationAngle, 0.0, 0.0, 0.0);