[WIP] Add more thorough DGF equilibrium test.

Moco/Moco/Components/DeGrooteFregly2016Muscle.h

@@ -94,9 +94,23 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle {
     OpenSim_DECLARE_PROPERTY(default_activation, double,
             "Value of activation in the default state returned by "
             "initSystem().");
+    OpenSim_DECLARE_PROPERTY(clamp_activation, bool,
+            "Clamp activations between the value set by the "
+            "'minimum_activation' property and 1.");
+    OpenSim_DECLARE_PROPERTY(minimum_activation, double, 
+            "The enforced activation lower bound if 'clamp_activation' is set "
+            "to true. Default: 0.");
     OpenSim_DECLARE_PROPERTY(default_normalized_tendon_force, double,
             "Value of normalized tendon force in the default state returned by "
             "initSystem().");
+    OpenSim_DECLARE_PROPERTY(clamp_normalized_tendon_length, bool,
+            "Clamp normalized tendon length between the value set by the "
+            "'minimum_normalized_tendon_length' and "
+            "'maximum_normalized_tendon_length' properties.");
+    OpenSim_DECLARE_PROPERTY(minimum_normalized_tendon_length, double, 
+            "Normalized tendon length lower bound. Default: 1.");
+    OpenSim_DECLARE_PROPERTY(maximum_normalized_tendon_length, double,
+            "Normalized tendon length upper bound. Default: 2.5.");
     OpenSim_DECLARE_PROPERTY(active_force_width_scale, double,
             "Scale factor for the width of the active force-length curve. "
             "Larger values make the curve wider. Default: 1.0.");
@@ -261,10 +275,13 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle {
 
     /// The residual (i.e. error) in the muscle-tendon equilibrium equation:
     ///         residual = tendonForce - fiberForce * cosPennationAngle
+    /// This is always computed using implicit form of the model since the 
+    /// explicit form will produce a zero residual for any guess of normalized 
+    /// tendon force.
     double getEquilibriumResidual(const SimTK::State& s) const {
-        const auto& mdi = getMuscleDynamicsInfo(s);
-        return calcEquilibriumResidual(
-                mdi.tendonForce, mdi.fiberForceAlongTendon);
+        return calcEquilibriumResidual(getLength(s), getLengtheningSpeed(s),
+                getActivation(s), getNormalizedTendonForce(s),
+                getNormalizedTendonForceDerivative(s));
     }
 
     /// The residual (i.e. error) in the time derivative of the linearized
@@ -274,13 +291,10 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle {
     ///                    (muscleTendonVelocity - fiberVelocityAlongTendon)
     double getLinearizedEquilibriumResidualDerivative(
             const SimTK::State& s) const {
-        const auto& muscleTendonVelocity = getLengtheningSpeed(s);
-        const FiberVelocityInfo& fvi = getFiberVelocityInfo(s);
-        const MuscleDynamicsInfo& mdi = getMuscleDynamicsInfo(s);
-
-        return calcLinearizedEquilibriumResidualDerivative(muscleTendonVelocity,
-                fvi.fiberVelocityAlongTendon, mdi.tendonStiffness,
-                mdi.fiberStiffnessAlongTendon);
+        return calcLinearizedEquilibriumResidualDerivative(getLength(s),
+                getLengtheningSpeed(s), getActivation(s),
+                getNormalizedTendonForce(s),
+                getNormalizedTendonForceDerivative(s));
     }
 
     static std::string getActivationStateName() {
@@ -578,7 +592,7 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle {
         // The stiffness of the fiber along the direction of the tendon. For
         // small changes in length parallel to the fiber, this quantity is
         // d_fiberForceAlongTendon / d_fiberLength =
-        //      d/d_fiberLength(fiberForce * cosPenneationAngle)
+        //      d/d_fiberLength(fiberForce * cosPennationAngle)
         return fiberStiffness * cosPennationAngle +
                fiberForce * partialCosPennationAnglePartialFiberLength;
     }
@@ -635,22 +649,47 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle {
     }
 
     /// @copydoc getEquilibriumResidual()
-    SimTK::Real calcEquilibriumResidual(const SimTK::Real& tendonForce,
-            const SimTK::Real& fiberForceAlongTendon) const {
+    SimTK::Real calcEquilibriumResidual(const SimTK::Real& muscleTendonLength,
+            const SimTK::Real& muscleTendonVelocity, 
+            const SimTK::Real& activation, 
+            const SimTK::Real& normTendonForce,
+            const SimTK::Real& normTendonForceDerivative) const {
+
+        MuscleLengthInfo mli;
+        FiberVelocityInfo fvi;
+        MuscleDynamicsInfo mdi;
+        calcMuscleLengthInfoHelper(
+                muscleTendonLength, false, mli, normTendonForce);
+        calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, false,
+                false, mli, fvi, normTendonForce, 
+                normTendonForceDerivative);
+        calcMuscleDynamicsInfoHelper(activation, muscleTendonVelocity, false,
+                mli, fvi, mdi, normTendonForce);
 
-        return tendonForce - fiberForceAlongTendon;
+        return mdi.tendonForce - mdi.fiberForceAlongTendon;
     }
 
     /// @copydoc getLinearizedEquilibriumResidualDerivative()
     SimTK::Real calcLinearizedEquilibriumResidualDerivative(
-            const SimTK::Real muscleTendonVelocity,
-            const SimTK::Real& fiberVelocityAlongTendon,
-            const SimTK::Real& tendonStiffness,
-            const SimTK::Real& fiberStiffnessAlongTendon) const {
-
-        return fiberStiffnessAlongTendon * fiberVelocityAlongTendon -
-               tendonStiffness *
-                       (muscleTendonVelocity - fiberVelocityAlongTendon);
+            const SimTK::Real& muscleTendonLength,
+            const SimTK::Real& muscleTendonVelocity,
+            const SimTK::Real& activation, const SimTK::Real& normTendonForce,
+            const SimTK::Real& normTendonForceDerivative) const {
+
+        MuscleLengthInfo mli;
+        FiberVelocityInfo fvi;
+        MuscleDynamicsInfo mdi;
+        calcMuscleLengthInfoHelper(
+                muscleTendonLength, false, mli, normTendonForce);
+        calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, false,
+                m_isTendonDynamicsExplicit, mli, fvi, normTendonForce, 
+                normTendonForceDerivative);
+        calcMuscleDynamicsInfoHelper(activation, muscleTendonVelocity, false,
+                mli, fvi, mdi, normTendonForce);
+
+        return mdi.fiberStiffnessAlongTendon * fvi.fiberVelocityAlongTendon -
+               mdi.tendonStiffness *
+                       (muscleTendonVelocity - fvi.fiberVelocityAlongTendon);
     }
     /// @}
 

Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.cpp

@@ -17,45 +17,45 @@
  * -------------------------------------------------------------------------- */
 
 #include "MocoInitialForceEquilibriumGoal.h"
-#include "../Components/DeGrooteFregly2016Muscle.h"
 
 using namespace OpenSim;
 
 void MocoInitialForceEquilibriumGoal::initializeOnModelImpl(
     const Model& model) const {
 
-    for (const auto& muscle : model.getComponentList<Muscle>()) {
-        if (!muscle.get_ignore_tendon_compliance()) {
-            if (const auto dgfmuscle =
-                    dynamic_cast<const DeGrooteFregly2016Muscle*>(&muscle)) {
-                if (dgfmuscle->get_tendon_compliance_dynamics_mode() 
-                        == "explicit") {
-                    m_muscleRefs.emplace_back(&muscle);
-                }
-            } else {
-                    m_muscleRefs.emplace_back(&muscle);
+    for (const auto& dgfmuscle :
+            model.getComponentList<DeGrooteFregly2016Muscle>()) {
+        if (!dgfmuscle.get_ignore_tendon_compliance() && 
+             dgfmuscle.get_tendon_compliance_dynamics_mode() == "explicit") {
+                m_muscleRefs.emplace_back(dgfmuscle);
             }
         }
-    }
 
     setRequirements(0, (int)m_muscleRefs.size());
 }
 
+
+
 void MocoInitialForceEquilibriumGoal::calcGoalImpl(
     const GoalInput& input, SimTK::Vector& goal) const {
     const auto& s = input.initial_state;
     getModel().realizeVelocity(s);
-    if (getModeIsCost()) {
-        for (int i = 0; i < (int)m_muscleRefs.size(); ++i) {
-            const auto residual = m_muscleRefs[i]->getTendonForce(s) -
-                                  m_muscleRefs[i]->getFiberForceAlongTendon(s);
-            goal[i] = residual * residual;
-        }
-    } else {
-        for (int i = 0; i < (int)m_muscleRefs.size(); ++i) {
-            const auto residual = m_muscleRefs[i]->getTendonForce(s) -
-                                  m_muscleRefs[i]->getFiberForceAlongTendon(s);
-            goal[i] = residual;
+
+    for (int i = 0; i < (int)m_muscleRefs.size(); ++i) {
+        const auto muscleTendonLength = m_muscleRefs[i]->getLength(s);
+        const auto muscleTendonVelocity = 
+            m_muscleRefs[i]->getLengtheningSpeed(s);
+        const auto activation = m_muscleRefs[i]->getActivation(s);
+        const auto normTendonForce = 
+            m_muscleRefs[i]->getNormalizedTendonForce(s);
+        // Assuming normalized tendon force derivative is zero.
+        const auto residual = m_muscleRefs[i]->calcEquilibriumResidual(
+            muscleTendonLength, muscleTendonVelocity, activation, 
+            normTendonForce, 0);
+        goal[i] = residual;
+        if (getModeIsCost()) {
+            goal[i] *= goal[i];
         }
     }
+
 }

Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.h

@@ -19,6 +19,7 @@
  * -------------------------------------------------------------------------- */
 
 #include "MocoGoal.h"
+#include "../Components/DeGrooteFregly2016Muscle.h"
 
 namespace OpenSim {
 
@@ -51,7 +52,7 @@ class OSIMMOCO_API MocoInitialForceEquilibriumGoal : public MocoGoal {
         const GoalInput& input, SimTK::Vector& goal) const override;
 
 private:
-    mutable std::vector<SimTK::ReferencePtr<const Muscle>>
+    mutable std::vector<SimTK::ReferencePtr<const DeGrooteFregly2016Muscle>>
         m_muscleRefs;
 };
 

Moco/Moco/MocoInverse.cpp

@@ -39,6 +39,8 @@ void MocoInverse::constructProperties() {
     constructProperty_constraint_tolerance(1e-3);
     constructProperty_output_paths();
     constructProperty_reserves_weight(1.0);
+    constructProperty_auxiliary_derivatives_weight(0.01);
+    constructProperty_auxiliary_derivatives_bound(10.0);
 }
 
 MocoStudy MocoInverse::initialize() const { return initializeInternal().first; }
@@ -104,7 +106,11 @@ std::pair<MocoStudy, TimeSeriesTable> MocoInverse::initializeInternal() const {
     solver.set_multibody_dynamics_mode("implicit");
     solver.set_interpolate_control_midpoints(false);
     solver.set_minimize_implicit_auxiliary_derivatives(true);
-    solver.set_implicit_auxiliary_derivatives_weight(0.01);
+    solver.set_implicit_auxiliary_derivatives_weight(
+            get_auxiliary_derivatives_weight());
+    solver.set_implicit_auxiliary_derivative_bounds(
+            {-get_auxiliary_derivatives_bound(), 
+              get_auxiliary_derivatives_bound()});
     solver.set_optim_convergence_tolerance(get_convergence_tolerance());
     solver.set_optim_constraint_tolerance(get_constraint_tolerance());
     // The sparsity detection works fine with DeGrooteFregly2016Muscle.

Moco/Moco/MocoInverse.h

@@ -133,6 +133,9 @@ class OSIMMOCO_API MocoInverse : public MocoTool {
             "the model operator ModOpAddReserves, which names each appended "
             "actuator in this format. Default weight: 1.")
 
+    OpenSim_DECLARE_PROPERTY(auxiliary_derivatives_weight, double, "TODO")
+    OpenSim_DECLARE_PROPERTY(auxiliary_derivatives_bound, double, "TODO")
+
     MocoInverse() { constructProperties(); }
 
     void setKinematics(TableProcessor kinematics) {

Moco/Moco/MocoUtilities.cpp

@@ -851,17 +851,17 @@ SimTK::Real OpenSim::solveBisection(
             maxIterations);
 
     const bool sameSign = calcResidual(left) * calcResidual(right) >= 0;
-    if (sameSign) {
-        const auto x = createVectorLinspace(1000, left, right);
-        TimeSeriesTable table;
-        table.setColumnLabels({"residual"});
-        SimTK::RowVector row(1);
-        for (int i = 0; i < x.nrow(); ++i) {
-            row[0] = calcResidual(x[i]);
-            table.appendRow(x[i], row);
-        }
-        // writeTableToFile(table, "DEBUG_solveBisection_residual.sto");
+    //if (sameSign) {
+    const auto x = createVectorLinspace(1000, left, right);
+    TimeSeriesTable table;
+    table.setColumnLabels({"residual"});
+    SimTK::RowVector row(1);
+    for (int i = 0; i < x.nrow(); ++i) {
+        row[0] = calcResidual(x[i]);
+        table.appendRow(x[i], row);
     }
+    writeTableToFile(table, "DEBUG_solveBisection_residual.sto");
+    //}
     OPENSIM_THROW_IF(sameSign, Exception,
             "Function has same sign at bounds of {} and {}.", left, right);
 

Moco/Moco/ModelOperators.h

@@ -228,7 +228,50 @@ class OSIMMOCO_API ModOpScaleMaxIsometricForce : public ModelOperator {
         model.finalizeFromProperties();
         for (auto& muscle : model.updComponentList<Muscle>()) {
             muscle.set_max_isometric_force(
-                    get_scale_factor() * muscle.get_max_isometric_force());
+                get_scale_factor() * muscle.get_max_isometric_force());
+        }
+    }
+};
+
+/// Scale the tendon slack length for all muscles in the model.
+class OSIMMOCO_API ModOpScaleTendonSlackLength : public ModelOperator {
+    OpenSim_DECLARE_CONCRETE_OBJECT(ModOpScaleTendonSlackLength, ModelOperator);
+    OpenSim_DECLARE_PROPERTY(
+            scale_factor, double, "The tendon slack length scale factor.");
+
+public:
+    ModOpScaleTendonSlackLength() { constructProperty_scale_factor(1); }
+    ModOpScaleTendonSlackLength(double scaleFactor)
+            : ModOpScaleTendonSlackLength() {
+        set_scale_factor(scaleFactor);
+    }
+    void operate(Model& model, const std::string&) const override {
+        model.finalizeFromProperties();
+        for (auto& muscle : model.updComponentList<Muscle>()) {
+            muscle.set_tendon_slack_length(
+                get_scale_factor() * muscle.get_tendon_slack_length());
+        }
+    }
+};
+
+/// Scale the optimal fiber length for all muscles in the model.
+class OSIMMOCO_API ModOpScaleOptimalFiberLength : public ModelOperator {
+    OpenSim_DECLARE_CONCRETE_OBJECT(
+            ModOpScaleOptimalFiberLength, ModelOperator);
+    OpenSim_DECLARE_PROPERTY(
+            scale_factor, double, "The optimal fiber length scale factor.");
+
+public:
+    ModOpScaleOptimalFiberLength() { constructProperty_scale_factor(1); }
+    ModOpScaleOptimalFiberLength(double scaleFactor)
+            : ModOpScaleOptimalFiberLength() {
+        set_scale_factor(scaleFactor);
+    }
+    void operate(Model& model, const std::string&) const override {
+        model.finalizeFromProperties();
+        for (auto& muscle : model.updComponentList<Muscle>()) {
+            muscle.set_optimal_fiber_length(
+                get_scale_factor() * muscle.get_optimal_fiber_length());
         }
     }
 };

Moco/Sandbox/DeGrooteFregly2016MuscleDerivatives.m

@@ -1,32 +1,50 @@
-syms b1 b2 b3 b4 lMtilde min_lMtilde c1 c2 c3 lTtilde kT kPE e0
+syms b1 b2 b3 b4 lMtilde min_lMtilde c1 c2 c3 lTtilde kT kPE e0 vMtilde d1 d2 d3 d4
 
 fprintf('Derivative of tendon force length curve \n')
 fprintf('======================================= \n')
 
 f_t = c1*exp(kT*(lTtilde - c2)) - c3;
 simplify(diff(f_t, lTtilde))
 
+fprintf('Integral of tendon force length curve \n')
+fprintf('===================================== \n')
+
+f_t = c1*exp(kT*(lTtilde - c2)) - c3;
+simplify(int(f_t, lTtilde))
+
 fprintf('Derivative of active fiber force length curve \n')
 fprintf('============================================= \n')
 
 f_act = b1 * exp((-0.5*(lMtilde-b2)^2) / ((b3 + b4*lMtilde)^2));
 simplify(diff(f_act, lMtilde))
 
+fprintf('Derivative of active fiber force velocity curve \n')
+fprintf('=============================================== \n')
+tempV = d2 * vMtilde + d3;
+tempLogArg = tempV + sqrt(tempV^2 + 1.0);
+f_v = d1 * log(tempLogArg) + d4;
+simplify(diff(f_v, vMtilde))
+
 fprintf('Derivative of passive fiber force length curve \n')
 fprintf('============================================== \n')
 
-f_pas = (exp(kPE * (lMtilde - 1.0) / e0) - exp(kPE * (min_lMtilde - 1) / e0)) / ...
-        (exp(kPE) - exp(kPE * (min_lMtilde - 1) / e0));
+offset = exp(kPE * (min_lMtilde - 1.0) / e0);
+denom = exp(kPE) - offset;
+f_pas = (exp(kPE * (lMtilde - 1.0) / e0) - offset) / denom;
 simplify(diff(f_pas, lMtilde))
 
-fprintf('Integral of tendon force length curve \n')
-fprintf('===================================== \n')
-
-f_t = c1*exp(kT*(lTtilde - c2)) - c3;
-simplify(int(f_t, lTtilde))
-
 fprintf('Integral of passive fiber force length curve \n')
 fprintf('============================================ \n')
 
-f_pas_int = simplify(int(f_pas, lMtilde))
+f_pas_int = simplify(int(f_pas, lMtilde));
 pretty(f_pas_int)
+
+temp1 = exp(kPE * lMtilde / e0);
+temp2 = exp(kPE * min_lMtilde / e0);
+temp3 = exp(kPE * (e0 + 1.0) / e0);
+numer = - e0 * temp1 + kPE * lMtilde * temp2;
+denom = kPE * (temp2 - temp3);
+f_pas_int_decomp = numer / denom;
+
+fprintf('Expressions equal: ')
+isequal(f_pas_int, f_pas_int_decomp)