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..9ab2fbf671 --- /dev/null +++ b/Bindings/Java/Matlab/tests/testVandenBogert2011Muscle.m @@ -0,0 +1,27 @@ + +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, 0); +% out should have 3 elements: +%assert(out.size() == 3); +%disp(out.get(0)) +%disp(out.get(1)) +%disp(out.get(2)) +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/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..c8b84d4b63 100644 --- a/Bindings/actuators.i +++ b/Bindings/actuators.i @@ -22,3 +22,7 @@ %include %include %include + +%feature("flatnested") OpenSim::VandenBogert2011Muscle::ImplicitResidual; +%rename(VandenBogert2011MuscleImplicitResidual) OpenSim::VandenBogert2011Muscle::ImplicitResidual; +%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/Bindings/simbody.i b/Bindings/simbody.i index e4efee1d9e..ba926dc6e4 100644 --- a/Bindings/simbody.i +++ b/Bindings/simbody.i @@ -20,6 +20,7 @@ namespace SimTK { %include %include // for typedefs like Mat33. namespace SimTK { +%template(Mat22) Mat<2, 2>; %template(Mat33) Mat<3, 3>; } %include diff --git a/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp b/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp index 5e2202adc8..812bdc38c6 100644 --- a/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp +++ b/OpenSim/Actuators/Millard2012EquilibriumMuscle.cpp @@ -201,6 +201,7 @@ double aTendonSlackLength, double aPennationAngle) setName(aName); setMaxIsometricForce(aMaxIsometricForce); + setMaxIsometricForce(aMaxIsometricForce); setOptimalFiberLength(aOptimalFiberLength); setTendonSlackLength(aTendonSlackLength); setPennationAngleAtOptimalFiberLength(aPennationAngle); 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 new file mode 100644 index 0000000000..a740f28c60 --- /dev/null +++ b/OpenSim/Actuators/VandenBogert2011Muscle.cpp @@ -0,0 +1,1312 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: VandenBogert.cpp * + * -------------------------------------------------------------------------- */ + + +//============================================================================= +// INCLUDES +//============================================================================= +#include "VandenBogert2011Muscle.h" +#include + + +//============================================================================= +// STATICS +//============================================================================= +using namespace std; +using namespace OpenSim; + + + +//============================================================================= +// CONSTRUCTOR(S) AND DESTRUCTOR +//============================================================================= +//_____________________________________________________________________________ +/* + * Default constructor + */ +VandenBogert2011Muscle::VandenBogert2011Muscle() +{ + constructProperties(); + finalizeFromProperties(); +} +//_____________________________________________________________________________ +/* + * Constructor. + */ +VandenBogert2011Muscle::VandenBogert2011Muscle + (const std::string& name, double maxIsometricForce, + double optimalFiberLength,double tendonSlackLength, + double pennationAngle) +{ ; + constructProperties(); + setName(name); + setMaxIsometricForce(maxIsometricForce); + setOptimalFiberLength(optimalFiberLength); + setTendonSlackLength(tendonSlackLength); + setPennationAngleAtOptimalFiberLength(pennationAngle); + finalizeFromProperties(); +} + + + +//_____________________________________________________________________________ +/* + * 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("A. van den Bogert, B. Humphreys, C. Dembia"); + setReferences("van den Bogert A, Blana D, Heinrich D, Implicit methods for efficient musculoskeletal simulation and optimal control. Procedia IUTAM 2011; 2:297-316"); + + constructProperty_tendon_strain_at_max_iso_force(0.033); + constructProperty_active_force_length_curve_width(0.63); + constructProperty_force_velocity_hill_constant(0.25); + constructProperty_force_velocity_max_lengthening_force_norm(1.5); + constructProperty_fiber_damping(0.01); + constructProperty_fiber_slack_length_norm(1.0); + constructProperty_activation_time_constant(0.01); + constructProperty_deactivation_time_constant(0.04); + constructProperty_pennation_at_optimal_fiber_length(0); + constructProperty_default_activation(0.1); + constructProperty_default_fiber_length(1.0); + + // These come from muscle.h + //constructProperty_max_isometric_force(1000); + //constructProperty_optimal_fiber_length(0.1); + //constructProperty_tendon_slack_length(0.2); + //constructProperty_max_contraction_velocity(10*optimal_fiber_length); +} + + + + +void VandenBogert2011Muscle::extendFinalizeFromProperties() +{ + Super::extendFinalizeFromProperties(); + +} + +//-------------------------------------------------------------------------- +// GET & SET Properties +//-------------------------------------------------------------------------- +void VandenBogert2011Muscle::setFMaxTendonStrain(double fMaxTendonStrain) { + set_tendon_strain_at_max_iso_force(fMaxTendonStrain); } +double VandenBogert2011Muscle::getFMaxTendonStrain() const { + return get_tendon_strain_at_max_iso_force(); } + +void VandenBogert2011Muscle::setFlWidth(double flWidth) { + set_active_force_length_curve_width(flWidth); } +double VandenBogert2011Muscle::getFlWidth() const + { return get_active_force_length_curve_width(); } + +void VandenBogert2011Muscle::setFvAHill(double fvAHill) { + set_force_velocity_hill_constant(fvAHill); } +double VandenBogert2011Muscle::getFvAHill() const { + return get_force_velocity_hill_constant(); } + +void VandenBogert2011Muscle::setFvmaxMultiplier(double fvMaxMultiplier) { + set_force_velocity_max_lengthening_force_norm(fvMaxMultiplier); } +double VandenBogert2011Muscle::getFvmaxMultiplier() const { + return get_force_velocity_max_lengthening_force_norm(); } + +void VandenBogert2011Muscle::setDampingCoefficient(double dampingCoefficient) { + set_fiber_damping(dampingCoefficient); } +double VandenBogert2011Muscle::getDampingCoefficient() const { + return get_fiber_damping(); } + +void VandenBogert2011Muscle::setNormFiberSlackLength(double + normFiberSlackLength) { + set_fiber_slack_length_norm(normFiberSlackLength); } +double VandenBogert2011Muscle::getNormFiberSlackLength() const { + return get_fiber_slack_length_norm(); } + +void VandenBogert2011Muscle::setActivTimeConstant(double activTimeConstant) { + set_activation_time_constant(activTimeConstant); } +double VandenBogert2011Muscle::getActivTimeConstant() const { + return get_activation_time_constant(); } + +void VandenBogert2011Muscle::setDeactivationTimeConstant(double + deactivationTimeConstant) { + set_deactivation_time_constant(deactivationTimeConstant); } +double VandenBogert2011Muscle::getDeactivationTimeConstant() const { + return get_deactivation_time_constant(); } + +void VandenBogert2011Muscle::setPennAtOptFiberLength(double + pennAtOptFiberLength) { + set_pennation_at_optimal_fiber_length(pennAtOptFiberLength); } +double VandenBogert2011Muscle::getPennAtOptFiberLength() const { + return get_pennation_at_optimal_fiber_length(); } + + +void VandenBogert2011Muscle::setDefaultActivation(double + defaultActivation) { + set_default_activation(defaultActivation); } +double VandenBogert2011Muscle::getDefaultActivation() const { + return get_default_activation(); } + +void VandenBogert2011Muscle::setDefaultFiberLength(double fiberLength) +{ + set_default_fiber_length(fiberLength); +} + +double VandenBogert2011Muscle::getDefaultFiberLength() const +{ return get_default_fiber_length(); } + + +void VandenBogert2011Muscle::setProjFiberLengthNorm(SimTK::State& s, double projFibLenNorm) + const +{ + //In other muscles this is setFiberLength + setStateVariableValue(s, "projected_fiber_length_normalized",projFibLenNorm); + markCacheVariableInvalid(s,"lengthInfo"); + markCacheVariableInvalid(s,"velInfo"); + markCacheVariableInvalid(s,"dynamicsInfo"); + } + + +// Set the activation +void VandenBogert2011Muscle::setActivation(SimTK::State& s,double activation) +const +//TODO: Add clamping? Add ignore_activation_dynamics +{ + setStateVariableValue(s, "activation", activation); + + //markCacheVariableInvalid(s,"velInfo"); + //markCacheVariableInvalid(s,"dynamicsInfo"); +} + +// Get the Residual of muscle +// TODO: This breaks naming convention in that it does not get an already +// evaluated constant, it actually performs the calculation. Without using a +// state variable or adding to the cache variables, I think I have to do it this +// way? +SimTK::Vec2 VandenBogert2011Muscle::getResidual(const SimTK::State& s, + double projFibVelNorm_guess, + double activdot_guess, + double excitation) + const +{ + ImplicitResidual residualStruct = calcImplicitResidual(s, + projFibVelNorm_guess, + activdot_guess, + excitation, 0); + + SimTK::Vec2 residual = {residualStruct.forceResidual, + residualStruct.activResidual}; + return(residual); +} + + + +//These are hacks because I am not using muscle.h mli cache variables, yet: +double VandenBogert2011Muscle::getProjFiberLengthNorm(const SimTK::State& s) const +{ + return getStateVariableValue(s, "projected_fiber_length_normalized"); +} +double VandenBogert2011Muscle::getActivation(const SimTK::State& s) const +{ + return getStateVariableValue(s, "activation"); +} + + + +//============================================================================== +// Muscle.h Interface +//============================================================================== + + +double VandenBogert2011Muscle::computeActuation(const SimTK::State& s) const +{ + const MuscleDynamicsInfo& mdi = getMuscleDynamicsInfo(s); + setActuation(s, mdi.tendonForce); + return mdi.tendonForce; +} + + + +void VandenBogert2011Muscle::computeInitialFiberEquilibrium(SimTK::State& s) +const +{ + // TODO: this is the same code as computeFiberEquilibriumAtZeroVelocity + //Need to update for v~=0 + //Let's start with the default activation and the fiberVelocity=0 + + computeFiberEquilibriumAtZeroVelocity(s); + +} + + + +void VandenBogert2011Muscle:: +computeFiberEquilibriumAtZeroVelocity(SimTK::State& s) const +{ + + double activation = getActivation(s); + double projFibLenNorm = getProjFiberLengthNorm(s); + SimTK::Vec2 lenghAndForce = calcFiberStaticEquilbirum(projFibLenNorm, + activation); + + setActuation(s, lenghAndForce[1]); + setProjFiberLengthNorm(s,lenghAndForce[0]); + + //TODO: Millard Muscle handles non-convergence here and zero muscle length. + // Need to consider adding. + +} + + +void VandenBogert2011Muscle:: +postScale(const SimTK::State& s, const ScaleSet& aScaleSet) +{ + //TODO: I have not really thought through if there are special cases for this muscle + GeometryPath& path = upd_GeometryPath(); + path.postScale(s, aScaleSet); + + if (path.getPreScaleLength(s) > 0.0) { + double scaleFactor = getLength(s) / path.getPreScaleLength(s); + upd_optimal_fiber_length() *= scaleFactor; + upd_tendon_slack_length() *= scaleFactor; + path.setPreScaleLength(s, 0.0); + } +} + +//============================================================================== +// MODELCOMPONENT INTERFACE REQUIREMENTS +//============================================================================== + + +void VandenBogert2011Muscle::extendConnectToModel(Model& model) +{ + Super::extendConnectToModel(model); +} + + +// Define new states and their derivatives in the underlying system +void VandenBogert2011Muscle::extendAddToSystem(SimTK::MultibodySystem& system) +const +{ + + Super::extendAddToSystem(system); + + SimTK_ASSERT(isObjectUpToDateWithProperties(), + "VandenBogert2011Muscle: Muscle properties not up-to-date"); + + addStateVariable("projected_fiber_length_normalized"); + addStateVariable("activation"); + +} + +void VandenBogert2011Muscle::extendInitStateFromProperties(SimTK::State& s) +const +{ + Super::extendInitStateFromProperties(s); + + setActivation(s, getDefaultActivation()); + + double projFiberLength = fiberLengthToProjectedLength(getDefaultFiberLength(), false); + setProjFiberLengthNorm(s, projFiberLength/getOptimalFiberLength()); +} + +void VandenBogert2011Muscle::extendSetPropertiesFromState(const SimTK::State& s) +{ + Super::extendSetPropertiesFromState(s); + + setDefaultActivation(getStateVariableValue(s,"activation")); + setDefaultFiberLength(getStateVariableValue(s,"projected_fiber_length_normalized")); + +} + + +void VandenBogert2011Muscle::computeStateVariableDerivatives(const SimTK::State& s) const +{ + + double excitation = getControl(s); + // For now we will start with static guess + // TODO: figure out how to use previous state as guess + SimTK::Vector ydotInitialGuess(2); + ydotInitialGuess[0] = 0.0; + ydotInitialGuess[1] = 0.0; + double projFibVelNorm = calcSolveMuscle(s, excitation, ydotInitialGuess); + setStateVariableDerivativeValue(s, "projected_fiber_length_normalized", projFibVelNorm ); + + + double adot =getActivationDerivative(s); + setStateVariableDerivativeValue(s, "activation", adot); + + + +} + + +//============================================================================= +// COMPUTATION +//============================================================================= + + + +class ImplicitSystemForwardEulerStep : public SimTK::OptimizerSystem { +public: + + //TODO: Add back in option to use NR solver (and perform benchmark) + /* Constructor class. Parameters passed are accessed in the objectiveFunc() + * class. */ + ImplicitSystemForwardEulerStep(int numParameters, SimTK::State& s, double& u): SimTK::OptimizerSystem(numParameters), numControls(numParameters), si(s), excitation(u) + { + + //VandenBogert2011Muscle::ImplicitResidual Results1 = VandenBogert2011Muscle::calcImplicitResidual(s,0.0,0.0,0.0,0); + + } + + int objectiveFunc(const SimTK::Vector &new_yDotGuess, bool new_params, + SimTK::Real& f) const override { + // No objective function + f = 0; + return 0; + } + + int constraintFunc(const SimTK::Vector &new_yDotGuess, bool newParams, + SimTK::Vector& constraints) const override { + //The constraints function is the residuals + + double projFibVelNorm_guess = new_yDotGuess[0]; + double activdot = 0; //BTH - big hack here. Remove this + + VandenBogert2011Muscle::ImplicitResidual results = p_muscle->calcImplicitResidual(si,projFibVelNorm_guess,activdot,excitation,0); + + + constraints[0] = results.forceResidual; + //constraints[1] = results.activResidual; + + return 0; + } + +private: + int numControls; + SimTK::State& si; + double excitation; + SimTK::ReferencePtr p_muscle; +}; + + + +double VandenBogert2011Muscle::fiberLengthToProjectedLength + (double fiberLength , bool argsAreNormalized) const { + + /* When argsAreNormalized= True the function expects normalized values and + * returns normalized values. The function though is internally structured + * to utilize non-normalized values (therefore for it will convert + * normalized arguments to non-normalized values, perform calculations, + * and do then convert the return values to normalized + * when argsAreNormalized= True.)*/ + + double pennAtOptFiberLength = getPennAtOptFiberLength(); + double projFibLen = 0; + + + // If there is pennation, compute fiberLength and muscleWidth from state s using the constant volume constraint: Lce*sin(p) = Lceopt*sin(popt) + // Lce is dimensionless (normalized to Lceopt) + // If pennation is zero, we can't do this because volume is zero, and fiberLength = projFibLen + if (pennAtOptFiberLength < 0.01) return fiberLength; + + //else: + double optimalFiberLength = getOptimalFiberLength(); + + if (argsAreNormalized) fiberLength = fiberLength*optimalFiberLength; + + double muscleWidth = sin(pennAtOptFiberLength); + //b is the fixed distance of the fiber perpindicular to the tendon (width) + + /*TODO: muscleWidth as cache variable + * recalculated. Should be moved info muscleLengthInfo cache */ + + // 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 >= muscleWidth) { + projFibLen = sqrt(pow(fiberLength, 2) - pow(muscleWidth, 2)); + } else { + projFibLen = SimTK::NaN; + } //TODO: Doing this for now, but need to + // come up with a clamping scheme (Millard has one) + + if (argsAreNormalized) projFibLen = projFibLen/optimalFiberLength; + + + return projFibLen; +}; + + +//------------------------------------------------------------------------------ +double VandenBogert2011Muscle::projFibLenToFiberLength (double projFibLen, + bool argsAreNormalized) + const { + + /* When argsAreNormalized= True the function expects normalized values and + * returns normalized values. The function though is internally structured + * to utilize non-normalized values (therefore for it will convert + * normalized arguments to non-normalized values, perform calculations, + * and do then convert the return values to normalized + * when argsAreNormalized= True.)*/ + + + double pennAtOptFiberLength = getPennAtOptFiberLength(); + double fiberLength = 0; + + double optimalFiberLength = getOptimalFiberLength(); + + if (argsAreNormalized) projFibLen = projFibLen * + optimalFiberLength; //Convert from Norm + + + // If pennation is zero, we can't do this because volume is zero, and fiberLength = projFibLen + if (pennAtOptFiberLength < 0.01) { + fiberLength = projFibLen; + } else { + double static muscleWidth = sin(pennAtOptFiberLength); + fiberLength = sqrt(pow(projFibLen, 2) + pow(muscleWidth, 2)); + } + + //Convert back to Norm if needed + if (argsAreNormalized) fiberLength = fiberLength / optimalFiberLength; + + return fiberLength; +} + +//------------------------------------------------------------------------------ +double VandenBogert2011Muscle::fiberVelocityToProjFibVel + (double fiberVelocity, double fiberLength, double projFiberLength, + bool argsAreNormalized) const { + + /* When argsAreNormalized= True the function expects normalized values and + * returns normalized values. The function though is internally structured + * to utilize non-normalized values (therefore for it will convert + * normalized arguments to non-normalized values, perform calculations, + * and do then convert the return values to normalized + * when argsAreNormalized= True.)*/ + + // Note that this normalized by fiber optimal length (not by max velocity) + double optimalFiberLength = getOptimalFiberLength(); + + if (argsAreNormalized) { + fiberLength = fiberLength * getOptimalFiberLength(); + fiberVelocity = fiberVelocity * optimalFiberLength; + projFiberLength = projFiberLength * optimalFiberLength; + } + + double projFibVel = 0; + + if (fiberVelocity != 0) { + projFibVel = fiberVelocity / cos(projFiberLength / fiberLength); + + } else { + projFibVel = 0; } + + if (argsAreNormalized) projFibVel = projFibVel / optimalFiberLength; + + return projFibVel; +}; + +//------------------------------------------------------------------------------ +double VandenBogert2011Muscle::fiberVelocityToProjFibVel + (double fiberVelocity, double fiberLength, bool argsAreNormalized) + const { + + //overload method when projFiberLength is not known/provided + + double projFiberLength = fiberLengthToProjectedLength(fiberLength, + argsAreNormalized); + double projFiberVelocity = fiberVelocityToProjFibVel (fiberVelocity, + fiberLength, + projFiberLength, + argsAreNormalized); + return projFiberVelocity; +}; + + +//------------------------------------------------------------------------------ +double VandenBogert2011Muscle::projFibVelToFiberVelocity(double projFibVel, + double fiberLength, + double projFiberLength, + bool argsAreNormalized) + const { + + /* When argsAreNormalized= True the function expects normalized values and + * returns normalized values. The function though is internally structured + * to utilize non-normalized values (therefore for it will convert + * normalized arguments to non-normalized values, perform calculations, + * and do then convert the return values to normalized + * when argsAreNormalized= True.)*/ + +// Note that this normalized by fiber optimal length (not by max velocity) + double optimalFiberLength = getOptimalFiberLength(); + + if (argsAreNormalized) { + fiberLength = fiberLength * getOptimalFiberLength(); + projFibVel = projFibVel * optimalFiberLength; + projFiberLength = projFiberLength * optimalFiberLength; + } + + double fiberVelocity = 0; + + if (projFibVel != 0) { + fiberVelocity = projFibVel * cos(projFiberLength / fiberLength); + } else { + fiberVelocity = 0; } + + if (argsAreNormalized) fiberVelocity = fiberVelocity / optimalFiberLength; + + return fiberVelocity;} + +//------------------------------------------------------------------------------ +double VandenBogert2011Muscle::projFibVelToFiberVelocity + (double projFibVel, double projFibLength, bool argsAreNormalized) + const { + + //overload method when fiberLength is not known/provided + + double fiberLength = projFibLenToFiberLength(fiberLength, argsAreNormalized); + double fiberVelocity = projFibVelToFiberVelocity(projFibVel, fiberLength, + projFibLength, + argsAreNormalized); + + return fiberVelocity; +}; + + + +//------------------------------------------------------------------------------ +VandenBogert2011Muscle::ImplicitResidual +VandenBogert2011Muscle::calcImplicitResidual(const SimTK::State& s, + double projFibVelNorm_guess, + double activdot_guess, + double excitation, + bool returnJacobians) +const +{ + // Overload method for state as parameters + + VandenBogert2011Muscle::ImplicitResidual results = calcImplicitResidual( + getLength(s), getProjFiberLengthNorm(s), getActivation(s), projFibVelNorm_guess, + activdot_guess, excitation, returnJacobians); + + return results; +} + + +//------------------------------------------------------------------------------ +VandenBogert2011Muscle::ImplicitResidual +VandenBogert2011Muscle::calcImplicitResidual(SimTK::Vec2 y, + SimTK::Vec2 ydot_guess, + double muscleLength, + double excitation, bool returnJacobians) +const +{ + // Overload method for state vectors as parameters + VandenBogert2011Muscle::ImplicitResidual results = calcImplicitResidual( + muscleLength, y[0], y[1], ydot_guess[0], ydot_guess[1], excitation, + returnJacobians); + return results; + +} + +//------------------------------------------------------------------------------ +VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle:: +calcImplicitResidual(double muscleLength, double projFibLenNorm, double activ, + double projFibVelNorm, double activdot, double excitation, + bool returnJacobians) const { + + + //TODO: Match symbols to doxygen diagram and Add symbolic equations comments + //TODO: May want to make this into a separate function + + // -------------------------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 = getFlWidth(); + + //AHill (dimensionless) 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 + + // 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 fiberSlackLengthNorm = getNormFiberSlackLength(); + + //L_{slack,tendon} (m) slack length of the tendon + double tendonSlackLength = getTendonSlackLength(); + + //T_{act} (s) Activation time + double activTimeConstant = getActivTimeConstant(); + + //T_{deact} (s) Deactivation time + double deactivationTimeConstant = getDeactivationTimeConstant(); + + //phi_{opt} pennation at Optimal Fiber Length + double pennAtOptFiberLength = getPennAtOptFiberLength(); + + + // constants derived from the muscle parameters + + + // Jacobian Matrices + SimTK::Mat22 df_dy; + SimTK::Mat22 df_dydot; + + + //-------Convert projFiberLength & Velocity to fiberLength & Velocity------/ + double cosPenn; + double dcosPenn_dprojFibLen; + double fiberLengthNorm; + double dfiberLength_dprojFibLen; + + fiberLengthNorm=projFibLenToFiberLength(projFibLenNorm,true); + + if (pennAtOptFiberLength<0.01) { + // If pennation is zero, we can't do this because volume is zero, + // and fiberLength ~ projFiberLength + cosPenn = 1.0; + dfiberLength_dprojFibLen = 1; + dcosPenn_dprojFibLen = 0; + } else { + double b = sin(pennAtOptFiberLength); + cosPenn = projFibLenNorm/fiberLengthNorm; + dfiberLength_dprojFibLen = cosPenn; + dcosPenn_dprojFibLen = pow(b,2) / pow(fiberLengthNorm,3); + } + + + // Compute fiberVelocity and its derivatives wrt projFibLen and projFibVel + double fiberLengtheningVelocityNorm = projFibVelNorm*cosPenn; + double dfiberLengtheningVelocityNorm_dprojFibVelNorm = cosPenn; + double dfiberLengtheningVelocityNorm_dprojFibLenNorm = + projFibVelNorm * dcosPenn_dprojFibLen; + + + //---F1 is the normalized isometric force-length relationship at maximum + // activation--------------// + double fiberExp = (fiberLengthNorm - 1.0) / fl_width; // [dimensionless] + double F1 = exp(-pow(fiberExp, 2)); // Gaussian force-length curve + + double dF1_dfiberLengthNorm = 0; + + getDebugLevel(); + + if (returnJacobians) { + dF1_dfiberLengthNorm = -2.0 * fiberExp * F1 / fl_width; + double dF1_dprojFibLenNorm = + dF1_dfiberLengthNorm * dfiberLength_dprojFibLen; + } + + + + //-------- F2 is the dimensionless force-velocity relationship -------// + double F2; + double dF2_dfiberLengtheningVelocityNorm; + double dF2_dactiv; + double dF2_dprojFibVelNorm; + double dF2_dprojFibLenNorm; + double df_dmuscleLength; + + // 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 (fiberLengtheningVelocityNorm < 0) { + //Hill's equation for concentric contraction + // F2 = (V_{max} + V_{fiber}) / (V_{max} - V_{fiber}/a_{Hill}) + + double hillDenom = (lambda*getMaxContractionVelocity() - fiberLengtheningVelocityNorm/fv_AHill); + F2 = (lambda*getMaxContractionVelocity()+ fiberLengtheningVelocityNorm) / hillDenom; + + if (returnJacobians) { + dF2_dfiberLengtheningVelocityNorm = (1.0 + F2 / fv_AHill) / hillDenom; + dF2_dactiv = - dlambda_da * getMaxContractionVelocity() * fiberLengtheningVelocityNorm * + (1.0 + 1.0/fv_AHill) / pow(hillDenom,2); + } + } else { + //Katz Curve for eccentric contraction + // c is Katz Constant + double c3 = getMaxContractionVelocity()* 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 = (fiberLengtheningVelocityNorm + c); + F2 = (fv_maxMultiplier * fiberLengtheningVelocityNorm + c) / katzDenom ; + if (returnJacobians) { + dF2_dfiberLengtheningVelocityNorm = (fv_maxMultiplier - F2) / katzDenom ; + dF2_dactiv = dlambda_da * c3 * fiberLengtheningVelocityNorm * + (1-fv_maxMultiplier) / pow(katzDenom,2); + } + } + if (returnJacobians){ + dF2_dprojFibVelNorm = + dF2_dfiberLengtheningVelocityNorm * dfiberLengtheningVelocityNorm_dprojFibVelNorm; + dF2_dprojFibLenNorm = + dF2_dfiberLengtheningVelocityNorm * dfiberLengtheningVelocityNorm_dprojFibLenNorm; + } + + //------F3 is the dimensionless fiber (PEE) force (in units of Fmax)------// + double dF3_dprojFibLenNorm; + + // stiffness of the linear term is 1 N/m, convert to Fmax/Lceopt units + double kPEENorm = 1.0 / maxIsoForce * optFiberLength; + // elongation of fiber (PEE), relative to Lceopt + double elongationFiberNorm = (fiberLengthNorm - fiberSlackLengthNorm); + double F3 = kPEENorm * elongationFiberNorm; + // low stiffness linear term + double dF3_dfiberLengthNorm = kPEENorm; + double kPEE2Norm =0; + if (elongationFiberNorm > 0) { + kPEE2Norm = 1 / pow(fl_width, 2); //Fiber (PEE) quadratic stiffness, + // so Fpee = Fmax when Lce = Lce*(1+W) + //add quadratic term for positive elongation + F3 = F3 + kPEE2Norm * pow(elongationFiberNorm, 2); + if (returnJacobians) { + dF3_dfiberLengthNorm = + dF3_dfiberLengthNorm + 2 * kPEE2Norm * elongationFiberNorm; + } + } + if (returnJacobians) { + dF3_dprojFibLenNorm = dF3_dfiberLengthNorm * dfiberLength_dprojFibLen; + } + + + //--------F4 is the dimensionless SEE force (in units of Fmax)----------// + + + // stiffness of the linear term is 1 N/m, convert to Fmax/m (so normalized + // by Fmax) + double kSEE = 1.0 / maxIsoForce; + + // elongation of tendon (SEE), in meters + double elongationTendon = muscleLength - projFibLenNorm * + optFiberLength - tendonSlackLength; + // low stiffness linear term + double F4 = kSEE * elongationTendon; + double dF4_dmuscleLength; + double dF4_dprojFibLenNorm; + if (returnJacobians) { + dF4_dprojFibLenNorm = -kSEE * optFiberLength; + dF4_dmuscleLength = kSEE; + } + + double kSEE2 = SimTK::NaN; + if (elongationTendon > 0) { + // Tendon (SEE) quadratic stiffness, so Fsee = Fmax at strain of umax + // This is normalized by Fmax + kSEE2 = 1 / pow(tendonSlackLength * fMaxTendonStrain, 2); + + // add quadratic term for positive deformation + F4 = F4 + (kSEE2 * pow(elongationTendon, 2)); + if (returnJacobians) { + dF4_dprojFibLenNorm = dF4_dprojFibLenNorm - 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 * projFibVelNorm ; + double dF5_dprojFibVelNorm = dampingCoeff; + + + // ---------Calculate the Muscle Force Residual ---------------------- // + //The muscle dynamics equation: f = Fsee - (a*Fce - Fpee)*cos(Penn) - + // Fdamping = 0 + // (Reminder the units of fRes are (N/N), needs to be *Fmax to get to N) + double fRes = F4 - (activ * F1 * F2 + F3)*cosPenn - F5; + + + +if (getDebugLevel()>0){ + cout << "-------------------------" << endl; + cout << "activ: " << activ << endl; + cout << "F1 (FL): " << F1 << endl; + cout << "F2 (FV): " << F2 << endl; + cout << "F3 (PEE) : " << F3 << endl; + cout << "F4 (SEE): " << F4 << endl; + cout << "F5 (Damping): " << F5 << endl; + cout << "fRes: " << fRes << endl; + cout << "------------------" << endl; + cout << "cosPenn: " << cosPenn << endl; + cout << "muscleLength: " << muscleLength << endl; + cout << " SEE" << endl; + cout << " kSEE: " << kSEE << endl; + cout << " kSEE2: " << kSEE2 << endl; + cout << " elongationTendon: " << elongationTendon << endl; + cout << " tendonSlackLength: " << tendonSlackLength << endl; + cout << " PEE" << endl; + cout << " fiberLengthNorm: " << fiberLengthNorm << endl; + cout << " optFiberLength: " << optFiberLength << endl; + cout << " projFibLenNorm: " << projFibLenNorm << endl; + cout << " kPEENorm: " << kPEENorm << endl; + cout << " kPEE2Norm: " << kPEE2Norm << endl; + cout << " elongationFiberNorm: " << elongationFiberNorm << endl; + cout << "" << endl; +} + + // --------------- 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 = activdot - calcActivationDerivative(activ,excitation); + SimTK::Vec2 df_du; + double dActRes_dactiv = 0; + double dActRes_dactivdot = 0; + + if (returnJacobians) { + dActRes_dactiv = (excitation / activTimeConstant + (1 - excitation) / deactivationTimeConstant); + dActRes_dactivdot = 1; + + df_du[0] = 0; + df_du[1] = -(excitation / activTimeConstant + (1 - excitation) / deactivationTimeConstant ) + - (excitation - activ) * (1 / activTimeConstant - 1 / deactivationTimeConstant ); + } + + + + //---------------------Assemble Jacobians---------------------------// + if (returnJacobians) { + double dfRes_dactiv = -(F1*F2 + activ*F1*dF2_dactiv )*cosPenn; + double dfRes_dprojFibLengthNorm = dF4_dprojFibLenNorm - + (activ*(dF1_dfiberLengthNorm*F2 + F1*dF2_dprojFibLenNorm) + + dF3_dprojFibLenNorm) * cosPenn - (activ*F1*F2 + F3) * + dcosPenn_dprojFibLen; + double dfRes_dprojFibVelNorm = + - activ*F1*dF2_dprojFibVelNorm - dF5_dprojFibVelNorm; + double dfRes_dmuscleLength = dF4_dmuscleLength; + + + //y=(projFiberLength,activation) <-States + + //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_dprojFibLengthNorm; + df_dy[0][1] = dfRes_dactiv; + // Row 2 - df_dy (activation) + df_dy[1][0] = 0; + df_dy[1][1] = dActRes_dactiv ; + + + // Row 1 - df_dydot (force) + 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] = dActRes_dactivdot; + + df_dmuscleLength = dfRes_dmuscleLength; + } + + + + VandenBogert2011Muscle::ImplicitResidual results; + + results.forceResidual = fRes; + results.activResidual = activationResidual; + results.forceTendon = Fsee; + 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; + results.F4 = F4; + results.F5 = F5; + +return results; } + + + +double VandenBogert2011Muscle:: +calcActivationDerivative(double activation, double excitation) const +{ + + double activationDerivative=(excitation - activation) * + (excitation / getActivTimeConstant() + (1 - excitation) / getDeactivationTimeConstant()); + + return activationDerivative; +} + + +double VandenBogert2011Muscle:: +getActivationDerivative(const SimTK::State& s) const +{ + double activationDerivative = + calcActivationDerivative(getActivation(s),getExcitation(s)); + + return activationDerivative; +} + + + + + +//============================================================================== +// MUSCLE INTERFACE REQUIREMENTS -- MUSCLE LENGTH INFO +//============================================================================== +void VandenBogert2011Muscle::calcMuscleLengthInfo(const SimTK::State& s, + MuscleLengthInfo& mli) const +{ + // Get musculotendon actuator properties. + //double maxIsoForce = getMaxIsometricForce(); + double optFiberLength = getOptimalFiberLength(); + double tendonSlackLen = getTendonSlackLength(); + + try { + + double projFibLengthNorm = getStateVariableValue(s, "projected_fiber_length_normalized"); + + mli.normFiberLength = projFibLenToFiberLength(projFibLengthNorm,true); + mli.fiberLength = mli.normFiberLength * optFiberLength ; + + mli.pennationAngle = penMdl.calcPennationAngle(mli.fiberLength); + mli.cosPennationAngle = cos(mli.pennationAngle); + mli.sinPennationAngle = sin(mli.pennationAngle); + mli.fiberLengthAlongTendon = mli.fiberLength * mli.cosPennationAngle; + + // Necessary even for the rigid tendon, as it might have gone slack. + mli.tendonLength = penMdl.calcTendonLength(mli.cosPennationAngle, + mli.fiberLength, getLength(s)); + mli.normTendonLength = mli.tendonLength / tendonSlackLen; + mli.tendonStrain = mli.normTendonLength - 1.0; + + mli.fiberPassiveForceLengthMultiplier = + fpeCurve.calcValue(mli.normFiberLength); + mli.fiberActiveForceLengthMultiplier = + falCurve.calcValue(mli.normFiberLength); + + } catch(const std::exception &x) { + std::string msg = "Exception caught in Millard2012EquilibriumMuscle::" + "calcMuscleLengthInfo from " + getName() + "\n" + + x.what(); + throw OpenSim::Exception(msg); + } +} + + + + + + double VandenBogert2011Muscle::getProjFiberVelNorm(const SimTK::State& s) const + { + double excitation = getExcitation(s); + SimTK::Vector projFibVelNormGuess; //BTH - Should make this use the current state derivative value if possible + projFibVelNormGuess[0]=0; + double projFiberVelocityNorm = calcSolveMuscle(s, excitation, projFibVelNormGuess); + + return projFiberVelocityNorm; + } + + + + + +//------------------------------------------------------------------------------ +VandenBogert2011Muscle::ImplicitResidual +VandenBogert2011Muscle::calcJacobianByFiniteDiff(double muscleLength, + double projFibLenNorm, + double activ, + double projFibVelNorm, + double activdot, double excitation, + double stepSize) 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, excitation, stepSize ); + + return results; +} + + +//------------------------------------------------------------------------------ +VandenBogert2011Muscle::ImplicitResidual VandenBogert2011Muscle:: +calcJacobianByFiniteDiff(SimTK::Vec2 y, SimTK::Vec2 ydot, double muscleLength, + double excitation, double stepSize ) const { + + + + //TODO: In the optimizer class, you do not have to supply the Jacobian + // function. The must mean there is code in Simbody that does finite diff. + // Should probably look at calling that in place of this code. + + // Jacobian Matrices + SimTK::Mat22 df_dy; + SimTK::Mat22 df_dydot; + SimTK::Vec2 df_du; + + VandenBogert2011Muscle::ImplicitResidual opPoint; + opPoint = calcImplicitResidual (y,ydot,muscleLength,excitation,0); + double opForceResidual = opPoint.forceResidual; + double opActivResidual = opPoint.activResidual; + + VandenBogert2011Muscle::ImplicitResidual del; + + //----------df_dy------------// + SimTK::Vec2 dh = {stepSize,0}; + del = calcImplicitResidual(y+dh,ydot,muscleLength,excitation,0); + df_dy[0][0] = (del.forceResidual-opForceResidual)/stepSize; + df_dy[1][0] = (del.activResidual-opActivResidual)/stepSize; + + dh = {0,stepSize}; + del = calcImplicitResidual(y+dh,ydot,muscleLength,excitation,0); + df_dy[1][0] = (del.forceResidual-opForceResidual)/stepSize; + df_dy[1][1] = (del.activResidual-opActivResidual)/stepSize; + + //----------df_dydot------------// + dh = {stepSize,0}; + del = calcImplicitResidual(y,ydot+dh,muscleLength,excitation,0); + df_dydot[0][0] = (del.forceResidual-opForceResidual)/stepSize; + df_dydot[1][0] = (del.activResidual-opActivResidual)/stepSize; + + dh = {0,stepSize}; + del = calcImplicitResidual(y,ydot+dh,muscleLength,excitation,0); + df_dydot[1][0] = (del.forceResidual-opForceResidual)/stepSize; + df_dydot[1][1] = (del.activResidual-opActivResidual)/stepSize; + + //----------df_du----------------// + del = calcImplicitResidual(y,ydot,muscleLength,excitation+stepSize,0); + df_du[0] = (del.forceResidual-opForceResidual)/stepSize; + df_du[1] = (del.activResidual-opActivResidual)/stepSize; + +/* + 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 + + + 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; + opPoint.df_du = df_du; + + return opPoint; +} + + +//------------------------------------------------------------------------------ +// Just a quick convenience method to get the force residual +// under static conditions +SimTK::Vec3 VandenBogert2011Muscle::calcFiberStaticEquilibResidual( + double projFibLenNorm, double muscleLength, double activ) const +{ + VandenBogert2011Muscle::ImplicitResidual results = calcImplicitResidual( + muscleLength, projFibLenNorm, 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::Vec2 VandenBogert2011Muscle::calcFiberStaticEquilbirum( + double muscleLength, double activ) const { + + // This code assumes that the muscle lengthing speed is 0. + // It utilizes a Newton-Raphson Method to root solve to obtain the + //fiber length. + + + //TODO: Code is not 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-8; //TODO : Look at changing to proportional of eps + double a = 0; + double b = 10; //10 muscle lengths (more will slow convergence by adding steps) + + double x = (a + b) / 2.0; + double dx = 2 * tol; + + int neval = 0; + + SimTK::Vec3 forceResAndDerivative; + + //Perform a Newton Line Search + while ((abs(dx) >= tol) && (neval < 100)) { + + neval++; + + // Set a to be lower value and b to be upper value + a = min(a, b); + b = max(a, b); + + + 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]; + + 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 = + calcFiberStaticEquilibResidual(xdx, muscleLength, activ); + bool largeDeriv = abs(forceResAndDerivative[0]) > (0.5 * abs(fx)); + + if (~inInterval || largeDeriv) { + x = (a + b) / 2; + dx = (a - b) / 2; + } else { + x = xdx; + } + + // TODO: Need to handle condition when number of loop iterations reaches + // neval limit. See Millard2012 Muscle Error Handling + } + } + SimTK::Vec2 vout; + vout[0] = x; //projFiberLengthNorm + vout[1] = forceResAndDerivative[2]; // muscleForce + return vout; +}; + + +//------------------------------------------------------------------------------ +//Calculate the ydot values to drive the residuals to 0 and "balance" the muscle + +double VandenBogert2011Muscle::calcSolveMuscle(const SimTK::State& s, + double excitation, SimTK::Vector projFibVelNormGuess) const { + +//SimTK::Vector VandenBogert2011Muscle::calcSolveMuscle(const SimTK::State& s, +// double activ, SimTK::Vector yDotInitialGuess) { + + SimTK::State stemp=s; + + //ImplicitSystemForwardEulerStep sys(2, stemp, activ); + ImplicitSystemForwardEulerStep sys(1, stemp, excitation); + + //TODO: Need come up with reasonable bounds + //SimTK::Vector lower_bounds(2); + SimTK::Vector lower_bounds(1); + lower_bounds[0] = -SimTK::Infinity; + //lower_bounds[1] = -SimTK::Infinity; + + //SimTK::Vector upper_bounds(2); + SimTK::Vector upper_bounds(1); + upper_bounds[0] = SimTK::Infinity; + //upper_bounds[1] = SimTK::Infinity; + + sys.setParameterLimits(lower_bounds, upper_bounds); + + + SimTK::Optimizer opt(sys, SimTK::InteriorPoint); //Create the optimizer + + // Specify settings for the optimizer + opt.setConvergenceTolerance(0.1); + opt.useNumericalGradient(true, 1e-5); + opt.useNumericalJacobian(true); + opt.setMaxIterations(100); + opt.setLimitedMemoryHistory(500); + + opt.optimize(projFibVelNormGuess); // Optimize + + + double projFibVelNorm=projFibVelNormGuess[0]; + //ydot[1]=yDotInitialGuess[1]; + + return projFibVelNorm; +}; + + + + + + + + + + + diff --git a/OpenSim/Actuators/VandenBogert2011Muscle.h b/OpenSim/Actuators/VandenBogert2011Muscle.h new file mode 100644 index 0000000000..913d532a27 --- /dev/null +++ b/OpenSim/Actuators/VandenBogert2011Muscle.h @@ -0,0 +1,887 @@ +#ifndef OPENSIM_VANDENBOGERT2011MUSCLE_H_ +#define OPENSIM_VANDENBOGERT2011MUSCLE_H_ + +/* -------------------------------------------------------------------------- * +* OpenSim: VandenBogert2011Muscle.h * +* -------------------------------------------------------------------------- * +* The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * +* See http://opensim.stanford.edu and the NOTICE file for more information. * +* OpenSim is developed at Stanford University and supported by the US * +* National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * +* through the Warrior Web program. * +* * +* Copyright (c) 2016 Stanford University and the Authors * +* Author(s): Brad Humphreys, Chris Dembia, Antoine van den Bogert * +* * +* Licensed under the Apache License, Version 2.0 (the "License"); you may * +* not use this file except in compliance with the License. You may obtain a * +* copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * +* * +* Unless required by applicable law or agreed to in writing, software * +* distributed under the License is distributed on an "AS IS" BASIS, * +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * +* See the License for the specific language governing permissions and * + * limitations under the License. * +* -------------------------------------------------------------------------- */ + +#include +#include + +/* +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 +*/ +#include +#include + +namespace OpenSim { +/** +This class implements a muscle with implicit dynamics capabilities as described + in van den Bogert et al[1]. + + The muscle is comprised of 4 components and is show below: +- CE: Contractile Element +- DE: Damping Element +- PE: Parallel Elastic Element +- SE: Series Elastic Element (tendon) + +\image html fig_VandenBogertMuscle.png + + + +The dimensions are: + - \f$h\f$ : Muscle height + - \f$ l_{m} \f$ : Length of the muscle contractile and parallel elastic element (also know as the + "muscle fiber length") + - \f$ l_{mt} \f$ - Muscle and tendon fiber length + - \f$ l_{p} \f$ - Length of the fiber projected on to the line of action + of the tendon (also the length of the damping elements) + - \f$ l_{t} \f$ - Length of the tendon + - \f$ \phi \f$ - Pennation angle of the muscle fiber + + +Nomenclature
+ - Symbols with the a \f$ \bar{bar} \f$ indicate that the quantity has been normalized by a constant parameter. For forces, this will all ways be \f$F_{m,opt}\f$. For velocities and lengths this will be \f$l_{m,opt}\f$ (velocities will therefore have units of "optimal lengths per second") + - Muscle velocities sign convention is the positive is lengthening (eccentric) + + + +|Symbol | Description | Units | +|--------|---------|----------| +|a | Activation (State Variable) | unitless | +|\f$b_{de}\f$ | Damping, % of \f$F_{m,opt}\f$ per 1 optimal length per second (Parameter: fiber_damping) | unitless | +|\f$A_{hill} \f$ | Hill Constant (Parameter: force_velocity_hill_constant)| unitless | +|\f$ F_{se} \f$| Force in the Series Elastic Element (Calculated Value) | N | +|\f$ F_{ce} \f$| Force in the Contractile Element (Calculated Value) | N | +|\f$ F_{de} \f$| Force in the Damping Element (Calculated Value) |N| +|\f$ F_{m,opt} \f$| Force in the contractile element under isometric conditions, at optimal length (Parameter: max_isometric_force) |N| +|\f$ k_{pe} \f$| Parallel elastic element linear stiffness (Derived Parameter) | N/m | +|\f$ k_{pe,quad} \f$| Parallel elastic element linear stiffness (Derived Parameter) | \f$ N/m^{2} \f$| +|\f$ k_{se} \f$| Series elastic element (tendon) linear stiffness (Derived Parameter) | N/m | +|\f$ k_{se,quad} \f$| Series elastic element (tendon) linear stiffness (Derived Parameter) | \f$ N/m^{2} \f$| +|\f$ \lambda \f$| Activation-Velocity Parameter (Derived Parameter)| unitless | +|\f$ \lambda_{m} \f$|Activation-Velocity Parameter Slope (Parameter: activation_velocity_slope)| unitless | +|\f$ \lambda_{o} \f$|Lambda value at zero Activation (Parameter: activation_velocity_intercept)| unitless | +|\f$ \phi_{ref} \f$|Pennation at optimal fiber length (Parameter: pennation_at_optimal_fiber_length) |rad| +|\f$ \bar{l_{m,slack}} \f$ | length of muscle when parallel elastic element is slack (Parameter: fiber_slack_length_norm)| unitless | +|\f$ l_{t,slack} \f$ | Slack Length of the tendon (Parameter: tendon_slack_length )| m | +|\f$ \tau_{a} \f$| Activation Time Constant (Parameter: activation_time_constant)| 1/s | +|\f$ \tau_{d} \f$ | Deactivation Time Constant (Parameter: deactivation_time_constant) |1/s| +|\f$ u_{max} \f$ | Strain in tendon at \f$F_{m,opt}\f$ (Parameter: tendon_strain_at_max_iso_force) | m/m | +|\f$ \bar{v_{max}} \f$ | Normalized maximum velocity (Parameter: force_velocity_max_lengthening_force_norm) | unitless| +|\f$ \bar{f_{v,max}} \f$ | Normalized maximal lengthening (eccentric) force (Parameter: force_velocity_max_lengthening_force_norm) | unitless | +|w | force-length width (Parameter: active_force_length_curve_width); This is the kShapeActive parameter in Thelen2003Muscle| unitless | + +
+
+ +------------------------------------------------------------------------------ + Force Balance
+ The implicit muscle determines it's operating state by calculating the forces + which result in the tendon and muscle complexes being balanced (equivalent): + \f[ + F_{se} - (F_{ce} + F_{pe})\cos\phi - F_{de} = 0 + \f] + + + Series Elastic Element Force, \f$ F_{se} \f$
+ This is the elastic force in the tendon and is a function of the length of the + tendon (\f$ l_{mt} - l_{m} \cos\phi \f$) which is : + \f[ + F_{se} = \mathbf{f_{se}} (l_{mt} - l_{m} \cos\phi) + \f] + + + Contractile Element Force, \f$ F_{ce} \f$
+ The contractile element force is: + \f[ + F_{ce} = a*F_{m,opt} \mathbf{f_{l}}(l_{m}) \mathbf{f_{v}} (\dot{l_{m}}) + \f] + \f$ \mathbf{f_{l}} \f$ and \f$ \mathbf{f_{v}} \f$ are the muscle force-length and force-velocity relationships and are detailed below. + + + Parallel Elastic Element Force, \f$ F_{pe} \f$
+ The elastic force in the muscle fiber is a function of the length of the contractile element: + \f[ + F_{pe} = \mathbf{f_{pe}}(l_{m}) + \f] + + + ParallelDamping Element Force, \f$ F_{de} \f$
+ A dissipative force in the muscle fiber is modeled as a function of the velocity of the velocity of + the muscle fiber projected along the line of action of the tendon: + \f[ + F_{de}=\mathbf{f_{de}}(\dot{l_{m}}) +\f] + + ------------------------------------------------------------------------------ + Muscle State Equation
+ The state equation for the muscle is then: + + \f[ + \mathbf{f_{se}} (l_{mt} - l_{m} \cos\phi) + + - [aF_{m,opt} \mathbf{f_{l}}(l_{m}) \mathbf{f_{v}} (\dot{l_{m}}) + + + \mathbf{f_{pe}}(l_{ce})] + \cos\phi + - \mathbf{f_{de}}(\dot{l_{m}}) + =0 + \f] + + The muscle length state variable is then chosen: + +\f[ + x_{1}= l_{m}\cos\phi = l_{p} + \f] + + (This differs from other muscle models that use a state variable of \f$ l_{m} \f$.) + + Due to the \f$ l_{m} \f$ and \f$ \dot{l_{m}}\f$ terms in the force-length and force-velocity relationship, + along with the \f$ \cos\phi \f$ term, intermediate equations are needed to convert from the state variable \f$ x_{1} \f$ to + \f$ \bar{l_{p}} \f$. + + + + To remove the pennation angle \f$ \phi \f$ as a variable, a constant muscle volume assumption is used[2]: + \f[ + l_{m} \sin\phi = l_{m,ref} \sin\phi_{ref} = h + \f] + + This muscle uses the reference length of \f$ l_{m,ref}=l_{m,opt} \f$. \f$ h \f$ is referred to as the muscle height (a constant). \f$ \phi_{ref} \f$ and \f$ l_{ce,ref} \f$ are the pennation angle at a reference fiber length respectively. If the pennation is less than 0.01 radians, the projected fiber length equals the fiber length (if pennation is zero, the muscle volume would be zero). + + An implicit first order differential equation of state \f$ x_{1} \f$ can then be formed using the following two equations: + \f[ + l_{m} = ({l_{p}}^2 + h^{2})^{0.5} = ({x_{1}}^2 + h^{2})^{0.5} + \f] +\f[ + \cos\phi=\frac {l_{p}}{l_{m}}=\frac {x_{1}}{l_{m}} + \f] + + +Muscle activation, \f$ a \f$ is a function of the control signal (neural excitation), is modeled[3] as: + + \f[ + \dot{a}=(u-a) (\frac{u}{\tau_{a}} + \frac{1-u}{\tau_{d}}) +\f] +This is a first order differential equation and therefore the muscle has a second state variable \f$ x_{2}=a \f$. + + + Implicit formulation of muscles have the following advantages:
+ 1) This muscle does not have a singularity at zero activation, \f$ a=0\f$
+ 2) This muscle does not have a singularity at pennation=90 deg, \f$ \phi=\pi \f$
+ 3) This muscle has continuous derivatives. This allows for use of gradient based solvers such as those commonly used in optimization.
+ + The above singularities can cause very small time stepping by integrators when they are approached in simulation.
+ + In the formulation of each of the force components, normalized values are used to help insure numerical stability and to allow comparison to standard muscle curves. Also, the implementation of the \f$ x_{1} \f$ state variable uses a normalization: +\f[ + x_{1}= \frac{l_{m}\cos\phi} {l_{m,opt}} = \frac {l_{p}} {l_{m,opt}} = \bar{l_{p}} + \f] + +------------------------------------------------------------------------------ + + Force Length Relationship, \f$ f_{l} \f$
+ The force-length relationship of the muscle is modeled with a Gaussian curve [4]: + \f[ + \mathbf{f_{l}} = e^{-d^{2}} + \f] + + where: + \f[ + d=\frac{l_{m}-l_{m,opt}}{w \cdot l_{m,opt}} +\f] + + + \image html fig_VandenBogert2011Muscle_fl.png + + + Force Velocity Relationship, \f$ f_{v} \f$
+ The force-velocity relationship of the muscle is broken into two regimes: + + For Muscle Shortening (\f$ \dot{l_{m}}<0 \f$), the Hill[5] concentric model is utilized: + \f[ + \mathbf{f_{v}}= \frac {\lambda v_{max} +\dot{l_{m}}} + {\lambda v_{max} -\frac{\dot{l_{m}}}{A_{Hill}}} +\f] + + For muscle elongating (\f$ \dot{l_{m}}>=0 \f$), the Katz[6] eccentric model is utilized: + \f[ +\mathbf{f_{v}} = \frac{\bar{f_{v,max}} \dot{l_{m}} +c} +{ \dot{l_{m}} +c} + \f] + + where: + \f[ +c=\frac{\lambda \cdot v_{max} A_{hill} (f_{v,max}-1)} +{A_{hill}+1} + \f] + + The parameter \f$ \lambda \f$ in the force-velocity relationship provides a scalar to adjust the maximum lengthening velocity based upon muscle activation[7]: + \f[ + \lambda = \lambda_{m}a + \lambda_{o} + \f] + + + \image html fig_VandenBogert2011Muscle_fv.png + + + Parallel Elastic Element Force, \f$ F_{pe} \f$
+ + Calculation of the force in the parallel elastic element are expressed in terms of normalized force: + \f[ + F_{pe} = F_{m,opt} \cdot \bar{F_{pe}} + \f] + + Elongation of the parallel element from it's slack length (and relative to fiber's optimal length): + \f[ + \bar{dl_{m}} = \frac {l_{m}} {l_{m,opt}} - \frac {l_{m,slack}} {l_{m,opt}} = \bar{l_{m}} - \bar{l_{m,slack}} + \f] + + If the parallel element's length is less than it's slack length (\f$ l_{m}l_{m,slack} \f$, so \f$ \bar{dl_{m}} > 0 \f$) a quadratic stiffness is added: + \f[ + \bar{k_{pe,quad}} = \frac{1}{w^{2}} + \f] + This insures that \f$ F_{pe} = F_{m,opt} \f$ when \f$ l_{m}=l_{m,opt}(1+w)\f$ + \f[ + \bar{F_{pe}} = \bar{k_{pe}} \bar{dl_{m}} + \bar{k_{pe,quad}} \bar{dl_{m}}^{2} + \f] + + + \image html fig_VandenBogert2011Muscle_PeeForce.png + + Series Elastic Element (Tendon) Force, \f$ F_{se} \f$
+ + +The length of the tendon is: + \f[ + l_{t} = l_{mt} - l_{p} + \f] + + The elongation of the tendon is then + \f[ + dl_{t}=l_{t}-l_{t,slack} + \f] + + The force in the tendon is: + \f[ + F_{se} = k_{se}dl_{t} + k_{se,quad}(dl_{t})^{2} + \f] + +If the tendon is not being stretched, a very light spring rate of 1N/m is used: + \f[ + k_{se} = 1 + \f] + + If the tendon is being stretched an additional quadratic term is used so that + force in the tendon: \f$F_{se} = F_{m,opt}\f$ when the strain in the tendon = \f$u_{max}\f$: + + \f[ + k_{se,quad} = \frac{F_{m,opt}}{(u_{max}l_{t,slack})^{2}} + \f] + + + +To normalize the force (note that due to the quadratic terms, non-normalized lengths must be used): + \f[ + \bar{F_{se}} = \frac{F_{se}}{F_{m,opt}} =\frac{k_{se}dl_{t}}{F_{m,opt}} + \frac{dl_{t}^2}{(u_{max}l_{t,slack})^{2}} + \f] + + \image html fig_VandenBogert2011Muscle_SeeForce.png + + Parralel Damping Element Force, \f$ F_{de} \f$
+ +\f$ F_{de} \f$ is the viscous damping parallel to the contractile element. The damping is set such that it equals \f$b_{de}\f$ percent of \f$F_{m,opt}\f$ when the muscle velocity is 1 optimal length per second: + + \f[ + F_{de} = b_{de} F_{m,opt} \frac {\dot{l_p}}{l_{m,opt}} + \f] + + or in normalized terms: + + \f[ + \bar{F_{de}} = b_{de} \bar{\dot{l_p}} + \f] + + This damping is present in the model to provide numerical stability when activation of the muscle is zero. + + +------------------------------------------------------------------------------ + References
+ [1] van den Bogert A, Blana D, Heinrich D, Implicit methods for efficient musculoskeletal simulation and optimal control. Procedia IUTAM 2011; 2:297-316
+ [2] Otten E. Concepts and models of functional architecture in skeletal muscle. Exerc Sport Sci Rev 1988; 16:89-137
+ [3] He J, Levine WS, Loeb GE. Feedback gains for correcting small perturbations to standing posture. IEEE Trans Auto Control 1991; 36:322-32.
+ [4] Walker S, Schrodt I, Segment lengths and thin filament periods in skeletal muscle fibers of the rhesus monkey and the human. The Anatom Rec 1974; 178:63-81
+ [5] Hill A, The heat of shortening and the dynamic constants of Muscle. Proc Royal Society 1938 126
+ [6] Katz B, The relation between force and speed in muscular contraction. J Physiol 1939; 96:45-64
+ [7] Chow J, Darling W, The maximum shortening velocity of muscle should be scaled with activation. J Appl Physiol 1999; 86(3):1025-31
+ + */ + //TODO: Add detailed muscle discription + +// Derive new class from Muscle.h +class OSIMACTUATORS_API VandenBogert2011Muscle : public Muscle { + OpenSim_DECLARE_CONCRETE_OBJECT(VandenBogert2011Muscle, Muscle); + +public: +//============================================================================= +// PROPERTIES +//============================================================================= + + //OpenSim_DECLARE_PROPERTY(max_isometric_force, double, + // F_{max} (N) "Maximum isometric force that the fibers can generate"); + // Comes from parent class, Muscle.h + + OpenSim_DECLARE_PROPERTY(tendon_strain_at_max_iso_force, double, + "tendon strain at maximum isometric muscle force"); + //u_{max} (dimensionless) strain in the series elastic element at load + // of maxIsometricForce + + OpenSim_DECLARE_PROPERTY(active_force_length_curve_width, double, + "force-length shape factor"); + //w (dimensionless) width parameter of the force-length relationship of + // the muscle fiber + + OpenSim_DECLARE_PROPERTY(force_velocity_hill_constant, double, + "force-velocity shape factor"); + //A_{Hill} (dimensionless) Hill parameter of the force-velocity + // relationship + + OpenSim_DECLARE_PROPERTY(force_velocity_max_lengthening_force_norm, double, + "maximum normalized lengthening force"); + //\bar{f_{v,max}} (dimensionless) maximal eccentric force + + //OpenSim_DECLARE_PROPERTY(optimal_fiber_length, double, + // "Optimal Length of Contractile Element" + //Lceopt (m) + // Comes from parent class, Muscle.h + + OpenSim_DECLARE_PROPERTY(fiber_damping, double, + "The linear damping of the fiber"); + //b_{de} damping coefficient of damper parallel to the fiber + // + + OpenSim_DECLARE_PROPERTY(fiber_slack_length_norm, double, + "(dimensionless) slack length of the parallel " + "elastic element, divided by Lceopt"); + //L_{m,slack} (dimensionless) slack length of the fiber (PEE) + + + OpenSim_DECLARE_PROPERTY(activation_time_constant, double, + "Activation time(s)"); + //T_{a} (s) Activation time + + OpenSim_DECLARE_PROPERTY(deactivation_time_constant, double, + "Deactivation time(s)"); + //T_{d} (s) Deactivation time + + OpenSim_DECLARE_PROPERTY(pennation_at_optimal_fiber_length, double, + "pennation at optimal fiber length " + "equilibrium"); + //\psi_{ref} (rad) + + + OpenSim_DECLARE_PROPERTY(default_activation, double, + "default activation"); + + OpenSim_DECLARE_PROPERTY(default_fiber_length, double, + "Assumed initial fiber length if none is " + "assigned."); + + +//============================================================================= +// Construction +//============================================================================= + /** Constructs a functional muscle using default parameters. */ + explicit VandenBogert2011Muscle(); + + /** Constructs a functional muscle using default curves and activation + model parameters. + @param name The name of the muscle. + @param maxIsometricForce The force generated by the muscle when fully + activated at its optimal resting length with a contraction velocity of + zero. + @param optimalFiberLength The optimal length of the muscle fiber. + @param tendonSlackLength The resting length of the tendon. + @param pennationAngle The angle of the fiber (in radians) relative to + the tendon when the fiber is at its optimal resting length. */ + VandenBogert2011Muscle(const std::string &name, + double maxIsometricForce, + double optimalFiberLength, + double tendonSlackLength, + double pennationAngle); + + + +//------------------------------------------------------------------------- +// GET & SET Properties +//------------------------------------------------------------------------- + /** @param tendon_strain_at_max_iso_force The tendon strain at a load of Fmax*/ + void setFMaxTendonStrain(double tendon_strain_at_max_iso_force); + /** @returns The tendon strain at a load of Fmax*/ + double getFMaxTendonStrain() const; + + /** @param flWidth The width parameter (W) of the force-length relationship of + the muscle fiber (dimensionless)*/ + void setFlWidth(double flWidth); + /** @returns The width parameter (W) of the force-length relationship of + the muscle fiber (dimensionless)*/ + double getFlWidth() const; + + /** @param fvAHill The Hill parameter of the force-velocity relationship + (dimensionless)*/ + void setFvAHill(double fvAHill); + /** @returns The Hill parameter of the force-velocity relationship + (dimensionless)*/ + double getFvAHill() const; + + /** @param fvMaxMultiplier The maximal eccentric force multiplier FV_{max} + (dimensionless) */ + void setFvmaxMultiplier(double fvMaxMultiplier); + /** @returns The maximal eccentric force multiplier FV_{max} + (dimensionless) */ + double getFvmaxMultiplier() const; + + /** @param dampingCoefficient The damping coefficient of damper parallel to the fiber + (normalized to maxIsometricForce), b (s/m)*/ + void setDampingCoefficient(double dampingCoefficient); + /** @returns The damping coefficient of damper parallel to the fiber + (normalized to maxIsometricForce), b (s/m)*/ + double getDampingCoefficient() const; + + /** @param normFiberSlackLength The slack length of the parallel elastic element, divided by + fiber optimal length, L_{slack,fiber} (dimensionless)*/ + void setNormFiberSlackLength(double normFiberSlackLength); + /** @returns The slack length of the parallel elastic element, divided + by fiber optimal length, L_{slack,fiber} (dimensionless)*/ + double getNormFiberSlackLength() const; + + /** @param activTimeConstant The activation time constant, T_{act} (s)*/ + void setActivTimeConstant(double activTimeConstant); + /** @returns The activation time constant, T_{act} (s)*/ + double getActivTimeConstant() const; + + /** @param deactivationTimeConstant The deactivation time constant, T_{deact} (s)*/ + void setDeactivationTimeConstant(double deactivationTimeConstant); + /** @returns The deactivation time constant, T_{deact} (s)*/ + double getDeactivationTimeConstant() const; + + /** @param pennAtOptFiberLength The pennation angel at optimal fiber length, + phi_{opt} (rad)*/ + void setPennAtOptFiberLength(double pennAtOptFiberLength); + /** @returns The pennation angel at optimal fiber length, + phi_{opt} (rad)*/ + double getPennAtOptFiberLength() const; + + /** @param defaultActivation The default muscle activation, (dimensionless)*/ + void setDefaultActivation(double defaultActivation); + /** @returns The default muscle activation, (dimensionless)*/ + double getDefaultActivation() const; + + /** @param fiberLength The default muscle fiber length, ()*/ + void setDefaultFiberLength(double fiberLength); + /** @returns The default muscle fiber length, ()*/ + double getDefaultFiberLength() const; + + + /**@param s The state of the system. + @param activation The muscle activation, (dimensionless)*/ + void setActivation(SimTK::State& s,double activation) const override; + /**@param s The state of the system. + @param projFibLenNorm The muscle fiber length projected on the tendon (normalized by the optimal muscle length), (dimensionless)*/ + void setProjFiberLengthNorm(SimTK::State& s, double projFibLenNorm) const; + + /* Calculate the muscle implicit residual. Returns a state Vec2 + containing the force residual and activation residual. When these + values are equal to zero, the muscle equations are "balanced" and the + muscle is in a valid state. + @param s The state of the system. + @param projFibVelNorm_guess The muscle fiber velocity, projected + in line with tendon and normalized by the optimal fiber length, + (dimensionless) + @param activdot_guess The muscle activation time derivative, + (1/s) + @param excitation The muscle excitation + @returns Muscle residual vector: + \li 0: force (N) + \li 1: activation (unitless)*/ + SimTK::Vec2 getResidual(const SimTK::State& s, + double projFibVelNorm_guess, + double activdot_guess, double excitation) const; + + + //Hacks because cache variables not implemented yet + + /**@param s The state of the system. + @returns The muscle fiber length projected on the tendon .*/ + double getProjFiberLengthNorm(const SimTK::State& s) const; //BTH + /**@param s The state of the system. + @returns The activation of the muscle.*/ + double getActivation(const SimTK::State& s) const override; // BTH + + + //Temporary struct for troubleshooting + struct ImplicitResidual { + double forceResidual = SimTK::NaN; + double activResidual = SimTK::NaN; + double forceTendon = 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; + double F3 = SimTK::NaN; + double F4 = SimTK::NaN; + double F5 = SimTK::NaN; }; + +//============================================================================== +// MUSCLE.H INTERFACE +//============================================================================== + + double computeActuation(const SimTK::State& s) const override final; + + void computeInitialFiberEquilibrium(SimTK::State& s) const override; + + void computeFiberEquilibriumAtZeroVelocity(SimTK::State& s) const + override; + + +//============================================================================= +// COMPUTATION +//============================================================================= + + // TODO: These should move to protected at some point + + // Methods to calculate muscle residual: + + + /** Calculates the muscle residuals. + @param muscleLength Length of muscle (m) + @param projFibLengthNorm Length of the muscle fiber projected inline + with tendon and normalized by the optimal fiber length (m/m) + @param activ The muscle activation + @param projFibVelNorm Velocity of the muscle fiber projected + inline with tendon and normalized by the optimal fiber length (1/s) + @param activdot Time derivative of the muscle activation + @param excitation The muscle control excitation + @param returnJacobians If = true, calculate and return the + Jacobains. default is = false, Jacobians are not calculated + @returns TBD for now see ImplicitResidual structure*/ + ImplicitResidual calcImplicitResidual(double muscleLength, + double projFibLengthNorm, + double activ, + double projFibVelNorm, + double activdot, + double excitation, + bool returnJacobians=false) const; + + /** Calculates the muscle residuals. (State equation form for vector + manipulation) + @param y vector of: + \li 0: projected fiber length + \li 1: muscle activation + @param ydot_guess Vector of the guess for the derivative of y + @param muscleLength Length of muscle (m) + @param excitation Muscle control excitation + @param returnJacobians If = true, calculate and return the + Jacobains. default is = false, Jacobians are not calculated + @returns TBD for now see ImplicitResidual structure*/ + ImplicitResidual calcImplicitResidual(SimTK::Vec2 y, + SimTK::Vec2 ydot_guess, + double muscleLength, + double excitation, + bool returnJacobians=false) const; + + /** Calculates the muscle residuals. (method for getting from + state) + @param s The state of the system. (provides projected fiber + length and muscle activation) + @param projFibVel_guess Guess of the fiber velocity + (projected in-line with the tendon) + @param activdot_guess Guess of the time derivative of activation + @param excitation Muscle control excitation + @param returnJacobians If = true, calculate and return the + Jacobains. default is = false, Jacobians are not calculated + @returns TBD for now see ImplicitResidual structure*/ + ImplicitResidual calcImplicitResidual(const SimTK::State& s, + double projFibVel_guess, + double activdot_guess, + double excitation, + bool returnJacobians=false) const; + + + /** Calculates the time derivative of activation. + @param activation Muscle activation + @param excitation Muscle control excitation + @returns activationDerivative The derivative of the activation wrt time*/ + double calcActivationDerivative(double activation, double excitation) const; + + /** Get the time derivative of activation of the state. + @param s The state of the system. (provides activation and excitation) + @returns activationDerivative The derivative of the activation wrt time*/ + double getActivationDerivative(const SimTK::State& s) const; + + double getProjFiberVelNorm(const SimTK::State& s) const; + + //Convience methods for converting between muscle length, projected + // muscle length, and normalized values. Need to do this because I am + // sticking with the convention that external to the muscle, + // muscle length is the state variable. Inside this muscle's code it + // is using the projected muscle length. + + //TODO: Maybe cache variables? + + /** @param fiberLength The fiber length + @param argsAreNormalized T: The values are normalized by the muscle's + optimal length, F: Not normalized + @returns The projected fiber length + (will be normalized if argsAreNormalized = true)*/ + double fiberLengthToProjectedLength (double fiberLength , + bool argsAreNormalized=false) const; + + + /**@param projFibLen The projected (inline with tendon) length + @param argsAreNormalized T: The values provided are normalized by the + muscle's optimal length, F: Not normalized + @returns The fiber length + (will be normalized if argsAreNormalized = true)*/ + double projFibLenToFiberLength (double projFibLen, + bool argsAreNormalized=false) const; + + /**@param fiberVelocity Velocity of the fiber + @param fiberLength Fiber Length + @param projFiberLength Projected fiber velocity (in-line with tendon) + @param argsAreNormalized T: The values provided are normalized by the + muscle's optimal length, F: Not normalized + @returns The projected fiber velocity + (will be normalized if argsAreNormalized = true)*/ + double fiberVelocityToProjFibVel (double fiberVelocity, + double fiberLength, + double projFiberLength, + bool argsAreNormalized=false) const; + + /**@param fiberVelocity Velocity of the fiber + @param fiberLength Fiber Length + @param argsAreNormalized T: The values provided are normalized by the + muscle's optimal length, false: Not normalized + @returns The projected fiber velocity + (will be normalized if argsAreNormalized = true)*/ + double fiberVelocityToProjFibVel (double fiberVelocity, + double fiberLength, + bool argsAreNormalized=false) const; + + /**@param projFibVel Projected velocity of the fiber + @param fiberLength Fiber Length + @param projFiberLength Projected fiber velocity (in-line with tendon) + @param argsAreNormalized true: The values provided are normalized by the + muscle's optimal length, false: Not normalized + @returns The fiber velocity + (will be normalized if argsAreNormalized = true)*/ + double projFibVelToFiberVelocity(double projFibVel, double fiberLength, + double projFiberLength, + bool argsAreNormalized=false) const; + + /**@param projFibVel Projected velocity of the fiber + @param projFiberLength Projected fiber velocity (in-line with tendon) + @param argsAreNormalized true: The values provided are normalized by the + muscle's optimal length, false: Not normalized + @returns The fiber velocity + (will be normalized if argsAreNormalized = true)*/ + double projFibVelToFiberVelocity(double projFibVel, + double projFiberLength, + bool argsAreNormalized=false) const; + + + //During tests we want to insure that the anlytical jacobians are + // correct. To do this we can compare them to finite difference + //calcualted jacobians. + + /** Calculate Jacobian by finite difference + @param y Vector of: + \li 0: projected fiber length + \li 1: muscle activation + @param ydot Vector of: + \li 0: projected fiber velocity + \li 1: muscle activation time derivative + @param muscleLength The muscle length + @param excitation The muscle control excitation + @param h The peturbation size of the finite step + @returns TBD for now see ImplicitResidual structure*/ + ImplicitResidual calcJacobianByFiniteDiff(SimTK::Vec2 y, + SimTK::Vec2 ydot, + double muscleLength, + double excitation, double stepSize ) const; + /** Calculate Jacobian by finite difference + @param muscleLength Muscle Length + @param projFibLenNorm The projected fiber length (inline with + tendon) and normalized by muscle optimal length. + @param activ The muscle activation + @param projFibVelNorm The projected fiber velocity (inline with + tendon) and normalized by muscle optimal length. + @param activdot The time derivative of the muscle activation + @param excitation The muscle control excitation + @param stepSize The peturbation size of the finite step + @returns TBD for now see ImplicitResidual structure*/ + ImplicitResidual calcJacobianByFiniteDiff(double muscleLength, + double projFibLenNorm, + double activ, + double projFibVelNorm, + double activdot, double excitation, + double stepSize) const; + + + + + /** Calculate the muscle static equilibrium. The muscle length is + static, the fiber velocity is zero, and the activation derivative is + zero. + @param muscleLength The muscle length + @param activ The muscle activation + @returns Vector: + \li 0: projected fiber length normalized + \li 1: muscle force */ + SimTK::Vec2 calcFiberStaticEquilbirum(double muscleLength, + double activ) const; + + /** Calculate the muscle residual at static equilibrium. The muscle + length is static, the fiber velocity is zero, and the activation + derivative is zero. + @param projFibLen The projected fiber length + @param muscleLength The muscle length + @param activ The muscle activation + @returns Vector: + \li 0: muscle force residual + \li 1: derivative of residual wrt activation + \li 2: muscle force*/ + SimTK::Vec3 calcFiberStaticEquilibResidual(double projFibLen, + double muscleLength, + double activ) const; + + + + //TODO: Need doxygen + double calcSolveMuscle(const SimTK::State& s, + double activ, + SimTK::Vector yDotInitialGuess) + const; + + + +//============================================================================= +// PROTECTED METHODS +//============================================================================= + protected: + + void postScale(const SimTK::State& s, const ScaleSet& aScaleSet) override; + + +//============================================================================== +// MUSCLE INTERFACE REQUIREMENTS +//============================================================================== + + + void calcMuscleLengthInfo(const SimTK::State& s, MuscleLengthInfo& mli) const; + /* Not implmented (yet) + void calcMuscleLengthInfo(const SimTK::State& s, + MuscleLengthInfo& mli) const override; + + + void calcFiberVelocityInfo(const SimTK::State& s, + FiberVelocityInfo& fvi) const override; + + + void calcMuscleDynamicsInfo(const SimTK::State& s, + MuscleDynamicsInfo& mdi) const override; + + + void calcMusclePotentialEnergyInfo(const SimTK::State& s, + MusclePotentialEnergyInfo& mpei) + const override; + */ + +//============================================================================== +// MODELCOMPONENT INTERFACE REQUIREMENTS +//============================================================================== + + /** Sets up the ModelComponent from the model, if necessary */ + void extendConnectToModel(Model& model) override; + //Removed per Chris Dembia comments + + /** 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; + + /** Computes state variable derivatives */ + void computeStateVariableDerivatives(const SimTK::State& s) + const override; + + + +//============================================================================= +// DATA +//============================================================================= + + private: + /** construct the new properties and set their default values */ + void constructProperties(); + + + // Rebuilds muscle model if any of its properties have changed. + void extendFinalizeFromProperties() override; + +//============================================================================== +// PRIVATE UTILITY CLASS MEMBERS +//============================================================================== + + // This object defines the pennation model used for this muscle model. Using + // a ClonePtr saves us from having to write a destructor, copy constructor, + // or copy assignment. This will be zeroed on construction, cleaned up on + // destruction, and cloned when copying. + MuscleFixedWidthPennationModel penMdl; + + }; // END of class VandenBogert2011Muscle +} // end of namespace OpenSim +#endif // OPENSIM_THELEN_2003_MUSCLE_H_ 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 diff --git a/OpenSim/Common/Component.cpp b/OpenSim/Common/Component.cpp index 76007f6fc4..baa33bf45f 100644 --- a/OpenSim/Common/Component.cpp +++ b/OpenSim/Common/Component.cpp @@ -323,6 +323,32 @@ void Component::addToSystem(SimTK::MultibodySystem& system) const extendAddToSystem(system); componentsAddToSystem(system); extendAddToSystemAfterSubcomponents(system); + + // Make sure, for this component (not including subcomponents), that either + // all state variables have an implicit form or that none of them do. + + // The reason is that the explicit form is computed for all state variables + // in a component at once--it is not possible to evaluate the explicit form + // for only a single state variable. So if we allowed only some of the + // state variables to have an implicit form, then when computing the trivial + // implicit form (ydot - ydotguess) for the ones that don't have an + // implicit form, we'll waste computation evaluating the explicit form for + // state variables that DO have an implicit form. + // TODO document elsewhere + int numStateVarsWithImplicitForm = 0; + int numStateVars = 0; + for (const auto& it : _namedStateVariableInfo) { + const auto& stateVar = it.second.stateVariable; + numStateVars += 1; + numStateVarsWithImplicitForm += stateVar->hasImplicitForm(); + } + OPENSIM_THROW_IF_FRMOBJ(numStateVarsWithImplicitForm != 0 && + numStateVarsWithImplicitForm != numStateVars, + Exception, "Of the " + std::to_string(numStateVars) + " state variables" + + " in this component, " + + std::to_string(numStateVarsWithImplicitForm) + " have an" + + " implicit form. We require that either all of a component's" + + " state variables have an implicit form or none of them do."); } // Base class implementation of virtual method. @@ -419,6 +445,73 @@ void Component::computeStateVariableDerivatives(const SimTK::State& s) const } } +// TODO void Component::computeImplicitResiduals(const SimTK::State& s) const +// TODO { +// TODO // TODO it's really bad if obtaining the trivial implicit form of +// TODO // auxiliary state variables requires realizing to acceleration (if +// TODO // the explicit fiber velocity is only available at acceleration); then +// TODO // computing the residual will still require computing the explicit +// TODO // multibody dynamics. +// TODO // OPENSIM_THROW_FRMOBJ(Exception, "TODO"); +// TODO // TODO bool requireExplicitForm = false; +// TODO // TODO for (const auto& it : _namedStateVariableInfo) { +// TODO // TODO const StateVariable& sv = *it.second.stateVariable; +// TODO // TODO if (!sv.hasImplicitForm()) requireExplicitForm = true; break; +// TODO // TODO } +// TODO // TODO +// TODO if (!hasFullImplicitForm()) computeStateVariableDerivatives(s); +// TODO +// TODO // TODO it seems that if you want implicit form, you should define all state +// TODO // variables implicitly. Otherwise, we will need to evaluate the explicit +// TODO // dynamics, which will waste time. TODO what about going up the inheritance +// TODO // hierarchy? +// TODO +// TODO // TODO based on what Ajay said, you might actually want to override your base +// TODO // class and thus not call Super::extend...(). So this should maybe be +// TODO // implementCompute...(). +// TODO extendComputeImplicitResiduals(s); +// TODO +// TODO +// TODO // TODO do this before or after extendCompute...()? +// TODO for (const auto& it : _namedStateVariableInfo) { +// TODO const StateVariable& sv = *it.second.stateVariable; +// TODO if (!sv.hasImplicitForm()) { +// TODO const Real yDot = sv.getDerivative(s); +// TODO const Real yDotGuess = sv.getDerivativeGuess(s); +// TODO sv.setImplicitResidual(s, yDot - yDotGuess); +// TODO } +// TODO // TODO set implicit residual for explicit components. +// TODO // TODO ensure that the user set the residual for those providing one. +// TODO } +// TODO } + +void Component::calcImplicitResidualsInternal(const SimTK::State& s, + const SimTK::Vector& yDotGuess, + const SimTK::Vector& lambdaGuess, + SimTK::Vector& residuals) const { + // TODO check size of input arguments? + // TODO assert(residuals.size() == + // TODO very awkward, maybe very inefficient! + SimTK::Array_ indices; + for (const auto& it : _namedStateVariableInfo) { + indices.push_back(it.second.stateVariable->getSystemYIndex()); + } + SimTK::VectorView componentResiduals = residuals.updIndex(indices); + componentResiduals = calcImplicitResidualsLocal(s, yDotGuess, lambdaGuess); +} + +void Component::setImplicitResidual(const std::string& name, + const double& thisResidual, SimTK::VectorView& componentResiduals) + const { + std::map::const_iterator it; + it = _namedStateVariableInfo.find(name); + + OPENSIM_THROW_IF_FRMOBJ(it == _namedStateVariableInfo.end(), + Exception, "State variable '" + name + "' not found."); + + assert(componentResiduals.size() > it->second.order); + componentResiduals[it->second.order] = thisResidual; +} void Component:: addModelingOption(const std::string& optionName, int maxFlagValue) const @@ -438,6 +531,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 +541,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); } @@ -467,6 +561,9 @@ void Component::addStateVariable(Component::StateVariable* stateVariable) const int order = (int)_namedStateVariableInfo.size(); + // TODO "componentVarIndex" duplicates "order" in StateVariableInfo. + // TODO stateVariable->setComponentVarIndex(order); + // assign a "slot" for a state variable by name // state variable index will be invalid by default // upon allocation during realizeTopology the index will be set @@ -1038,6 +1135,127 @@ setDiscreteVariableValue(SimTK::State& s, const std::string& name, double value) } } +bool Component::hasImplicitFormLocal() 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::hasImplicitForm() const { + if (!hasImplicitFormLocal()) return false; + + for (unsigned int i = 0; i<_memberSubcomponents.size(); i++) + if (!_memberSubcomponents[i]->hasImplicitForm()) return false; + + for(unsigned int i=0; i<_propertySubcomponents.size(); i++) + if (!_propertySubcomponents[i]->hasImplicitForm()) return false; + + for (unsigned int i = 0; i<_adoptedSubcomponents.size(); i++) + if (!_adoptedSubcomponents[i]->hasImplicitForm()) return false; + + return true; +} + +Vector Component::calcImplicitResidualsLocal(const SimTK::State& s, + const SimTK::Vector& yDotGuess, + const SimTK::Vector& lambdaGuess + ) const { + // TODO leave in these asserts? + assert(yDotGuess.size() == s.getNY()); + assert(lambdaGuess.size() == s.getNMultipliers()); + + Vector componentResiduals(getNumStateVariablesAddedByComponent()); + computeImplicitResiduals(s, yDotGuess, componentResiduals); + + // Handle the case where the user has not provided the implicit form. + bool computedExplicitForm = false; + for (const auto& it : _namedStateVariableInfo) { + const StateVariable& sv = *it.second.stateVariable; + if (!sv.hasImplicitForm()) { + if (!computedExplicitForm) { + // Evaluate the explicit form for all of this component's + // state variables. + computeStateVariableDerivatives(s); + } + const Real svYDot = sv.getDerivative(s); + const Real svYDotGuess = yDotGuess[sv.getSystemYIndex()]; + componentResiduals[it.second.order] = svYDot - svYDotGuess; + // TODO could be cleaner. + } + } + return componentResiduals; +} + +const double& Component::getStateVariableDerivativeGuess( + const std::string& name, const SimTK::Vector& allYDotGuess) const { + // TODO make sure the guess exists? + + std::map::const_iterator it; + it = _namedStateVariableInfo.find(name); + + if (it == _namedStateVariableInfo.end()) { + const StateVariable* sv = it->second.stateVariable.get(); + assert(allYDotGuess.size() > sv->getSystemYIndex()); + return allYDotGuess[sv->getSystemYIndex()]; + // TODO sv->getDerivativeGuess2(allYDotGuess); + } + else { + const StateVariable* rsv = findStateVariable(name); + if (rsv) { + assert(allYDotGuess.size() > rsv->getSystemYIndex()); + return allYDotGuess[rsv->getSystemYIndex()]; + // TODO rsv->getDerivativeGuess2(allYDotGuess); + } + } + + OPENSIM_THROW_FRMOBJ(Exception, "State variable '" + name + "' not found."); +} + +void Component::setStateVariableDerivativeGuess(const std::string& name, + const double& derivGuess, + SimTK::Vector& allYDotGuess + ) const +{ + // TODO make sure the guess exists? + + std::map::const_iterator it; + it = _namedStateVariableInfo.find(name); + + if (it == _namedStateVariableInfo.end()) { + const StateVariable* sv = it->second.stateVariable.get(); + assert(allYDotGuess.size() > sv->getSystemYIndex()); + allYDotGuess[sv->getSystemYIndex()] = derivGuess; + return; + } + else { + const StateVariable* rsv = findStateVariable(name); + if (rsv) { + assert(allYDotGuess.size() > rsv->getSystemYIndex()); + allYDotGuess[rsv->getSystemYIndex()] = derivGuess; + return; + } + } + + OPENSIM_THROW_FRMOBJ(Exception, "State variable '" + name + "' not found."); +} + +const double& Component::getImplicitResidual(const std::string& name, + const SimTK::Vector& allResiduals + ) const { + std::map::const_iterator it; + it = _namedStateVariableInfo.find(name); + + OPENSIM_THROW_IF_FRMOBJ(it == _namedStateVariableInfo.end(), + Exception, "State variable '" + name + "' not found."); + + // TODO should throw a real exception. + assert(allResiduals.size() > it->second.stateVariable->getSystemYIndex()); + return allResiduals[it->second.stateVariable->getSystemYIndex()]; +} + bool Component::constructOutputForStateVariable(const std::string& name) { auto func = [name](const Component* comp, @@ -1286,9 +1504,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 +1540,18 @@ 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) { + // 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); + } +} //------------------------------------------------------------------------------ // REALIZE ACCELERATION @@ -1372,8 +1602,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 +1655,15 @@ void Component::AddedStateVariable:: return getOwner().setCacheVariableValue(state, getName()+"_deriv", deriv); } +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 { diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index 7e1b3a16f6..16abfd395f 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -1289,7 +1289,47 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); } } // End of Model Component State Accessors. - //@} + //@} + + + /** @name Component interface for implicit form of dynamics */ + // @{ + /** Does this component provide implicit differential equations for each + of its state variables? This method does not check subcomponents. + @ingroup implicitdiffeq */ + bool hasImplicitFormLocal() const; + /** Do this component and its subcomponents provide implicit differential + equations for each of their state variables? + @ingroup implicitdiffeq */ + bool hasImplicitForm() const; + + /** TODO + @ingroup implicitdiffeq */ + SimTK::Vector calcImplicitResidualsLocal(const SimTK::State& s, + const SimTK::Vector& yDotGuess, const SimTK::Vector& lambdaGuess) + const; + + /** TODO + @ingroup implicitdiffeq */ + // TODO where to put this? + const double& getStateVariableDerivativeGuess( + const std::string& name, const SimTK::Vector& allYDotGuess) const; + + /** TODO + @ingroup implicitdiffeq */ + // TODO where to put this? + void setStateVariableDerivativeGuess(const std::string& name, + const double& derivGuess, + SimTK::Vector& allYDotGuess) const; + + /** TODO + @ingroup implicitdiffeq */ + // TODO where to put this method? + // TODO "getSingleImplicitResidual"? getImplicitResidualEntry? ByName + // TODO getImplicitResidualByName(). + const double& getImplicitResidual(const std::string& name, + const SimTK::Vector& allResiduals) const; + // @} /** @name Dump debugging information to the console */ /// @{ @@ -1626,6 +1666,22 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); */ void setStateVariableDerivativeValue(const SimTK::State& state, const std::string& name, double deriv) const; + + /** TODO + @ingroup implicitdiffeq */ + virtual void computeImplicitResiduals(const SimTK::State& s, + const SimTK::Vector& allYDotGuess, + SimTK::Vector& componentResiduals) const {} + /** TODO + @ingroup implicitdiffeq */ + void setImplicitResidual(const std::string& name, const double& thisResidual, + SimTK::VectorView& componentResiduals) const; + /** TODO don't document? + @ingroup implicitdiffeq */ + void calcImplicitResidualsInternal(const SimTK::State& s, + const SimTK::Vector& yDotGuess, + const SimTK::Vector& lambdaGuess, + SimTK::Vector& residuals) const; // End of Component Extension Interface (protected virtuals). @@ -1730,12 +1786,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 +2345,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 +2368,22 @@ 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; } + // TODO void setComponentVarIndex(int index) { componentVarIndex = index; } + 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; @@ -2324,8 +2392,11 @@ 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; - + private: + virtual SimTK::SystemYIndex implementDetermineSystemYIndex( + const SimTK::State& s) const = 0; + std::string name; SimTK::ReferencePtr owner; @@ -2338,6 +2409,12 @@ 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 The index of this state variable in the Component that contains it. + // TODO Used for setting the appropriate entry of the residual. + // TODO int componentVarIndex = SimTK::NaN; + + // TODO + bool implicitForm; // flag indicating if state variable is hidden to the outside world bool hidden; @@ -2402,19 +2479,20 @@ 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) {} + 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,15 +2501,18 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); double getDerivative(const SimTK::State& state) const override; void setDerivative(const SimTK::State& state, double deriv) 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. 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/OpenSimDoxygenMain.h b/OpenSim/OpenSimDoxygenMain.h index e5d1e28714..931f53828d 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,20 @@ 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 + + @see OpenSim::InverseDynamicsSolver + SimTK::SimbodyMatterSubsystem::calcResidualForce +*/ + /** @mainpage Overview \if developer diff --git a/OpenSim/Sandbox/HumphreysImplicitDynamics/forwardImplicitTest.cpp b/OpenSim/Sandbox/HumphreysImplicitDynamics/forwardImplicitTest.cpp new file mode 100644 index 0000000000..b8371a10c6 --- /dev/null +++ b/OpenSim/Sandbox/HumphreysImplicitDynamics/forwardImplicitTest.cpp @@ -0,0 +1,368 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: forwardImplicitTest.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * * + * 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. * + * -------------------------------------------------------------------------- */ + +/* + * Below is an example of an OpenSim application that provides its own + * main() routine. + */ + +//============================================================================== +//============================================================================== +#include + +using namespace OpenSim; +using namespace SimTK; +using namespace std; + +struct momentArmAndLength { + SimTK::Vec2 length; + SimTK::Vec2 momentArm; +}; + + + + +struct bodyResidual { + double forceResidual = SimTK::NaN; + double velocityResidual = SimTK::NaN; + double forceMass = SimTK::NaN; + SimTK::Mat22 df_dy = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; + SimTK::Mat22 df_dydot = {SimTK::NaN,SimTK::NaN,SimTK::NaN,SimTK::NaN}; +}; + + + + +// Get the Residual of the mass body. +bodyResidual calcBodyImplicitResidual(Model& osimModel, State& s, double velocityGuess, double accelGuess, Vec2 muscleForces) +{ + + // Tried to use new implicit interface: + // (going to just use equation implementation for now) + //const double qDotGuess = 0.0; const double uDotGuess = 0.0; + // Set derivative guess. + //coord.setStateVariableDerivativeGuess(s, "value", qDotGuess); + //coord.setStateVariableDerivativeGuess(s, "speed", uDotGuess); + + /*Returns: + /home/humphreysb/vanDenBogertMuscleTestSource/vanDenBogertMuscleTest.cpp:220:43: error: no viable conversion from 'SimTK::State' to + 'const std::string' (aka 'const basic_string') + coord.setStateVariableDerivativeGuess(s, "value", qDotGuess); + */ + + // Get the "Rigid Body" Dynamics + + Vec2 momentArms={-1, 1}; //Moment Arms are hard coded here. + //Ajay suggests trying to see cost having + //opensim calculate them. + + //Calculate the mass force residual (for now by equations) + double m=20; + double forceMass = m*accelGuess; //F=ma + double forceRes = momentArms[0]*muscleForces[0] + momentArms[1]*muscleForces[1] - forceMass; + + //Calculate the Velocity Residual + const CoordinateSet& modelCoordinateSet = osimModel.getCoordinateSet(); + double velRes = modelCoordinateSet[0].getSpeedValue(s) - velocityGuess; + + //Analytical Jacobian + SimTK::Mat22 df_dydot = {0,-m,-1,0}; + + //Create Output + bodyResidual Results; + Results.forceResidual=forceRes; + Results.velocityResidual=velRes; + Results.df_dydot=df_dydot; + + return Results; + +} + + +//Compute the residuals on the full model. This also noes the forward euler +// to go from yPGuess (y-prime guess) to ydotGuess +SimTK::Vector computeImplcitResiduals(Model& osimModel, State& s, Vector yPGuess, Vector u, double h){ + + + //Need to calculate yDotGuess using forward euler appoximation: ydot=(yp-y)/h + // (Note that I did this in 2 steps in my MATLAB implmentation. Here + // I am doing it in one function so I only need to dynamic cast the + // muscle once.) + + // So let's get y (from s): + + //First get the muscle and recast + const Set& muscleSet=osimModel.getMuscles(); + const auto* muscle1 = dynamic_cast(&muscleSet[0]); + const auto* muscle2 = dynamic_cast(&muscleSet[1]); + + Vector y(6); + y[0]=muscle1->getFiberLength(s); + y[1]=muscle1->getActivation(s); + y[2]=muscle2->getFiberLength(s); + y[3]=muscle2->getActivation(s); + + const CoordinateSet& modelCoordinateSet = osimModel.getCoordinateSet(); + y[4]=modelCoordinateSet[0].getValue(s); + y[5]=modelCoordinateSet[0].getSpeedValue(s); + + // Perform Forward Approximation + Vector yDotGuess(6); + yDotGuess=(yPGuess-y)/h; + + + //Now let's get the residuals + // Muscle1's Residuals + const VandenBogert2011Muscle::ImplicitResidual muscle1Residual =muscle1->calcImplicitResidual(s, yDotGuess[0], yDotGuess[1], u[0],0); + //Muscle2's Residuals + const VandenBogert2011Muscle::ImplicitResidual muscle2Residual =muscle2->calcImplicitResidual(s, yDotGuess[2], yDotGuess[3], u[1],0); + + // Mass Residual + Vec2 muscleForces = {muscle1Residual.forceTendon, muscle2Residual.forceTendon}; + bodyResidual massResidual=calcBodyImplicitResidual(osimModel, s, yDotGuess[4], yDotGuess[5], muscleForces); + + // Residual Vector + SimTK::Vector residuals(6); + residuals[0]=muscle1Residual.forceResidual, + residuals[1]=muscle1Residual.activResidual, + residuals[2]=muscle2Residual.forceResidual, + residuals[3]=muscle2Residual.activResidual, + residuals[4]=massResidual.forceResidual, + residuals[5]=massResidual.velocityResidual; + + return residuals; +} + + + + + +// ============================================================================= +// ImplicitSystemDerivativeSolver +// (adapted from Chris's testImplicitDifferentialEquations.cpp) +// ============================================================================= + +class ImplicitSystemDerivativeSolver : public OptimizerSystem { +public: + + /* Constructor class. Parameters passed are accessed in the objectiveFunc() class. */ + ImplicitSystemDerivativeSolver(int numParameters, State& s, Model& model, Vector& u, double h): + OptimizerSystem(numParameters), + numControls(numParameters), + si(s), + osimModel(model), + excitation(u), + timestep(h) + {} + + int objectiveFunc(const Vector &new_yDotGuess, bool new_params, Real& f) const override { + // No objective function + f = 0; + return 0; + } + + int constraintFunc(const Vector &new_yDotGuess, bool newParams, Vector& constraints) const override { + //The constraints function is just residuals + Vector cons=computeImplcitResiduals(osimModel,si,new_yDotGuess,excitation,timestep); + constraints=cons; + return 0; + } + +private: + int numControls; + State& si; + Model& osimModel; + Vector& excitation; + double timestep; +}; + + + + + +// Perform a forward dynamic step +SimTK::Vector forwardImplict(Model& osimModel, State& s, Vector yPGuess, Vector u, double h) { + + int numControls =6; + + // There must be a better way of doing this instead of creating a new + // solver at each time step (how do I handle u) + ImplicitSystemDerivativeSolver sys(6, s, osimModel, u, h); + + Vector lower_bounds(6); + lower_bounds[0]=0.0; + lower_bounds[1]=0.0; + lower_bounds[2]=0.0; + lower_bounds[3]=0.0; + lower_bounds[4]=-1.0; + lower_bounds[5]=-SimTK::Infinity; + + Vector upper_bounds(6); + upper_bounds[0] = 10.0; + upper_bounds[1] = 1.0; + upper_bounds[2] = 10.0; + upper_bounds[3] = 1.0; + upper_bounds[4] = 10.0; + upper_bounds[5] = SimTK::Infinity; + + sys.setParameterLimits( lower_bounds, upper_bounds ); + + Optimizer opt(sys, SimTK::InteriorPoint); //Create the optimizer + + // Specify settings for the optimizer + opt.setConvergenceTolerance(0.1); + opt.useNumericalGradient(true, 1e-5); + opt.useNumericalJacobian(true); + opt.setMaxIterations(100); + opt.setLimitedMemoryHistory(500); + + + + opt.optimize(yPGuess); // Optimize it! + + + //Now that we have a good new solution, let's update the state to match + //First get the muscle and recast + const Set& muscleSet=osimModel.getMuscles(); + const auto* muscle1 = dynamic_cast(&muscleSet[0]); + const auto* muscle2 = dynamic_cast(&muscleSet[1]); + muscle1->setFiberLength(s,yPGuess[0]); + muscle1->setActivation(s,yPGuess[1]); + muscle2->setFiberLength(s,yPGuess[2]); + muscle2->setActivation(s,yPGuess[3]); + //Now the mass + const CoordinateSet& modelCoordinateSet = osimModel.getCoordinateSet(); + modelCoordinateSet[0].setValue(s,yPGuess[4]); + modelCoordinateSet[0].setSpeedValue(s,yPGuess[5]); + + return yPGuess; + +} + +//______________________________________________________________________________ +/** + */ +int main() +{ + + + //Create a model + // BLOCK BODY + const double mass = 20.0; + Model model; model.setName("tug_of_war"); + model.setGravity(Vec3(0, 0, 0)); // No gravity + 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); + // Get a reference to the model's ground frame + Ground& ground = model.updGround(); + + //Add muscles + VandenBogert2011Muscle* muscle1 = new VandenBogert2011Muscle(); + VandenBogert2011Muscle* muscle2 = new VandenBogert2011Muscle(); + + // Specify the paths for the two muscles + // Path for muscle 1 + muscle1->addNewPathPoint("muscle1-point1", ground, Vec3(0.0,0.05,-0.35)); + muscle1->addNewPathPoint("muscle1-point2", *body, Vec3(0.0,0.0,-0.05)); + // Path for muscle 2 + muscle2->addNewPathPoint("muscle2-point1", ground, Vec3(0.0,0.05,0.35)); + muscle2->addNewPathPoint("muscle2-point2", *body, Vec3(0.0,0.0,0.05)); + + // Add the two muscles (as forces) to the model + model.addForce(muscle1); + model.addForce(muscle2); + + model.setUseVisualizer(false); + + // Initialize the system and get the default state + SimTK::State& s = model.initSystem(); + + SimTK::Vec2 activInit = {0.2,0.2}; + muscle1->setActivation(s,activInit[0]); + muscle2->setActivation(s,activInit[1]); + + + //Static Equilibrate Muscle + //model.equilibrateMuscles(s); + muscle1->computeInitialFiberEquilibrium(s); + muscle2->computeInitialFiberEquilibrium(s); + + // Double check that residuals are 0 + SimTK::Vec2 muscle1Res = muscle1->getResidual(s,0.0,0.0,activInit[0]); + SimTK::Vec2 muscle2Res = muscle2->getResidual(s,0.0,0.0,activInit[1]); + + double ml1 = muscle1->getFiberLength(s); + double ml2 = muscle2->getFiberLength(s); + double a1 = muscle1->getActivation(s); + double a2 = muscle2->getActivation(s); + + cout << "muscleLength1: " << ml1 << " a1: " << a1 <0.01 +% error('Exact and Finite Diff Jacobians do not seem to agree.') +% end + +useFiniteDiff=useFiniteDiffIn; + + +%IPOPT Initial Parameters +options.lb=[0 0 0 0 -1 -Inf]; +options.ub=[Inf 1 Inf 1 1 Inf]; +options.cl=[0 0 0 0 0 0]; +options.cu=[0 0 0 0 0 0]; + +funcs.objective = @ipoptObj; +funcs.constraints = @ipoptConstraints; +funcs.gradient = @ipoptGradObj; +funcs.jacobian = @ipoptConstraintsJacobian; +funcs.jacobianstructure = @() sparse(ones(6,6)); + +options.ipopt.jac_c_constant = 'yes'; +options.ipopt.hessian_approximation = 'limited-memory'; +options.ipopt.mu_strategy = 'adaptive'; +options.ipopt.tol = 1e-5; +options.ipopt.print_level = 0; + + +%Loop through the times steps to do forward solution +n=size(controlSig,2); +for i=1:n + + %Set the controls + u=controlSig(:,i); + + if solver==0 + [yp, info] = ipopt(ypGuess,funcs,options); + else + dif=useFiniteDiff; + [yp] = nrsolve(@implicitDynamics,dif,ypGuess); + end + + %If other parameters are needed: + %[residuals,muscleForces,massForce,yvec]=implicitDynamics(y2); + + %Store the results for later + yout(:,i)=y; + ydotout(:,i)=(yp-y)/h; + + + ypGuess=yp; %Make the next guess the current value + y=yp; %Make the current value the new starting point + + fprintf('Step %g of %g.\n',i,n) + +end + + +end + + +%% ==================================================== +%---------------Approximate the system dynamics-------------------% +function [residuals, df_dyp, muscleForces,massForce,yvec]=implicitDynamics(yp) + +%Run implicit dynamics of the full system (this is a forward euler +%appoximation of the system.) Note that "p" here is for prime, which means +%the guess or estimate. +% +%Inputs: +% residuals - residuals of the implicit equation, row vector of: +% [Force Res Muscle 1; Act Muscle 1; Force Res Muscle 2; +% Act Res Muscle 2; Force Residual Body; Velocity Res Body +% +%Outputs: +% df_dyp - Jacobian from yp to residuals +% muscleForce - force in each muscle [N] +% massForce - mass "acceleration" force [N] +% yvec - large vector of [y;yp;ydot] for troubleshooting/plotting + +global y u h + +ypdot=(yp-y)/h; %<-Forward Euler + +%Give the model the current state, the guess of ydot, and the controls +[residuals,muscleForces,massForce, df_dy, df_dydot]=tug_of_war(y,ypdot,u); + +% The system jacobian, J = df_dypdot = df_dydot * df_dydot/dypdot +% ^--- = 1/h + + +df_dyp=df_dydot/h; + + +yvec=[y;yp;ypdot]; + + + + +end + + +%% ==================================================== +%---------------muscle length and moments-------------------% +function [lm,r]=muscleLengthAndMomentArm(q) +%To book keep where moment arms will be needed +% Inputs +% q - joint position +% Outputs +% lm - muscle length +% r - moment arm +lm(1)=q+0.3; +r(1)=-1; %moment arm = -dlm_dq + +lm(2)=-q+0.3; +r(2)=1; +end + +%% ==================================================== +%---------------System Dynamics-------------------% +function [residuals, muscleForces, massForce, df_dy, df_dydot]=tug_of_war(y,ydot,u) +%Calculate the system dyanmics (the mass and muscles as a system) +% +%Inputs: +% y - state vector of full system: +% [muscleLengthNorm1; activation1; muscleLengthNorm2; activation2;... +% mass position; mass velocity] +% ydot - state vector derivative +% u - muscles excitation [excitation1; excitation2] +% +%Outputs: +% residuals - implicit residuals: +% [muscleForce1; muscleActivation1;muscleForce2; muscleActivation2;.. +% forceOnMass;massVeclocity] +% muscleForce - force applied by each muscle +% massForce - rigid body force on mass (accleration driven) +% df_dy - Jacobian of residuals wrt to state y +% df_dydot - Jacobian of residuals wrt to state ydot +% + +[muscleLength, momentArm]=muscleLengthAndMomentArm(y(5)); + +% Calculate the Residuals first for the 2 muscles and then the mass +[muscle1Res1, forceMuscle1, df_dy_Muscle1, df_dydot_Muscle1]=calcmuscle1Residual(y(1:2),ydot(1:2),u(1),muscleLength(1)); +[muscle1Res2, forceMuscle2, df_dy_Muscle2, df_dydot_Muscle2]=calcmuscle1Residual(y(3:4),ydot(3:4),u(2),muscleLength(2)); +[massRes, massForce, df_dy_Mass, df_dydot_Mass]=calcMassResidual(y(5:6),ydot(5:6),[forceMuscle1;forceMuscle2],momentArm); + +residuals=[muscle1Res1;muscle1Res2;massRes]; +muscleForces=[forceMuscle1;forceMuscle2]; + +%Assemble Jacobians +df_dy=zeros(6,6); +df_dydot=zeros(6,6); + +df_dy(1:2,1:2)=df_dy_Muscle1; +df_dydot(1:2,1:2)=df_dydot_Muscle1; +df_dy(3:4,3:4)=df_dy_Muscle2; +df_dydot(3:4,3:4)=df_dydot_Muscle2; +df_dy(5:6,5:6)=df_dy_Mass; +df_dydot(5:6,5:6)=df_dydot_Mass; + +end + + +%% ==================================================== +%---------------Muscle Dynamics-------------------% +function [muscleRes,forceMuscle,df_dy,df_dydot]=calcmuscle1Residual(y,ydot,u,muscleLength) +%Calculate the muscle residuals. +% +%Inputs: +% y - state vector: [muscleLengthNorm; activation] +% ydot - state vector derivative +% u - muscle excitation +% muscleLength - length of muscle [m] +%Outputs: +% muscleRes - muscle residual (force;activation) +% forceMuscle - force in Muscle [N] +% df_dy - residual jacobian (residuals/y) +% df_dydot - residual jacobian (residuals/ydot) + + +% Orginize variables for OpenSim API +import org.opensim.modeling.* +global muscle + +y=Vec2(y(1),y(2)); +ydot=Vec2(ydot(1),ydot(2)); + +%Run the Opensim Muscle code +out=muscle.calcImplicitResidual(y,ydot,muscleLength,u,1); + +%Get the output +forceRes=out.getForceResidual; +actRes=out.getActivResidual; +muscleRes=[forceRes;actRes]; + +forceMuscle=out.getForceTendon; + +[df_dy,muscle]=unFlattenMat22(out.getDf_dy(),muscle); %Need to unhack this with Chris's fixed Mat22 +[df_dydot,muscle]=unFlattenMat22(out.getDf_dydot(),muscle); %Need to unhack this with Chris's fixed Mat22 + +end + + +%% ==================================================== +%---------------Rigid Body Dynamics-------------------% +function [massRes,massForce,df_dy,df_dydot]=calcMassResidual(y,ydot,F,momentArm) +%% Calculate the mass residual +%Outputs +% massRes - mass Residual, 2x1: [force residual; velocity residual] +% massForce - rigid body force on mass +% df_dy - Jacobian between residuals and y state vector +% df_dydot - Jacobian between residuals and ydot state vector + +%Inputs +% y - state vector of [x;v]; +% ydot - state vector [xdot,vdotdot] +% momentArms - 2x1 vector with muscle moment arms + +m=10; +massForce = m*ydot(2); %mass * acceleration + +%Fres= muscle forces * moment arms - mass*accel +forceRes= momentArm(1)*F(1) + momentArm(2)*F(2) - massForce; + +velRes=y(2)-ydot(1); % Velocity equation + +massRes=[forceRes;velRes]; + +df_dy=[NaN NaN;NaN NaN]; %<<<< Not needed yet. Place holder +df_dydot=[0 -m; -1 0]; + +end + +%% ==================================================== +%---------------Jacobian by Finite Diff--------------% +function J=jacFiniteDiff(x,func,dx) + +%Calculate the Jacobians by finite diff +% +%Inputs: +% x - input vector +% func - handle to function to calculate the jacobian of +% dx- finite difference size +% +% Outputs: +% J - Jacoboan df/dx (inputs in columns) + +x=x(:); +m=length(x); + +yo=func(x); +yo=yo(:)'; + +for i=1:m + xTemp=x; + xTemp(i)=xTemp(i)+dx; + y=func(xTemp); + y=y(:)'; + J(:,i)=(y-yo)/dx; +end + +%J=J'; +end + +%% ==================================================== +%----------------------NR Solver---------------------% +function [x,info] = nrsolve(func,useFiniteDiff,x,options) +% Solves f(x)=0 using the N-dimensional Newton-Raphson method +% with half-step backtracking +% +% Input: +% func.............Function handle for F(x) +% x................Initial guess +% options..........Struct with solver options, can have these fields: +% tolerance... Tolerance +% print........Controls printing (0: none, 1: print everything) +% +% Output: +% x................Solution + + +%set up toptions +if nargin<4 %Default options + options.maxIter=100; + options.tolerance=1e-4; +end + +optFields=fieldnames(options); +for i=1:length(optFields) + opt.(optFields{i})=options.(optFields{i}); +end + +% evaluate F(x) at the initial guess +[F,J] = func(x); +%fprintf('Initial guess: x=%f %f F=%f %f\n',x,F); + +% do the Newton iterations +for i = 1:opt.maxIter + + if useFiniteDiff + %do finite diff jacobian + h=1e-7; + J=jacFiniteDiff(x,func,h); + else + [~,J] = func(x); % and the Jacobian matrix dF/dx + + end + + dx = -J\F; % solve dx from J*dx = -F + newx = x + dx; % do the Newton step + [newF,newJ] = func(newx); + if (norm(dx) < opt.tolerance) + x=newx; + break; % if within tolerance, jump out of the "for" + end + %fprintf(' Iteration %d: dx=%10.6f %10.6f\n',i,dx); + %fprintf(' x=%10.6f %10.6f\n',newx); + %fprintf(' F=%10.6f %10.6f (norm %10.6f)\n',newF,norm(newF)); + + % backtrack by half if the norm of F did not decrease during the step + while norm(newF) >= norm(F) + dx = dx/2; % reduce the Newton step by half + if norm(dx) < opt.tolerance + error('step became too small during backtracking') + end + newx = x + dx; % try the reduced step + newF = func(newx); + %fprintf(' Backtracking: dx=%10.6f %10.6f norm(F)=%10.6f\n',dx,norm(newF)); + end + + % starting point for next iteration + F = newF; + x = newx; +end + +% if we did the maximum number of iterations, we probably failed +if i == opt.maxIter; + error('nrsolve: maximum number of iterations was done.') +end +end + +%% ==================================================== +%%-------- Functions needed by IPOPT ---------------% +function obj=ipoptObj(x) +%IPOPT Objective Function (All 0) +obj=0; +end + +function gradObj=ipoptGradObj(x) +%IPOPT Objectove Gradiant +gradObj=[1;1;1;1;1;1]; +end + +function constraints=ipoptConstraints(yp) +%IPOPT Contsraints Functions +% Drive all residuals to 0 +global y u h +residuals=implicitDynamics(yp); +constraints=residuals; +end + +function constJac=ipoptConstraintsJacobian(yp) +%IPOPT Constraints Jacobian +global y u h useFiniteDiff +if useFiniteDiff + df_dyp=jacFiniteDiff(yp,@implicitDynamics,1e-7); +else + [~, df_dyp]=implicitDynamics(yp); +end +constJac=sparse(df_dyp); +end + + + diff --git a/OpenSim/Sandbox/HumphreysImplicitDynamics/unFlattenMat22.m b/OpenSim/Sandbox/HumphreysImplicitDynamics/unFlattenMat22.m new file mode 100644 index 0000000000..1a8cd43001 --- /dev/null +++ b/OpenSim/Sandbox/HumphreysImplicitDynamics/unFlattenMat22.m @@ -0,0 +1,11 @@ +function [matOut,muscle]=unFlattenMat22(matIn,muscle) + +import org.opensim.modeling.* + +b=muscle.flattenMat22(matIn); + +for i=0:3 + matOut(i+1)=b.get(i); +end + +matOut=reshape(matOut,2,2)'; \ No newline at end of file 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 c27da59ec3..3e8162eac1 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" @@ -768,6 +769,206 @@ 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, 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::Dynamics, nans); + _yDotGuessIndex = yDotGuess.getSubsystemMeasureIndex(); + + // TODO how to set size of lambdaGuess? shouldn't be numStateVars. + Measure_::Variable lambdaGuess(_system->updDefaultSubsystem(), + Stage::Dynamics, Vector(0)); + _lambdaGuessIndex = lambdaGuess.getSubsystemMeasureIndex(); + + // Storing the residual. + // We can only compute the residual once realized to Dynamics, since + // 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). + // 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::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. + auto measure = Measure_::Result::getAs( + _system->getDefaultSubsystem().getMeasure(_implicitResidualIndex)); + + // Multibody states. + + + 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? + const auto& yDotGuess = getYDotGuess(state); + const auto& lambdaGuess = getMultipliersGuess(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 alternatives: + // TODO getMatterSubsystem().calcQDot(...) + // 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, lambdaGuess); + } + + // 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); + } + + measure.markAsValid(state); // TODO + } + + return measure.getValue(state); +} +*/ +// TODO based on conversations with Sherm and Brad, we should not compute +// constraint errors here; users can compute them on their own if they'd like. +// But different methods may require different ways of solving the constraint +// errors, so we shouldn't try to do it here. + +void Model::calcImplicitResiduals(const SimTK::State& state, + const SimTK::Vector& yDotGuess, const SimTK::Vector& lambdaGuess, + SimTK::Vector& residuals) const { + + // Check sizes of yDotGuess and lambdaGuess. + OPENSIM_THROW_IF_FRMOBJ( yDotGuess.size() != 0 + && yDotGuess.size() != state.getNY(), + Exception, "Length of yDotGuess argument was " + + std::to_string(yDotGuess.size()) + + " but should have been either zero or the same as the" + + " number of mobilities nu=" + + std::to_string(state.getNY()) + "."); + OPENSIM_THROW_IF_FRMOBJ( lambdaGuess.size() != 0 + && lambdaGuess.size() != state.getNMultipliers(), + Exception, "Length of lambdaGuess argument was " + + std::to_string(lambdaGuess.size()) + + " but should have been either zero or the same as the" + + " number of acceleration-level constraints m=" + + std::to_string(state.getNMultipliers()) + "."); + + + residuals.resize(state.getNY()); + if (state.getNY() == 0) return; + + // Multibody states. + // ================= + if (state.getNQ() > 0) { + + // N u - qdot + // ---------- + Vector qDot; // TODO = state.getQDot() TODO profile? + getMatterSubsystem().calcQDot(state, state.getU(), qDot); + VectorView qResiduals = residuals(state.getQStart(), state.getNQ()); + if (yDotGuess.size() == 0) { + qResiduals = qDot; // interpret qDotGuess as 0. + } else { + const VectorView& qDotGuess = yDotGuess(0, state.getNQ()); + qResiduals = qDot - qDotGuess; + } + // TODO does realizing to Velocity set QDot? + // TODO do we want to use QDot here?? I think so, otherwise + // we are recomputing something that should already be cached for us. + // The alternatives: + // TODO getMatterSubsystem().calcQDot(...) + + // M u_dot - f + // ----------- + Vector uDotGuess; + if (yDotGuess.size() > 0) { + uDotGuess.viewAssign(yDotGuess(state.getUStart(), state.getNU())); + } + InverseDynamicsSolver idSolver(*this); + VectorView uResiduals = residuals(state.getUStart(), state.getNU()); + // TODO is there an unnecessary copy here, and is it expensive? + uResiduals = idSolver.solve(state, uDotGuess, lambdaGuess); + } + + + // Auxiliary states + // ================ + if (state.getNZ() > 0) { + // We can't expect the auxiliary states to handle empty guess vectors. + // So if the guess vectors are empty, we turn them into + // appropriately-sized vectors of 0. + Vector yDotGuessSpace; + Vector lambdaGuessSpace; + const Vector* yDotGuessp; + const Vector* lambdaGuessp; + + if (state.getNY() == 0 || yDotGuess.size() != 0) { + yDotGuessp = &yDotGuess; + } else { + yDotGuessSpace = Vector(state.getNY(), 0.0); + yDotGuessp = &yDotGuessSpace; + } + if (state.getNMultipliers() == 0 || lambdaGuess.size() != 0) { + lambdaGuessp = &lambdaGuess; + } else { + lambdaGuessSpace = Vector(state.getNMultipliers(), 0.0); + lambdaGuessp = &lambdaGuessSpace; + } + + // Compute residuals for auxiliary state variables. + for (const auto& comp : getComponentList()) { + comp.calcImplicitResidualsInternal(state, yDotGuess, lambdaGuess, + residuals); + } + } +} + //_____________________________________________________________________________ /** diff --git a/OpenSim/Simulation/Model/Model.h b/OpenSim/Simulation/Model/Model.h index 2193cd30f6..09e6002e80 100644 --- a/OpenSim/Simulation/Model/Model.h +++ b/OpenSim/Simulation/Model/Model.h @@ -793,6 +793,81 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Model, ModelComponent); double calcPotentialEnergy(const SimTK::State &s) const { return getMultibodySystem().calcPotentialEnergy(s); } + + /** Computes the residual (error) in the implicit form of all of the + dynamics for this model. The dynamics of a model are typically expressed in + explicit form as \f$ \dot{y} = f_\mathrm{exp}(t, y) \f$. + It is sometimes preferable to express the dynamics in implicit form + \f$ f_\mathrm{imp}(t, y, \dot{y}) = 0 \f$. + This function returns \f$ f_\mathrm{imp} \f$. So instead of getting the + derivative, you provide an estimate of the derivative and this function + tells you how wrong you are. For the multibody system, this is the same as + inverse dynamics. + + This is an operator and does not affect the state's cache. This function + does not use SimTK::State::getYDot() or SimTK::State::getMultipliers(). + + Most components provide their (auxiliary) dynamics only in explicit form + \f$ \dot{z} = f_\mathrm{z,exp}(t, y) \f$; for those components, OpenSim + computes a trivial implicit form as + \f$ f_\mathrm{z,imp}(t,y,\dot{y}) = f_\mathrm{z,exp}(t,y) - \dot{z} \f$. + All models can provide an implicit form, but it is much more useful if + its components provide their own implicit form. + + @see implicitdiffeq + + @par Examples + + @code{.cpp} + model.realizeDynamics(state); // Must compute forces to apply to model. + Vector yDotGuess = createYDotGuess(); // Perhaps a guess from an optimizer. + Vector lambdaGuess = createMultipliersGuess(); // Guess from an optimizer. + Vector residuals; // This will hold the output. + model.calcImplicitResiduals(state, yDotGuess, lambdaGuess, residuals); + assert(residuals.size() == state.getNY()); + @endcode + + If you provide the state derivatives and multipliers from forward dynamics, + then the residual should be zero. + @code{.cpp} + model.realizeAcceleration(state); + Vector residuals; + model.calcImplicitResiduals(state, state.getYDot(), state.getMultipliers(), + residuals); + @endcode + + + @param[in] state + The state at which the residual is computed. The state must be realized + to SimTK::Stage::Dynamics. This function does not affect the state + cache, and does not use or modify the YDot or (Lagrange) Multipliers + contained in the state cache---those are for the reuslts from forward + dynamics. + @param[in] yDotGuess + The value of the derivatives of the continuous state variables to be + used when computing the implicit form. If this is zero length it will + be treated as all-zero; otherwise the length must be the number of + continuous state variables; SimTK::State::getNY(). + @param[in] lambdaGuess + The value of the Lagrange multipliers, used when computing the residual + for the generalized accelerations (inverse dynamics) to account for the + forces applied by the constraints. Provide an empty vector for models + that do not have constraints. If this is zero length it will be + treated as all-zero; otherwise, the length must be the number of + acceleration-level constraints; SimTK::State::getNMultipliers(). + @param[out] residuals + The residuals are in the same order as the state variables in the + SimTK::State: \f$ q, u, z \f$. + This will be resized so that its length is the number of continuous + state variables; SimTK::State::getNY(). + + @see InverseDynamicsSolver + @ingroup implicitdiffeq */ + void calcImplicitResiduals(const SimTK::State& state, + const SimTK::Vector& yDotGuess, + const SimTK::Vector& lambdaGuess, + SimTK::Vector& residuals) const; + //-------------------------------------------------------------------------- // STATES //-------------------------------------------------------------------------- @@ -873,10 +948,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Model, ModelComponent); void removeAnalysis(Analysis* analysis, bool deleteIt=true); /** Remove a Controller from the %Model. **/ void removeController(Controller *aController); - - //-------------------------------------------------------------------------- - // DERIVATIVES - //-------------------------------------------------------------------------- + + //-------------------------------------------------------------------------- // OPERATIONS //-------------------------------------------------------------------------- @@ -970,7 +1043,7 @@ 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; void extendInitStateFromProperties(SimTK::State& state) const override; /**@}**/ diff --git a/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp b/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp index c9decf6c6f..982b4f619a 100644 --- a/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp +++ b/OpenSim/Simulation/SimbodyEngine/Coordinate.cpp @@ -226,16 +226,6 @@ 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::extendInitStateFromProperties(State& s) const { // Cannot enforce the constraint, since state of constraints may still be undefined @@ -645,6 +635,23 @@ void Coordinate::CoordinateStateVariable:: throw Exception(msg); } +// Invoked by Model::extendRealizeInstance(). +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); +} //----------------------------------------------------------------------------- // Coordinate::SpeedStateVariable @@ -680,3 +687,21 @@ void Coordinate::SpeedStateVariable:: msg += "Generalized speed derivative (udot) can only be set by the Multibody system."; throw Exception(msg); } + +// Invoked by Model::extendRealizeInstance(). +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 2eeda5a596..2e748935ca 100644 --- a/OpenSim/Simulation/SimbodyEngine/Coordinate.h +++ b/OpenSim/Simulation/SimbodyEngine/Coordinate.h @@ -227,7 +227,6 @@ 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; void extendInitStateFromProperties(SimTK::State& s) const override; void extendSetPropertiesFromState(const SimTK::State& state) override; @@ -241,38 +240,44 @@ 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; 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; + + 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; 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; + + 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_back b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp_back new file mode 100644 index 0000000000..0c20b2d755 --- /dev/null +++ b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp_back @@ -0,0 +1,727 @@ +/* -------------------------------------------------------------------------- * + * 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 +*/ + +// 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. +// 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(). +// 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 +// 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 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 test passing empty lambdas treats them as 0, same for ydot, with +// and without constraints. + +// TODO mock up the situation where we are minimizing joint contact load +// while obeying dynamics (in direct collocation fashion--the entire +// trajectory). + +// TODO make sure residual using forward dynamics yDot and lambda gives 0 residual. + +// TODO yDotGuess vs ydotGuess + +// 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 handle quaternion constraints. + +// SparseMatrix jacobian; +// model.calcImplicitResidualsJacobian(state, yDotGuess, lambdaGuess, +// jacobian); +// +// // IN MODEL +// calcPartsOfJacobian(s, dMultibody_dudot, dKinematics_du, dKinematics_dqdot) +// for (comp : all components) { +// n, I, J, compJac = comp.computeJacobian(state, yDotGuess); +// +// I = [1]; +// J = [10]; +// +// J = comp.getYIndexForStateVariable("fiber_length"); +// for (k = 1:n) { +// jacobian[I[k], J[k]] = compJac[k] +// } +// } + +// TODO sketch out solver-like interface (using IPOPT). + + +#include +#include + +using namespace OpenSim; +using namespace SimTK; + + +/** This component provides the differential equation ydot*y = c. In its +implicit form, it has no singularities. In its explicit form ydot = c/y, it +has a singularity at y = 0. The solutions are: + + y(t) = +/-sqrt(y0^2 + 2*c*t). + +It is important that the explicit and implicit forms for this component are +different so that we can ensure the implicit form is used when provided +(in lieu of the trivial implicit form ydot - ydotguess). */ +class DynamicsExplicitImplicit : public Component { +OpenSim_DECLARE_CONCRETE_OBJECT(DynamicsExplicitImplicit, Component); +public: + OpenSim_DECLARE_PROPERTY(default_activ, double, + "Default value of state variable."); + OpenSim_DECLARE_PROPERTY(coeff, double, + "Coefficient in the differential equation."); + DynamicsExplicitImplicit() { + constructProperty_default_activ(1.0); + constructProperty_coeff(-2.0); + } +protected: + void extendAddToSystem(SimTK::MultibodySystem& system) const override { + Super::extendAddToSystem(system); + addStateVariable("activ", SimTK::Stage::Dynamics, true); + } + void computeStateVariableDerivatives(const SimTK::State& s) const override { + const Real& activ = getStateVariableValue(s, "activ"); + setStateVariableDerivativeValue(s, "activ", get_coeff() / activ); + } + // TODO rename to implementComputeImplicitResiduals? + // TODO provide lambdaGuess? + void computeImplicitResiduals(const SimTK::State& s, + const SimTK::Vector& yDotGuess, + SimTK::Vector& componentResiduals + ) const override { + const Real& activ = getStateVariableValue(s, "activ"); + double activDotGuess = getStateVariableDerivativeGuess("activ", yDotGuess); + // TODO yDotGuess[_activIndex] + const Real activResidual = activ * activDotGuess - get_coeff(); + setImplicitResidual("activ", activResidual, componentResiduals); + // TODO residuals[0] = ...; + } + void extendInitStateFromProperties(SimTK::State& s) const override { + Super::extendInitStateFromProperties(s); + setStateVariableValue(s, "activ", get_default_activ()); + } + void extendSetPropertiesFromState(const SimTK::State& s) override { + Super::extendSetPropertiesFromState(s); + set_default_activ(getStateVariableValue(s, "activ")); + } +}; + +/** Differential equation: ydot = y^2. Only explicit form is provided. +We use this to test that the Component interface is able to produce a +trivial implicit form when the component does not provide an implicit form. */ +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")); + } +}; + +/// 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); + ts.stepTo(finalTime); + state = ts.getState(); +} + +// Ensure that explicit forward integration works for a component that also +// 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* comp = new DynamicsExplicitImplicit(); + 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(); + + // Check boolean indicators. + // TODO rename to "hasImplicitForm()" + SimTK_TEST(comp->hasImplicitFormLocal()); + SimTK_TEST(comp->hasImplicitForm()); + SimTK_TEST(model.hasImplicitFormLocal()); + SimTK_TEST(model.hasImplicitForm()); + + // Check initial values. + SimTK_TEST(comp->getStateVariableValue(s, "activ") == initialValue); + model.realizeAcceleration(s); + 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); + // Solution is sqrt(y0^2 + 2*c*t). + SimTK_TEST_EQ_TOL(comp->getStateVariableValue(s, "activ"), + sqrt(pow(initialValue, 2) + 2 * coeff * finalTime), 1e-5); +} + +void testEmptyResidualsIfNoDynamics() { + Model model; + State s = model.initSystem(); + Vector residuals(5, 0.0); + model.calcImplicitResiduals(s, Vector(), Vector(), residuals); + SimTK_TEST(residuals.size() == 0); +} + +void testSingleCustomImplicitEquation() { + const Real initialValue = 3.5; + const Real coeff = -0.28; + const Real activDotGuess = 5.3; + const Real expectedResidual = initialValue * activDotGuess - coeff; + + Model model; model.setName("model"); + auto* comp = new DynamicsExplicitImplicit(); + comp->setName("foo"); + comp->set_default_activ(initialValue); + comp->set_coeff(coeff); + model.addComponent(comp); + + State s = model.initSystem(); + + // Set guess. + Vector yDotGuess(s.getNY()); + comp->setStateVariableDerivativeGuess("activ", activDotGuess, yDotGuess); + + // TODO resizing yDotGuess appropriately? + SimTK_TEST_EQ(yDotGuess, Vector(1, activDotGuess)); + + // Compute the entire residual vector. + { + Vector residuals; + // TODO rename calcAllImplicitResiduals(). + model.calcImplicitResiduals(s, yDotGuess, Vector(), residuals); + SimTK_TEST_EQ(residuals, Vector(1, expectedResidual)); + + // Access specific residual by name from the component: + SimTK_TEST(comp->getImplicitResidual("activ", residuals) == + expectedResidual); + } + + // Compute the residuals for just the one component. + { + Vector componentResiduals = + comp->calcImplicitResidualsLocal(s, yDotGuess, Vector()); + SimTK_TEST_EQ(componentResiduals, Vector(1, expectedResidual)); + } +} + +// 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("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"); + 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]; + + SimTK_TEST(coord.hasImplicitFormLocal()); + SimTK_TEST(model.hasImplicitForm()); + + coord.setSpeedValue(s, u_i); + + const double qDotGuess = 5.6; const double uDotGuess = 8.3; + + // Check implicit form. + // -------------------- + // Set derivative guess. + Vector yDotGuess(s.getNY()); + // TODO be consistent with "addInControls()" etc. + coord.setStateVariableDerivativeGuess("value", qDotGuess, yDotGuess); + coord.setStateVariableDerivativeGuess("speed", uDotGuess, yDotGuess); + + // Calculate residual. + Vector expectedResiduals(2); + expectedResiduals[0] = u_i - qDotGuess; + expectedResiduals[1] = mass * uDotGuess - mass * (-g); // M u_dot-f_applied + Vector residuals; + model.realizeDynamics(s); // TODO test this requirement. + model.calcImplicitResiduals(s, yDotGuess, Vector(), residuals); + // Check the entire residuals vector. + SimTK_TEST_EQ(residuals, expectedResiduals); + // Check individual elements of the residual. + SimTK_TEST_EQ(coord.getImplicitResidual("value", residuals), + expectedResiduals[0]); + SimTK_TEST_EQ(coord.getImplicitResidual("speed", residuals), + expectedResiduals[1]); + + // TODO set YDotGuess all at once. + + + // Check explicit form. + // -------------------- + model.realizeAcceleration(s); + Vector expectedYDot(2); + expectedYDot[0] = u_i /* = qdot*/; expectedYDot[1] = -g /* = udot*/; + SimTK_TEST_EQ(s.getYDot(), expectedYDot); + + // Passing empty yDotGuess treats them as zeros. + // --------------------------------------------- + { + Vector resEmptyYDot; + model.calcImplicitResiduals(s, Vector(), Vector(), resEmptyYDot); + Vector resZerosYDot; + model.calcImplicitResiduals(s, Vector(2, 0.), Vector(), resZerosYDot); + SimTK_TEST_EQ(resEmptyYDot, resZerosYDot); + } + + // Error-checking. + // --------------- + Vector output; + // Size of yDotGuess must be correct. + SimTK_TEST_MUST_THROW_EXC( // yDotGuess too small. + model.calcImplicitResiduals(s, Vector(1, 0.), Vector(), output), + OpenSim::Exception); + SimTK_TEST_MUST_THROW_EXC( // yDotGuess too large. + model.calcImplicitResiduals(s, Vector(3, 0.), Vector(), output), + OpenSim::Exception); + // Size of lambdaGuess must be correct. + SimTK_TEST_MUST_THROW_EXC( // lambdaGuess too large. + model.calcImplicitResiduals(s, Vector(2, 0.), Vector(1, 0.), output), + OpenSim::Exception); +} + +// TODO test model with constraints here! + +// 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.hasImplicitFormLocal()); + SimTK_TEST(!comp.hasImplicitForm()); + SimTK_TEST(model.hasImplicitFormLocal()); + SimTK_TEST(!model.hasImplicitForm()); + + // Set guess. + Vector yDotGuess(s.getNY()); + comp.setStateVariableDerivativeGuess("length", lengthDotGuess, yDotGuess); + + // Calculate residual. + model.realizeDynamics(s); + Vector residuals; + model.calcImplicitResiduals(s, yDotGuess, Vector(), residuals); + + // Check the entire residuals vector. + SimTK_TEST_EQ(residuals, Vector(1, expectedResidual)); + + // Check individual elements of the residual by name. + SimTK_TEST(comp.getImplicitResidual("length", residuals) == + expectedResidual); + } +} + +/** If only some of a component's state variables provide an explicit form, +the user should get an error .*/ +void testMustHaveFullImplicitForm() { + + // Explicit-only and implicit state var in a single component. + // ----------------------------------------------------------- + class NotFullImplicitForm : public Component { + OpenSim_DECLARE_CONCRETE_OBJECT(NotFullImplicitForm, Component); + void extendAddToSystem(SimTK::MultibodySystem& system) const override { + Super::extendAddToSystem(system); + addStateVariable("var_explicit_only"); + addStateVariable("var_explicit_implicit", SimTK::Stage::Dynamics, + true); + } + }; + + { + Model model; + model.addComponent(new NotFullImplicitForm()); + SimTK_TEST_MUST_THROW_EXC(model.initSystem(), OpenSim::Exception); + } + + + // Base component does not have implicit form. + // ------------------------------------------- + class ExplicitOnlyBase : public Component { + OpenSim_DECLARE_CONCRETE_OBJECT(ExplicitOnlyBase, Component); + protected: + void extendAddToSystem(SimTK::MultibodySystem& system) const override { + Super::extendAddToSystem(system); + addStateVariable("var_explicit_only"); + } + }; + class FullImplicitDerived : public ExplicitOnlyBase { + OpenSim_DECLARE_CONCRETE_OBJECT(FullImplicitDerived, ExplicitOnlyBase); + void extendAddToSystem(SimTK::MultibodySystem& system) const override { + Super::extendAddToSystem(system); + addStateVariable("var_explicit_implicit", SimTK::Stage::Dynamics, + true); + } + }; + + { + Model model; + model.addComponent(new FullImplicitDerived()); + SimTK_TEST_MUST_THROW_EXC(model.initSystem(), OpenSim::Exception); + } +} + + + +// ============================================================================= +// 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, and the Lagrange + // multipliers, lambda. + void solve(const State& s, Vector& yDot, Vector& lambda); +private: + Model m_model; + std::unique_ptr m_problem; + std::unique_ptr m_opt; +}; +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& guess, bool newParams, + Real& f) const override { + f = 0; return 0; + } + int constraintFunc(const Vector& guess, bool newParams, + Vector& constraints) const override { + const int ny = m_workingState.getNY(); + const int nu = m_workingState.getNU(); + + 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)); + + // Differential equations. + m_parent.m_model.realizeDynamics(m_workingState); + m_parent.m_model.calcImplicitResiduals(m_workingState, + yDotGuess, lambdaGuess, + residuals); + + // Constraints. + const auto& matter = m_parent.m_model.getMatterSubsystem(); + const auto& uDotGuess = yDotGuess(m_workingState.getUStart(), nu); + matter.calcConstraintAccelerationErrors(m_workingState, uDotGuess, + pvaerrs); + + // 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; + State m_workingState; +}; +ImplicitSystemDerivativeSolver::ImplicitSystemDerivativeSolver( + const Model& model) : m_model(model), m_problem(new Problem(*this)) { + // Set up Problem. + State state = m_model.initSystem(); + const int N = state.getNY() + state.getNMultipliers(); + m_problem->setNumParameters(N); + m_problem->setNumEqualityConstraints(N); + Vector limits(N, 50.0); // arbitrary. + m_problem->setParameterLimits(-limits, limits); + + // Set up Optimizer. + m_opt.reset(new Optimizer(*m_problem, SimTK::InteriorPoint)); + m_opt->useNumericalGradient(true); m_opt->useNumericalJacobian(true); +} +void ImplicitSystemDerivativeSolver::solve(const State& s, + Vector& yDot, Vector& lambda) { + m_problem->setWorkingState(s); + Vector results(m_problem->getNumParameters(), 0.0); + m_opt->optimize(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 DynamicsExplicitImplicit(); + comp->setName("foo"); + const Real initialValue = 3.5; + comp->set_default_activ(initialValue); + const Real coeff = -0.21; + comp->set_coeff(coeff); + model.addComponent(comp); + State s = model.initSystem(); + + // Computing yDot using implicit form. + 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. + 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() { + // TODO constraints? does not require error message. + // TODO prescribed motion. +} + +int main() { + SimTK_START_TEST("testImplicitDifferentialEquations"); + /* + SimTK_SUBTEST(testExplicitFormOfImplicitComponent); + SimTK_SUBTEST(testEmptyResidualsIfNoDynamics); + SimTK_SUBTEST(testSingleCustomImplicitEquation); + SimTK_SUBTEST(testImplicitMultibodyDynamics1DOF); + SimTK_SUBTEST(testImplicitFormOfExplicitOnlyComponent); + SimTK_SUBTEST(testMustHaveFullImplicitForm); + // TODO SimTK_SUBTEST(testImplicitFormOfCombinedImplicitAndExplicitComponents); + // TODO SimTK_SUBTEST(testMultibody1DOFAndCustomComponent); + SimTK_SUBTEST(testGenericInterfaceForImplicitSolver); + SimTK_SUBTEST(testImplicitSystemDerivativeSolverMultibody1DOF); + SimTK_SUBTEST(testCoordinateCouplerConstraint); // TODO rename + SimTK_SUBTEST(testErrorsForUnsupportedModels); + */ + SimTK_END_TEST(); +} + + + + + diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index 6038bd320c..ac98cf0eff 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -122,7 +122,7 @@ AddDependency(NAME BTK AddDependency(NAME simbody URL https://github.com/simbody/simbody.git - TAG 76ca55bcfc169a1ddf3b13903d3cd1aa15d6e957 + TAG 3469006e99a94e9a3c7201a5308d298d20acf07f CMAKE_ARGS -DBUILD_EXAMPLES:BOOL=OFF -DBUILD_TESTING:BOOL=OFF) diff --git a/doc/doxygen-layout-developer.xml b/doc/doxygen-layout-developer.xml index 03e53bac99..d8fc88313e 100644 --- a/doc/doxygen-layout-developer.xml +++ b/doc/doxygen-layout-developer.xml @@ -39,8 +39,8 @@ - + diff --git a/doc/doxygen-layout-user.xml b/doc/doxygen-layout-user.xml index 9d96a3fafc..879abcb37a 100644 --- a/doc/doxygen-layout-user.xml +++ b/doc/doxygen-layout-user.xml @@ -39,8 +39,8 @@ - + diff --git a/doc/images/fig_VandenBogert2011Muscle_PeeForce.png b/doc/images/fig_VandenBogert2011Muscle_PeeForce.png new file mode 100644 index 0000000000..2def455ef9 Binary files /dev/null and b/doc/images/fig_VandenBogert2011Muscle_PeeForce.png differ diff --git a/doc/images/fig_VandenBogert2011Muscle_SeeForce.png b/doc/images/fig_VandenBogert2011Muscle_SeeForce.png new file mode 100644 index 0000000000..18265438aa Binary files /dev/null and b/doc/images/fig_VandenBogert2011Muscle_SeeForce.png differ diff --git a/doc/images/fig_VandenBogert2011Muscle_fl.png b/doc/images/fig_VandenBogert2011Muscle_fl.png new file mode 100644 index 0000000000..947c9a3eaf Binary files /dev/null and b/doc/images/fig_VandenBogert2011Muscle_fl.png differ diff --git a/doc/images/fig_VandenBogert2011Muscle_fv.png b/doc/images/fig_VandenBogert2011Muscle_fv.png new file mode 100644 index 0000000000..df98fe3371 Binary files /dev/null and b/doc/images/fig_VandenBogert2011Muscle_fv.png differ diff --git a/doc/images/fig_VandenBogertMuscle.png b/doc/images/fig_VandenBogertMuscle.png new file mode 100644 index 0000000000..01fe35003b Binary files /dev/null and b/doc/images/fig_VandenBogertMuscle.png differ