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/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 b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp new file mode 100644 index 0000000000..46c15a9df9 --- /dev/null +++ b/OpenSim/Simulation/Test/testImplicitDifferentialEquations.cpp @@ -0,0 +1,725 @@ +/* -------------------------------------------------------------------------- * + * 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 @@ - +