From 58ac46c4cb3c22531969b9b7f0a7298599a74983 Mon Sep 17 00:00:00 2001 From: humphreysb Date: Thu, 30 Jun 2016 23:24:33 -0700 Subject: [PATCH 01/40] Add VandenBogert2011Muscle (calculation only) --- OpenSim/Actuators/VandenBogert2011Muscle.cpp | 315 +++++++++++++++++++ OpenSim/Actuators/VandenBogert2011Muscle.h | 147 +++++++++ 2 files changed, 462 insertions(+) create mode 100644 OpenSim/Actuators/VandenBogert2011Muscle.cpp create mode 100644 OpenSim/Actuators/VandenBogert2011Muscle.h diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp new file mode 100644 index 0000000000..30f1e8043f --- /dev/null +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -0,0 +1,315 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: VandenBogert.cpp * + * -------------------------------------------------------------------------- */ + + +//============================================================================= +// INCLUDES +//============================================================================= +#include "VandenBogert2011Muscle.h" + +//============================================================================= +// STATICS +//============================================================================= +using namespace std; +using namespace OpenSim; + +//============================================================================= +// CONSTRUCTOR(S) AND DESTRUCTOR +//============================================================================= +//_____________________________________________________________________________ +/* + * Default constructor + */ +VandenBogert2011Muscle::VandenBogert2011Muscle() +{ + constructProperties(); +} + +//_____________________________________________________________________________ +/* + * Constructor. + */ +VandenBogert2011Muscle::VandenBogert2011Muscle(const std::string &name) : + Super(name) +{ + constructProperties(); +} + +//_____________________________________________________________________________ +/* + * Construct and initialize properties. + * All properties are added to the property set. Once added, they can be + * read in and written to files. + */ +void VandenBogert2011Muscle::constructProperties() +{ + setAuthors("Brad Humphreys"); + //constructProperty_max_isometric_force(1000); + constructProperty_strain_at_max_iso_force_SEE(0.04); + constructProperty_fl_width_parameter(0.63); + constructProperty_fv_AHill(0.25); + constructProperty_fv_max_multiplier(1.4); + //constructProperty_optimal_fiber_length(0.1); + constructProperty_dampingCoeff_PEE(0.01); + //constructProperty_tendon_slack_length(0.2); + constructProperty_t_act(0.01); + constructProperty_t_deact(0.04); + +} + +// Define new states and their derivatives in the underlying system +void VandenBogert2011Muscle::extendAddToSystem(SimTK::MultibodySystem& system) const +{ + + // No States to add, yet + /* Allow Millard2012EquilibriumMuscle to add its states, before extending + Super::extendAddToSystem(system); + + // Now add the states necessary to implement the fatigable behavior + addStateVariable("target_activation"); + addStateVariable("active_motor_units"); + addStateVariable("fatigued_motor_units"); + // and their corresponding derivatives + 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);*/ +} + +void VandenBogert2011Muscle::extendInitStateFromProperties(SimTK::State& s) const +{ + // No States Yet + /*Super::extendInitStateFromProperties(s); + setTargetActivation(s, getDefaultTargetActivation()); + setActiveMotorUnits(s, getDefaultActiveMotorUnits()); + setFatiguedMotorUnits(s, getDefaultFatiguedMotorUnits());*/ +} + +void VandenBogert2011Muscle::extendSetPropertiesFromState(const SimTK::State& s) +{ + // No States Yet + /*Super::extendSetPropertiesFromState(s); + setDefaultTargetActivation(getTargetActivation(s)); + setDefaultActiveMotorUnits(getActiveMotorUnits(s)); + setDefaultFatiguedMotorUnits(getFatiguedMotorUnits(s));*/ +} + +//-------------------------------------------------------------------------- +// GET & SET Properties +//-------------------------------------------------------------------------- +void VandenBogert2011Muscle::setStrainAtMaxIsoForceSee(double StrainAtMaxIsoForceSee) + { set_strain_at_max_iso_force_SEE(StrainAtMaxIsoForceSee); } +double VandenBogert2011Muscle::getStrainAtMaxIsoForceSee() const + { return get_strain_at_max_iso_force_SEE(); } + +void VandenBogert2011Muscle::setFlWidth(double FlWidth) + { set_fl_width(FlWidth); } +double VandenBogert2011Muscle::getFlWidth() const + { return get_fl_width(); } + +void VandenBogert2011Muscle::setFvAHill()(double FvAHill) + { set_fv_AHill(FvAHill); } +double VandenBogert2011Muscle::getFvAHill() const + { return get_fv_AHill(); } + +void VandenBogert2011Muscle::setFvMaxMultiplier()(double FvMaxMultiplier) + { set_fv_max_multiplier(FvMaxMultiplier); } +double VandenBogert2011Muscle::getFvMaxMultiplier() const + { return get_fv_max_multiplier(); } + +void VandenBogert2011Muscle::setDampingCoeffPee()(double DampingCoeffPee) + { set_dampingCoeff_PEE(DampingCoeffPee); } +double VandenBogert2011Muscle::getDampingCoeffPee() const + { return get_dampingCoeff_PEE(); } + +void VandenBogert2011Muscle::setLengthSlackPee()(double LengthSlackPee) + { set_length_slack_PEE(LengthSlackPee); } +double VandenBogert2011Muscle::getLengthSlackPee() const + { return get_length_slack_PEE(); } + +void VandenBogert2011Muscle::setTact(double Tact) + { set_t_act(Tact); } +double VandenBogert2011Muscle::getTact() const + { return get_t_act(); } + +void VandenBogert2011Muscle::setTdeact(double Tdeact) + { set_t_deact(Tdeact); } +double VandenBogert2011Muscle::getTdeact() const + { return get_t_deact(); } + + + + +//-------------------------------------------------------------------------- +// GET & SET States and their derivatives +//-------------------------------------------------------------------------- + +//No states yet +/*double FatigableMuscle::getTargetActivation(const SimTK::State& s) const +{ return getStateVariableValue(s, "target_activation"); } + +void FatigableMuscle::setTargetActivation(SimTK::State& s, + double fatiguedAct) const +{ setStateVariableValue(s, "target_activation", fatiguedAct); } + +double FatigableMuscle::getTargetActivationDeriv(const SimTK::State& s) const +{ return getStateVariableDerivativeValue(s, "target_activation"); } + +void FatigableMuscle::setTargetActivationDeriv(const SimTK::State& s, + double fatiguedActDeriv) const +{ setStateVariableDerivativeValue(s, "target_activation", fatiguedActDeriv); } + +double FatigableMuscle::getActiveMotorUnits(const SimTK::State& s) const +{ return getStateVariableValue(s, "active_motor_units"); } + +void FatigableMuscle::setActiveMotorUnits(SimTK::State& s, + double activeMotorUnits) const +{ setStateVariableValue(s, "active_motor_units", activeMotorUnits); } + +double FatigableMuscle::getActiveMotorUnitsDeriv(const SimTK::State& s) const +{ return getStateVariableDerivativeValue(s, "active_motor_units"); } + +void FatigableMuscle::setActiveMotorUnitsDeriv(const SimTK::State& s, + double activeMotorUnitsDeriv) const +{ setStateVariableDerivativeValue(s, "active_motor_units", activeMotorUnitsDeriv); } + +double FatigableMuscle::getFatiguedMotorUnits(const SimTK::State& s) const +{ return getStateVariableValue(s, "fatigued_motor_units"); } + +void FatigableMuscle::setFatiguedMotorUnits(SimTK::State& s, + double fatiguedMotorUnits) const +{ setStateVariableValue(s, "fatigued_motor_units", fatiguedMotorUnits); } + +double FatigableMuscle::getFatiguedMotorUnitsDeriv(const SimTK::State& s) const +{ return getStateVariableDerivativeValue(s, "fatigued_motor_units"); } + +void FatigableMuscle::setFatiguedMotorUnitsDeriv(const SimTK::State& s, + double fatiguedMotorUnitsDeriv) const +{ setStateVariableDerivativeValue(s, "fatigued_motor_units", fatiguedMotorUnitsDeriv); + }*/ + + +//============================================================================= +// COMPUTATION +//============================================================================= +//_____________________________________________________________________________ +/** + * Compute the derivatives of the muscle states. + * + * @param s system state + */ + + +array calcImplicitResidual(const SimTK::State& s) const +{ + + //State Variables + double Lm = getLength(s); + double Lce = getNormalizedFiberLength(s); //Ton: Lce is dimensionless, it is the muscle fiber length divided by Lceopt + double a = getActivation(s); + double Lcedot = getNormalizedFiberVelocity(s); + + // Parameters + double Fmax = getMaxIsometricForce(); //Maximum isometric force that the fibers can generate + double umax = getStrainAtMaxIsoForceSee(); //Strain in the series elastic element at load of Fmax + double W = getFlWidthParameter(); //Width parameter of the force-length relationship of the contractile element + double AHill = getFvAHill(); //Hill parameter of the force-velocity relationship + double FVmax = getFvMaxMultiplier(); //Maximal eccentric force multiplier + double Lceopt = getOptimalFiberLength(); //Optimal Length of Contractile Element + double b = getDampingCoeffPee(); //Damping coefficient of damper parallel to the CE (normalized to Fmax) + double SEELslack = getTendonSlackLength(); //Slack length of the series elastic element + double PEELslack = getLengthSlackPee(); //Slack length of the parallel elastic element, divided by Lceopt + double Tact = getTact(); // Activation time(s) + double Tdeact = getTdeact(); // Deactivation time(s) + + // constants derived from the muscle parameters + double Vmax = 10*model.Lceopt; //Maximum shortening velocity (m/s) is 10 fiber lengths per sec + double d1 = Vmax*AHill*(FVmax-1)/(AHill+1); // parameter in the eccentric force-velocity equation + + + //F1 is the normalized isometric force-length relationship at maximum activation + double ff = (Lce - 1.0)/W; // [dimensionless] + double F1 = exp(pow(-ff,2)); // Gaussian force-length curve + //dF1_dLce = -2.0*ff*F1 / W; + + // F2 is the dimensionless force-velocity relationship + if (Lcedot < 0) { + //concentric contraction + ff = Vmax - Lcedot/AHill; + double F2 = (Vmax + Lcedot)/ff; + //dF2_dLcedot = (1.0 + F2/AHill)/ff; + } + else { + //eccentric contraction + double c = Vmax * AHill * (FVmax - 1.0) / (AHill + 1.0); // parameter in the eccentric force-velocity equation + ff = Lcedot + c; + double F2 = (FVmax*Lcedot + c) / ff; + //dF2_dLcedot = (FVmax - F2)/ff; + } + + //F3 is the dimensionless PEE force (in units of Fmax) + double kPEE = 1.0/Fmax*Lceopt; // stiffness of the linear term is 1 N/m, convert to Fmax/Lceopt units + ff = (Lce - PEELslack); // elongation of PEE, relative to Lceopt + double F3 = kPEE*ff; // low stiffness linear term + //dF3_dLce = kPEE; + if (ff>0) { + //add quadratic term for positive elongation + F3 = F3 + kPEE2 * pow(ff, 2); + //dF3_dLce = dF3_dLce + 2*kPEE2*ff; + } + + + //F4 is the dimensionless SEE force (in units of Fmax) + double kSEE = 1.0/Fmax; // stiffness of the linear term is 1 N/m, convert to Fmax/m + ff = Lm - Lce*Lceopt - SEELslack; // elongation of SEE, in meters + double F4 = kSEE*ff; // low stiffness linear term + //dF4_dLce = -kSEE*Lceopt; + //dF4_dLm = kSEE; + if (ff>0) { + // add quadratic term for positive deformation + F4 = F4 + kSEE2 * pow(ff, 2); + //dF4_dLce = dF4_dLce - 2 * kSEE2 * Lceopt * ff; + //dF4_dLm = dF4_dLm + 2 * kSEE2 * ff; + } + + //F5 is viscous damping parallel to the CE (0.001 of Fmax at 1 Lceopt/s) to + // ensure that df/dLcedot is never zero + double F5 = 0.001*Lcedot; // TODO: is 0.001 suppossed to b? + //dF5_dLcedot = 0.001; + + //The muscle dynamics equation: f = Fsee - Fce - Fpee - Fdamping = 0 + double muscleForceResidual = F4 - a*F1*F2 - F3 - F5; + + + /*if (nargout > 1) + dfdy(5,6) = -F1*F2; % df/da + dfdy(5,5) = dF4_dLce - a*dF1_dLce*F2 - dF3_dLce; % df/dLce + dfdydot(5,5) = -a*F1*dF2_dLcedot - dF5_dLcedot; % df/dLcedot + df_dLm = dF4_dLm; + dfdy(5,3) = -(d/L)* df_dLm; % derivative of f with respect to hand position + end*/ + + // Force in SEE is Fmax*F4 + double Fsee = Fmax*F4; + /*if (nargout > 1) + dFsee_dLm = Fmax * dF4_dLm; + dFsee_dLce = Fmax * dF4_dLce; + dFsee_dy = [0;0;(-d/L)*dFsee_dLm; 0;dFsee_dLce;0;0]; % Fsee is a function of y(3) & y(5) + end*/ + + //Activation dynamics equation + double activationResidual = adot-(u-a)*(u/Tact + (1-u)/Tdeact); + /*if (nargout > 1) + dfdy(6,6) = (y(7)/model.Tact + (1-y(7))/model.Tdeact); + dfdy(6,7) = -(y(7)/model.Tact + (1-y(7))/model.Tdeact) ... + -(y(7)-y(6))*(1/model.Tact - 1/model.Tdeact); + dfdydot(6,6) = 1; + end*/ + + array outArr; + outArr[0]=muscleForceResidual; + outArr[1]=activationResidual ; + outArr[2]=Fsee; + + return outArr; +} diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h new file mode 100644 index 0000000000..c2b6edfe0b --- /dev/null +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -0,0 +1,147 @@ +#ifndef OPENSIM_VANDENBOGERT2011MUSCLE_H_ +#define OPENSIM_VANDENBOGERT2011MUSCLE_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: VandenBogert2011Muscle.h * + * -------------------------------------------------------------------------- */ + + +// INCLUDE +#include + +#include +#include + + + +namespace OpenSim { + +// Derive new class from Muscle.h +class VandenBogert2011Muscle : public Muscle { //TODO Do I need: class OSIMACTUATORS_API VandenBogert2011Muscle...... + OpenSim_DECLARE_CONCRETE_OBJECT(VandenBogert2011Muscle, Muscle); + +public: +//============================================================================= +// PROPERTIES +//============================================================================= +/* +The parent class, Muscle.h, provides + 1. max_isometric_force + 2. optimal_fiber_length + 3. tendon_slack_length + 4. pennation_angle_at_optimal + 5. max_contraction_velocity +*/ + +//OpenSim_DECLARE_PROPERTY(max_isometric_force, double, <-----Comes from parent class, Muscle.h +// "Maximum isometric force that the fibers can generate"); //Fmax (N) + + OpenSim_DECLARE_PROPERTY(strain_at_max_iso_force_SEE, double, + "Strain in the series elastic element at load of Fmax"); //umax (dimensionless) + + OpenSim_DECLARE_PROPERTY(fl_width_parameter, double, + "Width parameter of the force-length relationship of the contractile element"); //W (dimensionless) + + OpenSim_DECLARE_PROPERTY(fv_AHill, double, + "Hill parameter of the force-velocity relationship"); //AHill + + OpenSim_DECLARE_PROPERTY(fv_max_multiplier, double, + "Maximal eccentric force multiplier"); //FVmax + + //OpenSim_DECLARE_PROPERTY(optimal_fiber_length, double, <-----Comes from parent class, Muscle.h + // "Optimal Length of Contractile Element"); //Lceopt (m) + + OpenSim_DECLARE_PROPERTY(dampingCoeff_PEE, double, + "Damping coefficient of damper parallel to the CE (normalized to Fmax)"); //b (s/m) + + //OpenSim_DECLARE_PROPERTY(tendon_slack_length, double, <-----Comes from parent class, Muscle.h + // "Slack length of the series elastic element"); //SEELslack (m) + + OpenSim_DECLARE_PROPERTY(t_act, double, + "Activation time(s)"); //Tact (s) + + OpenSim_DECLARE_PROPERTY(t_deact, double, + "Deactivation time(s)"); //Tdeact (s) + +//============================================================================== +// PUBLIC METHODS +//============================================================================== + + +//============================================================================= +// Construction +//============================================================================= + /**Default constructor: produces a non-functional empty muscle*/ + VandenBogert2011Muscle(); + VandenBogert2011Muscle(const std::string &name); + + + // TODO: Uses default (compiler-generated) destructor, copy constructor, copy assignment operator? + + +//------------------------------------------------------------------------- +// GET & SET Properties +//------------------------------------------------------------------------- + // Properties + void VandenBogert2011Muscle::setStrainAtMaxIsoForceSee(double StrainAtMaxIsoForceSee); + double VandenBogert2011Muscle::getStrainAtMaxIsoForceSee() const; + + void VandenBogert2011Muscle::setFlWidth(double FlWidth); + double VandenBogert2011Muscle::getFlWidth() const; + + void VandenBogert2011Muscle::setFvAHill()(double FvAHill); + double VandenBogert2011Muscle::getFvAHill() const; + + void VandenBogert2011Muscle::setFvMaxMultiplier()(double FvMaxMultiplier); + double VandenBogert2011Muscle::getFvMaxMultiplier() const; + + void VandenBogert2011Muscle::setDampingCoeffPee()(double DampingCoeffPee); + double VandenBogert2011Muscle::getDampingCoeffPee() const; + + void VandenBogert2011Muscle::setLengthSlackPee()(double LengthSlackPee); + double VandenBogert2011Muscle::getLengthSlackPee() const; + + void VandenBogert2011Muscle::setTact(double Tact); + double VandenBogert2011Muscle::getTact() const; + + void VandenBogert2011Muscle::setTdeact(double Tdeact); + double VandenBogert2011Muscle::getTdeact() const; + +/** default values for states + double getDefaultActiveMotorUnits() const; + void setDefaultActiveMotorUnits(double activeMotorUnits); + double getDefaultFatiguedMotorUnits() const; + void setDefaultFatiguedMotorUnits(double fatiguedMotorUnits);*/ + +//============================================================================= +// COMPUTATION +//============================================================================= + +array calcImplicitResidual(const SimTK::State& s) const override; + +protected: +//============================================================================= +// PROTECTED METHODS +//============================================================================= + + + + // Model Component Interface + /** add new dynamical states to the multibody system corresponding + to this muscle */ + void extendAddToSystem(SimTK::MultibodySystem &system) const override; + + /** initialize muscle state variables from properties. For example, any + properties that contain default state values */ + void extendInitStateFromProperties(SimTK::State &s) const override; + + /** use the current values in the state to update any properties such as + default values for state variables */ + void extendSetPropertiesFromState(const SimTK::State &s) override; + + private: + /** construct the new properties and set their default values */ + void constructProperties(); + + }; // END of class VandenBogert2011Muscle +} // end of namespace OpenSim +#endif // OPENSIM_THELEN_2003_MUSCLE_H_ From 3da1730394023d7e8f9291355eef1247ae536dd8 Mon Sep 17 00:00:00 2001 From: humphreysb Date: Fri, 1 Jul 2016 11:33:26 -0700 Subject: [PATCH 02/40] A version that compiles for API wrapping --- OpenSim/Actuators/VandenBogert2011Muscle.cpp | 87 ++++++++++++++------ OpenSim/Actuators/VandenBogert2011Muscle.h | 50 ++++++----- 2 files changed, 93 insertions(+), 44 deletions(-) diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index 30f1e8043f..3207c007e4 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -30,8 +30,7 @@ VandenBogert2011Muscle::VandenBogert2011Muscle() /* * Constructor. */ -VandenBogert2011Muscle::VandenBogert2011Muscle(const std::string &name) : - Super(name) +VandenBogert2011Muscle::VandenBogert2011Muscle(const std::string &name) { constructProperties(); } @@ -44,14 +43,15 @@ VandenBogert2011Muscle::VandenBogert2011Muscle(const std::string &name) : */ void VandenBogert2011Muscle::constructProperties() { - setAuthors("Brad Humphreys"); + setAuthors("A. van den Bogert, B. Humphreys"); //constructProperty_max_isometric_force(1000); constructProperty_strain_at_max_iso_force_SEE(0.04); - constructProperty_fl_width_parameter(0.63); + constructProperty_fl_width(0.63); constructProperty_fv_AHill(0.25); constructProperty_fv_max_multiplier(1.4); //constructProperty_optimal_fiber_length(0.1); - constructProperty_dampingCoeff_PEE(0.01); + constructProperty_dampingCoeff_Pee(0.01); + constructProperty_length_slack_Pee(1.0); //constructProperty_tendon_slack_length(0.2); constructProperty_t_act(0.01); constructProperty_t_deact(0.04); @@ -97,8 +97,9 @@ void VandenBogert2011Muscle::extendSetPropertiesFromState(const SimTK::State& s) //-------------------------------------------------------------------------- // GET & SET Properties //-------------------------------------------------------------------------- -void VandenBogert2011Muscle::setStrainAtMaxIsoForceSee(double StrainAtMaxIsoForceSee) - { set_strain_at_max_iso_force_SEE(StrainAtMaxIsoForceSee); } +void VandenBogert2011Muscle::setStrainAtMaxIsoForceSee(double StrainAtMaxIsoForceSee) { + set_strain_at_max_iso_force_SEE(StrainAtMaxIsoForceSee); + } double VandenBogert2011Muscle::getStrainAtMaxIsoForceSee() const { return get_strain_at_max_iso_force_SEE(); } @@ -107,25 +108,25 @@ void VandenBogert2011Muscle::setFlWidth(double FlWidth) double VandenBogert2011Muscle::getFlWidth() const { return get_fl_width(); } -void VandenBogert2011Muscle::setFvAHill()(double FvAHill) +void VandenBogert2011Muscle::setFvAHill(double FvAHill) { set_fv_AHill(FvAHill); } double VandenBogert2011Muscle::getFvAHill() const { return get_fv_AHill(); } -void VandenBogert2011Muscle::setFvMaxMultiplier()(double FvMaxMultiplier) +void VandenBogert2011Muscle::setFvMaxMultiplier(double FvMaxMultiplier) { set_fv_max_multiplier(FvMaxMultiplier); } double VandenBogert2011Muscle::getFvMaxMultiplier() const { return get_fv_max_multiplier(); } -void VandenBogert2011Muscle::setDampingCoeffPee()(double DampingCoeffPee) - { set_dampingCoeff_PEE(DampingCoeffPee); } +void VandenBogert2011Muscle::setDampingCoeffPee(double DampingCoeffPee) + { set_dampingCoeff_Pee(DampingCoeffPee); } double VandenBogert2011Muscle::getDampingCoeffPee() const - { return get_dampingCoeff_PEE(); } + { return get_dampingCoeff_Pee(); } -void VandenBogert2011Muscle::setLengthSlackPee()(double LengthSlackPee) - { set_length_slack_PEE(LengthSlackPee); } +void VandenBogert2011Muscle::setLengthSlackPee(double LengthSlackPee) + { set_length_slack_Pee(LengthSlackPee); } double VandenBogert2011Muscle::getLengthSlackPee() const - { return get_length_slack_PEE(); } + { return get_length_slack_Pee(); } void VandenBogert2011Muscle::setTact(double Tact) { set_t_act(Tact); } @@ -189,6 +190,36 @@ void FatigableMuscle::setFatiguedMotorUnitsDeriv(const SimTK::State& s, }*/ + + + +//============================================================================== +// Muscle.h Interface +//============================================================================== + + +double VandenBogert2011Muscle::computeActuation(const SimTK::State& s) const +{return( 0.0);} + +void VandenBogert2011Muscle::setActivation(SimTK::State& s,double activation) const +{} + +void VandenBogert2011Muscle::computeInitialFiberEquilibrium(SimTK::State& s) const +{} + + + + + + + + + + + + + + //============================================================================= // COMPUTATION //============================================================================= @@ -200,19 +231,22 @@ void FatigableMuscle::setFatiguedMotorUnitsDeriv(const SimTK::State& s, */ -array calcImplicitResidual(const SimTK::State& s) const +//array calcImplicitResidual(const SimTK::State& s) const + +array VandenBogert2011Muscle::calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u) const { + // Later when using state variables: //State Variables - double Lm = getLength(s); - double Lce = getNormalizedFiberLength(s); //Ton: Lce is dimensionless, it is the muscle fiber length divided by Lceopt - double a = getActivation(s); - double Lcedot = getNormalizedFiberVelocity(s); + //double Lm = getLength(s); + //double Lce = getNormalizedFiberLength(s); //Ton: Lce is dimensionless, it is the muscle fiber length divided by Lceopt + //double a = getActivation(s); + //double Lcedot = getNormalizedFiberVelocity(s); // Parameters double Fmax = getMaxIsometricForce(); //Maximum isometric force that the fibers can generate double umax = getStrainAtMaxIsoForceSee(); //Strain in the series elastic element at load of Fmax - double W = getFlWidthParameter(); //Width parameter of the force-length relationship of the contractile element + double W = getFlWidth(); //Width parameter of the force-length relationship of the contractile element double AHill = getFvAHill(); //Hill parameter of the force-velocity relationship double FVmax = getFvMaxMultiplier(); //Maximal eccentric force multiplier double Lceopt = getOptimalFiberLength(); //Optimal Length of Contractile Element @@ -223,27 +257,32 @@ array calcImplicitResidual(const SimTK::State& s) const double Tdeact = getTdeact(); // Deactivation time(s) // constants derived from the muscle parameters - double Vmax = 10*model.Lceopt; //Maximum shortening velocity (m/s) is 10 fiber lengths per sec + double Vmax = 10*Lceopt; //Maximum shortening velocity (m/s) is 10 fiber lengths per sec double d1 = Vmax*AHill*(FVmax-1)/(AHill+1); // parameter in the eccentric force-velocity equation + double kPEE2 = 1/pow(W,2); // PEE quadratic stiffness, so Fpee = Fmax when Lce = Lce*(1+W) + double kSEE2 = 1/pow(SEELslack*umax,2); // SEE quadratic stiffness, so Fsee = Fmax at strain of umax + + //F1 is the normalized isometric force-length relationship at maximum activation double ff = (Lce - 1.0)/W; // [dimensionless] double F1 = exp(pow(-ff,2)); // Gaussian force-length curve //dF1_dLce = -2.0*ff*F1 / W; + double F2=0.0; // F2 is the dimensionless force-velocity relationship if (Lcedot < 0) { //concentric contraction ff = Vmax - Lcedot/AHill; - double F2 = (Vmax + Lcedot)/ff; + F2 = (Vmax + Lcedot)/ff; //dF2_dLcedot = (1.0 + F2/AHill)/ff; } else { //eccentric contraction double c = Vmax * AHill * (FVmax - 1.0) / (AHill + 1.0); // parameter in the eccentric force-velocity equation ff = Lcedot + c; - double F2 = (FVmax*Lcedot + c) / ff; + F2 = (FVmax*Lcedot + c) / ff; //dF2_dLcedot = (FVmax - F2)/ff; } diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index c2b6edfe0b..d1d61ca772 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -38,7 +38,7 @@ The parent class, Muscle.h, provides OpenSim_DECLARE_PROPERTY(strain_at_max_iso_force_SEE, double, "Strain in the series elastic element at load of Fmax"); //umax (dimensionless) - OpenSim_DECLARE_PROPERTY(fl_width_parameter, double, + OpenSim_DECLARE_PROPERTY(fl_width, double, "Width parameter of the force-length relationship of the contractile element"); //W (dimensionless) OpenSim_DECLARE_PROPERTY(fv_AHill, double, @@ -50,12 +50,19 @@ The parent class, Muscle.h, provides //OpenSim_DECLARE_PROPERTY(optimal_fiber_length, double, <-----Comes from parent class, Muscle.h // "Optimal Length of Contractile Element"); //Lceopt (m) - OpenSim_DECLARE_PROPERTY(dampingCoeff_PEE, double, + OpenSim_DECLARE_PROPERTY(dampingCoeff_Pee, double, "Damping coefficient of damper parallel to the CE (normalized to Fmax)"); //b (s/m) //OpenSim_DECLARE_PROPERTY(tendon_slack_length, double, <-----Comes from parent class, Muscle.h // "Slack length of the series elastic element"); //SEELslack (m) + + OpenSim_DECLARE_PROPERTY(length_slack_Pee, double, + "(dimensionless) slack length of the parallel elastic element, divided by Lceopt"); //b (s/m) + + + + OpenSim_DECLARE_PROPERTY(t_act, double, "Activation time(s)"); //Tact (s) @@ -65,7 +72,9 @@ The parent class, Muscle.h, provides //============================================================================== // PUBLIC METHODS //============================================================================== - + double computeActuation(const SimTK::State& s) const override; + void computeInitialFiberEquilibrium(SimTK::State& s) const override; + void setActivation(SimTK::State& s,double activation) const override; //============================================================================= // Construction @@ -82,29 +91,29 @@ The parent class, Muscle.h, provides // GET & SET Properties //------------------------------------------------------------------------- // Properties - void VandenBogert2011Muscle::setStrainAtMaxIsoForceSee(double StrainAtMaxIsoForceSee); - double VandenBogert2011Muscle::getStrainAtMaxIsoForceSee() const; + void setStrainAtMaxIsoForceSee(double StrainAtMaxIsoForceSee); + double getStrainAtMaxIsoForceSee() const; - void VandenBogert2011Muscle::setFlWidth(double FlWidth); - double VandenBogert2011Muscle::getFlWidth() const; + void setFlWidth(double FlWidth); + double getFlWidth() const; - void VandenBogert2011Muscle::setFvAHill()(double FvAHill); - double VandenBogert2011Muscle::getFvAHill() const; + void setFvAHill(double FvAHill); + double getFvAHill() const; - void VandenBogert2011Muscle::setFvMaxMultiplier()(double FvMaxMultiplier); - double VandenBogert2011Muscle::getFvMaxMultiplier() const; + void setFvMaxMultiplier(double FvMaxMultiplier); + double getFvMaxMultiplier() const; - void VandenBogert2011Muscle::setDampingCoeffPee()(double DampingCoeffPee); - double VandenBogert2011Muscle::getDampingCoeffPee() const; + void setDampingCoeffPee(double DampingCoeffPee); + double getDampingCoeffPee() const; - void VandenBogert2011Muscle::setLengthSlackPee()(double LengthSlackPee); - double VandenBogert2011Muscle::getLengthSlackPee() const; + void setLengthSlackPee(double LengthSlackPee); + double getLengthSlackPee() const; - void VandenBogert2011Muscle::setTact(double Tact); - double VandenBogert2011Muscle::getTact() const; + void setTact(double Tact); + double getTact() const; - void VandenBogert2011Muscle::setTdeact(double Tdeact); - double VandenBogert2011Muscle::getTdeact() const; + void setTdeact(double Tdeact); + double getTdeact() const; /** default values for states double getDefaultActiveMotorUnits() const; @@ -116,7 +125,8 @@ The parent class, Muscle.h, provides // COMPUTATION //============================================================================= -array calcImplicitResidual(const SimTK::State& s) const override; +//std::array calcImplicitResidual(const SimTK::State& s) const; +std::array calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u) const; protected: //============================================================================= From 78f084bc5da298e1b6dff3a8d3e1ea4f449906ad Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Fri, 1 Jul 2016 12:05:54 -0700 Subject: [PATCH 03/40] Wrap VandenBogert2011Muscle. --- Bindings/Java/Matlab/tests/CMakeLists.txt | 1 + .../Matlab/tests/testVandenBogert2011Muscle.m | 16 ++++++++++++++++ Bindings/OpenSimHeaders_actuators.h | 2 ++ Bindings/actuators.i | 2 ++ Bindings/preliminaries.i | 1 + .../Actuators/RegisterTypes_osimActuators.cpp | 4 ++++ OpenSim/Actuators/VandenBogert2011Muscle.cpp | 13 +++++++------ OpenSim/Actuators/VandenBogert2011Muscle.h | 4 ++-- OpenSim/Actuators/osimActuators.h | 2 ++ 9 files changed, 37 insertions(+), 8 deletions(-) create mode 100644 Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m diff --git a/Bindings/Java/Matlab/tests/CMakeLists.txt b/Bindings/Java/Matlab/tests/CMakeLists.txt index c2e44424b4..e751aa5257 100644 --- a/Bindings/Java/Matlab/tests/CMakeLists.txt +++ b/Bindings/Java/Matlab/tests/CMakeLists.txt @@ -58,6 +58,7 @@ add_dependencies(RunMatlabTests JavaBindings) OpenSimAddMatlabTest(TugOfWar ../examples/OpenSimCreateTugOfWarModel.m) OpenSimAddMatlabTest(ConnectorsInputsOutputs testConnectorsInputsOutputs.m) OpenSimAddMatlabTest(AccessSubcomponents testAccessSubcomponents.m) +OpenSimAddMatlabTest(VandenBogert2011Muscle testVandenBogert2011Muscle.m) # Copy resources. file(COPY "${OPENSIM_SHARED_TEST_FILES_DIR}/arm26.osim" diff --git a/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m b/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m new file mode 100644 index 0000000000..dace248501 --- /dev/null +++ b/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m @@ -0,0 +1,16 @@ + +import org.opensim.modeling.* + +muscle = VandenBogert2011Muscle(); +Lm = 0; +Lce = 0; +a = 0; +Lcedot = 0; +adot = 0; +u = 0; +out = muscle.calcImplicitResidual(Lm, Lce, a, Lcedot, adot, u); +% out should have 3 elements: +assert(out.size() == 3); +disp(out.get(0)) +disp(out.get(1)) +disp(out.get(2)) diff --git a/Bindings/OpenSimHeaders_actuators.h b/Bindings/OpenSimHeaders_actuators.h index cdc715af69..de2eec18d3 100644 --- a/Bindings/OpenSimHeaders_actuators.h +++ b/Bindings/OpenSimHeaders_actuators.h @@ -17,4 +17,6 @@ #include #include +#include + #endif // OPENSIM_OPENSIM_HEADERS_ACTUATORS_H_ diff --git a/Bindings/actuators.i b/Bindings/actuators.i index 07147a30a1..36d6b4857b 100644 --- a/Bindings/actuators.i +++ b/Bindings/actuators.i @@ -22,3 +22,5 @@ %include %include %include + +%include diff --git a/Bindings/preliminaries.i b/Bindings/preliminaries.i index 0886c3757e..13a9adb8ef 100644 --- a/Bindings/preliminaries.i +++ b/Bindings/preliminaries.i @@ -10,3 +10,4 @@ %include "std_map.i" %include + diff --git a/OpenSim/Actuators/RegisterTypes_osimActuators.cpp b/OpenSim/Actuators/RegisterTypes_osimActuators.cpp index c4e9105739..b105cf6e10 100644 --- a/OpenSim/Actuators/RegisterTypes_osimActuators.cpp +++ b/OpenSim/Actuators/RegisterTypes_osimActuators.cpp @@ -57,6 +57,8 @@ #include "Millard2012EquilibriumMuscle.h" #include "Millard2012AccelerationMuscle.h" +#include "VandenBogert2011Muscle.h" + // Awaiting new component architecture that supports subcomponents with states. //#include "ConstantMuscleActivation.h" //#include "ZerothOrderMuscleActivationDynamics.h" @@ -105,6 +107,8 @@ OSIMACTUATORS_API void RegisterTypes_osimActuators() Object::RegisterType(Millard2012EquilibriumMuscle()); Object::RegisterType(Millard2012AccelerationMuscle()); + Object::registerType(VandenBogert2011Muscle()); + //Object::RegisterType( ConstantMuscleActivation() ); //Object::RegisterType( ZerothOrderMuscleActivationDynamics() ); //Object::RegisterType( FirstOrderMuscleActivationDynamics() ); diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index 3207c007e4..096c0c033d 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -233,7 +233,7 @@ void VandenBogert2011Muscle::computeInitialFiberEquilibrium(SimTK::State& s) con //array calcImplicitResidual(const SimTK::State& s) const -array VandenBogert2011Muscle::calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u) const +SimTK::Vec3 VandenBogert2011Muscle::calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u) const { // Later when using state variables: @@ -345,10 +345,11 @@ array VandenBogert2011Muscle::calcImplicitResidual(double Lm, double L dfdydot(6,6) = 1; end*/ - array outArr; - outArr[0]=muscleForceResidual; - outArr[1]=activationResidual ; - outArr[2]=Fsee; + return SimTK::Vec3(muscleForceResidual, activationResidual, Fsee); + // array outArr; + // outArr[0]=muscleForceResidual; + // outArr[1]=activationResidual ; + // outArr[2]=Fsee; - return outArr; + // return outArr; } diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index d1d61ca772..4848096303 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -81,7 +81,7 @@ The parent class, Muscle.h, provides //============================================================================= /**Default constructor: produces a non-functional empty muscle*/ VandenBogert2011Muscle(); - VandenBogert2011Muscle(const std::string &name); + explicit VandenBogert2011Muscle(const std::string &name); // TODO: Uses default (compiler-generated) destructor, copy constructor, copy assignment operator? @@ -126,7 +126,7 @@ The parent class, Muscle.h, provides //============================================================================= //std::array calcImplicitResidual(const SimTK::State& s) const; -std::array calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u) const; + SimTK::Vec3 calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u) const; protected: //============================================================================= diff --git a/OpenSim/Actuators/osimActuators.h b/OpenSim/Actuators/osimActuators.h index 45bf01f77d..4a7487611d 100644 --- a/OpenSim/Actuators/osimActuators.h +++ b/OpenSim/Actuators/osimActuators.h @@ -39,6 +39,8 @@ #include "Millard2012EquilibriumMuscle.h" #include "Millard2012AccelerationMuscle.h" +#include "VandenBogert2011Muscle.h" + #include "McKibbenActuator.h" #include "RegisterTypes_osimActuators.h" // to expose RegisterTypes_osimActuators From 8ae97f7a7cd03fb00dea78f5f9f1de0e6aa6f10c Mon Sep 17 00:00:00 2001 From: humphreysb Date: Wed, 6 Jul 2016 19:00:56 -0700 Subject: [PATCH 04/40] Added Jacobian Calcs --- OpenSim/Actuators/VandenBogert2011Muscle.cpp | 181 +++++++++++-------- OpenSim/Actuators/VandenBogert2011Muscle.h | 25 ++- 2 files changed, 132 insertions(+), 74 deletions(-) diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index 096c0c033d..230784583f 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -210,16 +210,6 @@ void VandenBogert2011Muscle::computeInitialFiberEquilibrium(SimTK::State& s) con - - - - - - - - - - //============================================================================= // COMPUTATION //============================================================================= @@ -233,8 +223,10 @@ void VandenBogert2011Muscle::computeInitialFiberEquilibrium(SimTK::State& s) con //array calcImplicitResidual(const SimTK::State& s) const -SimTK::Vec3 VandenBogert2011Muscle::calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u) const -{ +//SimTK::Vec3 VandenBogert2011Muscle::calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u, int returnJacobians) const { + + +VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u, int returnJacobians) const { // Later when using state variables: //State Variables @@ -257,99 +249,144 @@ SimTK::Vec3 VandenBogert2011Muscle::calcImplicitResidual(double Lm, double Lce, double Tdeact = getTdeact(); // Deactivation time(s) // constants derived from the muscle parameters - double Vmax = 10*Lceopt; //Maximum shortening velocity (m/s) is 10 fiber lengths per sec - double d1 = Vmax*AHill*(FVmax-1)/(AHill+1); // parameter in the eccentric force-velocity equation + double Vmax = 10 * Lceopt; //Maximum shortening velocity (m/s) is 10 fiber lengths per sec + double d1 = Vmax * AHill * (FVmax - 1) / (AHill + 1); // parameter in the eccentric force-velocity equation - double kPEE2 = 1/pow(W,2); // PEE quadratic stiffness, so Fpee = Fmax when Lce = Lce*(1+W) - double kSEE2 = 1/pow(SEELslack*umax,2); // SEE quadratic stiffness, so Fsee = Fmax at strain of umax + double kPEE2 = 1 / pow(W, 2); // PEE quadratic stiffness, so Fpee = Fmax when Lce = Lce*(1+W) + double kSEE2 = 1 / pow(SEELslack * umax, 2); // SEE quadratic stiffness, so Fsee = Fmax at strain of umax - //F1 is the normalized isometric force-length relationship at maximum activation - double ff = (Lce - 1.0)/W; // [dimensionless] - double F1 = exp(pow(-ff,2)); // Gaussian force-length curve - //dF1_dLce = -2.0*ff*F1 / W; + // Jacobian Matrices + SimTK::Mat23 dfdy; + SimTK::Mat23 dfdydot; - double F2=0.0; - // F2 is the dimensionless force-velocity relationship - if (Lcedot < 0) { + + + + //----------------F1 is the normalized isometric force-length relationship at maximum activation--------------// + double ff = (Lce - 1.0) / W; // [dimensionless] + double F1 = exp(pow(-ff, 2)); // Gaussian force-length curve + + double dF1_dLce = 0; + if (returnJacobians) { + double dF1_dLce = -2.0 * ff * F1 / W; + } + + double F2 = 0.0; + + + + //----------------- F2 is the dimensionless force-velocity relationship --------------------------------// + double dF2_dLcedot = 0; + if (Lcedot < 0) { //concentric contraction - ff = Vmax - Lcedot/AHill; - F2 = (Vmax + Lcedot)/ff; - //dF2_dLcedot = (1.0 + F2/AHill)/ff; + ff = Vmax - Lcedot / AHill; + F2 = (Vmax + Lcedot) / ff; + if (returnJacobians) { + double dF2_dLcedot = (1.0 + F2 / AHill) / ff; } - else { + } + else { //eccentric contraction double c = Vmax * AHill * (FVmax - 1.0) / (AHill + 1.0); // parameter in the eccentric force-velocity equation - ff = Lcedot + c; - F2 = (FVmax*Lcedot + c) / ff; - //dF2_dLcedot = (FVmax - F2)/ff; + ff = Lcedot + c; + F2 = (FVmax * Lcedot + c) / ff; + if (returnJacobians) { + double dF2_dLcedot = (FVmax - F2) / ff; + } } - //F3 is the dimensionless PEE force (in units of Fmax) - double kPEE = 1.0/Fmax*Lceopt; // stiffness of the linear term is 1 N/m, convert to Fmax/Lceopt units + + + //----------F3 is the dimensionless PEE force (in units of Fmax)-------------// + double kPEE = 1.0 / Fmax * Lceopt; // stiffness of the linear term is 1 N/m, convert to Fmax/Lceopt units ff = (Lce - PEELslack); // elongation of PEE, relative to Lceopt - double F3 = kPEE*ff; // low stiffness linear term - //dF3_dLce = kPEE; - if (ff>0) { + double F3 = kPEE * ff; // low stiffness linear term + double dF3_dLce = kPEE; + if (ff > 0) { //add quadratic term for positive elongation F3 = F3 + kPEE2 * pow(ff, 2); - //dF3_dLce = dF3_dLce + 2*kPEE2*ff; + if (returnJacobians) { + double F3_dLce = dF3_dLce + 2 * kPEE2 * ff; + } } - //F4 is the dimensionless SEE force (in units of Fmax) - double kSEE = 1.0/Fmax; // stiffness of the linear term is 1 N/m, convert to Fmax/m - ff = Lm - Lce*Lceopt - SEELslack; // elongation of SEE, in meters - double F4 = kSEE*ff; // low stiffness linear term - //dF4_dLce = -kSEE*Lceopt; - //dF4_dLm = kSEE; - if (ff>0) { + + + //---------------F4 is the dimensionless SEE force (in units of Fmax)----------// + double kSEE = 1.0 / Fmax; // stiffness of the linear term is 1 N/m, convert to Fmax/m + ff = Lm - Lce * Lceopt - SEELslack; // elongation of SEE, in meters + double F4 = kSEE * ff; // low stiffness linear term + double dF4_dLce = 0; + double dF4_dLm = 0; + if (returnJacobians) { + double dF4_dLce = -kSEE * Lceopt; + double dF4_dLm = kSEE; + } + if (ff > 0) { // add quadratic term for positive deformation F4 = F4 + kSEE2 * pow(ff, 2); - //dF4_dLce = dF4_dLce - 2 * kSEE2 * Lceopt * ff; - //dF4_dLm = dF4_dLm + 2 * kSEE2 * ff; + if (returnJacobians) { + double dF4_dLce = dF4_dLce - 2 * kSEE2 * Lceopt * ff; + double dF4_dLm = dF4_dLm + 2 * kSEE2 * ff; + } } - //F5 is viscous damping parallel to the CE (0.001 of Fmax at 1 Lceopt/s) to + + + //-------------- F5 is viscous damping parallel to the CE (0.001 of Fmax at 1 Lceopt/s) to -----------// // ensure that df/dLcedot is never zero - double F5 = 0.001*Lcedot; // TODO: is 0.001 suppossed to b? - //dF5_dLcedot = 0.001; + double F5 = 0.001 * Lcedot; // TODO: is 0.001 suppossed to b? + double dF5_dLcedot = 0.001; //The muscle dynamics equation: f = Fsee - Fce - Fpee - Fdamping = 0 - double muscleForceResidual = F4 - a*F1*F2 - F3 - F5; + double muscleForceResidual = F4 - a * F1 * F2 - F3 - F5; - /*if (nargout > 1) - dfdy(5,6) = -F1*F2; % df/da - dfdy(5,5) = dF4_dLce - a*dF1_dLce*F2 - dF3_dLce; % df/dLce - dfdydot(5,5) = -a*F1*dF2_dLcedot - dF5_dLcedot; % df/dLcedot - df_dLm = dF4_dLm; - dfdy(5,3) = -(d/L)* df_dLm; % derivative of f with respect to hand position - end*/ + if (returnJacobians) { + dfdy[0][2] = -F1 * F2; // df/da + dfdy[0][1] = dF4_dLce - a * dF1_dLce * F2 - dF3_dLce; // df/dLce + dfdydot[0][1] = -a * F1 * dF2_dLcedot - dF5_dLcedot; // df/dLcedot + double df_dLm = dF4_dLm; + //dfdy[1][1] = -(d / L) * df_dLm; // derivative of f with respect to muscle length + dfdy[0][0] = -df_dLm; + } + - // Force in SEE is Fmax*F4 - double Fsee = Fmax*F4; + + // ---------------- Force in SEE is Fmax*F4 ---------------------------- // + double Fsee = Fmax * F4; + + // TODO: I don't think we need to implement dFsee_..... /*if (nargout > 1) dFsee_dLm = Fmax * dF4_dLm; dFsee_dLce = Fmax * dF4_dLce; dFsee_dy = [0;0;(-d/L)*dFsee_dLm; 0;dFsee_dLce;0;0]; % Fsee is a function of y(3) & y(5) end*/ - //Activation dynamics equation - double activationResidual = adot-(u-a)*(u/Tact + (1-u)/Tdeact); - /*if (nargout > 1) - dfdy(6,6) = (y(7)/model.Tact + (1-y(7))/model.Tdeact); - dfdy(6,7) = -(y(7)/model.Tact + (1-y(7))/model.Tdeact) ... - -(y(7)-y(6))*(1/model.Tact - 1/model.Tdeact); - dfdydot(6,6) = 1; - end*/ - return SimTK::Vec3(muscleForceResidual, activationResidual, Fsee); - // array outArr; - // outArr[0]=muscleForceResidual; - // outArr[1]=activationResidual ; - // outArr[2]=Fsee; - // return outArr; + //----------------------Activation dynamics equation-------------------// + double activationResidual = adot - (u - a) * (u / Tact + (1 - u) / Tdeact); + if (returnJacobians) { + dfdy[1][2] = (u / Tact + (1 - u) / Tdeact); + double dfdu = -(u / Tact + (1 - u) / Tdeact) - (u - a) * (1 / Tact - 1 / Tdeact); + dfdydot[1][2] = 1; + } + + + + VandenBogert2011Muscle::ImplicitResults Results; + + Results.forceResidual=muscleForceResidual; + Results.activationResidual=activationResidual; + Results.forceSee=Fsee; + Results.df_dy=dfdy; + Results.df_dydot=dfdydot; + + +return Results; + } diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index 4848096303..88351519c6 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -115,6 +115,16 @@ The parent class, Muscle.h, provides void setTdeact(double Tdeact); double getTdeact() const; + //struct ImplicitResults; + + struct ImplicitResults { //DIMENSION UNITS + double forceResidual = SimTK::NaN; //length/time m/s + double activationResidual = SimTK::NaN; //length/time m/s + double forceSee = SimTK::NaN; + SimTK::Mat23 df_dy = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; + SimTK::Mat23 df_dydot = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; }; + + /** default values for states double getDefaultActiveMotorUnits() const; void setDefaultActiveMotorUnits(double activeMotorUnits); @@ -126,8 +136,8 @@ The parent class, Muscle.h, provides //============================================================================= //std::array calcImplicitResidual(const SimTK::State& s) const; - SimTK::Vec3 calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u) const; - + //SimTK::Vec3 calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u, int returnJacobians) const; + ImplicitResults calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u, int returnJacobians) const; protected: //============================================================================= // PROTECTED METHODS @@ -148,6 +158,17 @@ The parent class, Muscle.h, provides default values for state variables */ void extendSetPropertiesFromState(const SimTK::State &s) override; + + + + + +//============================================================================= +// DATA +//============================================================================= + + + private: /** construct the new properties and set their default values */ void constructProperties(); From f355b138342ef22a8bf83401dfb152eabd836199 Mon Sep 17 00:00:00 2001 From: humphreysb Date: Thu, 7 Jul 2016 14:55:25 -0700 Subject: [PATCH 05/40] Addressed variable names and changed to opensim nomenclature --- .idea/misc.xml | 19 + .idea/modules.xml | 8 + .idea/opensim-core-source.iml | 811 +++++++++++ .idea/vcs.xml | 6 + .idea/workspace.xml | 1370 ++++++++++++++++++ OpenSim/Actuators/VandenBogert2011Muscle.cpp | 251 ++-- OpenSim/Actuators/VandenBogert2011Muscle.h | 98 +- 7 files changed, 2402 insertions(+), 161 deletions(-) create mode 100644 .idea/misc.xml create mode 100644 .idea/modules.xml create mode 100644 .idea/opensim-core-source.iml create mode 100644 .idea/vcs.xml create mode 100644 .idea/workspace.xml diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000000..87a6b246c1 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000000..97bceb9af7 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/opensim-core-source.iml b/.idea/opensim-core-source.iml new file mode 100644 index 0000000000..0ecde5ceed --- /dev/null +++ b/.idea/opensim-core-source.iml @@ -0,0 +1,811 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000000..94a25f7f4c --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/.idea/workspace.xml b/.idea/workspace.xml new file mode 100644 index 0000000000..6629559510 --- /dev/null +++ b/.idea/workspace.xml @@ -0,0 +1,1370 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + project + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1467305298891 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index 230784583f..37c5cce3de 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -45,16 +45,16 @@ void VandenBogert2011Muscle::constructProperties() { setAuthors("A. van den Bogert, B. Humphreys"); //constructProperty_max_isometric_force(1000); - constructProperty_strain_at_max_iso_force_SEE(0.04); + constructProperty_fMaxTendonStrain(0.04); constructProperty_fl_width(0.63); constructProperty_fv_AHill(0.25); - constructProperty_fv_max_multiplier(1.4); + constructProperty_fv_maxMultiplier(1.4); //constructProperty_optimal_fiber_length(0.1); - constructProperty_dampingCoeff_Pee(0.01); - constructProperty_length_slack_Pee(1.0); + constructProperty_dampingCoefficient(0.01); + constructProperty_normFiberSlackLength(1.0); //constructProperty_tendon_slack_length(0.2); - constructProperty_t_act(0.01); - constructProperty_t_deact(0.04); + constructProperty_activTimeConstant(0.01); + constructProperty_deactivTimeConstant(0.04); } @@ -97,46 +97,46 @@ void VandenBogert2011Muscle::extendSetPropertiesFromState(const SimTK::State& s) //-------------------------------------------------------------------------- // GET & SET Properties //-------------------------------------------------------------------------- -void VandenBogert2011Muscle::setStrainAtMaxIsoForceSee(double StrainAtMaxIsoForceSee) { - set_strain_at_max_iso_force_SEE(StrainAtMaxIsoForceSee); +void VandenBogert2011Muscle::setfMaxTendonStrain(double fMaxTendonStrain) { + setfMaxTendonStrain(fMaxTendonStrain); } -double VandenBogert2011Muscle::getStrainAtMaxIsoForceSee() const - { return get_strain_at_max_iso_force_SEE(); } +double VandenBogert2011Muscle::getfMaxTendonStrain() const + { return getfMaxTendonStrain(); } -void VandenBogert2011Muscle::setFlWidth(double FlWidth) - { set_fl_width(FlWidth); } -double VandenBogert2011Muscle::getFlWidth() const - { return get_fl_width(); } +void VandenBogert2011Muscle::setfl_width(double fl_width) + { setfl_width(fl_width); } +double VandenBogert2011Muscle::getfl_width() const + { return getfl_width(); } -void VandenBogert2011Muscle::setFvAHill(double FvAHill) - { set_fv_AHill(FvAHill); } -double VandenBogert2011Muscle::getFvAHill() const - { return get_fv_AHill(); } +void VandenBogert2011Muscle::setfv_AHill(double fv_AHill) + { setfv_AHill(fv_AHill); } +double VandenBogert2011Muscle::getfv_AHill() const + { return getfv_AHill(); } -void VandenBogert2011Muscle::setFvMaxMultiplier(double FvMaxMultiplier) - { set_fv_max_multiplier(FvMaxMultiplier); } -double VandenBogert2011Muscle::getFvMaxMultiplier() const - { return get_fv_max_multiplier(); } +void VandenBogert2011Muscle::setfv_maxMultiplier(double fv_maxMultiplier) + { setfv_maxMultiplier(fv_maxMultiplier); } +double VandenBogert2011Muscle::getfv_maxMultiplier() const + { return getfv_maxMultiplier(); } -void VandenBogert2011Muscle::setDampingCoeffPee(double DampingCoeffPee) - { set_dampingCoeff_Pee(DampingCoeffPee); } -double VandenBogert2011Muscle::getDampingCoeffPee() const - { return get_dampingCoeff_Pee(); } +void VandenBogert2011Muscle::setdampingCoefficient(double dampingCoefficient) + { setdampingCoefficient(dampingCoefficient); } +double VandenBogert2011Muscle::getdampingCoefficient() const + { return getdampingCoefficient(); } -void VandenBogert2011Muscle::setLengthSlackPee(double LengthSlackPee) - { set_length_slack_Pee(LengthSlackPee); } -double VandenBogert2011Muscle::getLengthSlackPee() const - { return get_length_slack_Pee(); } +void VandenBogert2011Muscle::setnormFiberSlackLength(double normFiberSlackLength) + { setnormFiberSlackLength(normFiberSlackLength); } +double VandenBogert2011Muscle::getnormFiberSlackLength() const + { return getnormFiberSlackLength(); } -void VandenBogert2011Muscle::setTact(double Tact) - { set_t_act(Tact); } -double VandenBogert2011Muscle::getTact() const - { return get_t_act(); } +void VandenBogert2011Muscle::setactivTimeConstant(double activTimeConstant) + { setactivTimeConstant(activTimeConstant); } +double VandenBogert2011Muscle::getactivTimeConstant() const + { return getactivTimeConstant(); } -void VandenBogert2011Muscle::setTdeact(double Tdeact) - { set_t_deact(Tdeact); } -double VandenBogert2011Muscle::getTdeact() const - { return get_t_deact(); } +void VandenBogert2011Muscle::setdeactivTimeConstant(double deactivTimeConstant) + { setdeactivTimeConstant(deactivTimeConstant); } +double VandenBogert2011Muscle::getdeactivTimeConstant() const + { return getdeactivTimeConstant(); } @@ -226,7 +226,7 @@ void VandenBogert2011Muscle::computeInitialFiberEquilibrium(SimTK::State& s) con //SimTK::Vec3 VandenBogert2011Muscle::calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u, int returnJacobians) const { -VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u, int returnJacobians) const { +VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResidual(double muslceLength, double fiberLength, double activ, double fiberVelocity, double activ_dot, double u, int returnJacobians) const { // Later when using state variables: //State Variables @@ -235,80 +235,104 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi //double a = getActivation(s); //double Lcedot = getNormalizedFiberVelocity(s); - // Parameters - double Fmax = getMaxIsometricForce(); //Maximum isometric force that the fibers can generate - double umax = getStrainAtMaxIsoForceSee(); //Strain in the series elastic element at load of Fmax - double W = getFlWidth(); //Width parameter of the force-length relationship of the contractile element - double AHill = getFvAHill(); //Hill parameter of the force-velocity relationship - double FVmax = getFvMaxMultiplier(); //Maximal eccentric force multiplier - double Lceopt = getOptimalFiberLength(); //Optimal Length of Contractile Element - double b = getDampingCoeffPee(); //Damping coefficient of damper parallel to the CE (normalized to Fmax) - double SEELslack = getTendonSlackLength(); //Slack length of the series elastic element - double PEELslack = getLengthSlackPee(); //Slack length of the parallel elastic element, divided by Lceopt - double Tact = getTact(); // Activation time(s) - double Tdeact = getTdeact(); // Deactivation time(s) + //TODO: Match symbols to doxygen diagram + // -------------------------Parameters----------------------------// + //F_{iso} Maximum isometric force that the fibers can generate + double maxIsoForce = getMaxIsometricForce(); + //u_{max} (dimensionless) strain in the series elastic element at load of maxIsometricForce + double fMaxTendonStrain = getfMaxTendonStrain(); //Strain in the series elastic element at load of Fmax + + //W (dimensionless) width parameter of the force-length relationship of the muscle fiber + double fl_width = getfl_width(); + + //AHill (dimensionless) Hill parameter of the force-velocity relationship + double fv_AHill= getfv_AHill(); //Hill parameter of the force-velocity relationship + + //FV_{max} (dimensionless) maximal eccentric force + double fv_maxMultiplier = getfv_maxMultiplier(); //Maximal eccentric force multiplier + + // L_{opt} (m) Optimal Length of Contractile Element; + double optFiberLength = getOptimalFiberLength(); //Optimal Length of Contractile Element + + //b (s/m) damping coefficient of damper parallel to the fiber (normalized to maxIsometricForce) + double dampingCoeff = getdampingCoefficient(); + + //L_{slack,fiber} Slack length of the parallel elastic element, divided by Lceopt + double fiberSlackLength= getnormFiberSlackLength(); + + //L_{slack,tendon} (dimensionless) slack length of the tendon + double tendonSlackLength = get_tendon_slack_length(); + + //T_{act} (s) Activation time + double activTimeConstant = getactivTimeConstant(); + + //T_{deact} (s) Deactivation time + double deactivTimeConstant = getdeactivTimeConstant(); + + //TODO: Make these constants // constants derived from the muscle parameters - double Vmax = 10 * Lceopt; //Maximum shortening velocity (m/s) is 10 fiber lengths per sec - double d1 = Vmax * AHill * (FVmax - 1) / (AHill + 1); // parameter in the eccentric force-velocity equation + double Vmax = 10 * optFiberLength; //Maximum shortening velocity (m/s) is 10 fiber lengths per sec - double kPEE2 = 1 / pow(W, 2); // PEE quadratic stiffness, so Fpee = Fmax when Lce = Lce*(1+W) - double kSEE2 = 1 / pow(SEELslack * umax, 2); // SEE quadratic stiffness, so Fsee = Fmax at strain of umax + double kPEE2 = 1 / pow(fl_width, 2); // Fiber (PEE) quadratic stiffness, so Fpee = Fmax when Lce = Lce*(1+W) + double kSEE2 = 1 / pow(fiberSlackLength * fMaxTendonStrain, 2); // Tendon (SEE) quadratic stiffness, so Fsee = Fmax at strain of umax // Jacobian Matrices - SimTK::Mat23 dfdy; - SimTK::Mat23 dfdydot; + SimTK::Mat23 df_dy; + SimTK::Mat23 df_dydot; //----------------F1 is the normalized isometric force-length relationship at maximum activation--------------// - double ff = (Lce - 1.0) / W; // [dimensionless] - double F1 = exp(pow(-ff, 2)); // Gaussian force-length curve + double fiberExp = (fiberLength - 1.0) / fl_width; // [dimensionless] + double F1 = exp(pow(-fiberExp, 2)); // Gaussian force-length curve double dF1_dLce = 0; if (returnJacobians) { - double dF1_dLce = -2.0 * ff * F1 / W; + double dF1_dfiberLength = -2.0 * fiberExp * F1 / fl_width; } - double F2 = 0.0; + //----------------- F2 is the dimensionless force-velocity relationship --------------------------------// - double dF2_dLcedot = 0; - if (Lcedot < 0) { - //concentric contraction - ff = Vmax - Lcedot / AHill; - F2 = (Vmax + Lcedot) / ff; + double F2 = 0.0; + double dF2_dfiberVelocity = 0; + if (fiberVelocity < 0) { + //Hill's equation for concentric contraction + // F2 = (V_{max} + V_{fiber}) / (V_{max} - V_{fiber}/a_{Hill}) + F2 = (Vmax + fiberVelocity ) / (Vmax - fiberVelocity / fv_AHill); if (returnJacobians) { - double dF2_dLcedot = (1.0 + F2 / AHill) / ff; + dF2_dfiberVelocity = (1.0 + F2 / fv_AHill) / (Vmax - fiberVelocity / fv_AHill); } } else { - //eccentric contraction - double c = Vmax * AHill * (FVmax - 1.0) / (AHill + 1.0); // parameter in the eccentric force-velocity equation - ff = Lcedot + c; - F2 = (FVmax * Lcedot + c) / ff; + //Katz Curve for eccentric contraction + // c is Katz Constant + double c = Vmax * fv_AHill * (fv_maxMultiplier - 1.0) / (fv_AHill + 1.0); // parameter in the eccentric force-velocity equation + //F2 = (g_{max} * V_{fiber} + c) / (V_{fiber} + c) + F2 = (fv_maxMultiplier * fiberVelocity + c) / (fiberVelocity + c); if (returnJacobians) { - double dF2_dLcedot = (FVmax - F2) / ff; + dF2_dfiberVelocity = (fv_maxMultiplier - F2) / (fiberVelocity + c); } } - //----------F3 is the dimensionless PEE force (in units of Fmax)-------------// - double kPEE = 1.0 / Fmax * Lceopt; // stiffness of the linear term is 1 N/m, convert to Fmax/Lceopt units - ff = (Lce - PEELslack); // elongation of PEE, relative to Lceopt - double F3 = kPEE * ff; // low stiffness linear term - double dF3_dLce = kPEE; - if (ff > 0) { + //----------F3 is the dimensionless fiber (PEE) force (in units of Fmax)-------------// + double kPEE = 1.0 / maxIsoForce * optFiberLength; // stiffness of the linear term is 1 N/m, convert to Fmax/Lceopt units + double elongationFiber = (fiberLength - fiberSlackLength); // elongation of fiber (PEE), relative to Lceopt + double F3 = kPEE * elongationFiber; // low stiffness linear term + double dF3_dfiberLength = kPEE; + if (elongationFiber > 0) { //add quadratic term for positive elongation - F3 = F3 + kPEE2 * pow(ff, 2); + F3 = F3 + kPEE2 * pow(elongationFiber, 2); if (returnJacobians) { - double F3_dLce = dF3_dLce + 2 * kPEE2 * ff; + dF3_dfiberLength = dF3_dfiberLength + 2 * kPEE2 * elongationFiber; } } @@ -316,21 +340,21 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi //---------------F4 is the dimensionless SEE force (in units of Fmax)----------// - double kSEE = 1.0 / Fmax; // stiffness of the linear term is 1 N/m, convert to Fmax/m - ff = Lm - Lce * Lceopt - SEELslack; // elongation of SEE, in meters - double F4 = kSEE * ff; // low stiffness linear term - double dF4_dLce = 0; - double dF4_dLm = 0; + double kSEE = 1.0 / maxIsoForce; // stiffness of the linear term is 1 N/m, convert to Fmax/m + double elongationTendon = muslceLength - fiberLength * optFiberLength - tendonSlackLength; // elongation of tendon (SEE), in meters + double F4 = kSEE * elongationTendon; // low stiffness linear term + double dF4_dfiberLength = 0; + double dF4_dmuscleLength = 0; if (returnJacobians) { - double dF4_dLce = -kSEE * Lceopt; - double dF4_dLm = kSEE; + dF4_dfiberLength = -kSEE * optFiberLength; + dF4_dmuscleLength = kSEE; } - if (ff > 0) { + if (elongationTendon > 0) { // add quadratic term for positive deformation - F4 = F4 + kSEE2 * pow(ff, 2); + F4 = F4 + kSEE2 * pow(elongationTendon, 2); if (returnJacobians) { - double dF4_dLce = dF4_dLce - 2 * kSEE2 * Lceopt * ff; - double dF4_dLm = dF4_dLm + 2 * kSEE2 * ff; + dF4_dfiberLength = dF4_dfiberLength - 2 * kSEE2 * optFiberLength* elongationTendon; + dF4_dmuscleLength = dF4_dmuscleLength + 2 * kSEE2 * elongationTendon; } } @@ -338,26 +362,29 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi //-------------- F5 is viscous damping parallel to the CE (0.001 of Fmax at 1 Lceopt/s) to -----------// // ensure that df/dLcedot is never zero - double F5 = 0.001 * Lcedot; // TODO: is 0.001 suppossed to b? - double dF5_dLcedot = 0.001; + double F5 = dampingCoeff * fiberVelocity ; + double dF5_dfiberVelocity = dampingCoeff; + + //The muscle dynamics equation: f = Fsee - a*Fce - Fpee - Fdamping = 0 + double muscleForceResidual = F4 - activ * F1 * F2 - F3 - F5; + - //The muscle dynamics equation: f = Fsee - Fce - Fpee - Fdamping = 0 - double muscleForceResidual = F4 - a * F1 * F2 - F3 - F5; + //---------------------Assemble Jacobians---------------------------// if (returnJacobians) { - dfdy[0][2] = -F1 * F2; // df/da - dfdy[0][1] = dF4_dLce - a * dF1_dLce * F2 - dF3_dLce; // df/dLce - dfdydot[0][1] = -a * F1 * dF2_dLcedot - dF5_dLcedot; // df/dLcedot - double df_dLm = dF4_dLm; - //dfdy[1][1] = -(d / L) * df_dLm; // derivative of f with respect to muscle length - dfdy[0][0] = -df_dLm; + df_dy[0][2] = -F1 * F2; // df/da + df_dy[0][1] = dF4_dfiberLength - activ * dF1_dLce * F2 - dF3_dfiberLength; // df/dLce + df_dydot[0][1] = -activ * F1 * dF2_dfiberVelocity - dF5_dfiberVelocity ; // df/dLcedot + double df_dLm = dF4_dmuscleLength; + //dfdy[0][0] = -(d / L) * df_dLm; // derivative of f with respect to muscle length + df_dy[0][0] = -df_dLm; } - // ---------------- Force in SEE is Fmax*F4 ---------------------------- // - double Fsee = Fmax * F4; + // ---------------- Force in tendon (SEE) is maxIsoForce*F4 ---------------------------- // + double Fsee = maxIsoForce * F4; // TODO: I don't think we need to implement dFsee_..... /*if (nargout > 1) @@ -369,11 +396,12 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi //----------------------Activation dynamics equation-------------------// - double activationResidual = adot - (u - a) * (u / Tact + (1 - u) / Tdeact); + double df_du = 0; + double activationResidual = activ_dot - (u - activ) * (u / activTimeConstant + (1 - u) / deactivTimeConstant ); if (returnJacobians) { - dfdy[1][2] = (u / Tact + (1 - u) / Tdeact); - double dfdu = -(u / Tact + (1 - u) / Tdeact) - (u - a) * (1 / Tact - 1 / Tdeact); - dfdydot[1][2] = 1; + df_dy[1][2] = (u / activTimeConstant + (1 - u) / deactivTimeConstant ); + double df_du = -(u / activTimeConstant + (1 - u) / deactivTimeConstant ) - (u - activ) * (1 / activTimeConstant - 1 / deactivTimeConstant ); + df_dydot[1][2] = 1; } @@ -381,10 +409,11 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi VandenBogert2011Muscle::ImplicitResults Results; Results.forceResidual=muscleForceResidual; - Results.activationResidual=activationResidual; - Results.forceSee=Fsee; - Results.df_dy=dfdy; - Results.df_dydot=dfdydot; + Results.activResidual=activationResidual; + Results.forceTendon=Fsee; + Results.df_dy=df_dy; + Results.df_dydot=df_dydot; + Results.df_du=df_du; return Results; diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index 88351519c6..54a716887d 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -33,41 +33,44 @@ The parent class, Muscle.h, provides */ //OpenSim_DECLARE_PROPERTY(max_isometric_force, double, <-----Comes from parent class, Muscle.h -// "Maximum isometric force that the fibers can generate"); //Fmax (N) +// "Maximum isometric force that the fibers can generate"); +// F_{max} (N) - OpenSim_DECLARE_PROPERTY(strain_at_max_iso_force_SEE, double, - "Strain in the series elastic element at load of Fmax"); //umax (dimensionless) + OpenSim_DECLARE_PROPERTY(fMaxTendonStrain, double, + "tendon strain at maximum isometric muscle force"); + //u_{max} (dimensionless) strain in the series elastic element at load of maxIsometricForce OpenSim_DECLARE_PROPERTY(fl_width, double, - "Width parameter of the force-length relationship of the contractile element"); //W (dimensionless) + "force-length shape factor"); + //W (dimensionless) width parameter of the force-length relationship of the muscle fiber OpenSim_DECLARE_PROPERTY(fv_AHill, double, - "Hill parameter of the force-velocity relationship"); //AHill + "force-velocity shape factor"); + //AHill (dimensionless) Hill parameter of the force-velocity relationship - OpenSim_DECLARE_PROPERTY(fv_max_multiplier, double, - "Maximal eccentric force multiplier"); //FVmax + OpenSim_DECLARE_PROPERTY(fv_maxMultiplier, double, + "maximum normalized lengthening force"); + //FV_{max} (dimensionless) maximal eccentric force //OpenSim_DECLARE_PROPERTY(optimal_fiber_length, double, <-----Comes from parent class, Muscle.h - // "Optimal Length of Contractile Element"); //Lceopt (m) + // "Optimal Length of Contractile Element" //Lceopt (m) - OpenSim_DECLARE_PROPERTY(dampingCoeff_Pee, double, - "Damping coefficient of damper parallel to the CE (normalized to Fmax)"); //b (s/m) + OpenSim_DECLARE_PROPERTY(dampingCoefficient, double, + "The linear damping of the fiber"); + //b (s/m) damping coefficient of damper parallel to the fiber (normalized to maxIsometricForce) - //OpenSim_DECLARE_PROPERTY(tendon_slack_length, double, <-----Comes from parent class, Muscle.h - // "Slack length of the series elastic element"); //SEELslack (m) + OpenSim_DECLARE_PROPERTY(normFiberSlackLength, double, + "(dimensionless) slack length of the parallel elastic element, divided by Lceopt"); + //L_{slack,fiber}(dimensionless) slack length of the fiber (PEE) - OpenSim_DECLARE_PROPERTY(length_slack_Pee, double, - "(dimensionless) slack length of the parallel elastic element, divided by Lceopt"); //b (s/m) + OpenSim_DECLARE_PROPERTY(activTimeConstant, double, + "Activation time(s)"); + //T_{act} (s) Activation time - - - - OpenSim_DECLARE_PROPERTY(t_act, double, - "Activation time(s)"); //Tact (s) - - OpenSim_DECLARE_PROPERTY(t_deact, double, - "Deactivation time(s)"); //Tdeact (s) + OpenSim_DECLARE_PROPERTY(deactivTimeConstant, double, + "Deactivation time(s)"); + //T_{deact} (s) Deactivation time //============================================================================== // PUBLIC METHODS @@ -91,45 +94,40 @@ The parent class, Muscle.h, provides // GET & SET Properties //------------------------------------------------------------------------- // Properties - void setStrainAtMaxIsoForceSee(double StrainAtMaxIsoForceSee); - double getStrainAtMaxIsoForceSee() const; + void setfMaxTendonStrain(double fMaxTendonStrain); + double getfMaxTendonStrain() const; - void setFlWidth(double FlWidth); - double getFlWidth() const; + void setfl_width(double fl_width); + double getfl_width() const; - void setFvAHill(double FvAHill); - double getFvAHill() const; + void setfv_AHill(double fv_AHill); + double getfv_AHill() const; - void setFvMaxMultiplier(double FvMaxMultiplier); - double getFvMaxMultiplier() const; + void setfv_maxMultiplier(double fv_maxMultiplier); + double getfv_maxMultiplier() const; - void setDampingCoeffPee(double DampingCoeffPee); - double getDampingCoeffPee() const; + void setdampingCoefficient(double dampingCoefficient); + double getdampingCoefficient() const; - void setLengthSlackPee(double LengthSlackPee); - double getLengthSlackPee() const; + void setnormFiberSlackLength(double normFiberSlackLength); + double getnormFiberSlackLength() const; - void setTact(double Tact); - double getTact() const; + void setactivTimeConstant(double activTimeConstant); + double getactivTimeConstant() const; - void setTdeact(double Tdeact); - double getTdeact() const; + void setdeactivTimeConstant(double deactivTimeConstant); + double getdeactivTimeConstant() const; //struct ImplicitResults; - struct ImplicitResults { //DIMENSION UNITS - double forceResidual = SimTK::NaN; //length/time m/s - double activationResidual = SimTK::NaN; //length/time m/s - double forceSee = SimTK::NaN; + struct ImplicitResults { + double forceResidual = SimTK::NaN; + double activResidual = SimTK::NaN; + double forceTendon = SimTK::NaN; SimTK::Mat23 df_dy = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; - SimTK::Mat23 df_dydot = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; }; - + SimTK::Mat23 df_dydot = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; + double df_du = SimTK::NaN;}; -/** default values for states - double getDefaultActiveMotorUnits() const; - void setDefaultActiveMotorUnits(double activeMotorUnits); - double getDefaultFatiguedMotorUnits() const; - void setDefaultFatiguedMotorUnits(double fatiguedMotorUnits);*/ //============================================================================= // COMPUTATION @@ -137,7 +135,7 @@ The parent class, Muscle.h, provides //std::array calcImplicitResidual(const SimTK::State& s) const; //SimTK::Vec3 calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u, int returnJacobians) const; - ImplicitResults calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u, int returnJacobians) const; + ImplicitResults calcImplicitResidual(double muslceLength, double fiberLength, double activ, double fiberVelocity, double activ_dot, double u, int returnJacobians) const; protected: //============================================================================= // PROTECTED METHODS From 047189545b6f62a1617cd199376064ed7e6fdf5a Mon Sep 17 00:00:00 2001 From: humphreysb Date: Fri, 8 Jul 2016 12:00:03 -0700 Subject: [PATCH 06/40] Modifed to include pennation. Also added activation-Vmax relationship. Need to tests Jacobians yet. --- .idea/workspace.xml | 51 +++++--- OpenSim/Actuators/VandenBogert2011Muscle.cpp | 130 ++++++++++++++----- OpenSim/Actuators/VandenBogert2011Muscle.h | 16 ++- 3 files changed, 144 insertions(+), 53 deletions(-) diff --git a/.idea/workspace.xml b/.idea/workspace.xml index 6629559510..c57ab5c128 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -140,6 +140,7 @@ + @@ -162,24 +163,34 @@ - - + + - - + + + + + + + + + + + + + + - - + + - - - - - + + + @@ -262,7 +273,7 @@ - + @@ -1348,10 +1359,18 @@ + + + + + + + + - - + + @@ -1360,8 +1379,8 @@ - - + + diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index 37c5cce3de..c54dfcb8bf 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -55,7 +55,7 @@ void VandenBogert2011Muscle::constructProperties() //constructProperty_tendon_slack_length(0.2); constructProperty_activTimeConstant(0.01); constructProperty_deactivTimeConstant(0.04); - + constructProperty_pennAtOptFiberLength(0.0); } // Define new states and their derivatives in the underlying system @@ -138,7 +138,10 @@ void VandenBogert2011Muscle::setdeactivTimeConstant(double deactivTimeConstant) double VandenBogert2011Muscle::getdeactivTimeConstant() const { return getdeactivTimeConstant(); } - +void VandenBogert2011Muscle::setpennAtOptFiberLength(double pennAtOptFiberLength) +{ setpennAtOptFiberLength(pennAtOptFiberLength); } +double VandenBogert2011Muscle::getpennAtOptFiberLength() const +{ return getpennAtOptFiberLength(); } //-------------------------------------------------------------------------- @@ -226,7 +229,7 @@ void VandenBogert2011Muscle::computeInitialFiberEquilibrium(SimTK::State& s) con //SimTK::Vec3 VandenBogert2011Muscle::calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u, int returnJacobians) const { -VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResidual(double muslceLength, double fiberLength, double activ, double fiberVelocity, double activ_dot, double u, int returnJacobians) const { +VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResidual(double muslceLength, double projFibLen, double activ, double projFibVel, double activ_dot, double u, int returnJacobians) const { // Later when using state variables: //State Variables @@ -270,7 +273,10 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi //T_{deact} (s) Deactivation time double deactivTimeConstant = getdeactivTimeConstant(); - //TODO: Make these constants + //phi_{opt} pennation at Optimal Fiber Length + double pennAtOptFiberLength = getpennAtOptFiberLength(); + + // constants derived from the muscle parameters double Vmax = 10 * optFiberLength; //Maximum shortening velocity (m/s) is 10 fiber lengths per sec @@ -284,46 +290,92 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi SimTK::Mat23 df_dydot; + //-------------------Convert projFiberLength & Velocity to fiberLength & Velocity------/ + //TODO: May want to make this into a seperate function + //TODO: Add symbolic equations comments + double cosPenn; + double dcosPenn_dprojFibLen; + double fiberLength; + double dfiberLength_dprojFibLen; + if (pennAtOptFiberLength<0.01) { + // If pennation is zero, we can't do this because volume is zero, and fiberLength ~ projFiberLength + cosPenn=1.0; + fiberLength=projFibLen; + dfiberLength_dprojFibLen=1; + dcosPenn_dprojFibLen=0; + } + else { + double b=sin(pennAtOptFiberLength); + fiberLength=sqrt(pow(projFibLen,2) + pow(b,2)); + cosPenn = projFibLen/fiberLength; + dfiberLength_dprojFibLen=cosPenn; + dcosPenn_dprojFibLen=pow(b,2) / pow(fiberLength,3); + + } + + // Compute fiberVelocity and its derivatives wrt projFibLen and projFibVel + double fiberVelocity = projFibVel*cosPenn; + double dfiberVelocity_dprojFibVel = cosPenn; + double dfiberVelocity_dprojFibLen = projFibVel * dcosPenn_dprojFibLen; + //----------------F1 is the normalized isometric force-length relationship at maximum activation--------------// double fiberExp = (fiberLength - 1.0) / fl_width; // [dimensionless] double F1 = exp(pow(-fiberExp, 2)); // Gaussian force-length curve - double dF1_dLce = 0; + double dF1_dfiberLength = 0; if (returnJacobians) { double dF1_dfiberLength = -2.0 * fiberExp * F1 / fl_width; + double dF1_dprojFibLen = dF1_dfiberLength * dfiberLength_dprojFibLen; } - - //----------------- F2 is the dimensionless force-velocity relationship --------------------------------// - double F2 = 0.0; - double dF2_dfiberVelocity = 0; + double F2; + double dF2_dfiberVelocity; + double dF2_dactiv; + double dF2_dprojFibVel; + double dF2_dprojFibLen; + + // Chow/Darling Vel-Activation Relationship //TODO: Add full reference + //double lambda = 0.5025 + 0.5341*activ; + //double dlambda_da = 0.5341; + double lambda = 1; //Turn it off for now as it seems to cause an issue with negative force large concentric vel + double dlambda_da =0; + if (fiberVelocity < 0) { //Hill's equation for concentric contraction // F2 = (V_{max} + V_{fiber}) / (V_{max} - V_{fiber}/a_{Hill}) - F2 = (Vmax + fiberVelocity ) / (Vmax - fiberVelocity / fv_AHill); + double hillDenom = (lambda*Vmax - fiberVelocity/fv_AHill); + F2 = (lambda*Vmax + fiberVelocity) / hillDenom; if (returnJacobians) { - dF2_dfiberVelocity = (1.0 + F2 / fv_AHill) / (Vmax - fiberVelocity / fv_AHill); + dF2_dfiberVelocity = (1.0 + F2 / fv_AHill) / hillDenom; + dF2_dactiv = - dlambda_da * Vmax * fiberVelocity * (1 + 1/fv_AHill) / pow(hillDenom,2); } } else { //Katz Curve for eccentric contraction // c is Katz Constant - double c = Vmax * fv_AHill * (fv_maxMultiplier - 1.0) / (fv_AHill + 1.0); // parameter in the eccentric force-velocity equation + double c3 = Vmax * fv_AHill * (fv_maxMultiplier - 1.0) / (fv_AHill + 1.0); // parameter in the eccentric force-velocity equation + double c = lambda*c3; //F2 = (g_{max} * V_{fiber} + c) / (V_{fiber} + c) - F2 = (fv_maxMultiplier * fiberVelocity + c) / (fiberVelocity + c); + double katzDenom = (fiberVelocity + c); + F2 = (fv_maxMultiplier * fiberVelocity + c) / katzDenom ; if (returnJacobians) { - dF2_dfiberVelocity = (fv_maxMultiplier - F2) / (fiberVelocity + c); + dF2_dfiberVelocity = (fv_maxMultiplier - F2) / katzDenom ; + dF2_dactiv = dlambda_da * c3 * fiberVelocity * (1-fv_maxMultiplier) / pow(katzDenom,2); } } - + if (returnJacobians){ + dF2_dprojFibVel = dF2_dfiberVelocity * dfiberVelocity_dprojFibVel; + dF2_dprojFibLen = dF2_dfiberVelocity * dfiberVelocity_dprojFibLen; + } //----------F3 is the dimensionless fiber (PEE) force (in units of Fmax)-------------// + double dF3_dprojFibLen; double kPEE = 1.0 / maxIsoForce * optFiberLength; // stiffness of the linear term is 1 N/m, convert to Fmax/Lceopt units double elongationFiber = (fiberLength - fiberSlackLength); // elongation of fiber (PEE), relative to Lceopt double F3 = kPEE * elongationFiber; // low stiffness linear term @@ -335,25 +387,27 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi dF3_dfiberLength = dF3_dfiberLength + 2 * kPEE2 * elongationFiber; } } - - + if (returnJacobians) { + dF3_dprojFibLen = dF3_dfiberLength * dfiberLength_dprojFibLen; + } //---------------F4 is the dimensionless SEE force (in units of Fmax)----------// double kSEE = 1.0 / maxIsoForce; // stiffness of the linear term is 1 N/m, convert to Fmax/m double elongationTendon = muslceLength - fiberLength * optFiberLength - tendonSlackLength; // elongation of tendon (SEE), in meters double F4 = kSEE * elongationTendon; // low stiffness linear term - double dF4_dfiberLength = 0; - double dF4_dmuscleLength = 0; + double dF4_dfiberLength; + double dF4_dmuscleLength; + double dF4_dprojFibLen; if (returnJacobians) { - dF4_dfiberLength = -kSEE * optFiberLength; + double dF4_dprojFibLen = -kSEE * optFiberLength; dF4_dmuscleLength = kSEE; } if (elongationTendon > 0) { // add quadratic term for positive deformation F4 = F4 + kSEE2 * pow(elongationTendon, 2); if (returnJacobians) { - dF4_dfiberLength = dF4_dfiberLength - 2 * kSEE2 * optFiberLength* elongationTendon; + dF4_dprojFibLen = dF4_dprojFibLen - 2 * kSEE2 * optFiberLength* elongationTendon; dF4_dmuscleLength = dF4_dmuscleLength + 2 * kSEE2 * elongationTendon; } } @@ -362,23 +416,33 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi //-------------- F5 is viscous damping parallel to the CE (0.001 of Fmax at 1 Lceopt/s) to -----------// // ensure that df/dLcedot is never zero - double F5 = dampingCoeff * fiberVelocity ; - double dF5_dfiberVelocity = dampingCoeff; + double F5 = dampingCoeff * projFibVel ; //TODO: Should this be: dampingCoeff * fiberVelocity (not proj)? + double dF5_dprojFibVel = dampingCoeff; + - //The muscle dynamics equation: f = Fsee - a*Fce - Fpee - Fdamping = 0 - double muscleForceResidual = F4 - activ * F1 * F2 - F3 - F5; + // ----------------------Calculate the Muscle Force Residual ------------------------- // + //The muscle dynamics equation: f = Fsee - (a*Fce - Fpee)*cos(Penn) - Fdamping = 0 + double fRes = F4 - (activ * F1 * F2 + F3)*cosPenn - F5; - //---------------------Assemble Jacobians---------------------------// + //---------------------Assemble Force Jacobian---------------------------// if (returnJacobians) { - df_dy[0][2] = -F1 * F2; // df/da - df_dy[0][1] = dF4_dfiberLength - activ * dF1_dLce * F2 - dF3_dfiberLength; // df/dLce - df_dydot[0][1] = -activ * F1 * dF2_dfiberVelocity - dF5_dfiberVelocity ; // df/dLcedot - double df_dLm = dF4_dmuscleLength; - //dfdy[0][0] = -(d / L) * df_dLm; // derivative of f with respect to muscle length - df_dy[0][0] = -df_dLm; + + double dfRes_dactiv = -(F1*F2 + activ*F1*dF2_dactiv )*cosPenn; + double dfRes_dprojFibLength = dF4_dprojFibLen - (activ*(dF1_dfiberLength*F2 + F1*dF2_dprojFibLen) + dF3_dprojFibLen) * cosPenn - (activ*F1*F2 + F3)*dcosPenn_dprojFibLen; + double dfRes_dprojFibVel = - activ*F1*dF2_dprojFibVel - dF5_dprojFibVel; + double df_dmuscleLength = dF4_dmuscleLength; + + df_dy[0][0] = df_dmuscleLength; + df_dy[0][1] = dfRes_dprojFibLength; + df_dy[0][1] = dfRes_dactiv; + + df_dy[1][0] = 0; + df_dy[1][1] = 0; + df_dy[1][2] = dfRes_dprojFibVel; + } @@ -408,7 +472,7 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi VandenBogert2011Muscle::ImplicitResults Results; - Results.forceResidual=muscleForceResidual; + Results.forceResidual=fRes; Results.activResidual=activationResidual; Results.forceTendon=Fsee; Results.df_dy=df_dy; diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index 54a716887d..82163fb5d1 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -72,6 +72,11 @@ The parent class, Muscle.h, provides "Deactivation time(s)"); //T_{deact} (s) Deactivation time + OpenSim_DECLARE_PROPERTY(pennAtOptFiberLength, double, + "pennation at optimal fiber length equilbrium"); + // phi_opt + + //============================================================================== // PUBLIC METHODS //============================================================================== @@ -118,6 +123,9 @@ The parent class, Muscle.h, provides void setdeactivTimeConstant(double deactivTimeConstant); double getdeactivTimeConstant() const; + void setpennAtOptFiberLength(double pennAtOptFiberLength); + double getpennAtOptFiberLength() const; + //struct ImplicitResults; struct ImplicitResults { @@ -133,10 +141,10 @@ The parent class, Muscle.h, provides // COMPUTATION //============================================================================= -//std::array calcImplicitResidual(const SimTK::State& s) const; - //SimTK::Vec3 calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u, int returnJacobians) const; - ImplicitResults calcImplicitResidual(double muslceLength, double fiberLength, double activ, double fiberVelocity, double activ_dot, double u, int returnJacobians) const; -protected: + //ImplicitResults calcImplicitResidual(double muslceLength, double fiberLength, double activ, double fiberVelocity, double activ_dot, double u, int returnJacobians) const; + ImplicitResults calcImplicitResidual(double muslceLength, double projFiberLength, double activ, double projFiberVelocity, double activ_dot, double u, int returnJacobians) const; + + protected: //============================================================================= // PROTECTED METHODS //============================================================================= From af5a276e95beefc3bf1a80cde4dce5d51110a0bd Mon Sep 17 00:00:00 2001 From: humphreysb Date: Fri, 8 Jul 2016 14:06:20 -0700 Subject: [PATCH 07/40] Add each of the muscle componet forces as individual outputs for troubleshooting (temporary) --- .idea/workspace.xml | 199 ++++++++++++++++-- .../Matlab/tests/testVandenBogert2011Muscle.m | 11 +- OpenSim/Actuators/VandenBogert2011Muscle.cpp | 6 + OpenSim/Actuators/VandenBogert2011Muscle.h | 11 +- 4 files changed, 202 insertions(+), 25 deletions(-) diff --git a/.idea/workspace.xml b/.idea/workspace.xml index c57ab5c128..d3d47e5e42 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -140,7 +140,7 @@ - + @@ -163,11 +163,11 @@ - + - - + + @@ -178,18 +178,58 @@ - + - + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -207,8 +247,9 @@ @@ -267,6 +308,78 @@ - + - + - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m b/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m index dace248501..40d84eb382 100644 --- a/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m +++ b/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m @@ -8,9 +8,10 @@ Lcedot = 0; adot = 0; u = 0; -out = muscle.calcImplicitResidual(Lm, Lce, a, Lcedot, adot, u); +out = muscle.calcImplicitResidual(Lm, Lce, a, Lcedot, adot, u, 0); % out should have 3 elements: -assert(out.size() == 3); -disp(out.get(0)) -disp(out.get(1)) -disp(out.get(2)) +%assert(out.size() == 3); +%disp(out.get(0)) +%disp(out.get(1)) +%disp(out.get(2)) +methods(out) \ No newline at end of file diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index c54dfcb8bf..5b6dd62995 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -478,6 +478,12 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi Results.df_dy=df_dy; Results.df_dydot=df_dydot; Results.df_du=df_du; + Results.F1=F1; //Output these for troubleshooting + Results.F2=F2; + Results.F3=F3; + Results.F4=F4; + Results.F5=F5; + return Results; diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index 82163fb5d1..5f31943ab9 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -134,7 +134,16 @@ The parent class, Muscle.h, provides double forceTendon = SimTK::NaN; SimTK::Mat23 df_dy = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; SimTK::Mat23 df_dydot = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; - double df_du = SimTK::NaN;}; + double df_du = SimTK::NaN; + double F1 = SimTK::NaN; + double F2 = SimTK::NaN; + double F3 = SimTK::NaN; + double F4 = SimTK::NaN; + double F5 = SimTK::NaN; + + + + }; //============================================================================= From 160092d01fd549b9e2f73837c969422c3cb1a615 Mon Sep 17 00:00:00 2001 From: humphreysb Date: Fri, 8 Jul 2016 23:42:49 -0700 Subject: [PATCH 08/40] Fixed setters and getters. --- .idea/workspace.xml | 77 ++++++------ OpenSim/Actuators/VandenBogert2011Muscle.cpp | 123 ++++++++++--------- OpenSim/Actuators/VandenBogert2011Muscle.h | 38 +++--- 3 files changed, 119 insertions(+), 119 deletions(-) diff --git a/.idea/workspace.xml b/.idea/workspace.xml index d3d47e5e42..6cc58294de 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -140,7 +140,6 @@ - @@ -163,11 +162,11 @@ - + - - + + @@ -185,16 +184,6 @@ - - - - - - - - - - @@ -235,6 +224,16 @@ + + + + + + + + + + @@ -248,8 +247,8 @@ @@ -260,10 +259,10 @@ DEFINITION_ORDER - @@ -1227,12 +1226,12 @@ - + - + - + @@ -1480,14 +1479,6 @@ - - - - - - - - @@ -1512,15 +1503,15 @@ - + - - + + - + @@ -1528,23 +1519,31 @@ - + - - + + - - + + + + + + + + + + \ No newline at end of file diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index 5b6dd62995..d79efcce97 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -45,17 +45,17 @@ void VandenBogert2011Muscle::constructProperties() { setAuthors("A. van den Bogert, B. Humphreys"); //constructProperty_max_isometric_force(1000); - constructProperty_fMaxTendonStrain(0.04); + constructProperty_fMaxTendonStrain(0.033); constructProperty_fl_width(0.63); constructProperty_fv_AHill(0.25); - constructProperty_fv_maxMultiplier(1.4); + constructProperty_fv_maxMultiplier(1.5); //constructProperty_optimal_fiber_length(0.1); constructProperty_dampingCoefficient(0.01); constructProperty_normFiberSlackLength(1.0); //constructProperty_tendon_slack_length(0.2); constructProperty_activTimeConstant(0.01); constructProperty_deactivTimeConstant(0.04); - constructProperty_pennAtOptFiberLength(0.0); + constructProperty_pennAtOptFiberLength(0.1745); } // Define new states and their derivatives in the underlying system @@ -97,51 +97,52 @@ void VandenBogert2011Muscle::extendSetPropertiesFromState(const SimTK::State& s) //-------------------------------------------------------------------------- // GET & SET Properties //-------------------------------------------------------------------------- -void VandenBogert2011Muscle::setfMaxTendonStrain(double fMaxTendonStrain) { - setfMaxTendonStrain(fMaxTendonStrain); - } -double VandenBogert2011Muscle::getfMaxTendonStrain() const - { return getfMaxTendonStrain(); } - -void VandenBogert2011Muscle::setfl_width(double fl_width) - { setfl_width(fl_width); } -double VandenBogert2011Muscle::getfl_width() const - { return getfl_width(); } - -void VandenBogert2011Muscle::setfv_AHill(double fv_AHill) - { setfv_AHill(fv_AHill); } -double VandenBogert2011Muscle::getfv_AHill() const - { return getfv_AHill(); } - -void VandenBogert2011Muscle::setfv_maxMultiplier(double fv_maxMultiplier) - { setfv_maxMultiplier(fv_maxMultiplier); } -double VandenBogert2011Muscle::getfv_maxMultiplier() const - { return getfv_maxMultiplier(); } - -void VandenBogert2011Muscle::setdampingCoefficient(double dampingCoefficient) - { setdampingCoefficient(dampingCoefficient); } -double VandenBogert2011Muscle::getdampingCoefficient() const - { return getdampingCoefficient(); } - -void VandenBogert2011Muscle::setnormFiberSlackLength(double normFiberSlackLength) - { setnormFiberSlackLength(normFiberSlackLength); } -double VandenBogert2011Muscle::getnormFiberSlackLength() const - { return getnormFiberSlackLength(); } - -void VandenBogert2011Muscle::setactivTimeConstant(double activTimeConstant) - { setactivTimeConstant(activTimeConstant); } -double VandenBogert2011Muscle::getactivTimeConstant() const - { return getactivTimeConstant(); } - -void VandenBogert2011Muscle::setdeactivTimeConstant(double deactivTimeConstant) - { setdeactivTimeConstant(deactivTimeConstant); } -double VandenBogert2011Muscle::getdeactivTimeConstant() const - { return getdeactivTimeConstant(); } - -void VandenBogert2011Muscle::setpennAtOptFiberLength(double pennAtOptFiberLength) -{ setpennAtOptFiberLength(pennAtOptFiberLength); } -double VandenBogert2011Muscle::getpennAtOptFiberLength() const -{ return getpennAtOptFiberLength(); } + + +void VandenBogert2011Muscle::setFMaxTendonStrain(double fMaxTendonStrain) { + set_fMaxTendonStrain(fMaxTendonStrain); } +double VandenBogert2011Muscle::getFMaxTendonStrain() const { + return get_fMaxTendonStrain(); } + +void VandenBogert2011Muscle::setFlWidth(double flWidth) { + set_fl_width(flWidth); } +double VandenBogert2011Muscle::getFlWidth() const + { return get_fl_width(); } + +void VandenBogert2011Muscle::setFvAHill(double fvAHill) { + set_fv_AHill(fvAHill); } +double VandenBogert2011Muscle::getFvAHill() const { + return get_fv_AHill(); } + +void VandenBogert2011Muscle::setFvmaxMultiplier(double fvMaxMultiplier) { + set_fv_maxMultiplier(fvMaxMultiplier); } +double VandenBogert2011Muscle::getFvmaxMultiplier() const { + return get_fv_maxMultiplier(); } + +void VandenBogert2011Muscle::setDampingCoefficient(double dampingCoefficient) { + set_dampingCoefficient(dampingCoefficient); } +double VandenBogert2011Muscle::getDampingCoefficient() const { + return get_dampingCoefficient(); } + +void VandenBogert2011Muscle::setNormFiberSlackLength(double normFiberSlackLength) { + set_normFiberSlackLength(normFiberSlackLength); } +double VandenBogert2011Muscle::getNormFiberSlackLength() const { + return get_normFiberSlackLength(); } + +void VandenBogert2011Muscle::setActivTimeConstant(double activTimeConstant) { + set_activTimeConstant(activTimeConstant); } +double VandenBogert2011Muscle::getActivTimeConstant() const { + return get_activTimeConstant(); } + +void VandenBogert2011Muscle::setDeactivTimeConstant(double deactivTimeConstant) { + set_deactivTimeConstant(deactivTimeConstant); } +double VandenBogert2011Muscle::getDeactivTimeConstant() const { + return get_deactivTimeConstant(); } + +void VandenBogert2011Muscle::setPennAtOptFiberLength(double pennAtOptFiberLength) { + set_pennAtOptFiberLength(pennAtOptFiberLength); } +double VandenBogert2011Muscle::getPennAtOptFiberLength() const { + return get_pennAtOptFiberLength(); } //-------------------------------------------------------------------------- @@ -244,37 +245,37 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi double maxIsoForce = getMaxIsometricForce(); //u_{max} (dimensionless) strain in the series elastic element at load of maxIsometricForce - double fMaxTendonStrain = getfMaxTendonStrain(); //Strain in the series elastic element at load of Fmax + double fMaxTendonStrain = getFMaxTendonStrain(); //Strain in the series elastic element at load of Fmax //W (dimensionless) width parameter of the force-length relationship of the muscle fiber - double fl_width = getfl_width(); + double fl_width = getFlWidth(); //AHill (dimensionless) Hill parameter of the force-velocity relationship - double fv_AHill= getfv_AHill(); //Hill parameter of the force-velocity relationship + double fv_AHill= getFvAHill(); //Hill parameter of the force-velocity relationship //FV_{max} (dimensionless) maximal eccentric force - double fv_maxMultiplier = getfv_maxMultiplier(); //Maximal eccentric force multiplier + double fv_maxMultiplier = getFvmaxMultiplier(); //Maximal eccentric force multiplier // L_{opt} (m) Optimal Length of Contractile Element; double optFiberLength = getOptimalFiberLength(); //Optimal Length of Contractile Element //b (s/m) damping coefficient of damper parallel to the fiber (normalized to maxIsometricForce) - double dampingCoeff = getdampingCoefficient(); + double dampingCoeff = getDampingCoefficient(); //L_{slack,fiber} Slack length of the parallel elastic element, divided by Lceopt - double fiberSlackLength= getnormFiberSlackLength(); + double fiberSlackLength= getNormFiberSlackLength(); //L_{slack,tendon} (dimensionless) slack length of the tendon - double tendonSlackLength = get_tendon_slack_length(); + double tendonSlackLength = getTendonSlackLength(); //T_{act} (s) Activation time - double activTimeConstant = getactivTimeConstant(); + double activTimeConstant = getActivTimeConstant(); //T_{deact} (s) Deactivation time - double deactivTimeConstant = getdeactivTimeConstant(); + double deactivTimeConstant = getDeactivTimeConstant(); //phi_{opt} pennation at Optimal Fiber Length - double pennAtOptFiberLength = getpennAtOptFiberLength(); + double pennAtOptFiberLength = getPennAtOptFiberLength(); // constants derived from the muscle parameters @@ -322,7 +323,7 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi //----------------F1 is the normalized isometric force-length relationship at maximum activation--------------// double fiberExp = (fiberLength - 1.0) / fl_width; // [dimensionless] - double F1 = exp(pow(-fiberExp, 2)); // Gaussian force-length curve + double F1 = exp(-pow(fiberExp, 2)); // Gaussian force-length curve double dF1_dfiberLength = 0; if (returnJacobians) { @@ -481,8 +482,8 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi Results.F1=F1; //Output these for troubleshooting Results.F2=F2; Results.F3=F3; - Results.F4=F4; - Results.F5=F5; + Results.F4=fl_width; + Results.F5=fiberExp; diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index 5f31943ab9..9c4341168b 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -99,32 +99,32 @@ The parent class, Muscle.h, provides // GET & SET Properties //------------------------------------------------------------------------- // Properties - void setfMaxTendonStrain(double fMaxTendonStrain); - double getfMaxTendonStrain() const; + void setFMaxTendonStrain(double fMaxTendonStrain); + double getFMaxTendonStrain() const; - void setfl_width(double fl_width); - double getfl_width() const; + void setFlWidth(double flWidth); + double getFlWidth() const; - void setfv_AHill(double fv_AHill); - double getfv_AHill() const; + void setFvAHill(double fvAHill); + double getFvAHill() const; - void setfv_maxMultiplier(double fv_maxMultiplier); - double getfv_maxMultiplier() const; + void setFvmaxMultiplier(double fvMaxMultiplier); + double getFvmaxMultiplier() const; - void setdampingCoefficient(double dampingCoefficient); - double getdampingCoefficient() const; + void setDampingCoefficient(double dampingCoefficient); + double getDampingCoefficient() const; - void setnormFiberSlackLength(double normFiberSlackLength); - double getnormFiberSlackLength() const; + void setNormFiberSlackLength(double normFiberSlackLength); + double getNormFiberSlackLength() const; - void setactivTimeConstant(double activTimeConstant); - double getactivTimeConstant() const; + void setActivTimeConstant(double activTimeConstant); + double getActivTimeConstant() const; - void setdeactivTimeConstant(double deactivTimeConstant); - double getdeactivTimeConstant() const; + void setDeactivTimeConstant(double deactivTimeConstant); + double getDeactivTimeConstant() const; - void setpennAtOptFiberLength(double pennAtOptFiberLength); - double getpennAtOptFiberLength() const; + void setPennAtOptFiberLength(double pennAtOptFiberLength); + double getPennAtOptFiberLength() const; //struct ImplicitResults; @@ -142,7 +142,7 @@ The parent class, Muscle.h, provides double F5 = SimTK::NaN; - + }; From 74278f06e3666ba4d87af3b272f41aef000f0c14 Mon Sep 17 00:00:00 2001 From: humphreysb Date: Mon, 11 Jul 2016 23:09:28 -0700 Subject: [PATCH 09/40] Added calcJacobianByFiniteDiff method --- .idea/codeStyleSettings.xml | 38 +++ .idea/workspace.xml | 329 +++++++++++-------- OpenSim/Actuators/VandenBogert2011Muscle.cpp | 309 ++++++++++++----- OpenSim/Actuators/VandenBogert2011Muscle.h | 7 +- 4 files changed, 460 insertions(+), 223 deletions(-) create mode 100644 .idea/codeStyleSettings.xml diff --git a/.idea/codeStyleSettings.xml b/.idea/codeStyleSettings.xml new file mode 100644 index 0000000000..23f6cae16e --- /dev/null +++ b/.idea/codeStyleSettings.xml @@ -0,0 +1,38 @@ + + + + + + \ No newline at end of file diff --git a/.idea/workspace.xml b/.idea/workspace.xml index 6cc58294de..7d3b4225fc 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -140,6 +140,7 @@ + @@ -165,8 +166,8 @@ - - + + @@ -177,18 +178,8 @@ - - - - - - - - - - - - + + @@ -207,28 +198,18 @@ - + - - - - - - - - - - - - + + @@ -259,10 +240,10 @@ DEFINITION_ORDER - @@ -307,86 +288,14 @@ - - + + @@ -1226,27 +1135,28 @@ - + - + + - + - - + + @@ -1284,6 +1194,156 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -1306,7 +1366,6 @@ - @@ -1314,7 +1373,6 @@ - @@ -1322,7 +1380,6 @@ - @@ -1330,7 +1387,6 @@ - @@ -1443,7 +1499,6 @@ - @@ -1451,7 +1506,6 @@ - @@ -1459,7 +1513,6 @@ - @@ -1467,15 +1520,6 @@ - - - - - - - - - @@ -1483,7 +1527,6 @@ - @@ -1491,19 +1534,17 @@ - - + - - - + + - + @@ -1511,10 +1552,10 @@ - + - - + + @@ -1527,10 +1568,18 @@ + + + + + + + + - - + + @@ -1539,8 +1588,8 @@ - - + + diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index d79efcce97..6dd7c5558a 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -59,7 +59,8 @@ void VandenBogert2011Muscle::constructProperties() } // Define new states and their derivatives in the underlying system -void VandenBogert2011Muscle::extendAddToSystem(SimTK::MultibodySystem& system) const +void VandenBogert2011Muscle::extendAddToSystem(SimTK::MultibodySystem& system) +const { // No States to add, yet @@ -73,10 +74,12 @@ void VandenBogert2011Muscle::extendAddToSystem(SimTK::MultibodySystem& system) c // and their corresponding derivatives 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("fatigued_motor_units_deriv", 0.0, SimTK::Stage::Dynamics); + */ } -void VandenBogert2011Muscle::extendInitStateFromProperties(SimTK::State& s) const +void VandenBogert2011Muscle::extendInitStateFromProperties(SimTK::State& s) +const { // No States Yet /*Super::extendInitStateFromProperties(s); @@ -124,7 +127,8 @@ void VandenBogert2011Muscle::setDampingCoefficient(double dampingCoefficient) { double VandenBogert2011Muscle::getDampingCoefficient() const { return get_dampingCoefficient(); } -void VandenBogert2011Muscle::setNormFiberSlackLength(double normFiberSlackLength) { +void VandenBogert2011Muscle::setNormFiberSlackLength(double + normFiberSlackLength) { set_normFiberSlackLength(normFiberSlackLength); } double VandenBogert2011Muscle::getNormFiberSlackLength() const { return get_normFiberSlackLength(); } @@ -134,12 +138,14 @@ void VandenBogert2011Muscle::setActivTimeConstant(double activTimeConstant) { double VandenBogert2011Muscle::getActivTimeConstant() const { return get_activTimeConstant(); } -void VandenBogert2011Muscle::setDeactivTimeConstant(double deactivTimeConstant) { +void VandenBogert2011Muscle::setDeactivTimeConstant(double + deactivTimeConstant) { set_deactivTimeConstant(deactivTimeConstant); } double VandenBogert2011Muscle::getDeactivTimeConstant() const { return get_deactivTimeConstant(); } -void VandenBogert2011Muscle::setPennAtOptFiberLength(double pennAtOptFiberLength) { +void VandenBogert2011Muscle::setPennAtOptFiberLength(double + pennAtOptFiberLength) { set_pennAtOptFiberLength(pennAtOptFiberLength); } double VandenBogert2011Muscle::getPennAtOptFiberLength() const { return get_pennAtOptFiberLength(); } @@ -175,8 +181,9 @@ double FatigableMuscle::getActiveMotorUnitsDeriv(const SimTK::State& s) const { return getStateVariableDerivativeValue(s, "active_motor_units"); } void FatigableMuscle::setActiveMotorUnitsDeriv(const SimTK::State& s, - double activeMotorUnitsDeriv) const -{ setStateVariableDerivativeValue(s, "active_motor_units", activeMotorUnitsDeriv); } + double activeMotorUnitsDeriv) const +{ setStateVariableDerivativeValue(s, "active_motor_units", + activeMotorUnitsDeriv); } double FatigableMuscle::getFatiguedMotorUnits(const SimTK::State& s) const { return getStateVariableValue(s, "fatigued_motor_units"); } @@ -189,8 +196,10 @@ double FatigableMuscle::getFatiguedMotorUnitsDeriv(const SimTK::State& s) const { return getStateVariableDerivativeValue(s, "fatigued_motor_units"); } void FatigableMuscle::setFatiguedMotorUnitsDeriv(const SimTK::State& s, - double fatiguedMotorUnitsDeriv) const -{ setStateVariableDerivativeValue(s, "fatigued_motor_units", fatiguedMotorUnitsDeriv); + double fatiguedMotorUnitsDeriv) + const +{ setStateVariableDerivativeValue(s, "fatigued_motor_units", + fatiguedMotorUnitsDeriv); }*/ @@ -205,10 +214,12 @@ void FatigableMuscle::setFatiguedMotorUnitsDeriv(const SimTK::State& s, double VandenBogert2011Muscle::computeActuation(const SimTK::State& s) const {return( 0.0);} -void VandenBogert2011Muscle::setActivation(SimTK::State& s,double activation) const +void VandenBogert2011Muscle::setActivation(SimTK::State& s,double activation) +const {} -void VandenBogert2011Muscle::computeInitialFiberEquilibrium(SimTK::State& s) const +void VandenBogert2011Muscle::computeInitialFiberEquilibrium(SimTK::State& s) +const {} @@ -227,15 +238,34 @@ void VandenBogert2011Muscle::computeInitialFiberEquilibrium(SimTK::State& s) con //array calcImplicitResidual(const SimTK::State& s) const -//SimTK::Vec3 VandenBogert2011Muscle::calcImplicitResidual(double Lm, double Lce, double a, double Lcedot, double adot, double u, int returnJacobians) const { +//SimTK::Vec3 VandenBogert2011Muscle::calcImplicitResidual(double Lm, +// double Lce, double a, double Lcedot, double adot, double u, +// int returnJacobians) const { -VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResidual(double muslceLength, double projFibLen, double activ, double projFibVel, double activ_dot, double u, int returnJacobians) const { +VandenBogert2011Muscle::ImplicitResidual +VandenBogert2011Muscle::calcImplicitResidual(SimTK::Vec3 y,SimTK::Vec3 ydot, + double u, int returnJacobians) const { + + VandenBogert2011Muscle::ImplicitResidual impRes= + calcImplicitResidual(y[0],y[1], y[2], ydot[1], ydot[2], u, + returnJacobians); + + return impRes; +} + + + +VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle:: +calcImplicitResidual(double muslceLength, double projFibLen, double activ, + double projFibVel, double activ_dot, double u, + int returnJacobians=0) const { // Later when using state variables: //State Variables //double Lm = getLength(s); - //double Lce = getNormalizedFiberLength(s); //Ton: Lce is dimensionless, it is the muscle fiber length divided by Lceopt + //double Lce = getNormalizedFiberLength(s); //Ton: Lce is dimensionless, + // it is the muscle fiber length divided by Lceopt //double a = getActivation(s); //double Lcedot = getNormalizedFiberVelocity(s); @@ -244,25 +274,32 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi //F_{iso} Maximum isometric force that the fibers can generate double maxIsoForce = getMaxIsometricForce(); - //u_{max} (dimensionless) strain in the series elastic element at load of maxIsometricForce - double fMaxTendonStrain = getFMaxTendonStrain(); //Strain in the series elastic element at load of Fmax + //u_{max} (dimensionless) strain in the series elastic element at load of + // maxIsometricForce + double fMaxTendonStrain = getFMaxTendonStrain(); //Strain in the series + // elastic element at load of Fmax - //W (dimensionless) width parameter of the force-length relationship of the muscle fiber + //W (dimensionless) width parameter of the force-length relationship of + // the muscle fiber double fl_width = getFlWidth(); //AHill (dimensionless) Hill parameter of the force-velocity relationship - double fv_AHill= getFvAHill(); //Hill parameter of the force-velocity relationship + double fv_AHill= getFvAHill(); //Hill parameter of the f-v relationship //FV_{max} (dimensionless) maximal eccentric force - double fv_maxMultiplier = getFvmaxMultiplier(); //Maximal eccentric force multiplier + double fv_maxMultiplier = getFvmaxMultiplier(); + //Maximal eccentric force multiplier // L_{opt} (m) Optimal Length of Contractile Element; - double optFiberLength = getOptimalFiberLength(); //Optimal Length of Contractile Element + double optFiberLength = getOptimalFiberLength(); + //Optimal Length of Contractile Element - //b (s/m) damping coefficient of damper parallel to the fiber (normalized to maxIsometricForce) + //b (s/m) damping coefficient of damper parallel to the fiber + // (normalized to maxIsometricForce) double dampingCoeff = getDampingCoefficient(); - //L_{slack,fiber} Slack length of the parallel elastic element, divided by Lceopt + //L_{slack,fiber} Slack length of the parallel elastic element, divided by + // Lceopt double fiberSlackLength= getNormFiberSlackLength(); //L_{slack,tendon} (dimensionless) slack length of the tendon @@ -279,19 +316,31 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi // constants derived from the muscle parameters - double Vmax = 10 * optFiberLength; //Maximum shortening velocity (m/s) is 10 fiber lengths per sec + double Vmax = 10 * optFiberLength; + //Maximum shortening velocity (m/s) is 10 fiber lengths per sec - double kPEE2 = 1 / pow(fl_width, 2); // Fiber (PEE) quadratic stiffness, so Fpee = Fmax when Lce = Lce*(1+W) - double kSEE2 = 1 / pow(fiberSlackLength * fMaxTendonStrain, 2); // Tendon (SEE) quadratic stiffness, so Fsee = Fmax at strain of umax + double kPEE2 = 1 / pow(fl_width, 2); // Fiber (PEE) quadratic stiffness, + // so Fpee = Fmax when Lce = Lce*(1+W) + double kSEE2 = 1 / pow(fiberSlackLength * fMaxTendonStrain, 2); + // Tendon (SEE) quadratic stiffness, so Fsee = Fmax at strain of umax // Jacobian Matrices SimTK::Mat23 df_dy; SimTK::Mat23 df_dydot; + //y=(Lm,projFiberLength,activation) <-States + + + //df_fy is 2x3: Where columns are states: + // [muscleLeLength, projFiberLength, activation] + // Rows are: + // [force residual; Activation Residual] + - //-------------------Convert projFiberLength & Velocity to fiberLength & Velocity------/ + + //-------Convert projFiberLength & Velocity to fiberLength & Velocity------/ //TODO: May want to make this into a seperate function //TODO: Add symbolic equations comments double cosPenn; @@ -299,7 +348,8 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi double fiberLength; double dfiberLength_dprojFibLen; if (pennAtOptFiberLength<0.01) { - // If pennation is zero, we can't do this because volume is zero, and fiberLength ~ projFiberLength + // If pennation is zero, we can't do this because volume is zero, + // and fiberLength ~ projFiberLength cosPenn=1.0; fiberLength=projFibLen; dfiberLength_dprojFibLen=1; @@ -321,7 +371,8 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi - //----------------F1 is the normalized isometric force-length relationship at maximum activation--------------// + //---F1 is the normalized isometric force-length relationship at maximum + // activation--------------// double fiberExp = (fiberLength - 1.0) / fl_width; // [dimensionless] double F1 = exp(-pow(fiberExp, 2)); // Gaussian force-length curve @@ -333,7 +384,7 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi - //----------------- F2 is the dimensionless force-velocity relationship --------------------------------// + //-------- F2 is the dimensionless force-velocity relationship -------// double F2; double dF2_dfiberVelocity; double dF2_dactiv; @@ -343,7 +394,8 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi // Chow/Darling Vel-Activation Relationship //TODO: Add full reference //double lambda = 0.5025 + 0.5341*activ; //double dlambda_da = 0.5341; - double lambda = 1; //Turn it off for now as it seems to cause an issue with negative force large concentric vel + double lambda = 1; //Turn it off for now as it seems to cause an issue + // with negative force large concentric vel double dlambda_da =0; if (fiberVelocity < 0) { @@ -353,20 +405,23 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi F2 = (lambda*Vmax + fiberVelocity) / hillDenom; if (returnJacobians) { dF2_dfiberVelocity = (1.0 + F2 / fv_AHill) / hillDenom; - dF2_dactiv = - dlambda_da * Vmax * fiberVelocity * (1 + 1/fv_AHill) / pow(hillDenom,2); + dF2_dactiv = - dlambda_da * Vmax * fiberVelocity * + (1 + 1/fv_AHill) / pow(hillDenom,2); } } else { //Katz Curve for eccentric contraction // c is Katz Constant - double c3 = Vmax * fv_AHill * (fv_maxMultiplier - 1.0) / (fv_AHill + 1.0); // parameter in the eccentric force-velocity equation + double c3 = Vmax * fv_AHill * (fv_maxMultiplier - 1.0) / + (fv_AHill + 1.0); // parameter in the eccentric f-v equation double c = lambda*c3; //F2 = (g_{max} * V_{fiber} + c) / (V_{fiber} + c) double katzDenom = (fiberVelocity + c); F2 = (fv_maxMultiplier * fiberVelocity + c) / katzDenom ; if (returnJacobians) { dF2_dfiberVelocity = (fv_maxMultiplier - F2) / katzDenom ; - dF2_dactiv = dlambda_da * c3 * fiberVelocity * (1-fv_maxMultiplier) / pow(katzDenom,2); + dF2_dactiv = dlambda_da * c3 * fiberVelocity * + (1-fv_maxMultiplier) / pow(katzDenom,2); } } if (returnJacobians){ @@ -375,11 +430,14 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi } - //----------F3 is the dimensionless fiber (PEE) force (in units of Fmax)-------------// + //------F3 is the dimensionless fiber (PEE) force (in units of Fmax)------// double dF3_dprojFibLen; - double kPEE = 1.0 / maxIsoForce * optFiberLength; // stiffness of the linear term is 1 N/m, convert to Fmax/Lceopt units - double elongationFiber = (fiberLength - fiberSlackLength); // elongation of fiber (PEE), relative to Lceopt - double F3 = kPEE * elongationFiber; // low stiffness linear term + // stiffness of the linear term is 1 N/m, convert to Fmax/Lceopt units + double kPEE = 1.0 / maxIsoForce * optFiberLength; + // elongation of fiber (PEE), relative to Lceopt + double elongationFiber = (fiberLength - fiberSlackLength); + double F3 = kPEE * elongationFiber; + // low stiffness linear term double dF3_dfiberLength = kPEE; if (elongationFiber > 0) { //add quadratic term for positive elongation @@ -393,10 +451,14 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi } - //---------------F4 is the dimensionless SEE force (in units of Fmax)----------// - double kSEE = 1.0 / maxIsoForce; // stiffness of the linear term is 1 N/m, convert to Fmax/m - double elongationTendon = muslceLength - fiberLength * optFiberLength - tendonSlackLength; // elongation of tendon (SEE), in meters - double F4 = kSEE * elongationTendon; // low stiffness linear term + //--------F4 is the dimensionless SEE force (in units of Fmax)----------// + // stiffness of the linear term is 1 N/m, convert to Fmax/m + double kSEE = 1.0 / maxIsoForce; + // elongation of tendon (SEE), in meters + double elongationTendon = muslceLength - fiberLength * + optFiberLength - tendonSlackLength; + // low stiffness linear term + double F4 = kSEE * elongationTendon; double dF4_dfiberLength; double dF4_dmuscleLength; double dF4_dprojFibLen; @@ -408,70 +470,106 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi // add quadratic term for positive deformation F4 = F4 + kSEE2 * pow(elongationTendon, 2); if (returnJacobians) { - dF4_dprojFibLen = dF4_dprojFibLen - 2 * kSEE2 * optFiberLength* elongationTendon; - dF4_dmuscleLength = dF4_dmuscleLength + 2 * kSEE2 * elongationTendon; + dF4_dprojFibLen = dF4_dprojFibLen - 2 * kSEE2 * + optFiberLength* elongationTendon; + dF4_dmuscleLength = dF4_dmuscleLength + 2 * kSEE2 * + elongationTendon; } } - //-------------- F5 is viscous damping parallel to the CE (0.001 of Fmax at 1 Lceopt/s) to -----------// - // ensure that df/dLcedot is never zero - double F5 = dampingCoeff * projFibVel ; //TODO: Should this be: dampingCoeff * fiberVelocity (not proj)? + //-- F5 is viscous damping parallel to the CE (0.001 of Fmax at 1 Lceopt/s) + // to ensure that df/dLcedot is never zero-----------// + double F5 = dampingCoeff * projFibVel ; + //TODO: Should this be: dampingCoeff * fiberVelocity (not proj)? double dF5_dprojFibVel = dampingCoeff; - // ----------------------Calculate the Muscle Force Residual ------------------------- // - //The muscle dynamics equation: f = Fsee - (a*Fce - Fpee)*cos(Penn) - Fdamping = 0 + // ---------Calculate the Muscle Force Residual ---------------------- // + //The muscle dynamics equation: f = Fsee - (a*Fce - Fpee)*cos(Penn) - + // Fdamping = 0 double fRes = F4 - (activ * F1 * F2 + F3)*cosPenn - F5; + // --------------- Force in tendon (SEE) is maxIsoForce*F4 -------------- // + double Fsee = maxIsoForce * F4; + + // TODO: I don't think we need to implement dFsee_..... + /*if (nargout > 1) + dFsee_dLm = Fmax * dF4_dLm; + dFsee_dLce = Fmax * dF4_dLce; + dFsee_dy = [0;0;(-d/L)*dFsee_dLm; 0;dFsee_dLce;0;0]; + % Fsee is a function of y(3) & y(5) + end*/ + + + //----------------------Activation dynamics equation-------------------// + + double activationResidual = activ_dot - (u - activ) * + (u / activTimeConstant + (1 - u) / deactivTimeConstant ); + double df_du = 0; + double dActRes_dactiv=0; + double dActRes_dactiv_dot = 0; + + if (returnJacobians) { + dActRes_dactiv= (u / activTimeConstant + (1 - u) / deactivTimeConstant); + + dActRes_dactiv_dot = 1; + + df_du = -(u / activTimeConstant + (1 - u) / deactivTimeConstant ) + - (u - activ) * (1 / activTimeConstant - 1 / + deactivTimeConstant ); + } + + + //---------------------Assemble Force Jacobian---------------------------// if (returnJacobians) { double dfRes_dactiv = -(F1*F2 + activ*F1*dF2_dactiv )*cosPenn; - double dfRes_dprojFibLength = dF4_dprojFibLen - (activ*(dF1_dfiberLength*F2 + F1*dF2_dprojFibLen) + dF3_dprojFibLen) * cosPenn - (activ*F1*F2 + F3)*dcosPenn_dprojFibLen; + double dfRes_dprojFibLength = dF4_dprojFibLen - + (activ*(dF1_dfiberLength*F2 + F1*dF2_dprojFibLen) + + dF3_dprojFibLen) * cosPenn - (activ*F1*F2 + F3) * + dcosPenn_dprojFibLen; double dfRes_dprojFibVel = - activ*F1*dF2_dprojFibVel - dF5_dprojFibVel; - double df_dmuscleLength = dF4_dmuscleLength; - - df_dy[0][0] = df_dmuscleLength; - df_dy[0][1] = dfRes_dprojFibLength; - df_dy[0][1] = dfRes_dactiv; + double dfRes_dmuscleLength = dF4_dmuscleLength; - df_dy[1][0] = 0; - df_dy[1][1] = 0; - df_dy[1][2] = dfRes_dprojFibVel; - - } + //y=(Lm,projFiberLength,activation) <-States + //df_fy is 2x3: Where columns are states: + // [muscleLeLength, projFiberLength, activation] + // Rows are: + // [force residual; Activation Residual] - // ---------------- Force in tendon (SEE) is maxIsoForce*F4 ---------------------------- // - double Fsee = maxIsoForce * F4; + // Row 1 - df_dy (force) + df_dy[0][0] = dfRes_dmuscleLength; + df_dy[0][1] = dfRes_dprojFibLength; + df_dy[0][2] = dfRes_dactiv; + // Row 2 - df_dy (activation) + df_dy[1][0] = 0; + df_dy[1][1] = 0; + df_dy[1][2] = dActRes_dactiv ; - // TODO: I don't think we need to implement dFsee_..... - /*if (nargout > 1) - dFsee_dLm = Fmax * dF4_dLm; - dFsee_dLce = Fmax * dF4_dLce; - dFsee_dy = [0;0;(-d/L)*dFsee_dLm; 0;dFsee_dLce;0;0]; % Fsee is a function of y(3) & y(5) - end*/ + // Row 1 - df_dydot (force) + df_dydot[0][0] = 0; + df_dydot[0][1] = dfRes_dprojFibVel; + df_dydot[0][2] = 0; + // Row 2 - df_dydot (activation) + df_dydot[1][0] = 0; + df_dydot[1][1] = 0; + df_dydot[1][2] = dActRes_dactiv_dot; - //----------------------Activation dynamics equation-------------------// - double df_du = 0; - double activationResidual = activ_dot - (u - activ) * (u / activTimeConstant + (1 - u) / deactivTimeConstant ); - if (returnJacobians) { - df_dy[1][2] = (u / activTimeConstant + (1 - u) / deactivTimeConstant ); - double df_du = -(u / activTimeConstant + (1 - u) / deactivTimeConstant ) - (u - activ) * (1 / activTimeConstant - 1 / deactivTimeConstant ); - df_dydot[1][2] = 1; } - VandenBogert2011Muscle::ImplicitResults Results; + VandenBogert2011Muscle::ImplicitResidual Results; Results.forceResidual=fRes; Results.activResidual=activationResidual; @@ -479,14 +577,63 @@ VandenBogert2011Muscle::ImplicitResults VandenBogert2011Muscle::calcImplicitResi Results.df_dy=df_dy; Results.df_dydot=df_dydot; Results.df_du=df_du; - Results.F1=F1; //Output these for troubleshooting + Results.F1=F1; //Output force components for troubleshooting Results.F2=F2; Results.F3=F3; - Results.F4=fl_width; - Results.F5=fiberExp; + Results.F4=F4; + Results.F5=F5; -return Results; +return Results; } -} + +VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle:: +calcJacobianByFiniteDiff(SimTK::Vec3 y,SimTK::Vec3 ydot, double u, double h ) +const { + + // Jacobian Matrices + SimTK::Mat23 df_dy; + SimTK::Mat23 df_dydot; + + VandenBogert2011Muscle::ImplicitResidual opPoint; + opPoint=calcImplicitResidual (y,ydot,u,0); + double opForceResidual=opPoint.forceResidual; + double opActivResidual=opPoint.activResidual; + + VandenBogert2011Muscle::ImplicitResidual del; + + // Make copies so we can modify them + SimTK::Vec3 yTemp = y; + SimTK::Vec3 ydotTemp = ydot; + + + for (int i =0; i<=2; i=i+1) { + yTemp[i]=y[i]+h; + del = calcImplicitResidual(yTemp,ydot,u,0); + + df_dy[0][i]=(del.forceResidual-opForceResidual)/h; //force residual + df_dy[1][i]=(del.activResidual-opActivResidual)/h; //activ residual + + + ydotTemp[i]=y[i]+h; + del = calcImplicitResidual(y,ydotTemp,u,0); + + df_dydot[0][i]=(del.forceResidual-opForceResidual)/h; //force residual + df_dydot[1][i]=(del.activResidual-opActivResidual)/h; //activ residual + + yTemp=y; + ydotTemp=ydot; + + } + + + del = calcImplicitResidual(y,ydot,u+h,0); + double df_du = (del.forceResidual-opForceResidual)/h; + + opPoint.df_dy=df_dy; + opPoint.df_dydot=df_dydot; + + return opPoint; + +} \ No newline at end of file diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index 9c4341168b..01b1d5435a 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -128,7 +128,7 @@ The parent class, Muscle.h, provides //struct ImplicitResults; - struct ImplicitResults { + struct ImplicitResidual { double forceResidual = SimTK::NaN; double activResidual = SimTK::NaN; double forceTendon = SimTK::NaN; @@ -151,7 +151,10 @@ The parent class, Muscle.h, provides //============================================================================= //ImplicitResults calcImplicitResidual(double muslceLength, double fiberLength, double activ, double fiberVelocity, double activ_dot, double u, int returnJacobians) const; - ImplicitResults calcImplicitResidual(double muslceLength, double projFiberLength, double activ, double projFiberVelocity, double activ_dot, double u, int returnJacobians) const; + ImplicitResidual calcImplicitResidual(double muslceLength, double projFiberLength, double activ, double projFiberVelocity, double activ_dot, double u, int returnJacobians) const; + ImplicitResidual calcImplicitResidual(SimTK::Vec3 y,SimTK::Vec3 ydot, double u, int returnJacobians=0) const; + + ImplicitResidual calcJacobianByFiniteDiff(SimTK::Vec3 y,SimTK::Vec3 ydot, double u, double h=1e-7 ) const; protected: //============================================================================= From a038284f31fefc4da5d2525ff65a5d7b12365547 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Wed, 13 Jul 2016 18:02:49 -0700 Subject: [PATCH 10/40] Start test for implicit differential equations. --- .../testImplicitDifferentialEquations.cpp | 116 ++++++++++++++++++ 1 file changed, 116 insertions(+) create mode 100644 OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp diff --git a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp new file mode 100644 index 0000000000..930fbc79ed --- /dev/null +++ b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp @@ -0,0 +1,116 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: testImplicitDifferentialEquations.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) 2016 Stanford University and the Authors * + * Author(s): Chris Dembia, Brad Humphreys * + * * + * 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. * + * -------------------------------------------------------------------------- */ + +/** TODO +*/ + +// TODO component w/multiple state vars, only some of which have implicit form. +// TODO model containing components with and without explicit form. +// TODO write custom implicit form solver. + + +#include + +using namespace OpenSim; +using namespace SimTK; + +/** TODO */ +class TrigDynamics : public Component { +OpenSim_DECLARE_CONCRETE_OBJECT(TrigDynamics, Component); +public: + OpenSim_DECLARE_PROPERTY(default_sine, double, + "Default value of state variable."); + OpenSim_DECLARE_OUTPUT_FOR_STATE_VARIABLE(sine); + TrigDynamics() { + constructProperty_default_sine(0); + } +protected: + void extendAddToSystem(SimTK::MultibodySystem& system) const override { + Super::extendAddToSystem(system); + addStateVariable("sine" /* , true has implicit form */); + } + void computeStateVariableDerivatives(const SimTK::State& s) const override { + // TODO Super::computeStateVariableDerivatives(s); + setStateVariableDerivativeValue(s, "sine", cos(s.getTime())); + } + /* TODO + void computeStateVariableImplicitResiduals(const SimTK::State& s) + const override { + Super::computeStateVariableDerivatives(s); + yDotGuess = getStateVariableDerivativeGuess(s, "sine"); + setStateVariableImplicitResidual(s, "sine", cos(s.getTime()) - yDotGuess); + } + */ + void extendInitStateFromProperties(SimTK::State& s) const override { + Super::extendInitStateFromProperties(s); + setStateVariableValue(s, "sine", get_default_sine()); + } + void extendSetPropertiesFromState(const SimTK::State& s) override { + Super::extendSetPropertiesFromState(s); + set_default_sine(getStateVariableValue(s, "sine")); + } +}; + +/// Integrates the given system and returns the final state via the `state` +/// argument. +void simulate(const Model& model, State& state, Real finalTime) { + SimTK::RungeKuttaMersonIntegrator integrator(model.getSystem()); + SimTK::TimeStepper ts(model.getSystem(), integrator); + ts.initialize(state); + // TODO ts.setReportAllSignificantStates(true); + // TODO integrator.setReturnEveryInternalStep(true); + ts.stepTo(finalTime); + state = ts.getState(); +} + +// Ensure that explicit forward integration works for a component that also +// provides an implicit form. +void testExplicitFormOfImplicitComponent() { + Model model; model.setName("model"); + auto* trig = new TrigDynamics(); + trig->setName("foo"); + const Real initialValue = 3.5; + trig->set_default_sine(initialValue); + model.addComponent(trig); + + // TODO auto* rep = new ConsoleReporter(); + // TODO rep->set_report_time_interval(0.01); + // TODO model.addComponent(rep); + // TODO rep->updInput("inputs").connect(trig->getOutput("sine")); + + auto s = model.initSystem(); + SimTK_TEST(trig->getStateVariableValue(s, "sine") == initialValue); + model.realizeAcceleration(s); + SimTK_TEST(trig->getStateVariableDerivativeValue(s, "sine") == cos(s.getTime())); + + const double finalTime = 0.23; + simulate(model, s, finalTime); + SimTK_TEST_EQ_TOL(trig->getStateVariableValue(s, "sine"), + initialValue + sin(finalTime), 1e-5); +} + +int main() { + SimTK_START_TEST("testImplicitDifferentialEquatiosns"); + SimTK_SUBTEST(testExplicitFormOfImplicitComponent); + SimTK_END_TEST(); +} \ No newline at end of file From 05870cf63887eaaed1137c535584204560f8320e Mon Sep 17 00:00:00 2001 From: humphreysb Date: Thu, 14 Jul 2016 09:27:09 -0700 Subject: [PATCH 11/40] Push for troubleshooting Mat wrapping issue --- .idea/workspace.xml | 111 ++++----- Bindings/actuators.i | 3 + Bindings/simbody.i | 1 + OpenSim/Actuators/VandenBogert2011Muscle.cpp | 227 +++++++++++++------ OpenSim/Actuators/VandenBogert2011Muscle.h | 16 +- 5 files changed, 222 insertions(+), 136 deletions(-) diff --git a/.idea/workspace.xml b/.idea/workspace.xml index 7d3b4225fc..b80dd57f08 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -109,8 +109,8 @@ - + @@ -141,6 +141,7 @@ + @@ -166,8 +167,8 @@ - - + + @@ -175,31 +176,21 @@ - - + + - - + + - - - - - - - - - - - - + + - - + + @@ -208,8 +199,8 @@ - - + + @@ -241,9 +232,8 @@ @@ -294,7 +284,7 @@ - + @@ -1135,13 +1125,13 @@ - + - + - + @@ -1190,7 +1180,9 @@ - + + @@ -1216,7 +1208,6 @@ - @@ -1224,7 +1215,6 @@ - @@ -1232,7 +1222,6 @@ - @@ -1266,7 +1255,6 @@ - @@ -1274,7 +1262,6 @@ - @@ -1282,7 +1269,6 @@ - @@ -1316,7 +1302,6 @@ - @@ -1324,7 +1309,6 @@ - @@ -1332,7 +1316,6 @@ - @@ -1509,13 +1492,6 @@ - - - - - - - @@ -1548,15 +1524,6 @@ - - - - - - - - - @@ -1564,7 +1531,6 @@ - @@ -1572,14 +1538,37 @@ + + + + + + + + + + + + + + + + + + + + + + + - - + + @@ -1588,8 +1577,8 @@ - - + + diff --git a/Bindings/actuators.i b/Bindings/actuators.i index 36d6b4857b..a66c3f73d4 100644 --- a/Bindings/actuators.i +++ b/Bindings/actuators.i @@ -24,3 +24,6 @@ %include %include +namespace OpenSim{ +%rename(VandenBogert2011MuscleImplicitResiduals) VandenBogert2011Muscle::ImplicitResiduals; +} diff --git a/Bindings/simbody.i b/Bindings/simbody.i index 7885cd96e6..58dcb4bf15 100644 --- a/Bindings/simbody.i +++ b/Bindings/simbody.i @@ -19,6 +19,7 @@ namespace SimTK { // Mat33 %include namespace SimTK { +%template(Mat22) Mat<2, 2>; %template(Mat33) Mat<3, 3>; } %include diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index 6dd7c5558a..4b08085849 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -43,7 +43,7 @@ VandenBogert2011Muscle::VandenBogert2011Muscle(const std::string &name) */ void VandenBogert2011Muscle::constructProperties() { - setAuthors("A. van den Bogert, B. Humphreys"); + setAuthors("A. van den Bogert, B. Humphreys, C. Dembia"); //constructProperty_max_isometric_force(1000); constructProperty_fMaxTendonStrain(0.033); constructProperty_fl_width(0.63); @@ -243,33 +243,116 @@ const // int returnJacobians) const { -VandenBogert2011Muscle::ImplicitResidual -VandenBogert2011Muscle::calcImplicitResidual(SimTK::Vec3 y,SimTK::Vec3 ydot, - double u, int returnJacobians) const { - VandenBogert2011Muscle::ImplicitResidual impRes= - calcImplicitResidual(y[0],y[1], y[2], ydot[1], ydot[2], u, - returnJacobians); - return impRes; + + +//------------------------------------------------------------------------------ +SimTK::Vec2 VandenBogert2011Muscle::fiberLengthToProjectedLength + (double fiberLength, double fiberVelocity) const + /* fiberLengthToProject - calculates the projected fiber length + & velocity of the contractile element. + + @param fiberLength - normalized length of the contractile fiber. + (Normalized by the optimal fiber length.) [fiber Lengths] + @param fiberVelocity - normalized velocity of the contractile fiber. + [optimal fiberLengths per sec] + + @return - a SIMTK:Vec2: + projFiberLength - normailzed length of the contractile fiber + projected in line with tendon [fiber Lengths] + projFiberVelocity - normailzed velocity of the contractile fiber + projected in line with tendon + [optimal fiberLengths per sec] */ + +{ +double pennAtOptFiberLength = getPennAtOptFiberLength(); + + double projFiberLength=0; + double projFiberVelocity=0; +if (pennAtOptFiberLength<0.01) { + projFiberLength=fiberLength; } +else { + double b=sin(pennAtOptFiberLength); //b is the fixed distance of the fiber + //perpindicular to the tendon (width) + + // The fiber can not be shorter than b; if it is, the projected length + // equation below will return a complex value. It also physically can not + //happen (the fiber being shorter than the fixed width + if (fiberLength >= b ) { + projFiberLength=sqrt(pow(fiberLength,2) + pow(b,2));} + else { + projFiberLength= SimTK::NaN;} //TODO: Doing this for now, but need to + // come up with a clamping scheme (Millard has one) + + if (fiberVelocity!=0) { + projFiberVelocity=fiberVelocity/cos(projFiberLength/fiberLength);} + + }; + SimTK::Vec2 output; + output[0] = projFiberLength; + output[1] = projFiberVelocity; + return output; +}; + + +//------------------------------------------------------------------------------ +VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle::calcImplicitResidual(SimTK::Vec2 y,SimTK::Vec2 ydot_guess, double muscleLength, double u, int returnJacobians) const { + + // Overload method for state vectors as parameters + +VandenBogert2011Muscle::ImplicitResidual Results= calcImplicitResidual(muscleLength, y[0], y[1], + ydot_guess[0], ydot_guess[1], u, returnJacobians); + +return Results; } +//------------------------------------------------------------------------------ +VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle::calcImplicitResidual(SimTK::State s, double projFibVel_guess, double activdot_guess, double u, int returnJacobians) const { + + // Overload method for SimTK as parameter + + double muscleLength = getLength(s); + double projFibLen = getNormalizedFiberLength(s); + double activ = getActivation(s); + + VandenBogert2011Muscle::ImplicitResidual Results= calcImplicitResidual(muscleLength, projFibLen, activ, + projFibVel_guess, activdot_guess, u, returnJacobians); + return Results; +} + + + +//------------------------------------------------------------------------------ VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle:: -calcImplicitResidual(double muslceLength, double projFibLen, double activ, - double projFibVel, double activ_dot, double u, +calcImplicitResidual(double muscleLength, double projFibLenNorm, double activ, + double projFibVelNorm, double activdot, double u, int returnJacobians=0) const { - // Later when using state variables: - //State Variables - //double Lm = getLength(s); - //double Lce = getNormalizedFiberLength(s); //Ton: Lce is dimensionless, - // it is the muscle fiber length divided by Lceopt - //double a = getActivation(s); - //double Lcedot = getNormalizedFiberVelocity(s); - //TODO: Match symbols to doxygen diagram + /* + @param muscleLength - muscle length (tendon + fiber) [m] + @param projFibLengthNorm - length of contractile element projected along line + of tendon. Normalized by optimal Fiber Length. + [unitless: length/optimal length] + @param activ - muscle activation: 0 to 1 [unitless] + @param projFibVelNorm - velocity of contractile element projected along line + of tendon. Normalized by optimal fiber length. + [length/optimal length/sec] + @param activdot - rate of change of muscle activation [1/sec] + @param u - control signal (neural excitation) 0.0 to 1.0 [unitless] + + @return - a structure: //TODO: finish defining + + + */ + + + //TODO: Match symbols to doxygen diagram and Add symbolic equations comments + //TODO: May want to make this into a seperate function + // -------------------------Parameters----------------------------// //F_{iso} Maximum isometric force that the fibers can generate double maxIsoForce = getMaxIsometricForce(); @@ -327,22 +410,11 @@ calcImplicitResidual(double muslceLength, double projFibLen, double activ, // Jacobian Matrices - SimTK::Mat23 df_dy; - SimTK::Mat23 df_dydot; - - //y=(Lm,projFiberLength,activation) <-States - - - //df_fy is 2x3: Where columns are states: - // [muscleLeLength, projFiberLength, activation] - // Rows are: - // [force residual; Activation Residual] - + SimTK::Mat33 df_dy; + SimTK::Mat33 df_dydot; //-------Convert projFiberLength & Velocity to fiberLength & Velocity------/ - //TODO: May want to make this into a seperate function - //TODO: Add symbolic equations comments double cosPenn; double dcosPenn_dprojFibLen; double fiberLength; @@ -351,23 +423,23 @@ calcImplicitResidual(double muslceLength, double projFibLen, double activ, // If pennation is zero, we can't do this because volume is zero, // and fiberLength ~ projFiberLength cosPenn=1.0; - fiberLength=projFibLen; + fiberLength=projFibLenNorm; dfiberLength_dprojFibLen=1; dcosPenn_dprojFibLen=0; } else { double b=sin(pennAtOptFiberLength); - fiberLength=sqrt(pow(projFibLen,2) + pow(b,2)); - cosPenn = projFibLen/fiberLength; + fiberLength=sqrt(pow(projFibLenNorm,2) + pow(b,2)); + cosPenn = projFibLenNorm/fiberLength; dfiberLength_dprojFibLen=cosPenn; dcosPenn_dprojFibLen=pow(b,2) / pow(fiberLength,3); } // Compute fiberVelocity and its derivatives wrt projFibLen and projFibVel - double fiberVelocity = projFibVel*cosPenn; - double dfiberVelocity_dprojFibVel = cosPenn; - double dfiberVelocity_dprojFibLen = projFibVel * dcosPenn_dprojFibLen; + double fiberVelocity = projFibVelNorm*cosPenn; + double dfiberVelocity_dprojFibVelNorm = cosPenn; + double dfiberVelocity_dprojFibLen = projFibVelNorm * dcosPenn_dprojFibLen; @@ -388,8 +460,9 @@ calcImplicitResidual(double muslceLength, double projFibLen, double activ, double F2; double dF2_dfiberVelocity; double dF2_dactiv; - double dF2_dprojFibVel; + double dF2_dprojFibVelNorm; double dF2_dprojFibLen; + double df_dmuscleLength; // Chow/Darling Vel-Activation Relationship //TODO: Add full reference //double lambda = 0.5025 + 0.5341*activ; @@ -425,7 +498,7 @@ calcImplicitResidual(double muslceLength, double projFibLen, double activ, } } if (returnJacobians){ - dF2_dprojFibVel = dF2_dfiberVelocity * dfiberVelocity_dprojFibVel; + dF2_dprojFibVelNorm = dF2_dfiberVelocity * dfiberVelocity_dprojFibVelNorm; dF2_dprojFibLen = dF2_dfiberVelocity * dfiberVelocity_dprojFibLen; } @@ -455,7 +528,7 @@ calcImplicitResidual(double muslceLength, double projFibLen, double activ, // stiffness of the linear term is 1 N/m, convert to Fmax/m double kSEE = 1.0 / maxIsoForce; // elongation of tendon (SEE), in meters - double elongationTendon = muslceLength - fiberLength * + double elongationTendon = muscleLength - fiberLength * optFiberLength - tendonSlackLength; // low stiffness linear term double F4 = kSEE * elongationTendon; @@ -481,9 +554,10 @@ calcImplicitResidual(double muslceLength, double projFibLen, double activ, //-- F5 is viscous damping parallel to the CE (0.001 of Fmax at 1 Lceopt/s) // to ensure that df/dLcedot is never zero-----------// - double F5 = dampingCoeff * projFibVel ; + double F5 = dampingCoeff * projFibVelNorm ; //TODO: Should this be: dampingCoeff * fiberVelocity (not proj)? - double dF5_dprojFibVel = dampingCoeff; + //TODO: Why are damping coeff used instead of damping ratio? + double dF5_dprojFibVelNorm = dampingCoeff; @@ -508,16 +582,16 @@ calcImplicitResidual(double muslceLength, double projFibLen, double activ, //----------------------Activation dynamics equation-------------------// - double activationResidual = activ_dot - (u - activ) * + double activationResidual = activdot - (u - activ) * (u / activTimeConstant + (1 - u) / deactivTimeConstant ); double df_du = 0; double dActRes_dactiv=0; - double dActRes_dactiv_dot = 0; + double dActRes_dactivdot = 0; if (returnJacobians) { dActRes_dactiv= (u / activTimeConstant + (1 - u) / deactivTimeConstant); - dActRes_dactiv_dot = 1; + dActRes_dactivdot = 1; df_du = -(u / activTimeConstant + (1 - u) / deactivTimeConstant ) - (u - activ) * (1 / activTimeConstant - 1 / @@ -526,44 +600,41 @@ calcImplicitResidual(double muslceLength, double projFibLen, double activ, - //---------------------Assemble Force Jacobian---------------------------// + //---------------------Assemble Jacobians---------------------------// if (returnJacobians) { double dfRes_dactiv = -(F1*F2 + activ*F1*dF2_dactiv )*cosPenn; - double dfRes_dprojFibLength = dF4_dprojFibLen - + double dfRes_dprojFibLengthNorm = dF4_dprojFibLen - (activ*(dF1_dfiberLength*F2 + F1*dF2_dprojFibLen) + dF3_dprojFibLen) * cosPenn - (activ*F1*F2 + F3) * dcosPenn_dprojFibLen; - double dfRes_dprojFibVel = - activ*F1*dF2_dprojFibVel - dF5_dprojFibVel; + double dfRes_dprojFibVelNorm = - activ*F1*dF2_dprojFibVelNorm - dF5_dprojFibVelNorm; double dfRes_dmuscleLength = dF4_dmuscleLength; - //y=(Lm,projFiberLength,activation) <-States + //y=(projFiberLength,activation) <-States - //df_fy is 2x3: Where columns are states: - // [muscleLeLength, projFiberLength, activation] + //df_fy is 2x2: Where columns are states: + // [projFiberLengthNorm, activation] // Rows are: // [force residual; Activation Residual] // Row 1 - df_dy (force) - df_dy[0][0] = dfRes_dmuscleLength; - df_dy[0][1] = dfRes_dprojFibLength; - df_dy[0][2] = dfRes_dactiv; + df_dy[0][0] = dfRes_dprojFibLengthNorm; + df_dy[0][1] = dfRes_dactiv; // Row 2 - df_dy (activation) df_dy[1][0] = 0; - df_dy[1][1] = 0; - df_dy[1][2] = dActRes_dactiv ; + df_dy[1][1] = dActRes_dactiv ; // Row 1 - df_dydot (force) - df_dydot[0][0] = 0; - df_dydot[0][1] = dfRes_dprojFibVel; - df_dydot[0][2] = 0; - + df_dydot[0][0] = dfRes_dprojFibVelNorm; + df_dydot[0][1] = 0; // Row 2 - df_dydot (activation) df_dydot[1][0] = 0; - df_dydot[1][1] = 0; - df_dydot[1][2] = dActRes_dactiv_dot; + df_dydot[1][1] = dActRes_dactivdot; + + df_dmuscleLength = dfRes_dmuscleLength; } @@ -577,6 +648,7 @@ calcImplicitResidual(double muslceLength, double projFibLen, double activ, Results.df_dy=df_dy; Results.df_dydot=df_dydot; Results.df_du=df_du; + Results.df_dmuscleLength=df_dmuscleLength; Results.F1=F1; //Output force components for troubleshooting Results.F2=F2; Results.F3=F3; @@ -588,30 +660,33 @@ calcImplicitResidual(double muslceLength, double projFibLen, double activ, return Results; } + +//------------------------------------------------------------------------------ VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle:: -calcJacobianByFiniteDiff(SimTK::Vec3 y,SimTK::Vec3 ydot, double u, double h ) +calcJacobianByFiniteDiff(SimTK::Vec2 y, SimTK::Vec2 ydot, double muscleLength, double u, double h ) + const { // Jacobian Matrices - SimTK::Mat23 df_dy; - SimTK::Mat23 df_dydot; + SimTK::Mat33 df_dy; + SimTK::Mat33 df_dydot; VandenBogert2011Muscle::ImplicitResidual opPoint; - opPoint=calcImplicitResidual (y,ydot,u,0); + opPoint=calcImplicitResidual (y,ydot,muscleLength,u,0); double opForceResidual=opPoint.forceResidual; double opActivResidual=opPoint.activResidual; VandenBogert2011Muscle::ImplicitResidual del; // Make copies so we can modify them - SimTK::Vec3 yTemp = y; - SimTK::Vec3 ydotTemp = ydot; + SimTK::Vec2 yTemp = y; + SimTK::Vec2 ydotTemp = ydot; + - for (int i =0; i<=2; i=i+1) { + for (int i =0; i<=1; i=i+1) { yTemp[i]=y[i]+h; del = calcImplicitResidual(yTemp,ydot,u,0); - df_dy[0][i]=(del.forceResidual-opForceResidual)/h; //force residual df_dy[1][i]=(del.activResidual-opActivResidual)/h; //activ residual @@ -636,4 +711,14 @@ const { return opPoint; -} \ No newline at end of file +} + + + + SimTK::Mat22 VandenBogert2011Muscle::fixMat22(SimTK::Mat22 matIn,SimTK::Mat22 matFixed) const{ + for (int m =0; m<=1; m=m+1) { + for (int n =0; n<=1; n=n+1) { + matFixed[m][n]=matIn[m][n]; + }} + return matFixed; + } \ No newline at end of file diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index 01b1d5435a..60ee188be0 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -132,9 +132,10 @@ The parent class, Muscle.h, provides double forceResidual = SimTK::NaN; double activResidual = SimTK::NaN; double forceTendon = SimTK::NaN; - SimTK::Mat23 df_dy = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; - SimTK::Mat23 df_dydot = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; + SimTK::Mat33 df_dy = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; + SimTK::Mat33 df_dydot = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; double df_du = SimTK::NaN; + double df_dmuscleLength = SimTK::NaN; double F1 = SimTK::NaN; double F2 = SimTK::NaN; double F3 = SimTK::NaN; @@ -152,9 +153,16 @@ The parent class, Muscle.h, provides //ImplicitResults calcImplicitResidual(double muslceLength, double fiberLength, double activ, double fiberVelocity, double activ_dot, double u, int returnJacobians) const; ImplicitResidual calcImplicitResidual(double muslceLength, double projFiberLength, double activ, double projFiberVelocity, double activ_dot, double u, int returnJacobians) const; - ImplicitResidual calcImplicitResidual(SimTK::Vec3 y,SimTK::Vec3 ydot, double u, int returnJacobians=0) const; + ImplicitResidual calcImplicitResidual(SimTK::Vec2 y,SimTK::Vec2 ydot_guess, double muscleLength, double u, int returnJacobians=0) const; + ImplicitResidual calcImplicitResidual(SimTK::State s, double projFibVel_guess, double activdot_guess, double u, int returnJacobians) const; + + + SimTK::Vec2 fiberLengthToProjectedLength(double fiberLength, double fiberVelocity) const; + + ImplicitResidual calcJacobianByFiniteDiff(SimTK::Vec2 y,SimTK::Vec2 ydot, double muscleLength, double u, double h ) const; + SimTK::Mat22 fixMat22(SimTK::Mat22 matIn,SimTK::Mat22 matFixed) const; + - ImplicitResidual calcJacobianByFiniteDiff(SimTK::Vec3 y,SimTK::Vec3 ydot, double u, double h=1e-7 ) const; protected: //============================================================================= From 17556642835ba0532546b83c519bd07afbbfee4b Mon Sep 17 00:00:00 2001 From: humphreysb Date: Thu, 14 Jul 2016 10:06:41 -0700 Subject: [PATCH 12/40] Fixing deleted source bindings dir --- .idea/workspace.xml | 18 ++++++++---------- OpenSim/Actuators/VandenBogert2011Muscle.cpp | 14 +++++++++++++- OpenSim/Actuators/VandenBogert2011Muscle.h | 3 ++- 3 files changed, 23 insertions(+), 12 deletions(-) diff --git a/.idea/workspace.xml b/.idea/workspace.xml index b80dd57f08..a72fb76dae 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -140,8 +140,6 @@ - - @@ -167,8 +165,8 @@ - - + + @@ -199,8 +197,8 @@ - - + + @@ -1567,8 +1565,8 @@ - - + + @@ -1577,8 +1575,8 @@ - - + + diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index 4b08085849..d8bbb4792f 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -721,4 +721,16 @@ const { matFixed[m][n]=matIn[m][n]; }} return matFixed; - } \ No newline at end of file + } + +SimTK::Mat33 VandenBogert2011Muscle::quickMat33() const { + SimTK::Mat33 m; + m[1][1]=1; + return m; +} + +SimTK::Mat22 VandenBogert2011Muscle::quickMat22() const { + SimTK::Mat22 m; + m[1][1]=2; + return m; +} \ No newline at end of file diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index 60ee188be0..a9160ecc6e 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -161,7 +161,8 @@ The parent class, Muscle.h, provides ImplicitResidual calcJacobianByFiniteDiff(SimTK::Vec2 y,SimTK::Vec2 ydot, double muscleLength, double u, double h ) const; SimTK::Mat22 fixMat22(SimTK::Mat22 matIn,SimTK::Mat22 matFixed) const; - + SimTK::Mat33 quickMat33() const; + SimTK::Mat33 quickMat22() const; protected: From 7e98c1a9058cd349fdce05161c678644194e1b4c Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Thu, 14 Jul 2016 15:17:12 -0700 Subject: [PATCH 13/40] Properly wrap Mat22, Mat33, etc. --- .../Java/Matlab/tests/testVandenBogert2011Muscle.m | 12 +++++++++++- Bindings/actuators.i | 5 +++++ Bindings/simbody.i | 1 + 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m b/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m index 40d84eb382..9ab2fbf671 100644 --- a/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m +++ b/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m @@ -14,4 +14,14 @@ %disp(out.get(0)) %disp(out.get(1)) %disp(out.get(2)) -methods(out) \ No newline at end of file +methods(out) + +% Make sure that we can access the Mat22: +df_dy = out.getDf_dy(); +df_dy.get(0, 0); +df_dy.get(1, 1); +df_dy.set(1, 1, 1.5); + + +% Ensure the nested struct is wrapped. +impRes = VandenBogert2011MuscleImplicitResidual() diff --git a/Bindings/actuators.i b/Bindings/actuators.i index a66c3f73d4..1d7d1bb58e 100644 --- a/Bindings/actuators.i +++ b/Bindings/actuators.i @@ -23,6 +23,11 @@ %include %include +//namespace OpenSim{ +//%rename(VandenBogert2011MuscleImplicitResidual) VandenBogert2011Muscle::ImplicitResidual; +//} +%feature("flatnested") OpenSim::VandenBogert2011Muscle::ImplicitResidual; +%rename(VandenBogert2011MuscleImplicitResidual) OpenSim::VandenBogert2011Muscle::ImplicitResidual; %include namespace OpenSim{ %rename(VandenBogert2011MuscleImplicitResiduals) VandenBogert2011Muscle::ImplicitResiduals; diff --git a/Bindings/simbody.i b/Bindings/simbody.i index 58dcb4bf15..5693cfb40a 100644 --- a/Bindings/simbody.i +++ b/Bindings/simbody.i @@ -18,6 +18,7 @@ namespace SimTK { // Mat33 %include +%include namespace SimTK { %template(Mat22) Mat<2, 2>; %template(Mat33) Mat<3, 3>; From 3c09cb24cf9aaed1fcb0d6ae6b8245dcc4311dcd Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Thu, 14 Jul 2016 15:28:23 -0700 Subject: [PATCH 14/40] Clean up leftover swig rename command. --- Bindings/actuators.i | 6 ------ 1 file changed, 6 deletions(-) diff --git a/Bindings/actuators.i b/Bindings/actuators.i index 1d7d1bb58e..c8b84d4b63 100644 --- a/Bindings/actuators.i +++ b/Bindings/actuators.i @@ -23,12 +23,6 @@ %include %include -//namespace OpenSim{ -//%rename(VandenBogert2011MuscleImplicitResidual) VandenBogert2011Muscle::ImplicitResidual; -//} %feature("flatnested") OpenSim::VandenBogert2011Muscle::ImplicitResidual; %rename(VandenBogert2011MuscleImplicitResidual) OpenSim::VandenBogert2011Muscle::ImplicitResidual; %include -namespace OpenSim{ -%rename(VandenBogert2011MuscleImplicitResiduals) VandenBogert2011Muscle::ImplicitResiduals; -} From b485c3878e39d1e0f9f80058d008b525cf80da58 Mon Sep 17 00:00:00 2001 From: humphreysb Date: Thu, 14 Jul 2016 23:55:07 -0700 Subject: [PATCH 15/40] Added static eq calc . --- .idea/workspace.xml | 150 ++++++++++----- OpenSim/Actuators/VandenBogert2011Muscle.cpp | 184 +++++++++++++++++-- OpenSim/Actuators/VandenBogert2011Muscle.h | 20 +- 3 files changed, 283 insertions(+), 71 deletions(-) diff --git a/.idea/workspace.xml b/.idea/workspace.xml index a72fb76dae..7c884a20cb 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -140,6 +140,7 @@ + @@ -161,12 +162,12 @@ - + - - + + @@ -174,31 +175,41 @@ + + + + + + + + + + - - + + - - + + - - + + - - + + - - + + @@ -230,8 +241,9 @@ @@ -276,15 +288,48 @@ - + + @@ -1123,20 +1168,20 @@ - + - + - + - + - + - + @@ -1147,23 +1192,25 @@ - - - - + + + + + + - - - - - - + + + + + + - - + + - - + + @@ -1490,13 +1537,6 @@ - - - - - - - @@ -1549,24 +1589,32 @@ - - + + - - + + + + + + + + + + - - + + @@ -1575,8 +1623,8 @@ - - + + diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index d8bbb4792f..5ab5fa354f 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -305,9 +305,29 @@ VandenBogert2011Muscle::ImplicitResidual Results= calcImplicitResidual(muscleLen ydot_guess[0], ydot_guess[1], u, returnJacobians); return Results; + + } +//------------------------------------------------------------------------------ +SimTK::Vec2 VandenBogert2011Muscle::calcImplicitResidual(double projFibLen, double muscleLength) const { + + // Overload method for when calculating static equilibrium + + + VandenBogert2011Muscle::ImplicitResidual Results = calcImplicitResidual( + muscleLength, projFibLen, 0.0, 0.0, 0.0, 0.0, 1.0); + + SimTK::Vec2 forceResAndDerivative; + + forceResAndDerivative[0]=Results.forceResidual; + forceResAndDerivative[1]=Results.df_dy[0][0]; + + return forceResAndDerivative; + +} + //------------------------------------------------------------------------------ VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle::calcImplicitResidual(SimTK::State s, double projFibVel_guess, double activdot_guess, double u, int returnJacobians) const { @@ -410,8 +430,8 @@ calcImplicitResidual(double muscleLength, double projFibLenNorm, double activ, // Jacobian Matrices - SimTK::Mat33 df_dy; - SimTK::Mat33 df_dydot; + SimTK::Mat22 df_dy; + SimTK::Mat22 df_dydot; //-------Convert projFiberLength & Velocity to fiberLength & Velocity------/ @@ -584,7 +604,7 @@ calcImplicitResidual(double muscleLength, double projFibLenNorm, double activ, double activationResidual = activdot - (u - activ) * (u / activTimeConstant + (1 - u) / deactivTimeConstant ); - double df_du = 0; + SimTK::Vec2 df_du; double dActRes_dactiv=0; double dActRes_dactivdot = 0; @@ -593,7 +613,9 @@ calcImplicitResidual(double muscleLength, double projFibLenNorm, double activ, dActRes_dactivdot = 1; - df_du = -(u / activTimeConstant + (1 - u) / deactivTimeConstant ) + + df_du[0]=0; + df_du[1] = -(u / activTimeConstant + (1 - u) / deactivTimeConstant ) - (u - activ) * (1 / activTimeConstant - 1 / deactivTimeConstant ); } @@ -661,6 +683,32 @@ return Results; } + + + +//------------------------------------------------------------------------------ +VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle::calcJacobianByFiniteDiff(double muscleLength, double projFibLenNorm, double activ, + double projFibVelNorm, double activdot, double u, + double h) const { + + SimTK::Vec2 y; + SimTK::Vec2 ydot; + y[0]=projFibLenNorm; + y[1]=activ; + ydot[0]=projFibVelNorm; + ydot[1]=activdot; + + // Overload method for state vectors as parameters + VandenBogert2011Muscle::ImplicitResidual Results= calcJacobianByFiniteDiff(y, ydot, muscleLength, u, h ); + + return Results; +} + + + + + + //------------------------------------------------------------------------------ VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle:: calcJacobianByFiniteDiff(SimTK::Vec2 y, SimTK::Vec2 ydot, double muscleLength, double u, double h ) @@ -668,8 +716,9 @@ calcJacobianByFiniteDiff(SimTK::Vec2 y, SimTK::Vec2 ydot, double muscleLength, d const { // Jacobian Matrices - SimTK::Mat33 df_dy; - SimTK::Mat33 df_dydot; + SimTK::Mat22 df_dy; + SimTK::Mat22 df_dydot; + SimTK::Vec2 df_du; VandenBogert2011Muscle::ImplicitResidual opPoint; opPoint=calcImplicitResidual (y,ydot,muscleLength,u,0); @@ -678,12 +727,34 @@ const { VandenBogert2011Muscle::ImplicitResidual del; - // Make copies so we can modify them - SimTK::Vec2 yTemp = y; - SimTK::Vec2 ydotTemp = ydot; - - + //----------df_dy------------// + SimTK::Vec2 dh = {h,0}; + del = calcImplicitResidual(y+dh,ydot,muscleLength,u,0); + df_dy[0][0]= (del.forceResidual-opForceResidual)/h; + df_dy[1][0]= (del.activResidual-opActivResidual)/h; + + dh = {0,h}; + del = calcImplicitResidual(y+dh,ydot,muscleLength,u,0); + df_dy[1][0]= (del.forceResidual-opForceResidual)/h; + df_dy[1][1]= (del.activResidual-opActivResidual)/h; + + //----------df_dydot------------// + dh = {h,0}; + del = calcImplicitResidual(y,ydot+dh,muscleLength,u,0); + df_dydot[0][0]= (del.forceResidual-opForceResidual)/h; + df_dydot[1][0]= (del.activResidual-opActivResidual)/h; + + dh = {0,h}; + del = calcImplicitResidual(y,ydot+dh,muscleLength,u,0); + df_dydot[1][0]= (del.forceResidual-opForceResidual)/h; + df_dydot[1][1]= (del.activResidual-opActivResidual)/h; + + //----------df_du----------------// + del = calcImplicitResidual(y,ydot,muscleLength,u+h,0); + df_du[0]= (del.forceResidual-opForceResidual)/h; + df_du[1]= (del.activResidual-opActivResidual)/h; +/* for (int i =0; i<=1; i=i+1) { yTemp[i]=y[i]+h; del = calcImplicitResidual(yTemp,ydot,u,0); @@ -700,14 +771,15 @@ const { yTemp=y; ydotTemp=ydot; - } + }*/ - del = calcImplicitResidual(y,ydot,u+h,0); - double df_du = (del.forceResidual-opForceResidual)/h; + //del = calcImplicitResidual(y,ydot,u+h,0); + //double df_du = (del.forceResidual-opForceResidual)/h; opPoint.df_dy=df_dy; opPoint.df_dydot=df_dydot; + opPoint.df_du=df_du; return opPoint; @@ -715,6 +787,72 @@ const { + +//------------------------------------------------------------------------------ +SimTK::Vec1 VandenBogert2011Muscle::calcStatic(double muscleLength ) const { + +double tol=1e-4; +double a=0; +double b=10; + +double x=(a+b)/2.0; +double dx=2*tol; + +int neval=0; + +while (abs(x)>=tol){ + +neval++; + +// Set a to be lower value and b to be upper value +a=min(a,b); +b=max(a,b); + + +SimTK::Vec2 forceResAndDerivative = calcImplicitResidual(x, muscleLength); +double fx = forceResAndDerivative[0]; + + + +forceResAndDerivative = calcImplicitResidual(a, muscleLength); +double funcA = forceResAndDerivative[0]; + +//After the 1st iteration, use the new guess as a new upper or lower bound +if (neval>1) { + if (funcA *fx>0){ + a = x;} + else { b = x;}} + + + +forceResAndDerivative = calcImplicitResidual(x, muscleLength); +double dfx= forceResAndDerivative[1]; +double forceRes=forceResAndDerivative[0]; + +double dx =-fx/dfx; +double xdx=x-dx; + + +bool inInterval=((xdx>=a) && (xdx<=b)); +forceResAndDerivative = calcImplicitResidual(xdx, muscleLength); +bool largeDeriv=abs(forceResAndDerivative[0])> 0.5*abs(fx); + +if (~inInterval || largeDeriv){ +x=(a+b)/2; +dx=(a-b)/2;} +else +{x=xdx;}; + + +}; + SimTK::Vec1 vout; + vout[0]=x; + return vout; +} + + + + SimTK::Mat22 VandenBogert2011Muscle::fixMat22(SimTK::Mat22 matIn,SimTK::Mat22 matFixed) const{ for (int m =0; m<=1; m=m+1) { for (int n =0; n<=1; n=n+1) { @@ -733,4 +871,20 @@ SimTK::Mat22 VandenBogert2011Muscle::quickMat22() const { SimTK::Mat22 m; m[1][1]=2; return m; -} \ No newline at end of file +} + + +SimTK::Vec4 VandenBogert2011Muscle::quickVec4() const { + SimTK::Vec4 m; + m[1]=2; + return m; +} + +SimTK::Vec4 VandenBogert2011Muscle::flattenMat22(SimTK::Mat22 matIn) const { + SimTK::Vec4 vecOut; + vecOut[0]=matIn[0][0]; + vecOut[1]=matIn[0][1]; + vecOut[2]=matIn[1][0]; + vecOut[3]=matIn[1][1]; + return vecOut; +} diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index a9160ecc6e..ca0a8b9475 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -132,9 +132,9 @@ The parent class, Muscle.h, provides double forceResidual = SimTK::NaN; double activResidual = SimTK::NaN; double forceTendon = SimTK::NaN; - SimTK::Mat33 df_dy = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; - SimTK::Mat33 df_dydot = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; - double df_du = SimTK::NaN; + SimTK::Mat22 df_dy = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; + SimTK::Mat22 df_dydot = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; + SimTK::Vec2 df_du = {SimTK::NaN,SimTK::NaN}; double df_dmuscleLength = SimTK::NaN; double F1 = SimTK::NaN; double F2 = SimTK::NaN; @@ -160,10 +160,20 @@ The parent class, Muscle.h, provides SimTK::Vec2 fiberLengthToProjectedLength(double fiberLength, double fiberVelocity) const; ImplicitResidual calcJacobianByFiniteDiff(SimTK::Vec2 y,SimTK::Vec2 ydot, double muscleLength, double u, double h ) const; + ImplicitResidual calcJacobianByFiniteDiff(double muscleLength, double projFibLenNorm, double activ, + double projFibVelNorm, double activdot, double u, + double h) const; + + SimTK::Vec2 calcImplicitResidual(double projFibLen, double muscleLength) const; + + SimTK::Vec1 calcStatic(double muscleLength) const; + + //Hacks for troubleshooting Mat22: SimTK::Mat22 fixMat22(SimTK::Mat22 matIn,SimTK::Mat22 matFixed) const; SimTK::Mat33 quickMat33() const; - SimTK::Mat33 quickMat22() const; - + SimTK::Mat22 quickMat22() const; + SimTK::Vec4 quickVec4() const; + SimTK::Vec4 flattenMat22(SimTK::Mat22 matIn) const; protected: //============================================================================= From 98a20b4662e68e65728efd9fc25ee3d995d2f745 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Fri, 15 Jul 2016 11:47:38 -0700 Subject: [PATCH 16/40] First successful implicit residual, with many serious flaws. --- OpenSim/Common/Component.cpp | 183 +++++++++++++++++- OpenSim/Common/Component.h | 85 +++++++- OpenSim/Simulation/Model/Model.cpp | 91 +++++++++ OpenSim/Simulation/Model/Model.h | 19 +- .../Simulation/SimbodyEngine/Coordinate.cpp | 47 +++++ OpenSim/Simulation/SimbodyEngine/Coordinate.h | 14 ++ .../testImplicitDifferentialEquations.cpp | 102 +++++++--- 7 files changed, 498 insertions(+), 43 deletions(-) diff --git a/OpenSim/Common/Component.cpp b/OpenSim/Common/Component.cpp index 76007f6fc4..924804dc22 100644 --- a/OpenSim/Common/Component.cpp +++ b/OpenSim/Common/Component.cpp @@ -419,6 +419,11 @@ void Component::computeStateVariableDerivatives(const SimTK::State& s) const } } +void Component::computeImplicitResiduals(const SimTK::State& s) const +{ + // OPENSIM_THROW_FRMOBJ(Exception, "TODO"); +} + void Component:: addModelingOption(const std::string& optionName, int maxFlagValue) const @@ -438,6 +443,7 @@ addModelingOption(const std::string& optionName, int maxFlagValue) const void Component::addStateVariable(const std::string& stateVariableName, const SimTK::Stage& invalidatesStage, + bool hasImplicitForm, bool isHidden) const { if( (invalidatesStage < Stage::Position) || @@ -447,7 +453,7 @@ void Component::addStateVariable(const std::string& stateVariableName, } // Allocate space for a new state variable AddedStateVariable* asv = - new AddedStateVariable(stateVariableName, *this, invalidatesStage, isHidden); + new AddedStateVariable(stateVariableName, *this, invalidatesStage, hasImplicitForm, isHidden); // Add it to the Component and let it take ownership addStateVariable(asv); } @@ -876,6 +882,71 @@ double Component:: return SimTK::NaN; } +double Component::getStateVariableDerivativeGuess(const SimTK::State& state, + const std::string& name) const +{ + // TODO make sure the guess exists? + + std::map::const_iterator it; + it = _namedStateVariableInfo.find(name); + + if (it == _namedStateVariableInfo.end()) { + return it->second.stateVariable->getDerivativeGuess(state); + } + else { + const StateVariable* rsv = findStateVariable(name); + if (rsv) { + return rsv->getDerivativeGuess(state); + } + } + + OPENSIM_THROW_FRMOBJ(Exception, "State variable '" + name + "' not found."); + return SimTK::NaN; +} + +void Component::setStateVariableDerivativeGuess(SimTK::State& state, + const std::string& name, + double derivGuess) const +{ + // TODO make sure the guess exists? + + std::map::const_iterator it; + it = _namedStateVariableInfo.find(name); + + if (it == _namedStateVariableInfo.end()) { + return it->second.stateVariable->setDerivativeGuess(state, derivGuess); + } + else { + const StateVariable* rsv = findStateVariable(name); + if (rsv) { + return rsv->setDerivativeGuess(state, derivGuess); + } + } + + OPENSIM_THROW_FRMOBJ(Exception, "State variable '" + name + "' not found."); +} + +double Component::getImplicitResidual(const SimTK::State& state, + const std::string& name) const +{ + // TODO compute residual? + std::map::const_iterator it; + it = _namedStateVariableInfo.find(name); + + if (it == _namedStateVariableInfo.end()) { + return it->second.stateVariable->getImplicitResidual(state); + } + else { + const StateVariable* rsv = findStateVariable(name); + if (rsv) { + return rsv->getImplicitResidual(state); + } + } + + OPENSIM_THROW_FRMOBJ(Exception, "State variable '" + name + "' not found."); + return SimTK::NaN; +} + // Set the value of a state variable allocated by this Component given its name // for this component. void Component:: @@ -1038,6 +1109,19 @@ setDiscreteVariableValue(SimTK::State& s, const std::string& name, double value) } } +void Component::setImplicitResidual(const State& state, + const std::string& name, double residual) const +{ + std::map::const_iterator it; + it = _namedStateVariableInfo.find(name); + + OPENSIM_THROW_IF_FRMOBJ(it == _namedStateVariableInfo.end(), + Exception, "State variable '" + name + "' not found."); + + const StateVariable& sv = *it->second.stateVariable; + sv.setImplicitResidual(state, residual); +} + bool Component::constructOutputForStateVariable(const std::string& name) { auto func = [name](const Component* comp, @@ -1286,9 +1370,9 @@ void Component::extendRealizeTopology(SimTK::State& s) const it != _namedStateVariableInfo.end(); ++it) { const StateVariable& sv = *it->second.stateVariable; - const AddedStateVariable* asv + const AddedStateVariable* asv = dynamic_cast(&sv); - + if(asv){// add index information for added state variables // make mutable just to update system allocated index ONLY! AddedStateVariable* masv = const_cast(asv); @@ -1322,6 +1406,28 @@ void Component::extendRealizeTopology(SimTK::State& s) const } } +// Update global indices for state variables. +void Component::extendRealizeInstance(const SimTK::State& s) const +{ + for (auto& entry : _namedStateVariableInfo) { + const StateVariable& sv = *entry.second.stateVariable; + const AddedStateVariable* asv + = dynamic_cast(&sv); + // TODO asv->setSystemYIndexFromTODO(); + if(asv){// add index information for added state variables + // make mutable just to update system allocated index ONLY! + AddedStateVariable* masv = const_cast(asv); + // This is only known at Model stage: + const int systemYIdxOfFirstZ = int(s.getZStart()); + const SubsystemIndex subsysIdx = masv->getSubsysIndex(); + const int systemZIdxOfFirstSubsystemZ = s.getZStart(subsysIdx); + const int subsystemZIdx = masv->getVarIndex(); + masv->setSystemYIndex(SystemYIndex(systemYIdxOfFirstZ + + systemZIdxOfFirstSubsystemZ + + subsystemZIdx)); // TODO + } + } +} //------------------------------------------------------------------------------ // REALIZE ACCELERATION @@ -1372,8 +1478,7 @@ const SimTK::MultibodySystem& Component::getSystem() const // Base class implementations of these virtual methods do nothing now but // could do something in the future. Users must still invoke Super::realizeXXX() // as the first line in their overrides to ensure future compatibility. -void Component::extendRealizeModel(SimTK::State& state) const {} -void Component::extendRealizeInstance(const SimTK::State& state) const {} +void Component::extendRealizeModel(SimTK::State& s) const {} void Component::extendRealizeTime(const SimTK::State& state) const {} void Component::extendRealizePosition(const SimTK::State& state) const {} void Component::extendRealizeVelocity(const SimTK::State& state) const {} @@ -1426,6 +1531,74 @@ void Component::AddedStateVariable:: return getOwner().setCacheVariableValue(state, getName()+"_deriv", deriv); } +double Component::AddedStateVariable:: + getImplicitResidual(const SimTK::State& state) const +{ + // TODO find another way to get the residual. + const Component* root = nullptr; + { + const Component* comp = &getOwner(); + while (comp->hasParent()) comp = &comp->getParent(); + root = comp; + } + + // TODO should returning a single entry of the residual require + // evaluating the entire vector? If not, might need a local cache variable. + const SimTK::Vector& residual = root->getImplicitResidual(state); + return residual[getSystemYIndex()]; + + // TODO OPENSIM_THROW(Exception, "TODO"); +} + +void Component::AddedStateVariable:: + setImplicitResidual(const SimTK::State& state, double residual) const +{ + // TODO find another way to get the residual. + const Component* root = nullptr; + { + const Component* comp = &getOwner(); + while (comp->hasParent()) comp = &comp->getParent(); + root = comp; + } + + root->updImplicitResidual(state)[getSystemYIndex()] = residual; + + // TODO OPENSIM_THROW(Exception, "TODO"); +} + +double Component::AddedStateVariable:: + getDerivativeGuess(const SimTK::State& state) const +{ + // TODO find another way to get the guess. + const Component* root = nullptr; + { + const Component* comp = &getOwner(); + while (comp->hasParent()) comp = &comp->getParent(); + root = comp; + } + + return root->getYDotGuess(state)[getSystemYIndex()]; + // TODO OPENSIM_THROW(Exception, "TODO"); +} + +void Component::AddedStateVariable:: + setDerivativeGuess(SimTK::State& state, double derivGuess) const +{ + // TODO find another way to get the guess. + const Component* root = nullptr; + { + const Component* comp = &getOwner(); + while (comp->hasParent()) comp = &comp->getParent(); + root = comp; + } + + // TODO very inefficient. + SimTK::Vector yDotGuess = root->getYDotGuess(state); + yDotGuess[getSystemYIndex()] = derivGuess; + root->setYDotGuess(state, yDotGuess); + + // TODO OPENSIM_THROW(Exception, "TODO"); +} void Component::dumpSubcomponents(int depth) const { diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index 7e1b3a16f6..e9b6295a4c 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -1072,6 +1072,29 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); * in the order returned by getStateVariableNames() */ SimTK::Vector getStateVariableValues(const SimTK::State& state) const; + + virtual const SimTK::Vector& getImplicitResidual(const SimTK::State& state) + const { + OPENSIM_THROW_FRMOBJ(Exception, + "Can only call on a root component (e.g., Model)."); + } + virtual const SimTK::Vector& getYDotGuess(const SimTK::State& state) const { + OPENSIM_THROW_FRMOBJ(Exception, + "Can only call on a root component (e.g., Model)."); + } +protected: // TODO move this around. + virtual SimTK::Vector& updImplicitResidual(const SimTK::State& state) + const { + OPENSIM_THROW_FRMOBJ(Exception, + "Can only call on a root component (e.g., Model)."); + } + // TODO really want "updYDotGuess". really inefficient this way... + virtual void setYDotGuess(SimTK::State& state, + const SimTK::Vector& yDotGuess) const { + OPENSIM_THROW_FRMOBJ(Exception, + "Can only call on a root component (e.g., Model)."); + } +public: /** * %Set all values of the state variables allocated by this Component. @@ -1091,6 +1114,18 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); */ double getStateVariableDerivativeValue(const SimTK::State& state, const std::string& name) const; + + /** TODO */ + double getStateVariableDerivativeGuess(const SimTK::State& state, + const std::string& name) const; + + /** TODO where to put this? */ + void setStateVariableDerivativeGuess(SimTK::State& state, + const std::string& name, double derivGuess) const; + + /** TODO where to put this? */ + double getImplicitResidual(const SimTK::State& state, + const std::string& name) const; /** * Get the value of a discrete variable allocated by this Component by name. @@ -1627,6 +1662,13 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); void setStateVariableDerivativeValue(const SimTK::State& state, const std::string& name, double deriv) const; + /** TODO */ + virtual void computeImplicitResiduals(const SimTK::State& s) const; + + /** TODO */ + void setImplicitResidual(const SimTK::State& state, + const std::string& name, double residual) const; + // End of Component Extension Interface (protected virtuals). ///@} @@ -1730,12 +1772,14 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); @param[in] stateVariableName string value to access variable by name @param[in] invalidatesStage the system realization stage that is invalidated when variable value is changed + @param[in] hasImplicitForm TODO @param[in] isHidden flag (bool) to optionally hide this state variable from being accessed outside this component as an Output */ void addStateVariable(const std::string& stateVariableName, const SimTK::Stage& invalidatesStage=SimTK::Stage::Dynamics, + bool hasImplicitForm = false, bool isHidden = false) const; /** The above method provides a convenient interface to this method, which @@ -2287,15 +2331,18 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); public: StateVariable() : name(""), owner(nullptr), subsysIndex(SimTK::InvalidIndex), varIndex(SimTK::InvalidIndex), - sysYIndex(SimTK::InvalidIndex), hidden(true) {} - explicit StateVariable(const std::string& name, //state var name + sysYIndex(SimTK::InvalidIndex), implicitForm(false), + hidden(true) {} + StateVariable(const std::string& name, //state var name const Component& owner, //owning component SimTK::SubsystemIndex sbsix,//subsystem for allocation int varIndex, //variable's index in subsystem + bool hasImplicitForm = false, //TODO bool hide = false) //state variable is hidden or not : name(name), owner(&owner), subsysIndex(sbsix), varIndex(varIndex), - sysYIndex(SimTK::InvalidIndex), hidden(hide) {} + sysYIndex(SimTK::InvalidIndex), implicitForm(hasImplicitForm), + hidden(hide) {} virtual ~StateVariable() {} @@ -2307,15 +2354,19 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); const SimTK::SubsystemIndex& getSubsysIndex() const { return subsysIndex; } // return the index in the global list of continuous state variables, Y const SimTK::SystemYIndex& getSystemYIndex() const { return sysYIndex; } + + bool hasImplicitForm() const { return implicitForm; } bool isHidden() const { return hidden; } void hide() { hidden = true; } void show() { hidden = false; } + // These setters are called in realizeTopology, realizeModel. void setVarIndex(int index) { varIndex = index; } void setSubsystemIndex(const SimTK::SubsystemIndex& sbsysix) { subsysIndex = sbsysix; } + void setSystemYIndex(const SimTK::SystemYIndex& ix) { sysYIndex = ix; } //Concrete Components implement how the state variable value is evaluated virtual double getValue(const SimTK::State& state) const = 0; @@ -2324,6 +2375,14 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); // The derivative a state should be a cache entry and thus does not // change the state virtual void setDerivative(const SimTK::State& state, double deriv) const = 0; + + // TODO + virtual double getImplicitResidual(const SimTK::State& state) const = 0; + // TODO + virtual void setImplicitResidual(const SimTK::State& state, double residual) const = 0; + // TODO get/set guess. + virtual double getDerivativeGuess(const SimTK::State& state) const = 0; + virtual void setDerivativeGuess(SimTK::State& state, double residual) const = 0; private: std::string name; @@ -2338,6 +2397,9 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); // Once allocated a state in the system will have a global index // and that can be stored here as well SimTK::SystemYIndex sysYIndex; + + // TODO + bool implicitForm; // flag indicating if state variable is hidden to the outside world bool hidden; @@ -2405,16 +2467,17 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); public: // Constructors AddedStateVariable() : StateVariable(), - invalidatesStage(SimTK::Stage::Empty) {} + invalidatesStage(SimTK::Stage::Empty) {} /** Convenience constructor for defining a Component added state variable */ - explicit AddedStateVariable(const std::string& name, //state var name + AddedStateVariable(const std::string& name, //state var name const Component& owner, //owning component SimTK::Stage invalidatesStage,//stage this variable invalidates + bool hasImplicitForm=false, bool hide=false) : StateVariable(name, owner, SimTK::SubsystemIndex(SimTK::InvalidIndex), - SimTK::InvalidIndex, hide), + SimTK::InvalidIndex, hasImplicitForm, hide), invalidatesStage(SimTK::Stage::Empty) {} //override virtual methods @@ -2423,6 +2486,14 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); double getDerivative(const SimTK::State& state) const override; void setDerivative(const SimTK::State& state, double deriv) const override; + + // TODO + double getImplicitResidual(const SimTK::State& state) const override; + // TODO + void setImplicitResidual(const SimTK::State& state, double residual) const override; + // TODO get/set guess. + double getDerivativeGuess(const SimTK::State& state) const override; + void setDerivativeGuess(SimTK::State& state, double residual) const override; private: // DATA // Changes in state variables trigger recalculation of appropriate cache @@ -2431,7 +2502,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); SimTK::Stage invalidatesStage; }; - // Structure to hold related info about discrete variables + // Structure to hold related info about state variables struct StateVariableInfo { StateVariableInfo() {} explicit StateVariableInfo(Component::StateVariable* sv, int order) : diff --git a/OpenSim/Simulation/Model/Model.cpp b/OpenSim/Simulation/Model/Model.cpp index c27da59ec3..d0c27db51a 100644 --- a/OpenSim/Simulation/Model/Model.cpp +++ b/OpenSim/Simulation/Model/Model.cpp @@ -768,7 +768,98 @@ void Model::extendAddToSystem(SimTK::MultibodySystem& system) const mutableThis->_modelControlsIndex = modelControls.getSubsystemMeasureIndex(); } +// TODO so that +void Model::extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) const +{ + Super::extendAddToSystemAfterSubcomponents(system); + // TODO determine if we have any state variables with an implicit form. + + // TODO would rather do s.getNY() but can't do that till after + // realizeTopology(). + // TODO what if Simbody has state variables that OpenSim doesn't know about? + const int numStateVars = getNumStateVariables(); + Vector nans(numStateVars, SimTK::NaN); + nans.lockShape(); // TODO where is the right place for this? + + // This Measure is basically a discrete state variable. + // Acceleration cache is invalid if this is updated. + // TODO what should default value be? what size should it have? + Measure_::Variable yDotGuess(_system->updDefaultSubsystem(), + Stage::Acceleration, nans); + _yDotGuessIndex = yDotGuess.getSubsystemMeasureIndex(); + + Measure_::Variable lambdaGuess(_system->updDefaultSubsystem(), + Stage::Acceleration, nans); + _lambdaGuessIndex = lambdaGuess.getSubsystemMeasureIndex(); + + // Storing the residual. + // We can only compute the residual once realized to Dynamics, since + // we will need to apply forces. + // None of the acceleration-level calculations depend on the residual + // (nothing should depend on the residual) so there is nothing to + // invalidate when the residual is changed (invalidated = Infinity). + // TODO do we depend on Acceleration stage, for constraint forces? + Measure_::Result implicitResidual(_system->updDefaultSubsystem(), + Stage::Dynamics, Stage::Infinity); + implicitResidual.setDefaultValue(nans); + _implicitResidualIndex = implicitResidual.getSubsystemMeasureIndex(); +} + +const SimTK::Vector& Model::getImplicitResidual(const SimTK::State& state) const { + OPENSIM_THROW_IF_FRMOBJ(!_system || !_implicitResidualIndex.isValid(), + Exception, "Prior call to Model::initSystem() is required."); + // TODO must make sure we are realized to Dynamics. + auto implicitResidual = Measure_::Result::getAs( + _system->getDefaultSubsystem().getMeasure(_implicitResidualIndex)); + + if (!implicitResidual.isValid(state)) { + // TODO should put this in a separate "realizeImplicitResidual"? + // TODO perhaps this is not the best way to invoke the actual calculation + // of residuals. + for (const auto& comp : getComponentList()) { + comp.computeImplicitResiduals(state); + } + implicitResidual.markAsValid(state); // TODO + } + + return implicitResidual.getValue(state); +} + +SimTK::Vector& Model::updImplicitResidual(const SimTK::State& state) + const { + OPENSIM_THROW_IF_FRMOBJ(!_system || !_implicitResidualIndex.isValid(), + Exception, "Prior call to Model::initSystem() is required."); + auto implicitResidual = Measure_::Result::getAs( + _system->getDefaultSubsystem().getMeasure(_implicitResidualIndex)); + return implicitResidual.updValue(state); + // TODO OPENSIM_THROW_FRMOBJ(Exception, "TODO"); +} + +const SimTK::Vector& Model::getYDotGuess(const SimTK::State& state) const { + OPENSIM_THROW_IF_FRMOBJ(!_system || !_yDotGuessIndex.isValid(), + Exception, "Prior call to Model::initSystem() is required."); + + auto yDotGuess = Measure_::Variable::getAs( + _system->getDefaultSubsystem().getMeasure(_yDotGuessIndex)); + + return yDotGuess.getValue(state); + + // TODO OPENSIM_THROW_FRMOBJ(Exception, "TODO"); +} +void Model::setYDotGuess(SimTK::State& state, + const SimTK::Vector& yDotGuess) const { + OPENSIM_THROW_IF_FRMOBJ(!_system || !_yDotGuessIndex.isValid(), + Exception, "Prior call to Model::initSystem() is required."); + + auto measure = Measure_::Variable::getAs( + _system->getDefaultSubsystem().getMeasure(_yDotGuessIndex)); + + // TODO ensure yDotGuess has the correct length. + return measure.setValue(state, yDotGuess); + + // TODO OPENSIM_THROW_FRMOBJ(Exception, "TODO"); +} //_____________________________________________________________________________ /** * Add any Component derived from ModelComponent to the Model. diff --git a/OpenSim/Simulation/Model/Model.h b/OpenSim/Simulation/Model/Model.h index 2193cd30f6..60e098422f 100644 --- a/OpenSim/Simulation/Model/Model.h +++ b/OpenSim/Simulation/Model/Model.h @@ -876,7 +876,14 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Model, ModelComponent); //-------------------------------------------------------------------------- // DERIVATIVES - //-------------------------------------------------------------------------- + //-------------------------------------------------------------------------- + const SimTK::Vector& getImplicitResidual(const SimTK::State& state) const + override; + const SimTK::Vector& getYDotGuess(const SimTK::State& state) const override; + SimTK::Vector& updImplicitResidual(const SimTK::State& state) const override; + void setYDotGuess(SimTK::State& state, + const SimTK::Vector& yDotGuess) const override; + //-------------------------------------------------------------------------- // OPERATIONS //-------------------------------------------------------------------------- @@ -970,7 +977,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Model, ModelComponent); void extendFinalizeFromProperties() override; void extendConnectToModel(Model& model) override; - void extendAddToSystem(SimTK::MultibodySystem& system) const override; + void extendAddToSystem(SimTK::MultibodySystem& system) const override; + // TODO + void extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) + const override; void extendInitStateFromProperties(SimTK::State& state) const override; /**@}**/ @@ -1101,6 +1111,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Model, ModelComponent); SimTK::MeasureIndex _modelControlsIndex; // Default values pooled from Actuators upon system creation. mutable SimTK::Vector _defaultControls; + + // TODO + mutable SimTK::ResetOnCopy _yDotGuessIndex; + mutable SimTK::ResetOnCopy _lambdaGuessIndex; + mutable SimTK::ResetOnCopy _implicitResidualIndex; // VISUALIZATION diff --git a/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp b/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp index c9decf6c6f..c14b8e90dd 100644 --- a/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp +++ b/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp @@ -645,6 +645,29 @@ void Coordinate::CoordinateStateVariable:: throw Exception(msg); } +double Coordinate::CoordinateStateVariable:: + getImplicitResidual(const SimTK::State& state) const +{ + OPENSIM_THROW(Exception, "TODO"); +} + +void Coordinate::CoordinateStateVariable:: + setImplicitResidual(const SimTK::State& state, double residual) const +{ + OPENSIM_THROW(Exception, "TODO"); +} + +double Coordinate::CoordinateStateVariable:: + getDerivativeGuess(const SimTK::State& state) const +{ + OPENSIM_THROW(Exception, "TODO"); +} + +void Coordinate::CoordinateStateVariable:: + setDerivativeGuess(SimTK::State& state, double derivGuess) const +{ + OPENSIM_THROW(Exception, "TODO"); +} //----------------------------------------------------------------------------- // Coordinate::SpeedStateVariable @@ -680,3 +703,27 @@ void Coordinate::SpeedStateVariable:: msg += "Generalized speed derivative (udot) can only be set by the Multibody system."; throw Exception(msg); } + +double Coordinate::SpeedStateVariable:: + getImplicitResidual(const SimTK::State& state) const +{ + OPENSIM_THROW(Exception, "TODO"); +} + +void Coordinate::SpeedStateVariable:: + setImplicitResidual(const SimTK::State& state, double residual) const +{ + OPENSIM_THROW(Exception, "TODO"); +} + +double Coordinate::SpeedStateVariable:: + getDerivativeGuess(const SimTK::State& state) const +{ + OPENSIM_THROW(Exception, "TODO"); +} + +void Coordinate::SpeedStateVariable:: + setDerivativeGuess(SimTK::State& state, double derivGuess) const +{ + OPENSIM_THROW(Exception, "TODO"); +} diff --git a/OpenSim/Simulation/SimbodyEngine/Coordinate.h b/OpenSim/Simulation/SimbodyEngine/Coordinate.h index 2eeda5a596..2cc1d00582 100644 --- a/OpenSim/Simulation/SimbodyEngine/Coordinate.h +++ b/OpenSim/Simulation/SimbodyEngine/Coordinate.h @@ -255,6 +255,13 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Coordinate, ModelComponent); void setValue(SimTK::State& state, double value) const override; double getDerivative(const SimTK::State& state) const override; void setDerivative(const SimTK::State& state, double deriv) const override; + + double getImplicitResidual(const SimTK::State& state) const override; + // TODO + void setImplicitResidual(const SimTK::State& state, double residual) const override; + // TODO get/set guess. + double getDerivativeGuess(const SimTK::State& state) const override; + void setDerivativeGuess(SimTK::State& state, double residual) const override; }; // Class for handling state variable added (allocated) by this Component @@ -273,6 +280,13 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Coordinate, ModelComponent); void setValue(SimTK::State& state, double value) const override; double getDerivative(const SimTK::State& state) const override; void setDerivative(const SimTK::State& state, double deriv) const override; + + double getImplicitResidual(const SimTK::State& state) const override; + // TODO + void setImplicitResidual(const SimTK::State& state, double residual) const override; + // TODO get/set guess. + double getDerivativeGuess(const SimTK::State& state) const override; + void setDerivativeGuess(SimTK::State& state, double residual) const override; }; // All coordinates (Simbody mobility) have associated constraints that diff --git a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp index 930fbc79ed..3740f21ea2 100644 --- a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp +++ b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp @@ -24,9 +24,18 @@ /** TODO */ +// Tests: // TODO component w/multiple state vars, only some of which have implicit form. // TODO model containing components with and without explicit form. // TODO write custom implicit form solver. +// TODO copying and (de)serializing models with implicit forms. +// TODO model containing multiple components with implicit form. +// TODO editing yDotGuess or lambdaGuess invalidates the residual cache. +// TODO calculation of YIndex must be correct. + +// Implementation: +// TODO Only create implicit cache/discrete vars if any components have an +// implicit form (place in extendAddToSystemAfterComponents()). #include @@ -35,39 +44,42 @@ using namespace OpenSim; using namespace SimTK; /** TODO */ -class TrigDynamics : public Component { -OpenSim_DECLARE_CONCRETE_OBJECT(TrigDynamics, Component); +class LinearDynamics : public Component { +OpenSim_DECLARE_CONCRETE_OBJECT(LinearDynamics, Component); public: - OpenSim_DECLARE_PROPERTY(default_sine, double, + OpenSim_DECLARE_PROPERTY(default_activ, double, "Default value of state variable."); - OpenSim_DECLARE_OUTPUT_FOR_STATE_VARIABLE(sine); - TrigDynamics() { - constructProperty_default_sine(0); + OpenSim_DECLARE_PROPERTY(coeff, double, + "Coefficient in the differential equation."); + OpenSim_DECLARE_OUTPUT_FOR_STATE_VARIABLE(activ); + LinearDynamics() { + constructProperty_default_activ(0.0); + constructProperty_coeff(-1.0); } protected: void extendAddToSystem(SimTK::MultibodySystem& system) const override { Super::extendAddToSystem(system); - addStateVariable("sine" /* , true has implicit form */); + addStateVariable("activ" /* , true has implicit form */); } void computeStateVariableDerivatives(const SimTK::State& s) const override { - // TODO Super::computeStateVariableDerivatives(s); - setStateVariableDerivativeValue(s, "sine", cos(s.getTime())); + // TODO invokes false error msg. Super::computeStateVariableDerivatives(s); + const Real& activ = getStateVariableValue(s, "activ"); + setStateVariableDerivativeValue(s, "activ", get_coeff() * activ); } - /* TODO - void computeStateVariableImplicitResiduals(const SimTK::State& s) + void computeImplicitResiduals(const SimTK::State& s) const override { - Super::computeStateVariableDerivatives(s); - yDotGuess = getStateVariableDerivativeGuess(s, "sine"); - setStateVariableImplicitResidual(s, "sine", cos(s.getTime()) - yDotGuess); + // TODO Super::computeStateVariableDerivatives(s); + const Real& activ = getStateVariableValue(s, "activ"); + double activDotGuess = getStateVariableDerivativeGuess(s, "activ"); + setImplicitResidual(s, "activ", get_coeff() * activ - activDotGuess); } - */ void extendInitStateFromProperties(SimTK::State& s) const override { Super::extendInitStateFromProperties(s); - setStateVariableValue(s, "sine", get_default_sine()); + setStateVariableValue(s, "activ", get_default_activ()); } void extendSetPropertiesFromState(const SimTK::State& s) override { Super::extendSetPropertiesFromState(s); - set_default_sine(getStateVariableValue(s, "sine")); + set_default_activ(getStateVariableValue(s, "activ")); } }; @@ -77,40 +89,72 @@ void simulate(const Model& model, State& state, Real finalTime) { SimTK::RungeKuttaMersonIntegrator integrator(model.getSystem()); SimTK::TimeStepper ts(model.getSystem(), integrator); ts.initialize(state); - // TODO ts.setReportAllSignificantStates(true); - // TODO integrator.setReturnEveryInternalStep(true); ts.stepTo(finalTime); state = ts.getState(); } // Ensure that explicit forward integration works for a component that also -// provides an implicit form. +// provides an implicit form. The model contains only one component, which +// contains one state variable. void testExplicitFormOfImplicitComponent() { + // Create model. Model model; model.setName("model"); - auto* trig = new TrigDynamics(); - trig->setName("foo"); + auto* comp = new LinearDynamics(); + comp->setName("foo"); const Real initialValue = 3.5; - trig->set_default_sine(initialValue); - model.addComponent(trig); + comp->set_default_activ(initialValue); + const Real coeff = -0.28; + comp->set_coeff(coeff); + model.addComponent(comp); // TODO auto* rep = new ConsoleReporter(); // TODO rep->set_report_time_interval(0.01); // TODO model.addComponent(rep); - // TODO rep->updInput("inputs").connect(trig->getOutput("sine")); + // TODO rep->updInput("inputs").connect(comp->getOutput("activ")); auto s = model.initSystem(); - SimTK_TEST(trig->getStateVariableValue(s, "sine") == initialValue); + + // Check initial values. + SimTK_TEST(comp->getStateVariableValue(s, "activ") == initialValue); model.realizeAcceleration(s); - SimTK_TEST(trig->getStateVariableDerivativeValue(s, "sine") == cos(s.getTime())); + SimTK_TEST_EQ(comp->getStateVariableDerivativeValue(s, "activ"), + coeff * initialValue); + // Simulate and check resulting value of state variable. const double finalTime = 0.23; simulate(model, s, finalTime); - SimTK_TEST_EQ_TOL(trig->getStateVariableValue(s, "sine"), - initialValue + sin(finalTime), 1e-5); + SimTK_TEST_EQ_TOL(comp->getStateVariableValue(s, "activ"), + initialValue * exp(coeff * finalTime), 1e-5); +} + +void testTODO() { + Model model; model.setName("model"); + auto* comp = new LinearDynamics(); + comp->setName("foo"); + const Real initialValue = 3.5; + comp->set_default_activ(initialValue); + const Real coeff = -0.28; + comp->set_coeff(coeff); + model.addComponent(comp); + auto s = model.initSystem(); + + // Access residual from the component: + model.realizeDynamics(s); // Must realize to dynamics to get residuals. + // TODO set yGuess. + const Real activDotGuess = 5.3; + comp->setStateVariableDerivativeGuess(s, "activ", activDotGuess); + SimTK_TEST(comp->getImplicitResidual(s, "activ") == + coeff * initialValue - activDotGuess); + + // TODO set derivative guess in one sweep. + // TODO SimTK_TEST(model.getImplicitResiduals(s) == Vector(1, activDotGuess)); + /* TODO + */ } int main() { SimTK_START_TEST("testImplicitDifferentialEquatiosns"); SimTK_SUBTEST(testExplicitFormOfImplicitComponent); + SimTK_SUBTEST(testTODO); SimTK_END_TEST(); } \ No newline at end of file From 0968cf028a3838fd85ee3eafbd908245aa27e51d Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Fri, 15 Jul 2016 13:34:12 -0700 Subject: [PATCH 17/40] Rename getImplicitResidual(). --- OpenSim/Common/Component.cpp | 5 ++--- OpenSim/Common/Component.h | 4 ++-- OpenSim/Simulation/Model/Model.cpp | 4 ++-- OpenSim/Simulation/Model/Model.h | 5 +++-- .../Test/testImplicitDifferentialEquations.cpp | 9 +++++---- 5 files changed, 14 insertions(+), 13 deletions(-) diff --git a/OpenSim/Common/Component.cpp b/OpenSim/Common/Component.cpp index 924804dc22..92bab3c8f5 100644 --- a/OpenSim/Common/Component.cpp +++ b/OpenSim/Common/Component.cpp @@ -1544,8 +1544,7 @@ double Component::AddedStateVariable:: // TODO should returning a single entry of the residual require // evaluating the entire vector? If not, might need a local cache variable. - const SimTK::Vector& residual = root->getImplicitResidual(state); - return residual[getSystemYIndex()]; + return root->getImplicitResiduals(state)[getSystemYIndex()]; // TODO OPENSIM_THROW(Exception, "TODO"); } @@ -1561,7 +1560,7 @@ void Component::AddedStateVariable:: root = comp; } - root->updImplicitResidual(state)[getSystemYIndex()] = residual; + root->updImplicitResiduals(state)[getSystemYIndex()] = residual; // TODO OPENSIM_THROW(Exception, "TODO"); } diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index e9b6295a4c..d2121ad5e3 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -1073,7 +1073,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); */ SimTK::Vector getStateVariableValues(const SimTK::State& state) const; - virtual const SimTK::Vector& getImplicitResidual(const SimTK::State& state) + virtual const SimTK::Vector& getImplicitResiduals(const SimTK::State& state) const { OPENSIM_THROW_FRMOBJ(Exception, "Can only call on a root component (e.g., Model)."); @@ -1083,7 +1083,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); "Can only call on a root component (e.g., Model)."); } protected: // TODO move this around. - virtual SimTK::Vector& updImplicitResidual(const SimTK::State& state) + virtual SimTK::Vector& updImplicitResiduals(const SimTK::State& state) const { OPENSIM_THROW_FRMOBJ(Exception, "Can only call on a root component (e.g., Model)."); diff --git a/OpenSim/Simulation/Model/Model.cpp b/OpenSim/Simulation/Model/Model.cpp index d0c27db51a..0135538b29 100644 --- a/OpenSim/Simulation/Model/Model.cpp +++ b/OpenSim/Simulation/Model/Model.cpp @@ -805,7 +805,7 @@ void Model::extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) _implicitResidualIndex = implicitResidual.getSubsystemMeasureIndex(); } -const SimTK::Vector& Model::getImplicitResidual(const SimTK::State& state) const { +const SimTK::Vector& Model::getImplicitResiduals(const SimTK::State& state) const { OPENSIM_THROW_IF_FRMOBJ(!_system || !_implicitResidualIndex.isValid(), Exception, "Prior call to Model::initSystem() is required."); // TODO must make sure we are realized to Dynamics. @@ -825,7 +825,7 @@ const SimTK::Vector& Model::getImplicitResidual(const SimTK::State& state) const return implicitResidual.getValue(state); } -SimTK::Vector& Model::updImplicitResidual(const SimTK::State& state) +SimTK::Vector& Model::updImplicitResiduals(const SimTK::State& state) const { OPENSIM_THROW_IF_FRMOBJ(!_system || !_implicitResidualIndex.isValid(), Exception, "Prior call to Model::initSystem() is required."); diff --git a/OpenSim/Simulation/Model/Model.h b/OpenSim/Simulation/Model/Model.h index 60e098422f..2a983d71a1 100644 --- a/OpenSim/Simulation/Model/Model.h +++ b/OpenSim/Simulation/Model/Model.h @@ -877,10 +877,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Model, ModelComponent); //-------------------------------------------------------------------------- // DERIVATIVES //-------------------------------------------------------------------------- - const SimTK::Vector& getImplicitResidual(const SimTK::State& state) const + // TODO rearrange according to public/private. + const SimTK::Vector& getImplicitResiduals(const SimTK::State& state) const override; const SimTK::Vector& getYDotGuess(const SimTK::State& state) const override; - SimTK::Vector& updImplicitResidual(const SimTK::State& state) const override; + SimTK::Vector& updImplicitResiduals(const SimTK::State& state) const override; void setYDotGuess(SimTK::State& state, const SimTK::Vector& yDotGuess) const override; diff --git a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp index 3740f21ea2..3fd484eba6 100644 --- a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp +++ b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp @@ -143,13 +143,14 @@ void testTODO() { // TODO set yGuess. const Real activDotGuess = 5.3; comp->setStateVariableDerivativeGuess(s, "activ", activDotGuess); - SimTK_TEST(comp->getImplicitResidual(s, "activ") == - coeff * initialValue - activDotGuess); + double expectedResidual = coeff * initialValue - activDotGuess; + SimTK_TEST(comp->getImplicitResidual(s, "activ") == expectedResidual); - // TODO set derivative guess in one sweep. - // TODO SimTK_TEST(model.getImplicitResiduals(s) == Vector(1, activDotGuess)); + // Make sure the residual is accessible from the entire residual vector. + SimTK_TEST_EQ(model.getImplicitResiduals(s), Vector(1, expectedResidual)); /* TODO */ + // TODO set derivative guess in one sweep. } int main() { From 69bd723234e402f2715faf4c474f1159fbf0bef1 Mon Sep 17 00:00:00 2001 From: humphreysb Date: Fri, 15 Jul 2016 15:39:26 -0700 Subject: [PATCH 18/40] With updates for static equib --- .idea/workspace.xml | 54 ++++----- OpenSim/Actuators/VandenBogert2011Muscle.cpp | 119 +++++++++++-------- OpenSim/Actuators/VandenBogert2011Muscle.h | 6 +- 3 files changed, 98 insertions(+), 81 deletions(-) diff --git a/.idea/workspace.xml b/.idea/workspace.xml index 7c884a20cb..3f9965324f 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -109,8 +109,8 @@ - + @@ -162,12 +162,12 @@ - + - - + + @@ -178,8 +178,8 @@ - - + + @@ -188,8 +188,8 @@ - - + + @@ -199,7 +199,7 @@ - + @@ -208,7 +208,7 @@ - + @@ -240,10 +240,8 @@ DEFINITION_ORDER - @@ -1168,13 +1166,13 @@ - + - + - + @@ -1589,32 +1587,32 @@ - + - + - - + + - + - - + + - - + + @@ -1623,8 +1621,8 @@ - - + + diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.cpp b/OpenSim/Actuators/VandenBogert2011Muscle.cpp index 5ab5fa354f..28a474e121 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.cpp +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -296,35 +296,21 @@ else { }; -//------------------------------------------------------------------------------ -VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle::calcImplicitResidual(SimTK::Vec2 y,SimTK::Vec2 ydot_guess, double muscleLength, double u, int returnJacobians) const { - - // Overload method for state vectors as parameters -VandenBogert2011Muscle::ImplicitResidual Results= calcImplicitResidual(muscleLength, y[0], y[1], - ydot_guess[0], ydot_guess[1], u, returnJacobians); -return Results; -} - //------------------------------------------------------------------------------ -SimTK::Vec2 VandenBogert2011Muscle::calcImplicitResidual(double projFibLen, double muscleLength) const { - - // Overload method for when calculating static equilibrium - +VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle::calcImplicitResidual(SimTK::Vec2 y,SimTK::Vec2 ydot_guess, double muscleLength, double u, int returnJacobians) const { - VandenBogert2011Muscle::ImplicitResidual Results = calcImplicitResidual( - muscleLength, projFibLen, 0.0, 0.0, 0.0, 0.0, 1.0); + // Overload method for state vectors as parameters - SimTK::Vec2 forceResAndDerivative; + VandenBogert2011Muscle::ImplicitResidual Results= calcImplicitResidual(muscleLength, y[0], y[1], + ydot_guess[0], ydot_guess[1], u, returnJacobians); - forceResAndDerivative[0]=Results.forceResidual; - forceResAndDerivative[1]=Results.df_dy[0][0]; + return Results; - return forceResAndDerivative; } @@ -584,7 +570,10 @@ calcImplicitResidual(double muscleLength, double projFibLenNorm, double activ, // ---------Calculate the Muscle Force Residual ---------------------- // //The muscle dynamics equation: f = Fsee - (a*Fce - Fpee)*cos(Penn) - // Fdamping = 0 + double fRes = F4 - (activ * F1 * F2 + F3)*cosPenn - F5; + //cout << fRes << "=" << F4 << "-(" << activ << "*" << F1 << "*" << F2 << "+" << F3 << ")*" << cosPenn << "-" << F5 << endl; + @@ -786,12 +775,37 @@ const { } +//------------------------------------------------------------------------------ +SimTK::Vec3 VandenBogert2011Muscle::calcFiberStaticEquilibResidual(double projFibLen, double muscleLength, double activ) const { + + // Just a quick convience method to get the force residual under static conditions + + VandenBogert2011Muscle::ImplicitResidual Results = calcImplicitResidual( + muscleLength, projFibLen, activ, 0.0, 0.0, activ, 1.0); + + SimTK::Vec3 resAndDerivative; + resAndDerivative[0]=Results.forceResidual; + resAndDerivative[1]=Results.df_dy[0][0]; + resAndDerivative[2]=Results.forceTendon; + + return resAndDerivative; + +} //------------------------------------------------------------------------------ -SimTK::Vec1 VandenBogert2011Muscle::calcStatic(double muscleLength ) const { +SimTK::Vec3 VandenBogert2011Muscle::calcFiberStaticEquilbirum(double muscleLength, double activ) const { + -double tol=1e-4; + + //TODO: Code is optimized. Specifically the number of calls to + //calcImplicitResidual can be reduced + + //TODO: Generalize with a Lambda function (will need help with that). + //TODO: calcImplicitResidual really only needs to calculate df_ds (single element) for this function + + +double tol=1e-5; double a=0; double b=10; @@ -800,53 +814,56 @@ double dx=2*tol; int neval=0; -while (abs(x)>=tol){ +SimTK::Vec3 forceResAndDerivative; -neval++; +while ((abs(dx)>=tol) && (neval<100)) { -// Set a to be lower value and b to be upper value -a=min(a,b); -b=max(a,b); + neval++; + // Set a to be lower value and b to be upper value + a=min(a,b); + b=max(a,b); -SimTK::Vec2 forceResAndDerivative = calcImplicitResidual(x, muscleLength); -double fx = forceResAndDerivative[0]; + forceResAndDerivative = calcFiberStaticEquilibResidual(x, muscleLength,activ); + double fx = forceResAndDerivative[0]; + //After the 1st iteration, use the new guess as a new upper or lower bound + if (neval>1) { + forceResAndDerivative = calcFiberStaticEquilibResidual(a, muscleLength,activ); + double funcA = forceResAndDerivative[0]; -forceResAndDerivative = calcImplicitResidual(a, muscleLength); -double funcA = forceResAndDerivative[0]; + if ((funcA *fx)>0){ + a = x;} + else { b = x;}} -//After the 1st iteration, use the new guess as a new upper or lower bound -if (neval>1) { - if (funcA *fx>0){ - a = x;} - else { b = x;}} + forceResAndDerivative = calcFiberStaticEquilibResidual(x, muscleLength,activ); + double dfx= forceResAndDerivative[1]; + //double forceRes=forceResAndDerivative[0]; + dx =-fx/dfx; + double xdx=x-dx; + bool inInterval=((xdx>=a) && (xdx<=b)); -forceResAndDerivative = calcImplicitResidual(x, muscleLength); -double dfx= forceResAndDerivative[1]; -double forceRes=forceResAndDerivative[0]; + forceResAndDerivative = calcFiberStaticEquilibResidual(xdx, muscleLength,activ); + bool largeDeriv=abs(forceResAndDerivative[0])> (0.5*abs(fx)); -double dx =-fx/dfx; -double xdx=x-dx; + if (~inInterval || largeDeriv) { + x=(a+b)/2; + dx=(a-b)/2;} + else + {x=xdx;}; + //cout << x << " " << dx << " " << fx << " " << dfx << endl; -bool inInterval=((xdx>=a) && (xdx<=b)); -forceResAndDerivative = calcImplicitResidual(xdx, muscleLength); -bool largeDeriv=abs(forceResAndDerivative[0])> 0.5*abs(fx); - -if (~inInterval || largeDeriv){ -x=(a+b)/2; -dx=(a-b)/2;} -else -{x=xdx;}; + }; -}; - SimTK::Vec1 vout; + SimTK::Vec3 vout; vout[0]=x; + vout[1]=neval; + vout[2]=forceResAndDerivative[2]; return vout; } diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h index ca0a8b9475..2bba465856 100644 --- a/OpenSim/Actuators/VandenBogert2011Muscle.h +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -164,9 +164,11 @@ The parent class, Muscle.h, provides double projFibVelNorm, double activdot, double u, double h) const; - SimTK::Vec2 calcImplicitResidual(double projFibLen, double muscleLength) const; - SimTK::Vec1 calcStatic(double muscleLength) const; + SimTK::Vec3 calcFiberStaticEquilbirum(double muscleLength, double activ) const; + SimTK::Vec3 calcFiberStaticEquilibResidual(double projFibLen, double muscleLength, double activ) const; + + //Hacks for troubleshooting Mat22: SimTK::Mat22 fixMat22(SimTK::Mat22 matIn,SimTK::Mat22 matFixed) const; From a7bb1932698cca61294a26ac89a74d6c61327dfe Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Fri, 15 Jul 2016 17:32:14 -0700 Subject: [PATCH 19/40] Added multibody system (no constraints) to residuals. --- OpenSim/Common/Component.cpp | 37 ++--- OpenSim/Common/Component.h | 18 ++- OpenSim/OpenSimDoxygenMain.h | 14 +- OpenSim/Simulation/Model/Model.cpp | 45 +++++- OpenSim/Simulation/Model/Model.h | 3 + .../Simulation/SimbodyEngine/Coordinate.cpp | 133 +++++++++++++++--- OpenSim/Simulation/SimbodyEngine/Coordinate.h | 32 +++-- .../testImplicitDifferentialEquations.cpp | 85 ++++++++++- 8 files changed, 308 insertions(+), 59 deletions(-) diff --git a/OpenSim/Common/Component.cpp b/OpenSim/Common/Component.cpp index 92bab3c8f5..5435dff9de 100644 --- a/OpenSim/Common/Component.cpp +++ b/OpenSim/Common/Component.cpp @@ -421,6 +421,11 @@ void Component::computeStateVariableDerivatives(const SimTK::State& s) const void Component::computeImplicitResiduals(const SimTK::State& s) const { + // TODO it's really bad if obtaining the trivial implicit form of + // auxiliary state variables requires realizing to acceleration (if + // the explicit fiber velocity is only available at acceleration); then + // computing the residual will still require computing the explicit + // multibody dynamics. // OPENSIM_THROW_FRMOBJ(Exception, "TODO"); } @@ -1410,22 +1415,12 @@ void Component::extendRealizeTopology(SimTK::State& s) const void Component::extendRealizeInstance(const SimTK::State& s) const { for (auto& entry : _namedStateVariableInfo) { - const StateVariable& sv = *entry.second.stateVariable; - const AddedStateVariable* asv - = dynamic_cast(&sv); - // TODO asv->setSystemYIndexFromTODO(); - if(asv){// add index information for added state variables - // make mutable just to update system allocated index ONLY! - AddedStateVariable* masv = const_cast(asv); - // This is only known at Model stage: - const int systemYIdxOfFirstZ = int(s.getZStart()); - const SubsystemIndex subsysIdx = masv->getSubsysIndex(); - const int systemZIdxOfFirstSubsystemZ = s.getZStart(subsysIdx); - const int subsystemZIdx = masv->getVarIndex(); - masv->setSystemYIndex(SystemYIndex(systemYIdxOfFirstZ + - systemZIdxOfFirstSubsystemZ + - subsystemZIdx)); // TODO - } + // make mutable just to update system allocated index ONLY! + auto* msv = const_cast(entry.second.stateVariable.get()); + // This must be called after Model stage has been realized + // b/c that's when getZStart(), etc. is available and those methods + // need to be used to determine the SystemYIndex. + msv->determineSystemYIndex(s); } } @@ -1599,6 +1594,16 @@ void Component::AddedStateVariable:: // TODO OPENSIM_THROW(Exception, "TODO"); } +SimTK::SystemYIndex Component::AddedStateVariable:: +implementDetermineSystemYIndex(const SimTK::State& s) const { + // TODO explain why this is so. + const int systemYIdxOfFirstZ = int(s.getZStart()); + const int systemZIdxOfFirstSubsystemZ = s.getZStart(getSubsysIndex()); + const int subsystemZIdx = getVarIndex(); + return SystemYIndex(systemYIdxOfFirstZ + systemZIdxOfFirstSubsystemZ + + subsystemZIdx); +} + void Component::dumpSubcomponents(int depth) const { std::string tabs; diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index d2121ad5e3..5fdbf9bd1e 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -1073,6 +1073,8 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); */ SimTK::Vector getStateVariableValues(const SimTK::State& state) const; + /** TODO + */ virtual const SimTK::Vector& getImplicitResiduals(const SimTK::State& state) const { OPENSIM_THROW_FRMOBJ(Exception, @@ -2366,7 +2368,9 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); void setSubsystemIndex(const SimTK::SubsystemIndex& sbsysix) { subsysIndex = sbsysix; } - void setSystemYIndex(const SimTK::SystemYIndex& ix) { sysYIndex = ix; } + void determineSystemYIndex(const SimTK::State& s) { + sysYIndex = implementDetermineSystemYIndex(s); + } //Concrete Components implement how the state variable value is evaluated virtual double getValue(const SimTK::State& state) const = 0; @@ -2383,8 +2387,11 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); // TODO get/set guess. virtual double getDerivativeGuess(const SimTK::State& state) const = 0; virtual void setDerivativeGuess(SimTK::State& state, double residual) const = 0; - + private: + virtual SimTK::SystemYIndex implementDetermineSystemYIndex( + const SimTK::State& s) const = 0; + std::string name; SimTK::ReferencePtr owner; @@ -2464,7 +2471,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); // Class for handling state variable added (allocated) by this Component class AddedStateVariable : public StateVariable { - public: + public: // Constructors AddedStateVariable() : StateVariable(), invalidatesStage(SimTK::Stage::Empty) {} @@ -2494,8 +2501,11 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); // TODO get/set guess. double getDerivativeGuess(const SimTK::State& state) const override; void setDerivativeGuess(SimTK::State& state, double residual) const override; + + SimTK::SystemYIndex implementDetermineSystemYIndex( + const SimTK::State& s) const override; - private: // DATA + private: // DATA // Changes in state variables trigger recalculation of appropriate cache // variables by automatically invalidating the realization stage specified // upon allocation of the state variable. diff --git a/OpenSim/OpenSimDoxygenMain.h b/OpenSim/OpenSimDoxygenMain.h index e5d1e28714..d85b42eb14 100644 --- a/OpenSim/OpenSimDoxygenMain.h +++ b/OpenSim/OpenSimDoxygenMain.h @@ -28,8 +28,7 @@ Mainpage, the first page that a user sees when entering the Doxygen- generated API documentation. This is not actually included as part of the OpenSim source and it is not installed with OpenSim. **/ -/** @page reporters Reporter Components - * @defgroup reporters Reporter Components +/** @defgroup reporters Reporter Components * These components allow you to report the quantities calculated by your * model in a unified way. You can wire the outputs of Component%s into one * of these reporters to either save the quantities to a DataTable_ (that you @@ -37,6 +36,17 @@ OpenSim source and it is not installed with OpenSim. **/ * Reporters have a single list Input named "inputs". */ +/** @defgroup implicitdiffeq Implicit form of dynamics + * Some components may provide their dynamics (e.g., activation dynamics, fiber + * dynamics) as implicit differential equations in addition to the + * traditional explicit form. + * This feature was added in OpenSim 4.0 (TODO). + * + * TODO only use with unconstrained systems. + * TODO not even prescribed motion or locked joints! + * TODO + */ + /** @mainpage Overview \if developer diff --git a/OpenSim/Simulation/Model/Model.cpp b/OpenSim/Simulation/Model/Model.cpp index 0135538b29..4057caed82 100644 --- a/OpenSim/Simulation/Model/Model.cpp +++ b/OpenSim/Simulation/Model/Model.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include "SimTKcommon/internal/SystemGuts.h" @@ -809,20 +810,56 @@ const SimTK::Vector& Model::getImplicitResiduals(const SimTK::State& state) cons OPENSIM_THROW_IF_FRMOBJ(!_system || !_implicitResidualIndex.isValid(), Exception, "Prior call to Model::initSystem() is required."); // TODO must make sure we are realized to Dynamics. - auto implicitResidual = Measure_::Result::getAs( + auto measure = Measure_::Result::getAs( _system->getDefaultSubsystem().getMeasure(_implicitResidualIndex)); + + // Multibody states. + - if (!implicitResidual.isValid(state)) { + if (!measure.isValid(state)) { + getMultibodySystem().realize(state, SimTK::Stage::Dynamics); + + // Multibody states. + // ================= + if (state.getNQ() > 0) { // Are there multibody states? + const auto& yDotGuess = getYDotGuess(state); + SimTK::Vector& residuals = measure.updValue(state); + + // qdot - N u + // ---------- + // Get a view into YDotGuess[0:nq]. + const auto& qDotGuess = yDotGuess(0, state.getNQ()); + VectorView qResiduals = residuals(0, state.getNQ()); + qResiduals = state.getQDot() - qDotGuess; + // TODO do we want to use QDot here?? I think so, otherwise + // we are recomputing something that should already be cached for us. + // The alternative: + // TODO getMatterSubsystem().multiplybyN(state, state.getU(), qResiduals); + + // M u_dot - f + // ----------- + // Get a view into YDotGuess[nq:nq+nu]. + const auto& uDotGuess = yDotGuess(state.getNQ(), state.getNU()); + InverseDynamicsSolver idSolver(*this); + // TODO improve InverseDynamicsSolver to take a lambda. + VectorView uResiduals = residuals(state.getNQ(), state.getNU()); + // TODO is there an unnecessary copy here, and is it expensive? + uResiduals = idSolver.solve(state, uDotGuess); + } + + // Auxiliary states. + // ================= // TODO should put this in a separate "realizeImplicitResidual"? // TODO perhaps this is not the best way to invoke the actual calculation // of residuals. for (const auto& comp : getComponentList()) { comp.computeImplicitResiduals(state); } - implicitResidual.markAsValid(state); // TODO + + measure.markAsValid(state); // TODO } - return implicitResidual.getValue(state); + return measure.getValue(state); } SimTK::Vector& Model::updImplicitResiduals(const SimTK::State& state) diff --git a/OpenSim/Simulation/Model/Model.h b/OpenSim/Simulation/Model/Model.h index 2a983d71a1..ec2ee25913 100644 --- a/OpenSim/Simulation/Model/Model.h +++ b/OpenSim/Simulation/Model/Model.h @@ -878,6 +878,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Model, ModelComponent); // DERIVATIVES //-------------------------------------------------------------------------- // TODO rearrange according to public/private. + /** TODO + @ingroup implicitdiffeq + */ const SimTK::Vector& getImplicitResiduals(const SimTK::State& state) const override; const SimTK::Vector& getYDotGuess(const SimTK::State& state) const override; diff --git a/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp b/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp index c14b8e90dd..c0f215b0dd 100644 --- a/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp +++ b/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp @@ -226,15 +226,15 @@ void Coordinate::extendAddToSystem(SimTK::MultibodySystem& system) const addStateVariable(ssv); } -void Coordinate::extendRealizeInstance(const SimTK::State& state) const -{ - //const MobilizedBody& mb - // = getModel().getMatterSubsystem().getMobilizedBody(_bodyIndex); - - //int uix = state.getUStart() + mb.getFirstUIndex(state) + _mobilizerQIndex; - - /* Set the YIndex on the StateVariable */ -} +// void Coordinate::extendRealizeInstance(const SimTK::State& state) const +// { +// //const MobilizedBody& mb +// // = getModel().getMatterSubsystem().getMobilizedBody(_bodyIndex); +// +// //int uix = state.getUStart() + mb.getFirstUIndex(state) + _mobilizerQIndex; +// +// /* Set the YIndex on the StateVariable */ +// } void Coordinate::extendInitStateFromProperties(State& s) const { @@ -648,25 +648,77 @@ void Coordinate::CoordinateStateVariable:: double Coordinate::CoordinateStateVariable:: getImplicitResidual(const SimTK::State& state) const { - OPENSIM_THROW(Exception, "TODO"); + // TODO find another way to get the residual. + const Component* root = nullptr; + { + const Component* comp = &getOwner(); + while (comp->hasParent()) comp = &comp->getParent(); + root = comp; + } + + return root->getImplicitResiduals(state)[getSystemYIndex()]; + + // TODO OPENSIM_THROW(Exception, "TODO"); } void Coordinate::CoordinateStateVariable:: setImplicitResidual(const SimTK::State& state, double residual) const { - OPENSIM_THROW(Exception, "TODO"); + OPENSIM_THROW(Exception, "Residual is computed by the Model."); } double Coordinate::CoordinateStateVariable:: getDerivativeGuess(const SimTK::State& state) const { - OPENSIM_THROW(Exception, "TODO"); + // TODO could this method be implemented in the base StateVariable class? + + // TODO find another way to get the guess. + const Component* root = nullptr; + { + const Component* comp = &getOwner(); + while (comp->hasParent()) comp = &comp->getParent(); + root = comp; + } + + return root->getYDotGuess(state)[getSystemYIndex()]; + + // OPENSIM_THROW(Exception, "TODO"); } void Coordinate::CoordinateStateVariable:: setDerivativeGuess(SimTK::State& state, double derivGuess) const { - OPENSIM_THROW(Exception, "TODO"); + // TODO find another way to get the guess. + const Component* root = nullptr; + { + const Component* comp = &getOwner(); + while (comp->hasParent()) comp = &comp->getParent(); + root = comp; + } + + // TODO very inefficient! + SimTK::Vector yDotGuess = root->getYDotGuess(state); + yDotGuess[getSystemYIndex()] = derivGuess; + root->setYDotGuess(state, yDotGuess); + + // OPENSIM_THROW(Exception, "TODO"); +} + +SimTK::SystemYIndex Coordinate::CoordinateStateVariable:: +implementDetermineSystemYIndex(const SimTK::State& s) const { + const Coordinate& owner = *((Coordinate *)&getOwner()); + const MobilizedBody& mb = owner.getModel().getMatterSubsystem() + .getMobilizedBody(owner.getBodyIndex()); + + const int systemYIdxOfFirstQ = int(s.getQStart()); + const int systemQIdxOfFirstSubsystemQ = s.getQStart(getSubsysIndex()); + const int qIdxOfFirstMobilizerQ = mb.getFirstQIndex(s); // local to subsys. + const int mobilizerQIdx = getVarIndex(); + + return SystemYIndex(systemYIdxOfFirstQ + + systemQIdxOfFirstSubsystemQ + + qIdxOfFirstMobilizerQ + + mobilizerQIdx); } //----------------------------------------------------------------------------- @@ -707,23 +759,70 @@ void Coordinate::SpeedStateVariable:: double Coordinate::SpeedStateVariable:: getImplicitResidual(const SimTK::State& state) const { - OPENSIM_THROW(Exception, "TODO"); + // TODO find another way to get the residual. + const Component* root = nullptr; + { + const Component* comp = &getOwner(); + while (comp->hasParent()) comp = &comp->getParent(); + root = comp; + } + + return root->getImplicitResiduals(state)[getSystemYIndex()]; } void Coordinate::SpeedStateVariable:: setImplicitResidual(const SimTK::State& state, double residual) const { - OPENSIM_THROW(Exception, "TODO"); + OPENSIM_THROW(Exception, + "Residual (inverse dynamics) is computed by the Model."); } double Coordinate::SpeedStateVariable:: getDerivativeGuess(const SimTK::State& state) const { - OPENSIM_THROW(Exception, "TODO"); + // TODO could this method be implemented in the base StateVariable class? + + // TODO find another way to get the guess. + const Component* root = nullptr; + { + const Component* comp = &getOwner(); + while (comp->hasParent()) comp = &comp->getParent(); + root = comp; + } + + return root->getYDotGuess(state)[getSystemYIndex()]; } void Coordinate::SpeedStateVariable:: setDerivativeGuess(SimTK::State& state, double derivGuess) const { - OPENSIM_THROW(Exception, "TODO"); + // TODO find another way to get the guess. + const Component* root = nullptr; + { + const Component* comp = &getOwner(); + while (comp->hasParent()) comp = &comp->getParent(); + root = comp; + } + + // TODO very inefficient! + SimTK::Vector yDotGuess = root->getYDotGuess(state); + yDotGuess[getSystemYIndex()] = derivGuess; + root->setYDotGuess(state, yDotGuess); +} + +SimTK::SystemYIndex Coordinate::SpeedStateVariable:: +implementDetermineSystemYIndex(const SimTK::State& s) const { + const Coordinate& owner = *((Coordinate *)&getOwner()); + const MobilizedBody& mb = owner.getModel().getMatterSubsystem() + .getMobilizedBody(owner.getBodyIndex()); + + const int systemYIdxOfFirstU = int(s.getUStart()); + const int systemUIdxOfFirstSubsystemU = s.getUStart(getSubsysIndex()); + const int uIdxOfFirstMobilizerU = mb.getFirstUIndex(s); // local to subsys. + const int mobilizerUIdx = getVarIndex(); + + return SystemYIndex(systemYIdxOfFirstU + + systemUIdxOfFirstSubsystemU + + uIdxOfFirstMobilizerU + + mobilizerUIdx); } diff --git a/OpenSim/Simulation/SimbodyEngine/Coordinate.h b/OpenSim/Simulation/SimbodyEngine/Coordinate.h index 2cc1d00582..f7574e8f5d 100644 --- a/OpenSim/Simulation/SimbodyEngine/Coordinate.h +++ b/OpenSim/Simulation/SimbodyEngine/Coordinate.h @@ -227,7 +227,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Coordinate, ModelComponent); //State structure is locked and now we can assign names to state variables //allocated by underlying components after modeling options have been //factored in. - void extendRealizeInstance(const SimTK::State& state) const override; + // TODO void extendRealizeInstance(const SimTK::State& state) const override; void extendInitStateFromProperties(SimTK::State& s) const override; void extendSetPropertiesFromState(const SimTK::State& state) override; @@ -241,14 +241,14 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Coordinate, ModelComponent); private: // Class for handling state variable added (allocated) by this Component class CoordinateStateVariable : public StateVariable { - public: + public: // Constructors /** Convenience constructor for defining a Component added state variable */ - explicit CoordinateStateVariable(const std::string& name, //state var name - const Component& owner, //owning component - SimTK::SubsystemIndex subSysIndex, - int index) : - StateVariable(name, owner, subSysIndex, index, false) {} + CoordinateStateVariable(const std::string& name, //state var name + const Component& owner, //owning component + SimTK::SubsystemIndex subSysIndex, + int index) : + StateVariable(name, owner, subSysIndex, index, true) {} //override StateVariable virtual methods double getValue(const SimTK::State& state) const override; @@ -262,18 +262,21 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Coordinate, ModelComponent); // TODO get/set guess. double getDerivativeGuess(const SimTK::State& state) const override; void setDerivativeGuess(SimTK::State& state, double residual) const override; + + SimTK::SystemYIndex implementDetermineSystemYIndex(const SimTK::State& s) + const override; }; // Class for handling state variable added (allocated) by this Component class SpeedStateVariable : public StateVariable { - public: + public: // Constructors /** Convenience constructor for defining a Component added state variable */ - explicit SpeedStateVariable(const std::string& name, //state var name - const Component& owner, //owning component - SimTK::SubsystemIndex subSysIndex, - int index) : - StateVariable(name, owner, subSysIndex, index, false) {} + SpeedStateVariable(const std::string& name, //state var name + const Component& owner, //owning component + SimTK::SubsystemIndex subSysIndex, + int index) : + StateVariable(name, owner, subSysIndex, index, true) {} //override StateVariable virtual methods double getValue(const SimTK::State& state) const override; @@ -287,6 +290,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Coordinate, ModelComponent); // TODO get/set guess. double getDerivativeGuess(const SimTK::State& state) const override; void setDerivativeGuess(SimTK::State& state, double residual) const override; + + SimTK::SystemYIndex implementDetermineSystemYIndex(const SimTK::State& s) + const override; }; // All coordinates (Simbody mobility) have associated constraints that diff --git a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp index 3fd484eba6..dd62c8ba47 100644 --- a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp +++ b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp @@ -32,6 +32,10 @@ // TODO model containing multiple components with implicit form. // TODO editing yDotGuess or lambdaGuess invalidates the residual cache. // TODO calculation of YIndex must be correct. +// TODO test implicit form with multibody system but no auxiliary dynamics, as +// well as only explicit auxiliary dynamics. +// TODO test given lambda. +// TODO debug by comparing directly to result of calcResidualForce(). // Implementation: // TODO Only create implicit cache/discrete vars if any components have an @@ -127,7 +131,7 @@ void testExplicitFormOfImplicitComponent() { initialValue * exp(coeff * finalTime), 1e-5); } -void testTODO() { +void testSingleCustomImplicitEquation() { Model model; model.setName("model"); auto* comp = new LinearDynamics(); comp->setName("foo"); @@ -138,6 +142,22 @@ void testTODO() { model.addComponent(comp); auto s = model.initSystem(); + // TODO now doing the realize internally to getImplicitResidual(); + // no need to do this check. + // If not realized to Velocity, can't call getQDot(), which is needed + // when computing the residuals. + //SimTK_TEST_MUST_THROW_EXC(comp->getImplicitResidual(s, "activ"), + // SimTK::Exception::StageTooLow); + //SimTK_TEST_MUST_THROW_EXC(model.getImplicitResiduals(s), + // SimTK::Exception::StageTooLow); + // + //// If not realized to Dynamics, get exception for trying to access residual. + //model.realizeVelocity(s); + //SimTK_TEST_MUST_THROW_EXC(comp->getImplicitResidual(s, "activ"), + // SimTK::Exception::ErrorCheck); + //SimTK_TEST_MUST_THROW_EXC(model.getImplicitResiduals(s), + // SimTK::Exception::ErrorCheck); + // Access residual from the component: model.realizeDynamics(s); // Must realize to dynamics to get residuals. // TODO set yGuess. @@ -151,11 +171,70 @@ void testTODO() { /* TODO */ // TODO set derivative guess in one sweep. + // TODO model.setYDotGuess(s, Vector(1, activDotGuess)); +} + +// Test implicit multibody dynamics (i.e., inverse dynamics) with a point +// mass that can move along the direction of gravity. +void testImplicitMultibodyDynamics1DOF() { + const double g = 9.81; + const double mass = 7.2; + const double u_i = 3.9; + + // Build model. + // ------------ + Model model; model.setName("ball"); + model.setGravity(Vec3(-g, 0, 0)); // gravity pulls in the -x direction. + auto* body = new OpenSim::Body("ptmass", mass, Vec3(0), Inertia(0)); + auto* slider = new SliderJoint(); slider->setName("slider"); + model.addBody(body); + model.addJoint(slider); + slider->updConnector("parent_frame").connect(model.getGround()); + slider->updConnector("child_frame").connect(*body); + + State s = model.initSystem(); + const auto& coord = slider->getCoordinateSet()[0]; + coord.setSpeedValue(s, u_i); + + const double qDotGuess = 5.6; const double uDotGuess = 8.3; + + // Check implicit form. + // -------------------- + // Set derivative guess. + coord.setStateVariableDerivativeGuess(s, "value", qDotGuess); + coord.setStateVariableDerivativeGuess(s, "speed", uDotGuess); + + // Get residual. + Vector expectedResiduals(2); + expectedResiduals[0] = u_i - qDotGuess; + expectedResiduals[1] = mass * uDotGuess - mass * (-g); // M u_dot-f_applied + model.realizeDynamics(s); + // Check individual elements of the residual. + SimTK_TEST_EQ(coord.getImplicitResidual(s, "value"), expectedResiduals[0]); + SimTK_TEST_EQ(coord.getImplicitResidual(s, "speed"), expectedResiduals[1]); + // Check the entire residuals vector. + SimTK_TEST_EQ(model.getImplicitResiduals(s), expectedResiduals); + + // TODO set YDotGuess all at once. + + + // Check explicit form. + // -------------------- + model.realizeAcceleration(s); + Vector expectedYDot(2); + expectedYDot[0] /* = qdot*/ = u_i; expectedYDot[1] /* = udot*/ = -g; + SimTK_TEST_EQ(s.getYDot(), expectedYDot); } int main() { SimTK_START_TEST("testImplicitDifferentialEquatiosns"); SimTK_SUBTEST(testExplicitFormOfImplicitComponent); - SimTK_SUBTEST(testTODO); + SimTK_SUBTEST(testSingleCustomImplicitEquation); + SimTK_SUBTEST(testImplicitMultibodyDynamics1DOF); SimTK_END_TEST(); -} \ No newline at end of file +} + + + + + From 82cabca01fa549127af65be833794bc9911f7576 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Thu, 21 Jul 2016 19:51:15 -0700 Subject: [PATCH 20/40] Test class ImplicitSystemDerivativeSolver(). --- .../testImplicitDifferentialEquations.cpp | 88 ++++++++++++++++++- 1 file changed, 86 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp index dd62c8ba47..eebee048e4 100644 --- a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp +++ b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp @@ -222,15 +222,99 @@ void testImplicitMultibodyDynamics1DOF() { // -------------------- model.realizeAcceleration(s); Vector expectedYDot(2); - expectedYDot[0] /* = qdot*/ = u_i; expectedYDot[1] /* = udot*/ = -g; + expectedYDot[0] = u_i /* = qdot*/; expectedYDot[1] = -g /* = udot*/; SimTK_TEST_EQ(s.getYDot(), expectedYDot); } +// ============================================================================= +// ImplicitSystemDerivativeSolver +// ============================================================================= +// TODO clarify: just solving system of nonlinear equations; no cost function. +// Given a state y and constraints f(y, ydot) = 0, solve for ydot. +class ImplicitSystemDerivativeSolver { +public: + class Problem; // Defined below. + ImplicitSystemDerivativeSolver(const Model& model); + // Solve for the derivatives of the system, ydot. + Vector solve(const State& s); +private: + Model m_model; + State m_state; + std::unique_ptr m_problem; + std::unique_ptr m_opt; +}; +class ImplicitSystemDerivativeSolver::Problem : public SimTK::OptimizerSystem { +public: + Problem(const ImplicitSystemDerivativeSolver& parent) : m_parent(parent) { + } + int objectiveFunc(const Vector& yDotGuess, bool newParams, + Real& f) const override { + f = 0; return 0; + } + int constraintFunc(const Vector& yDotGuess, bool newParams, + Vector& constraints) const override { + // Must create a copy of the state, since this is a const method. + State workingState = m_parent.m_state; // TODO expensive? + m_parent.m_model.setYDotGuess(workingState, yDotGuess); + constraints = m_parent.m_model.getImplicitResiduals(workingState); + // TODO lambdas will be NaN? residuals will be bad + // what to do with lambdas for a system without constraints? + return 0; + } +private: + const ImplicitSystemDerivativeSolver& m_parent; +}; +ImplicitSystemDerivativeSolver::ImplicitSystemDerivativeSolver( + const Model& model) : m_model(model), m_problem(new Problem(*this)) { + // Set up Problem. + m_state = m_model.initSystem(); + m_problem->setNumParameters(m_state.getNY()); + m_problem->setNumEqualityConstraints(m_state.getNY()); + Vector limits(m_state.getNY(), 10.0); // TODO arbitrary. + m_problem->setParameterLimits(-limits, limits); + + // Set up Optimizer. + m_opt.reset(new Optimizer(*m_problem)); + m_opt->useNumericalGradient(true); m_opt->useNumericalJacobian(true); +} +Vector ImplicitSystemDerivativeSolver::solve(const State& s) { + m_state = s; + Vector results(m_problem->getNumParameters(), 0.0); // TODO inefficient + m_opt->optimize(results); + return results; +} +// end ImplicitSystemDerivativeSolver........................................... + +void testGenericInterfaceForImplicitSolver() /*TODO rename test */ { + Model model; + auto* comp = new LinearDynamics(); + comp->setName("foo"); + const Real initialValue = 3.5; + comp->set_default_activ(initialValue); + const Real coeff = -0.28; + comp->set_coeff(coeff); + model.addComponent(comp); + State s = model.initSystem(); + + // Computing yDot using implicit form. + ImplicitSystemDerivativeSolver solver(model); + Vector yDotImplicit = solver.solve(s); + + // Computing yDot using explicit form. + model.realizeAcceleration(s); + const Vector& yDotExplicit = s.getYDot(); + + // Implicit and explicit forms give same yDot. + SimTK_TEST_EQ_TOL(yDotImplicit, yDotExplicit, 1e-11); +} + int main() { - SimTK_START_TEST("testImplicitDifferentialEquatiosns"); + SimTK_START_TEST("testImplicitDifferentialEquations"); SimTK_SUBTEST(testExplicitFormOfImplicitComponent); SimTK_SUBTEST(testSingleCustomImplicitEquation); SimTK_SUBTEST(testImplicitMultibodyDynamics1DOF); + // TODO SimTK_SUBTEST(testMultibody1DOFAndCustomComponent); + SimTK_SUBTEST(testGenericInterfaceForImplicitSolver); SimTK_END_TEST(); } From b1384774cf41e088b92042746ecff7df4e3390a6 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Fri, 22 Jul 2016 15:09:20 -0700 Subject: [PATCH 21/40] Change stages to cause proper invalidation of residual. --- OpenSim/Simulation/Model/Model.cpp | 19 ++- .../testImplicitDifferentialEquations.cpp | 136 +++++++++++------- 2 files changed, 100 insertions(+), 55 deletions(-) diff --git a/OpenSim/Simulation/Model/Model.cpp b/OpenSim/Simulation/Model/Model.cpp index 4057caed82..f11d8ae58c 100644 --- a/OpenSim/Simulation/Model/Model.cpp +++ b/OpenSim/Simulation/Model/Model.cpp @@ -783,19 +783,23 @@ void Model::extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) nans.lockShape(); // TODO where is the right place for this? // This Measure is basically a discrete state variable. - // Acceleration cache is invalid if this is updated. + // Acceleration cache is invalid if this is updated, since acceleration- + // dependent quantities should be computed with uDotGuess. + // Dynamics stage must also be invalidated, since residuals cache var + // depends on Dynamics stage. // TODO what should default value be? what size should it have? Measure_::Variable yDotGuess(_system->updDefaultSubsystem(), - Stage::Acceleration, nans); + Stage::Dynamics, nans); _yDotGuessIndex = yDotGuess.getSubsystemMeasureIndex(); Measure_::Variable lambdaGuess(_system->updDefaultSubsystem(), - Stage::Acceleration, nans); + Stage::Dynamics, nans); _lambdaGuessIndex = lambdaGuess.getSubsystemMeasureIndex(); // Storing the residual. // We can only compute the residual once realized to Dynamics, since - // we will need to apply forces. + // we will need to apply forces. Residual does not depend on accelerations. + // TODO this would change if realizeAcceleration() simply uses uDotGuess. // None of the acceleration-level calculations depend on the residual // (nothing should depend on the residual) so there is nothing to // invalidate when the residual is changed (invalidated = Infinity). @@ -833,7 +837,8 @@ const SimTK::Vector& Model::getImplicitResiduals(const SimTK::State& state) cons qResiduals = state.getQDot() - qDotGuess; // TODO do we want to use QDot here?? I think so, otherwise // we are recomputing something that should already be cached for us. - // The alternative: + // The alternatives: + // TODO getMatterSubsystem().calcQDot(...) // TODO getMatterSubsystem().multiplybyN(state, state.getU(), qResiduals); // M u_dot - f @@ -889,6 +894,10 @@ void Model::setYDotGuess(SimTK::State& state, OPENSIM_THROW_IF_FRMOBJ(!_system || !_yDotGuessIndex.isValid(), Exception, "Prior call to Model::initSystem() is required."); + SimTK_ASSERT2_ALWAYS(yDotGuess.size()==state.getNY(), + "Expected size of yDotGuess to be the number of state variables, %i, " + "but it was %i.", state.getNY(), yDotGuess.size()); + auto measure = Measure_::Variable::getAs( _system->getDefaultSubsystem().getMeasure(_yDotGuessIndex)); diff --git a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp index eebee048e4..6a0fcddb8f 100644 --- a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp +++ b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp @@ -36,10 +36,20 @@ // well as only explicit auxiliary dynamics. // TODO test given lambda. // TODO debug by comparing directly to result of calcResidualForce(). +// TODO test coupled system (auxiliary dynamics depends on q, u depends on +// auxiliary dynamics). +// TODO trying to set a yDotGuess or Lambda that is of the wrong size. +// smss.getRep().calcConstraintAccelerationErrors // Implementation: // TODO Only create implicit cache/discrete vars if any components have an // implicit form (place in extendAddToSystemAfterComponents()). +// TODO if we use the operator form (not storing residual etc in the state), +// then we have to be clear to implementors of the residual equation that they +// CANNOT depend on qdot, udot, zdot or ydot vectors in the state; that will +// invoke forward dynamics! + +// TODO sketch out solver-like interface (using IPOPT). #include @@ -132,46 +142,55 @@ void testExplicitFormOfImplicitComponent() { } void testSingleCustomImplicitEquation() { - Model model; model.setName("model"); - auto* comp = new LinearDynamics(); - comp->setName("foo"); + auto createModel = [](double initialValue, double coeff) -> Model { + Model model; model.setName("model"); + auto* comp = new LinearDynamics(); + comp->setName("foo"); + comp->set_default_activ(initialValue); + comp->set_coeff(coeff); + model.addComponent(comp); + return model; + }; + const Real initialValue = 3.5; - comp->set_default_activ(initialValue); const Real coeff = -0.28; - comp->set_coeff(coeff); - model.addComponent(comp); - auto s = model.initSystem(); - - // TODO now doing the realize internally to getImplicitResidual(); - // no need to do this check. - // If not realized to Velocity, can't call getQDot(), which is needed - // when computing the residuals. - //SimTK_TEST_MUST_THROW_EXC(comp->getImplicitResidual(s, "activ"), - // SimTK::Exception::StageTooLow); - //SimTK_TEST_MUST_THROW_EXC(model.getImplicitResiduals(s), - // SimTK::Exception::StageTooLow); - // - //// If not realized to Dynamics, get exception for trying to access residual. - //model.realizeVelocity(s); - //SimTK_TEST_MUST_THROW_EXC(comp->getImplicitResidual(s, "activ"), - // SimTK::Exception::ErrorCheck); - //SimTK_TEST_MUST_THROW_EXC(model.getImplicitResiduals(s), - // SimTK::Exception::ErrorCheck); - - // Access residual from the component: - model.realizeDynamics(s); // Must realize to dynamics to get residuals. - // TODO set yGuess. const Real activDotGuess = 5.3; - comp->setStateVariableDerivativeGuess(s, "activ", activDotGuess); - double expectedResidual = coeff * initialValue - activDotGuess; - SimTK_TEST(comp->getImplicitResidual(s, "activ") == expectedResidual); + const Real expectedResidual = coeff * initialValue - activDotGuess; + + { // Setting elements of guess by name. + Model model = createModel(initialValue, coeff); + State s = model.initSystem(); + const auto& comp = model.getComponent("foo"); + + // Set guess. + comp.setStateVariableDerivativeGuess(s, "activ", activDotGuess); + + // Access residual by name from the component: + SimTK_TEST(comp.getImplicitResidual(s, "activ") == expectedResidual); + + // Access residual from the entire residual vector. + SimTK_TEST_EQ(model.getImplicitResiduals(s), Vector(1, expectedResidual)); + } - // Make sure the residual is accessible from the entire residual vector. - SimTK_TEST_EQ(model.getImplicitResiduals(s), Vector(1, expectedResidual)); - /* TODO - */ - // TODO set derivative guess in one sweep. - // TODO model.setYDotGuess(s, Vector(1, activDotGuess)); + { // Setting entire guess vector at once. + Model model = createModel(initialValue, coeff); + State s = model.initSystem(); + const auto& comp = model.getComponent("foo"); + + // Set guess. + model.setYDotGuess(s, Vector(1, activDotGuess)); + + // Access residual by name from the component. + SimTK_TEST(comp.getImplicitResidual(s, "activ") == expectedResidual); + + // Access residual from the entire residual vector. + SimTK_TEST_EQ(model.getImplicitResiduals(s), Vector(1, expectedResidual)); + + // Editing guesses causes residual to be recalculated. + model.setYDotGuess(s, Vector(1, NaN)); // different guess. + SimTK_TEST_NOTEQ(model.getImplicitResiduals(s), + Vector(1, expectedResidual)); + } } // Test implicit multibody dynamics (i.e., inverse dynamics) with a point @@ -179,7 +198,7 @@ void testSingleCustomImplicitEquation() { void testImplicitMultibodyDynamics1DOF() { const double g = 9.81; const double mass = 7.2; - const double u_i = 3.9; + const double u_i = 3.9; // Build model. // ------------ @@ -208,7 +227,6 @@ void testImplicitMultibodyDynamics1DOF() { Vector expectedResiduals(2); expectedResiduals[0] = u_i - qDotGuess; expectedResiduals[1] = mass * uDotGuess - mass * (-g); // M u_dot-f_applied - model.realizeDynamics(s); // Check individual elements of the residual. SimTK_TEST_EQ(coord.getImplicitResidual(s, "value"), expectedResiduals[0]); SimTK_TEST_EQ(coord.getImplicitResidual(s, "speed"), expectedResiduals[1]); @@ -224,6 +242,18 @@ void testImplicitMultibodyDynamics1DOF() { Vector expectedYDot(2); expectedYDot[0] = u_i /* = qdot*/; expectedYDot[1] = -g /* = udot*/; SimTK_TEST_EQ(s.getYDot(), expectedYDot); + + // Error-checking. + // --------------- + // Size of yDotGuess must be correct. + SimTK_TEST_MUST_THROW_EXC(model.setYDotGuess(s, Vector(1, 0.)), // too small. + SimTK::Exception::Base); + SimTK_TEST_MUST_THROW_EXC(model.setYDotGuess(s, Vector(3, 0.)), // too large. + SimTK::Exception::Base); + + // TODO test size of residuals. + + // TODO test size of lambdaGuess. } // ============================================================================= @@ -239,38 +269,37 @@ class ImplicitSystemDerivativeSolver { Vector solve(const State& s); private: Model m_model; - State m_state; std::unique_ptr m_problem; std::unique_ptr m_opt; }; class ImplicitSystemDerivativeSolver::Problem : public SimTK::OptimizerSystem { public: - Problem(const ImplicitSystemDerivativeSolver& parent) : m_parent(parent) { - } + Problem(const ImplicitSystemDerivativeSolver& parent): m_parent(parent) {} + void setWorkingState(const State& s) { m_workingState = s; } int objectiveFunc(const Vector& yDotGuess, bool newParams, Real& f) const override { f = 0; return 0; } int constraintFunc(const Vector& yDotGuess, bool newParams, Vector& constraints) const override { - // Must create a copy of the state, since this is a const method. - State workingState = m_parent.m_state; // TODO expensive? - m_parent.m_model.setYDotGuess(workingState, yDotGuess); - constraints = m_parent.m_model.getImplicitResiduals(workingState); + m_parent.m_model.setYDotGuess(m_workingState, yDotGuess); + constraints = m_parent.m_model.getImplicitResiduals(m_workingState); // TODO lambdas will be NaN? residuals will be bad // what to do with lambdas for a system without constraints? return 0; } private: const ImplicitSystemDerivativeSolver& m_parent; + // Mutable since we edit the yDotGuess discrete state var in constraintFunc. + mutable State m_workingState; }; ImplicitSystemDerivativeSolver::ImplicitSystemDerivativeSolver( const Model& model) : m_model(model), m_problem(new Problem(*this)) { // Set up Problem. - m_state = m_model.initSystem(); - m_problem->setNumParameters(m_state.getNY()); - m_problem->setNumEqualityConstraints(m_state.getNY()); - Vector limits(m_state.getNY(), 10.0); // TODO arbitrary. + State state = m_model.initSystem(); + m_problem->setNumParameters(state.getNY()); + m_problem->setNumEqualityConstraints(state.getNY()); + Vector limits(state.getNY(), 10.0); // TODO arbitrary. m_problem->setParameterLimits(-limits, limits); // Set up Optimizer. @@ -278,9 +307,10 @@ ImplicitSystemDerivativeSolver::ImplicitSystemDerivativeSolver( m_opt->useNumericalGradient(true); m_opt->useNumericalJacobian(true); } Vector ImplicitSystemDerivativeSolver::solve(const State& s) { - m_state = s; + m_problem->setWorkingState(s); Vector results(m_problem->getNumParameters(), 0.0); // TODO inefficient m_opt->optimize(results); + m_opt->setDiagnosticsLevel(1); return results; } // end ImplicitSystemDerivativeSolver........................................... @@ -308,6 +338,11 @@ void testGenericInterfaceForImplicitSolver() /*TODO rename test */ { SimTK_TEST_EQ_TOL(yDotImplicit, yDotExplicit, 1e-11); } +void testErrorsForUnsupportedModels() { + // TODO constraints? does not require error message. + // TODO prescribed motion. +} + int main() { SimTK_START_TEST("testImplicitDifferentialEquations"); SimTK_SUBTEST(testExplicitFormOfImplicitComponent); @@ -315,6 +350,7 @@ int main() { SimTK_SUBTEST(testImplicitMultibodyDynamics1DOF); // TODO SimTK_SUBTEST(testMultibody1DOFAndCustomComponent); SimTK_SUBTEST(testGenericInterfaceForImplicitSolver); + SimTK_SUBTEST(testErrorsForUnsupportedModels); SimTK_END_TEST(); } From 7b2955efe7090c885bd6a021ec5ca81d17db6355 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Fri, 22 Jul 2016 15:29:08 -0700 Subject: [PATCH 22/40] Ensure residuals vector can't change size. --- OpenSim/Simulation/Model/Model.cpp | 7 +------ .../Simulation/Test/testImplicitDifferentialEquations.cpp | 8 +++++--- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/OpenSim/Simulation/Model/Model.cpp b/OpenSim/Simulation/Model/Model.cpp index f11d8ae58c..493aae5d1d 100644 --- a/OpenSim/Simulation/Model/Model.cpp +++ b/OpenSim/Simulation/Model/Model.cpp @@ -873,8 +873,8 @@ SimTK::Vector& Model::updImplicitResiduals(const SimTK::State& state) Exception, "Prior call to Model::initSystem() is required."); auto implicitResidual = Measure_::Result::getAs( _system->getDefaultSubsystem().getMeasure(_implicitResidualIndex)); + implicitResidual.updValue(state).lockShape(); // TODO relocate. return implicitResidual.updValue(state); - // TODO OPENSIM_THROW_FRMOBJ(Exception, "TODO"); } const SimTK::Vector& Model::getYDotGuess(const SimTK::State& state) const { @@ -885,8 +885,6 @@ const SimTK::Vector& Model::getYDotGuess(const SimTK::State& state) const { _system->getDefaultSubsystem().getMeasure(_yDotGuessIndex)); return yDotGuess.getValue(state); - - // TODO OPENSIM_THROW_FRMOBJ(Exception, "TODO"); } void Model::setYDotGuess(SimTK::State& state, @@ -901,10 +899,7 @@ void Model::setYDotGuess(SimTK::State& state, auto measure = Measure_::Variable::getAs( _system->getDefaultSubsystem().getMeasure(_yDotGuessIndex)); - // TODO ensure yDotGuess has the correct length. return measure.setValue(state, yDotGuess); - - // TODO OPENSIM_THROW_FRMOBJ(Exception, "TODO"); } //_____________________________________________________________________________ /** diff --git a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp index 6a0fcddb8f..31f2cc78da 100644 --- a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp +++ b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp @@ -247,11 +247,13 @@ void testImplicitMultibodyDynamics1DOF() { // --------------- // Size of yDotGuess must be correct. SimTK_TEST_MUST_THROW_EXC(model.setYDotGuess(s, Vector(1, 0.)), // too small. - SimTK::Exception::Base); + SimTK::Exception::Assert); SimTK_TEST_MUST_THROW_EXC(model.setYDotGuess(s, Vector(3, 0.)), // too large. - SimTK::Exception::Base); + SimTK::Exception::Assert); - // TODO test size of residuals. + // Size of residuals must be correct. + SimTK_TEST_MUST_THROW_EXC(model.updImplicitResiduals(s) = Vector(3, 0.), + SimTK::Exception::Cant); // TODO test size of lambdaGuess. } From 03da0b41d13387484b75f829a8ac96005f148741 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Mon, 25 Jul 2016 18:15:06 -0700 Subject: [PATCH 23/40] Handle constraints along with implicit form. This commit depends on a change in Simbody that is only on my computer! --- OpenSim/Simulation/InverseDynamicsSolver.cpp | 21 +- OpenSim/Simulation/InverseDynamicsSolver.h | 15 +- OpenSim/Simulation/Model/Model.cpp | 59 +++++- OpenSim/Simulation/Model/Model.h | 10 + .../testImplicitDifferentialEquations.cpp | 186 ++++++++++++++++-- 5 files changed, 260 insertions(+), 31 deletions(-) diff --git a/OpenSim/Simulation/InverseDynamicsSolver.cpp b/OpenSim/Simulation/InverseDynamicsSolver.cpp index f70c4c931d..c316c18312 100644 --- a/OpenSim/Simulation/InverseDynamicsSolver.cpp +++ b/OpenSim/Simulation/InverseDynamicsSolver.cpp @@ -43,7 +43,9 @@ InverseDynamicsSolver::InverseDynamicsSolver(const Model &model) : Solver(model) /** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau. Applied loads are computed by the model from the state. */ -Vector InverseDynamicsSolver::solve(const SimTK::State &s, const SimTK::Vector &udot) +Vector InverseDynamicsSolver::solve(const SimTK::State &s, + const SimTK::Vector &udot, + const SimTK::Vector& lambda) { // Default is a statics inverse dynamics analysis, udot = 0; Vector knownUdots(getModel().getNumSpeeds(), 0.0); @@ -66,7 +68,7 @@ Vector InverseDynamicsSolver::solve(const SimTK::State &s, const SimTK::Vector & const Vector_& appliedBodyForces = getModel().getMultibodySystem().getRigidBodyForces(s, Stage::Dynamics); // Perform general inverse dynamics - return solve(s, knownUdots, appliedMobilityForces, appliedBodyForces); + return solve(s, knownUdots, appliedMobilityForces, appliedBodyForces, lambda); } @@ -74,7 +76,9 @@ Vector InverseDynamicsSolver::solve(const SimTK::State &s, const SimTK::Vector & Applied loads are explicitly provided as generalized coordinate forces (MobilityForces) and/or a Vector of Spatial-body forces */ Vector InverseDynamicsSolver::solve(const SimTK::State &s, const SimTK::Vector &udot, - const SimTK::Vector &appliedMobilityForces, const SimTK::Vector_& appliedBodyForces) + const SimTK::Vector& appliedMobilityForces, + const SimTK::Vector_& appliedBodyForces, + const SimTK::Vector& lambda) { //Results of the inverse dynamics for the generalized forces to satisfy accelerations Vector residualMobilityForces; @@ -82,9 +86,16 @@ Vector InverseDynamicsSolver::solve(const SimTK::State &s, const SimTK::Vector & if(s.getSystemStage() < SimTK::Stage::Dynamics) getModel().getMultibodySystem().realize(s,SimTK::Stage::Dynamics); + // If valid Lagrange multipliers provided then use for inverse dynamics + OPENSIM_THROW_IF(lambda.size() != 0 && lambda.size() != s.getNMultipliers(), + Exception, "lambda has " + std::to_string(lambda.size()) + + " elements but must have 0 or " + + std::to_string(s.getNMultipliers()) + " element(s)."); + // Perform inverse dynamics - getModel().getMultibodySystem().getMatterSubsystem().calcResidualForceIgnoringConstraints(s, - appliedMobilityForces, appliedBodyForces, udot, residualMobilityForces); + getModel().getMultibodySystem().getMatterSubsystem().calcResidualForce(s, + appliedMobilityForces, appliedBodyForces, udot, lambda, + residualMobilityForces); return residualMobilityForces; } diff --git a/OpenSim/Simulation/InverseDynamicsSolver.h b/OpenSim/Simulation/InverseDynamicsSolver.h index 70a202d08c..19f9ebc31e 100644 --- a/OpenSim/Simulation/InverseDynamicsSolver.h +++ b/OpenSim/Simulation/InverseDynamicsSolver.h @@ -73,16 +73,25 @@ OpenSim_DECLARE_CONCRETE_OBJECT(InverseDynamicsSolver, Solver); according to the state. @param[in] s the system state specifying time, coordinates and speeds @param[in] udot the vector of generalized accelerations in the order + @param[in] lambda TODO */ virtual SimTK::Vector solve(const SimTK::State& s, - const SimTK::Vector& udot = SimTK::Vector(0)); + const SimTK::Vector& udot = SimTK::Vector(0), + const SimTK::Vector& lambda = SimTK::Vector(0)); + + // TODO if you call calcResidualForce with lambda.size() == 0, then + // it'll call the "ignoringConstraints" version. So we could do that here + // too! /** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau. Applied loads are explicitly provided as generalized coordinate forces (MobilityForces) - and/or a Vector of Spatial-body forces */ + and/or a Vector of Spatial-body forces + TODO lambda if lambda is empty, then it'll call calcResidualForceIgnoringConstraints(). + */ virtual SimTK::Vector solve(const SimTK::State& s, const SimTK::Vector& udot, const SimTK::Vector& appliedMobilityForces, - const SimTK::Vector_& appliedBodyForces); + const SimTK::Vector_& appliedBodyForces, + const SimTK::Vector& lambda); /** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau. Now the state is updated from known coordinates, q, as diff --git a/OpenSim/Simulation/Model/Model.cpp b/OpenSim/Simulation/Model/Model.cpp index 493aae5d1d..f6c23550c5 100644 --- a/OpenSim/Simulation/Model/Model.cpp +++ b/OpenSim/Simulation/Model/Model.cpp @@ -792,8 +792,9 @@ void Model::extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) Stage::Dynamics, nans); _yDotGuessIndex = yDotGuess.getSubsystemMeasureIndex(); + // TODO how to set size of lambdaGuess? shouldn't be numStateVars. Measure_::Variable lambdaGuess(_system->updDefaultSubsystem(), - Stage::Dynamics, nans); + Stage::Dynamics, Vector(0)); _lambdaGuessIndex = lambdaGuess.getSubsystemMeasureIndex(); // Storing the residual. @@ -827,6 +828,7 @@ const SimTK::Vector& Model::getImplicitResiduals(const SimTK::State& state) cons // ================= if (state.getNQ() > 0) { // Are there multibody states? const auto& yDotGuess = getYDotGuess(state); + const auto& lambdaGuess = getMultipliersGuess(state); SimTK::Vector& residuals = measure.updValue(state); // qdot - N u @@ -849,7 +851,7 @@ const SimTK::Vector& Model::getImplicitResiduals(const SimTK::State& state) cons // TODO improve InverseDynamicsSolver to take a lambda. VectorView uResiduals = residuals(state.getNQ(), state.getNU()); // TODO is there an unnecessary copy here, and is it expensive? - uResiduals = idSolver.solve(state, uDotGuess); + uResiduals = idSolver.solve(state, uDotGuess, lambdaGuess); } // Auxiliary states. @@ -867,8 +869,7 @@ const SimTK::Vector& Model::getImplicitResiduals(const SimTK::State& state) cons return measure.getValue(state); } -SimTK::Vector& Model::updImplicitResiduals(const SimTK::State& state) - const { +SimTK::Vector& Model::updImplicitResiduals(const SimTK::State& state) const { OPENSIM_THROW_IF_FRMOBJ(!_system || !_implicitResidualIndex.isValid(), Exception, "Prior call to Model::initSystem() is required."); auto implicitResidual = Measure_::Result::getAs( @@ -899,8 +900,56 @@ void Model::setYDotGuess(SimTK::State& state, auto measure = Measure_::Variable::getAs( _system->getDefaultSubsystem().getMeasure(_yDotGuessIndex)); - return measure.setValue(state, yDotGuess); + measure.setValue(state, yDotGuess); +} + +const SimTK::Vector& Model::getMultipliersGuess(const SimTK::State& state) const +{ + OPENSIM_THROW_IF_FRMOBJ(!_system || !_lambdaGuessIndex.isValid(), + Exception, "Prior call to Model::initSystem() is required."); + + auto lambdaGuess = Measure_::Variable::getAs( + _system->getDefaultSubsystem().getMeasure(_lambdaGuessIndex)); + + return lambdaGuess.getValue(state); +} + +void Model::setMultipliersGuess(SimTK::State& state, + const SimTK::Vector& lambdaGuess) const { + OPENSIM_THROW_IF_FRMOBJ(!_system || !_lambdaGuessIndex.isValid(), + Exception, "Prior call to Model::initSystem() is required."); + + SimTK_ASSERT2_ALWAYS(lambdaGuess.size()==state.getNMultipliers(), + "Expected size of lambdaGuess to be the number of multipliers, %i, " + "but it was %i.", state.getNMultipliers(), lambdaGuess.size()); + + auto measure = Measure_::Variable::getAs( + _system->getDefaultSubsystem().getMeasure(_lambdaGuessIndex)); + + measure.setValue(state, lambdaGuess); +} + +void Model::calcImplicitResidualsAndConstraintErrors(SimTK::State& state, + const SimTK::Vector& yDotGuess, const SimTK::Vector& lambdaGuess, + SimTK::Vector& residuals, SimTK::Vector& constraintAErrs) const { + + setYDotGuess(state, yDotGuess); + setMultipliersGuess(state, lambdaGuess); + residuals = getImplicitResiduals(state); + + const auto& matter = getMatterSubsystem(); + + // TODO best place to put this constraint-error code? + // TODO constraintAErrs.setToNaN(); + const auto& uDotGuess = yDotGuess(state.getNQ(), state.getNU()); + Vector_ A_GB; + matter.calcBodyAccelerationFromUDot(state, uDotGuess, A_GB); + Vector qDotDotGuess; + matter.calcQDotDot(state, uDotGuess, qDotDotGuess); + matter.calcConstraintAccelerationErrors(state, + A_GB, uDotGuess, qDotDotGuess, constraintAErrs); } + //_____________________________________________________________________________ /** * Add any Component derived from ModelComponent to the Model. diff --git a/OpenSim/Simulation/Model/Model.h b/OpenSim/Simulation/Model/Model.h index ec2ee25913..27f2111c61 100644 --- a/OpenSim/Simulation/Model/Model.h +++ b/OpenSim/Simulation/Model/Model.h @@ -887,6 +887,16 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Model, ModelComponent); SimTK::Vector& updImplicitResiduals(const SimTK::State& state) const override; void setYDotGuess(SimTK::State& state, const SimTK::Vector& yDotGuess) const override; + const SimTK::Vector& getMultipliersGuess(const SimTK::State& state) const; + void setMultipliersGuess(SimTK::State& state, + const SimTK::Vector& lambdaGuess) const; + /** TODO for this implementation, it sets the discrete state variables. + */ + void calcImplicitResidualsAndConstraintErrors(SimTK::State& state, + const SimTK::Vector& yDotGuess, const SimTK::Vector& lambdaGuess, + SimTK::Vector& residuals, SimTK::Vector& constraintAErrs) const; + + //-------------------------------------------------------------------------- // OPERATIONS diff --git a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp index 31f2cc78da..39e83d2f91 100644 --- a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp +++ b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp @@ -40,6 +40,16 @@ // auxiliary dynamics). // TODO trying to set a yDotGuess or Lambda that is of the wrong size. // smss.getRep().calcConstraintAccelerationErrors +// TODO ensure residuals are NaN if lambdas are not set to 0, unless the system +// does not have any constraints. +// TODO test calcImplicitResidualsAndConstraintErrors(); make sure constraintErrs +// is empty if there are no constraints. Make sure lambdaGuess is the +// correct size. +// TODO test prescribedmotion, model with non-implicit dynamics. + +// TODO mock up the situation where we are minimizing joint contact load +// while obeying dynamics (in direct collocation fashion--the entire +// trajectory). // Implementation: // TODO Only create implicit cache/discrete vars if any components have an @@ -48,10 +58,12 @@ // then we have to be clear to implementors of the residual equation that they // CANNOT depend on qdot, udot, zdot or ydot vectors in the state; that will // invoke forward dynamics! +// TODO handle quaternion constraints. // TODO sketch out solver-like interface (using IPOPT). +#include #include using namespace OpenSim; @@ -202,7 +214,7 @@ void testImplicitMultibodyDynamics1DOF() { // Build model. // ------------ - Model model; model.setName("ball"); + Model model; model.setName("model"); model.setGravity(Vec3(-g, 0, 0)); // gravity pulls in the -x direction. auto* body = new OpenSim::Body("ptmass", mass, Vec3(0), Inertia(0)); auto* slider = new SliderJoint(); slider->setName("slider"); @@ -267,8 +279,9 @@ class ImplicitSystemDerivativeSolver { public: class Problem; // Defined below. ImplicitSystemDerivativeSolver(const Model& model); - // Solve for the derivatives of the system, ydot. - Vector solve(const State& s); + // Solve for the derivatives of the system, ydot, and the Lagrange + // multipliers, lambda. + void solve(const State& s, Vector& yDot, Vector& lambda); private: Model m_model; std::unique_ptr m_problem; @@ -278,14 +291,22 @@ class ImplicitSystemDerivativeSolver::Problem : public SimTK::OptimizerSystem { public: Problem(const ImplicitSystemDerivativeSolver& parent): m_parent(parent) {} void setWorkingState(const State& s) { m_workingState = s; } - int objectiveFunc(const Vector& yDotGuess, bool newParams, + int objectiveFunc(const Vector& guess, bool newParams, Real& f) const override { f = 0; return 0; } - int constraintFunc(const Vector& yDotGuess, bool newParams, + int constraintFunc(const Vector& guess, bool newParams, Vector& constraints) const override { - m_parent.m_model.setYDotGuess(m_workingState, yDotGuess); - constraints = m_parent.m_model.getImplicitResiduals(m_workingState); + const int ny = m_workingState.getNY(); + Vector yDotGuess; yDotGuess.viewAssign(guess(0, ny)); + Vector lambdaGuess; lambdaGuess.viewAssign(guess(ny, guess.size() - ny)); + + Vector residuals; residuals.viewAssign(constraints(0, ny)); + Vector pvaerrs; pvaerrs.viewAssign(constraints(ny, guess.size() - ny)); + + m_parent.m_model.calcImplicitResidualsAndConstraintErrors(m_workingState, + yDotGuess, lambdaGuess, // inputs + residuals, pvaerrs); // outputs // TODO lambdas will be NaN? residuals will be bad // what to do with lambdas for a system without constraints? return 0; @@ -299,45 +320,172 @@ ImplicitSystemDerivativeSolver::ImplicitSystemDerivativeSolver( const Model& model) : m_model(model), m_problem(new Problem(*this)) { // Set up Problem. State state = m_model.initSystem(); - m_problem->setNumParameters(state.getNY()); - m_problem->setNumEqualityConstraints(state.getNY()); - Vector limits(state.getNY(), 10.0); // TODO arbitrary. + const int N = state.getNY() + state.getNMultipliers(); + m_problem->setNumParameters(N); + m_problem->setNumEqualityConstraints(N); + Vector limits(N, 50.0); // TODO arbitrary. m_problem->setParameterLimits(-limits, limits); // Set up Optimizer. - m_opt.reset(new Optimizer(*m_problem)); + m_opt.reset(new Optimizer(*m_problem, SimTK::InteriorPoint)); m_opt->useNumericalGradient(true); m_opt->useNumericalJacobian(true); } -Vector ImplicitSystemDerivativeSolver::solve(const State& s) { +void ImplicitSystemDerivativeSolver::solve(const State& s, + Vector& yDot, Vector& lambda) { m_problem->setWorkingState(s); Vector results(m_problem->getNumParameters(), 0.0); // TODO inefficient m_opt->optimize(results); m_opt->setDiagnosticsLevel(1); - return results; + yDot = results(0, s.getNY()); + lambda = results(s.getNY(), results.size() - s.getNY()); } // end ImplicitSystemDerivativeSolver........................................... +// TODO explain purpose of this test. void testGenericInterfaceForImplicitSolver() /*TODO rename test */ { Model model; auto* comp = new LinearDynamics(); comp->setName("foo"); const Real initialValue = 3.5; comp->set_default_activ(initialValue); - const Real coeff = -0.28; + const Real coeff = -0.21; comp->set_coeff(coeff); model.addComponent(comp); State s = model.initSystem(); // Computing yDot using implicit form. - ImplicitSystemDerivativeSolver solver(model); - Vector yDotImplicit = solver.solve(s); + Vector yDotImplicit; + { + State sImplicit = s; + ImplicitSystemDerivativeSolver solver(model); + Vector lambda; + solver.solve(sImplicit, yDotImplicit, lambda); + // There should be no multipliers. + SimTK_TEST(lambda.size() == 0); + } // Computing yDot using explicit form. - model.realizeAcceleration(s); - const Vector& yDotExplicit = s.getYDot(); + Vector yDotExplicit; + { + State sExplicit = s; + model.realizeAcceleration(sExplicit); + yDotExplicit = sExplicit.getYDot(); + } // Implicit and explicit forms give same yDot. SimTK_TEST_EQ_TOL(yDotImplicit, yDotExplicit, 1e-11); + // Also make sure the test is actually testing something. + SimTK_TEST(yDotExplicit.size() == 1); + SimTK_TEST_NOTEQ(yDotExplicit.norm(), 0); +} + +void testImplicitSystemDerivativeSolverMultibody1DOF() { + Model model; model.setName("model"); + model.setGravity(Vec3(-9.81, 0, 0)); // gravity pulls in the -x direction. + auto* body = new OpenSim::Body("ptmass", 1.3, Vec3(0), Inertia(0)); + auto* slider = new SliderJoint(); slider->setName("slider"); + model.addBody(body); + model.addJoint(slider); + slider->updConnector("parent_frame").connect(model.getGround()); + slider->updConnector("child_frame").connect(*body); + + State s = model.initSystem(); + const auto& coord = slider->getCoordinateSet()[0]; + coord.setSpeedValue(s, 1.7); + + // Computing yDot using implicit form. + Vector yDotImplicit; + { + State sImplicit = s; + ImplicitSystemDerivativeSolver solver(model); + Vector lambda; // unused. + solver.solve(sImplicit, yDotImplicit, lambda); + } + + // Computing yDot using explicit form. + Vector yDotExplicit; + { + State sExplicit = s; + model.realizeAcceleration(sExplicit); + yDotExplicit = sExplicit.getYDot(); + } + + // Implicit and explicit forms give same yDot. + SimTK_TEST_EQ_TOL(yDotImplicit, yDotExplicit, 1e-8); + + // Also make sure the test is actually testing something. + SimTK_TEST(yDotExplicit.size() == 2); + SimTK_TEST_NOTEQ(yDotExplicit.norm(), 0); +} + +void testCoordinateCouplerConstraint() { + Model model; model.setName("twodof"); + auto* body = new OpenSim::Body("ptmass", 1.5, Vec3(0.7, 0, 0), + Inertia::ellipsoid(1, 2, 3)); + // TODO BallJoint() causes issues. + auto* joint = new GimbalJoint(); joint->setName("joint"); + auto& c0 = joint->upd_CoordinateSet()[0]; c0.setName("c0"); + auto& c1 = joint->upd_CoordinateSet()[1]; c1.setName("c1"); + auto& c2 = joint->upd_CoordinateSet()[2]; c2.setName("c2"); + model.addBody(body); + model.addJoint(joint); + joint->updConnector("parent_frame").connect(model.getGround()); + joint->updConnector("child_frame").connect(*body); + + // Set up constraint using a linear relation between c0 and c1. + auto* coupler = new CoordinateCouplerConstraint(); coupler->setName("cplr"); + model.addConstraint(coupler); + Array indepCoords; indepCoords.append("c0"); + coupler->setIndependentCoordinateNames(indepCoords); + coupler->setDependentCoordinateName("c1"); + const Real slope = 5.1; const Real intercept = 2.31; + coupler->setFunction(LinearFunction(slope, intercept)); + + State s = model.initSystem(); + c0.setValue(s, 0.51, false); // We'll enforce constraints all at once. + c2.setValue(s, 0.36, false); + c0.setSpeedValue(s, 1.5); // The projection will change this value. + c2.setSpeedValue(s, 2.8); + model.assemble(s); + model.getSystem().projectU(s); // Enforce velocity constraints. + + // Ensure that the constraints are obeyed in the current state. + SimTK_TEST_EQ(c1.getValue(s), slope * c0.getValue(s) + intercept); + // Check the velocity-level constraint: + SimTK_TEST_EQ(c1.getSpeedValue(s), slope * c0.getSpeedValue(s)); + + // Computing yDot and lambda using implicit form. + Vector yDotImplicit, lambdaImplicit; + { + State sImplicit = s; + ImplicitSystemDerivativeSolver solver(model); + solver.solve(sImplicit, yDotImplicit, lambdaImplicit); + + // Acceleration-level constraint equation is satisfied. + SimTK_TEST_EQ_TOL(yDotImplicit[1], slope * yDotImplicit[0], 1e-12); + } + + // Computing yDot and lambda using explicit form. + Vector yDotExplicit, lambdaExplicit; + { + State sExplicit = s; + model.realizeAcceleration(sExplicit); + yDotExplicit = sExplicit.getYDot(); + lambdaExplicit = sExplicit.getMultipliers(); + + // Acceleration-level constraint equation is satisfied. + SimTK_TEST_EQ_TOL(c1.getAccelerationValue(sExplicit), + slope * c0.getAccelerationValue(sExplicit), 1e-9); + } + + + // Implicit and explicit forms give same yDot and lambda. + SimTK_TEST_EQ_TOL(yDotImplicit, yDotExplicit, 1e-9); + SimTK_TEST_EQ_TOL(lambdaImplicit, lambdaExplicit, 1e-9); + + // Also make sure the test is actually testing something. + SimTK_TEST(yDotExplicit.size() == 6); // 3 DOFs, 2 states per DOF. + SimTK_TEST(lambdaExplicit.size() == 1); } void testErrorsForUnsupportedModels() { @@ -352,6 +500,8 @@ int main() { SimTK_SUBTEST(testImplicitMultibodyDynamics1DOF); // TODO SimTK_SUBTEST(testMultibody1DOFAndCustomComponent); SimTK_SUBTEST(testGenericInterfaceForImplicitSolver); + SimTK_SUBTEST(testImplicitSystemDerivativeSolverMultibody1DOF); + SimTK_SUBTEST(testCoordinateCouplerConstraint); SimTK_SUBTEST(testErrorsForUnsupportedModels); SimTK_END_TEST(); } From c65d49fff05ef43be6bacdbc345af305cd1ba785 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Mon, 25 Jul 2016 22:20:11 -0700 Subject: [PATCH 24/40] hasFullImplicitForm() --- OpenSim/Common/Component.cpp | 52 ++++++ OpenSim/Common/Component.h | 9 +- OpenSim/Simulation/Model/Model.cpp | 2 + .../testImplicitDifferentialEquations.cpp | 172 ++++++++++++++++-- 4 files changed, 217 insertions(+), 18 deletions(-) diff --git a/OpenSim/Common/Component.cpp b/OpenSim/Common/Component.cpp index 5435dff9de..678ad2bcbe 100644 --- a/OpenSim/Common/Component.cpp +++ b/OpenSim/Common/Component.cpp @@ -427,8 +427,58 @@ void Component::computeImplicitResiduals(const SimTK::State& s) const // computing the residual will still require computing the explicit // multibody dynamics. // OPENSIM_THROW_FRMOBJ(Exception, "TODO"); + bool requireExplicitForm = false; + for (const auto& it : _namedStateVariableInfo) { + const StateVariable& sv = *it.second.stateVariable; + if (!sv.hasImplicitForm()) requireExplicitForm = true; break; + } + // TODO + if (!hasFullImplicitForm()) computeStateVariableDerivatives(s); + + // TODO it seems that if you want implicit form, you should define all state + // variables implicitly. Otherwise, we will need to evaluate the explicit + // dynamics, which will waste time. TODO what about going up the inheritance + // hierarchy? + + extendComputeImplicitResiduals(s); + + + // TODO do this before or after extendCompute...()? + for (const auto& it : _namedStateVariableInfo) { + const StateVariable& sv = *it.second.stateVariable; + if (!sv.hasImplicitForm()) { + const Real yDot = sv.getDerivative(s); + const Real yDotGuess = sv.getDerivativeGuess(s); + sv.setImplicitResidual(s, yDot - yDotGuess); + } + // TODO set implicit residual for explicit components. + // TODO ensure that the user set the residual for those providing one. + } } +bool Component::hasFullImplicitFormThisComponent() const { + // TODO compute this once in baseAddToSystem(). + for (const auto& it : _namedStateVariableInfo) { + const StateVariable& sv = *it.second.stateVariable; + if (!sv.hasImplicitForm()) return false; + } + return true; +} + +bool Component::hasFullImplicitForm() const { + if (!hasFullImplicitFormThisComponent()) return false; + + for (unsigned int i = 0; i<_memberSubcomponents.size(); i++) + if (!_memberSubcomponents[i]->hasFullImplicitForm()) return false; + + for(unsigned int i=0; i<_propertySubcomponents.size(); i++) + if (!_propertySubcomponents[i]->hasFullImplicitForm()) return false; + + for (unsigned int i = 0; i<_adoptedSubcomponents.size(); i++) + if (!_adoptedSubcomponents[i]->hasFullImplicitForm()) return false; + + return true; +} void Component:: addModelingOption(const std::string& optionName, int maxFlagValue) const @@ -1547,6 +1597,8 @@ double Component::AddedStateVariable:: void Component::AddedStateVariable:: setImplicitResidual(const SimTK::State& state, double residual) const { + // TODO be clever about whether hasImplicitResidual() is set. + // TODO find another way to get the residual. const Component* root = nullptr; { diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index 5fdbf9bd1e..159980a235 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -1084,6 +1084,11 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); OPENSIM_THROW_FRMOBJ(Exception, "Can only call on a root component (e.g., Model)."); } + + /** TODO */ + bool hasFullImplicitFormThisComponent() const; + /** TODO */ + bool hasFullImplicitForm() const; protected: // TODO move this around. virtual SimTK::Vector& updImplicitResiduals(const SimTK::State& state) const { @@ -1096,6 +1101,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); OPENSIM_THROW_FRMOBJ(Exception, "Can only call on a root component (e.g., Model)."); } + public: /** @@ -1665,7 +1671,8 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); const std::string& name, double deriv) const; /** TODO */ - virtual void computeImplicitResiduals(const SimTK::State& s) const; + void computeImplicitResiduals(const SimTK::State& s) const; + virtual void extendComputeImplicitResiduals(const SimTK::State& s) const {} /** TODO */ void setImplicitResidual(const SimTK::State& state, diff --git a/OpenSim/Simulation/Model/Model.cpp b/OpenSim/Simulation/Model/Model.cpp index f6c23550c5..02063c45ca 100644 --- a/OpenSim/Simulation/Model/Model.cpp +++ b/OpenSim/Simulation/Model/Model.cpp @@ -824,6 +824,8 @@ const SimTK::Vector& Model::getImplicitResiduals(const SimTK::State& state) cons if (!measure.isValid(state)) { getMultibodySystem().realize(state, SimTK::Stage::Dynamics); + // TODO do all of this in extendComputeImplicitResiduals(). + // Multibody states. // ================= if (state.getNQ() > 0) { // Are there multibody states? diff --git a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp index 39e83d2f91..9fe9d67cc1 100644 --- a/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp +++ b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp @@ -46,6 +46,10 @@ // is empty if there are no constraints. Make sure lambdaGuess is the // correct size. // TODO test prescribedmotion, model with non-implicit dynamics. +// TODO put multibody residuals calculation in extendComputeImplicitResiduals(). +// TODO test what happens when you provide an implicit from but don't set hasImplicitForm. +// TODO test what happens when you set hasImplicitForm() but don't provide it. +// TODO test inheritance hierarchy: multiple subclasses add state vars. // TODO mock up the situation where we are minimizing joint contact load // while obeying dynamics (in direct collocation fashion--the entire @@ -69,32 +73,33 @@ using namespace OpenSim; using namespace SimTK; -/** TODO */ -class LinearDynamics : public Component { -OpenSim_DECLARE_CONCRETE_OBJECT(LinearDynamics, Component); +/** TODO change this to have a different explicit vs implicit form. +TODO otherwise we can't tell if we are using the default implicit form or +the provided one. */ +class LinearDynamicsExplicitImplicit : public Component { +OpenSim_DECLARE_CONCRETE_OBJECT(LinearDynamicsExplicitImplicit, Component); public: OpenSim_DECLARE_PROPERTY(default_activ, double, "Default value of state variable."); OpenSim_DECLARE_PROPERTY(coeff, double, "Coefficient in the differential equation."); - OpenSim_DECLARE_OUTPUT_FOR_STATE_VARIABLE(activ); - LinearDynamics() { + LinearDynamicsExplicitImplicit() { constructProperty_default_activ(0.0); constructProperty_coeff(-1.0); } protected: void extendAddToSystem(SimTK::MultibodySystem& system) const override { Super::extendAddToSystem(system); - addStateVariable("activ" /* , true has implicit form */); + addStateVariable("activ", SimTK::Stage::Dynamics, true + /* TODO , true has implicit form */); } void computeStateVariableDerivatives(const SimTK::State& s) const override { // TODO invokes false error msg. Super::computeStateVariableDerivatives(s); const Real& activ = getStateVariableValue(s, "activ"); setStateVariableDerivativeValue(s, "activ", get_coeff() * activ); } - void computeImplicitResiduals(const SimTK::State& s) - const override { - // TODO Super::computeStateVariableDerivatives(s); + void extendComputeImplicitResiduals(const SimTK::State& s) const override { + Super::extendComputeImplicitResiduals(s); const Real& activ = getStateVariableValue(s, "activ"); double activDotGuess = getStateVariableDerivativeGuess(s, "activ"); setImplicitResidual(s, "activ", get_coeff() * activ - activDotGuess); @@ -109,6 +114,69 @@ OpenSim_DECLARE_CONCRETE_OBJECT(LinearDynamics, Component); } }; +// TODO +class QuadraticDynamicsExplicitOnly : public Component { +OpenSim_DECLARE_CONCRETE_OBJECT(QuadraticDynamicsExplicitOnly, Component); +public: + OpenSim_DECLARE_PROPERTY(default_length, double, + "Default value of state variable."); + OpenSim_DECLARE_PROPERTY(coeff, double, + "Coefficient in the differential equation."); + QuadraticDynamicsExplicitOnly() { + constructProperty_default_length(0.0); + constructProperty_coeff(-1.0); + } +protected: + void extendAddToSystem(SimTK::MultibodySystem& system) const override { + Super::extendAddToSystem(system); + addStateVariable("length", SimTK::Stage::Dynamics, false); + } + void computeStateVariableDerivatives(const SimTK::State& s) const override { + const Real& activ = getStateVariableValue(s, "length"); + setStateVariableDerivativeValue(s, "length", get_coeff() * activ * activ); + } + void extendInitStateFromProperties(SimTK::State& s) const override { + Super::extendInitStateFromProperties(s); + setStateVariableValue(s, "length", get_default_length()); + } + void extendSetPropertiesFromState(const SimTK::State& s) override { + Super::extendSetPropertiesFromState(s); + set_default_length(getStateVariableValue(s, "length")); + } +}; + +/** // TODO +class MixedExplicitImplicitDynamics : public Component { +OpenSim_DECLARE_CONCRETE_OBJECT(MixedExplicitImplicitDynamics, Component); +public: + OpenSim_DECLARE_PROPERTY(default_length, double, + "Default value of state variable."); + OpenSim_DECLARE_PROPERTY(coeff, double, + "Coefficient in the differential equation."); + MixedExplicitImplicitDynamics() { + constructProperty_default_length(0.0); + constructProperty_coeff(-1.0); + } +protected: + void extendAddToSystem(SimTK::MultibodySystem& system) const override { + Super::extendAddToSystem(system); + addStateVariable("length"); + } + void computeStateVariableDerivatives(const SimTK::State& s) const override { + const Real& activ = getStateVariableValue(s, "length"); + setStateVariableDerivativeValue(s, "length", get_coeff() * activ * activ); + } + void extendInitStateFromProperties(SimTK::State& s) const override { + Super::extendInitStateFromProperties(s); + setStateVariableValue(s, "length", get_default_length()); + } + void extendSetPropertiesFromState(const SimTK::State& s) override { + Super::extendSetPropertiesFromState(s); + set_default_length(getStateVariableValue(s, "length")); + } +}; +*/ + /// Integrates the given system and returns the final state via the `state` /// argument. void simulate(const Model& model, State& state, Real finalTime) { @@ -125,7 +193,7 @@ void simulate(const Model& model, State& state, Real finalTime) { void testExplicitFormOfImplicitComponent() { // Create model. Model model; model.setName("model"); - auto* comp = new LinearDynamics(); + auto* comp = new LinearDynamicsExplicitImplicit(); comp->setName("foo"); const Real initialValue = 3.5; comp->set_default_activ(initialValue); @@ -133,13 +201,14 @@ void testExplicitFormOfImplicitComponent() { comp->set_coeff(coeff); model.addComponent(comp); - // TODO auto* rep = new ConsoleReporter(); - // TODO rep->set_report_time_interval(0.01); - // TODO model.addComponent(rep); - // TODO rep->updInput("inputs").connect(comp->getOutput("activ")); - auto s = model.initSystem(); + // Check boolean indicators. + SimTK_TEST(comp->hasFullImplicitFormThisComponent()); + SimTK_TEST(comp->hasFullImplicitForm()); + SimTK_TEST(model.hasFullImplicitFormThisComponent()); + SimTK_TEST(model.hasFullImplicitForm()); + // Check initial values. SimTK_TEST(comp->getStateVariableValue(s, "activ") == initialValue); model.realizeAcceleration(s); @@ -156,7 +225,7 @@ void testExplicitFormOfImplicitComponent() { void testSingleCustomImplicitEquation() { auto createModel = [](double initialValue, double coeff) -> Model { Model model; model.setName("model"); - auto* comp = new LinearDynamics(); + auto* comp = new LinearDynamicsExplicitImplicit(); comp->setName("foo"); comp->set_default_activ(initialValue); comp->set_coeff(coeff); @@ -224,7 +293,12 @@ void testImplicitMultibodyDynamics1DOF() { slider->updConnector("child_frame").connect(*body); State s = model.initSystem(); + const auto& coord = slider->getCoordinateSet()[0]; + + SimTK_TEST(coord.hasFullImplicitFormThisComponent()); + SimTK_TEST(model.hasFullImplicitForm()); + coord.setSpeedValue(s, u_i); const double qDotGuess = 5.6; const double uDotGuess = 8.3; @@ -270,6 +344,68 @@ void testImplicitMultibodyDynamics1DOF() { // TODO test size of lambdaGuess. } +// When components provide dynamics in explicit form only, we compute the +// implicit form by using the explicit form. +void testImplicitFormOfExplicitOnlyComponent() { + auto createModel = [](double initialValue, double coeff) -> Model { + Model model; model.setName("model"); + auto* comp = new QuadraticDynamicsExplicitOnly(); + comp->setName("foo"); + comp->set_default_length(initialValue); + comp->set_coeff(coeff); + model.addComponent(comp); + return model; + }; + + const Real initialValue = 1.8; + const Real coeff = -0.73; + const Real lengthDotGuess = -4.5; + // The default implicit residual. + const Real expectedResidual = coeff * pow(initialValue, 2) - lengthDotGuess; + + { // Setting elements of guess by name. + // TODO + Model model = createModel(initialValue, coeff); + State s = model.initSystem(); + const auto& comp = model.getComponent("foo"); + + // Check boolean indicators. + SimTK_TEST(!comp.hasFullImplicitFormThisComponent()); + SimTK_TEST(!comp.hasFullImplicitForm()); + SimTK_TEST(model.hasFullImplicitFormThisComponent()); + SimTK_TEST(!model.hasFullImplicitForm()); + + // Set guess. + comp.setStateVariableDerivativeGuess(s, "length", lengthDotGuess); + + // Access residual by name from the component. + SimTK_TEST(comp.getImplicitResidual(s, "length") == expectedResidual); + + // Access residual from the entire residual vector. + SimTK_TEST_EQ(model.getImplicitResiduals(s), Vector(1, expectedResidual)); + } + + { // Setting entire guess vector at once. + Model model = createModel(initialValue, coeff); + State s = model.initSystem(); + const auto& comp = model.getComponent("foo"); + + // Set guess. + model.setYDotGuess(s, Vector(1, lengthDotGuess)); + + // Access residual by name from the component. + SimTK_TEST(comp.getImplicitResidual(s, "length") == expectedResidual); + + // Access residual from the entire residual vector. + SimTK_TEST_EQ(model.getImplicitResiduals(s), Vector(1, expectedResidual)); + + // Editing guesses causes residual to be recalculated. + model.setYDotGuess(s, Vector(1, NaN)); + SimTK_TEST_NOTEQ(model.getImplicitResiduals(s), + Vector(1, expectedResidual)); + } +} + // ============================================================================= // ImplicitSystemDerivativeSolver // ============================================================================= @@ -344,7 +480,7 @@ void ImplicitSystemDerivativeSolver::solve(const State& s, // TODO explain purpose of this test. void testGenericInterfaceForImplicitSolver() /*TODO rename test */ { Model model; - auto* comp = new LinearDynamics(); + auto* comp = new LinearDynamicsExplicitImplicit(); comp->setName("foo"); const Real initialValue = 3.5; comp->set_default_activ(initialValue); @@ -498,6 +634,8 @@ int main() { SimTK_SUBTEST(testExplicitFormOfImplicitComponent); SimTK_SUBTEST(testSingleCustomImplicitEquation); SimTK_SUBTEST(testImplicitMultibodyDynamics1DOF); + SimTK_SUBTEST(testImplicitFormOfExplicitOnlyComponent); + // TODO SimTK_SUBTEST(testImplicitFormOfCombinedImplicitAndExplicitComponents); // TODO SimTK_SUBTEST(testMultibody1DOFAndCustomComponent); SimTK_SUBTEST(testGenericInterfaceForImplicitSolver); SimTK_SUBTEST(testImplicitSystemDerivativeSolverMultibody1DOF); From 2f292031c33e48cbe3e7cdde68262336af0019e2 Mon Sep 17 00:00:00 2001 From: humphreysb Date: Tue, 26 Jul 2016 15:44:35 -0700 Subject: [PATCH 25/40] Adding Fwd Test (MATLAB) version for reference --- .idea/workspace.xml | 302 +++++------ Bindings/simbody.i | 4 - OpenSim/Actuators/VandenBogert2011Muscle.cpp | 21 +- .../runForwardImplicitTest.m | 480 ++++++++++++++++++ .../unFlattenMat22.m | 11 + 5 files changed, 619 insertions(+), 199 deletions(-) create mode 100644 OpenSim/Sandbox/HumphreysImplicitDynamics/runForwardImplicitTest.m create mode 100644 OpenSim/Sandbox/HumphreysImplicitDynamics/unFlattenMat22.m diff --git a/.idea/workspace.xml b/.idea/workspace.xml index 12851e25cb..684b2d941c 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -140,27 +140,9 @@ - - - - - - - - - - - - - - - - - - @@ -180,118 +162,80 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -363,27 +308,13 @@