diff --git a/OpenSim/Common/Component.cpp b/OpenSim/Common/Component.cpp index 450461c858..1fa772a729 100644 --- a/OpenSim/Common/Component.cpp +++ b/OpenSim/Common/Component.cpp @@ -573,23 +573,42 @@ void Component::addStateVariable(Component::StateVariable* stateVariable) const } - -void Component::addDiscreteVariable(const std::string& discreteVariableName, - SimTK::Stage invalidatesStage) const -{ - // don't add discrete var if there is another discrete variable with the - // same name for this component +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023) +// A variaible 'allocate' was added to the argument list of +// addDiscreteVariable(). This was done to prevent double allocation of a +// discrete variable that is allocated outside of class Component. Such an +// allocation can occur when a native Simbody class, wrapped as an OpenSim +// Component, allocates its own discrete variables. +// +// When 'allocate' is true (default), the discrete state is allocated normally +// in Component::extendRealizeTopology(). +// +// When 'allocate' is false, the discrete variable is assumed to be allocated +// elswhere. In this case, the derived Component is responsible for +// initializing the index of the discrete state, as well its Subsystem. +// This should be done by implementing an overriding extendRealizeTopology() +// method and, in that method, calling Component::updDiscreteVariableIndex(); +// +// See ExponentialContact::extendRealizeTopology() for an example. +void +Component:: +addDiscreteVariable(const std::string& discreteVariableName, + SimTK::Stage invalidatesStage, bool allocate) const +{ + // Don't add discrete var if there is another discrete variable with the + // same name for this component. std::map::const_iterator it; it = _namedDiscreteVariableInfo.find(discreteVariableName); - if(it != _namedDiscreteVariableInfo.end()){ + if(it != _namedDiscreteVariableInfo.end()) { throw Exception("Component::addDiscreteVariable: discrete variable '" + discreteVariableName + "' already exists."); } - // assign "slots" for the discrete variables by name - // discrete variable indices will be invalid by default - // upon allocation during realizeTopology the indices will be set + // Assign "slots" for the discrete variables by name. + // Discrete variable indices will be invalid by default. + // Upon allocation during realizeTopology, the indices will be set. _namedDiscreteVariableInfo[discreteVariableName] = - DiscreteVariableInfo(invalidatesStage); + DiscreteVariableInfo(invalidatesStage, allocate); } // Get the value of a ModelingOption flag for this Component. @@ -760,6 +779,7 @@ ComponentPath Component::getRelativePath(const Component& wrt) const return thisP.formRelativePath(wrtP); } + const Component::StateVariable* Component:: traverseToStateVariable(const std::string& pathName) const { @@ -792,6 +812,7 @@ const Component::StateVariable* Component::traverseToStateVariable( return found; } + // Get the names of "continuous" state variables maintained by the Component and // its subcomponents. Array Component::getStateVariableNames() const @@ -1007,9 +1028,75 @@ void Component:: } } -// Get the value of a discrete variable allocated by this Component by name. -double Component:: + +//_____________________________________________________________________________ +// F. C. Anderson (Feb 2023) +// Added so that discrete states can be serialized and deserialized. +// +// Get the names of discrete state variables maintained by the Component and +// its subcomponents. +Array Component::getDiscreteVariableNames() const { + // Must have already called initSystem. + OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); + + Array dvNames = getDiscreteVariableNamesAddedByComponent(); + + for (int i = 0; i < dvNames.size(); ++i) { + dvNames[i] = (getAbsolutePathString() + "/" + dvNames[i]); + } + + for (auto& comp : getComponentList()) { + const std::string& pathName = comp.getAbsolutePathString(); // *this); + Array subDVNames = + comp.getDiscreteVariableNamesAddedByComponent(); + for (int i = 0; i < subDVNames.size(); ++i) { + dvNames.append(pathName + "/" + subDVNames[i]); + } + } + + return dvNames; +} + +//_____________________________________________________________________________ +// F. C. Anderson (Feb 2023) +// Added so that discrete states can be serialized and deserialized. +Array Component::getDiscreteVariableNamesAddedByComponent() const { + std::map::const_iterator it; + it = _namedDiscreteVariableInfo.begin(); + + Array names("", (int)_namedDiscreteVariableInfo.size()); + + int i = 0; + while (it != _namedDiscreteVariableInfo.end()) { + names[i] = it->first; + it++; + i++; + } + return names; +} + +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023) +// Get the value (assumed to be type double) of a discrete variable allocated +// by this Component by name. So that existing code does not need to change, +// this method fulfills the API prior to Jan 2023. +double +Component:: getDiscreteVariableValue(const SimTK::State& s, const std::string& name) const +{ + double value = SimTK::NaN; + value = SimTK::Value::downcast( + getDiscreteVariableAbstractValue(s, name)); + return value; +} + +//_____________________________________________________________________________ +// Set the value (assumed to be double) of a discrete variable allocated by +// this Component by name. +void +Component:: +setDiscreteVariableValue(SimTK::State& s, const std::string& name, + double value) const { // Must have already called initSystem. OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); @@ -1019,43 +1106,153 @@ getDiscreteVariableValue(const SimTK::State& s, const std::string& name) const if(it != _namedDiscreteVariableInfo.end()) { SimTK::DiscreteVariableIndex dvIndex = it->second.index; - return SimTK::Value::downcast( - getDefaultSubsystem().getDiscreteVariable(s, dvIndex)).get(); + + // F. C. Anderson (Jan 2023) + // Previously, it was assumed that all discrete states were allocated + // from the default Subsystem. This is likely not the case when + // discrete states are allocated by native Simbody objects. For + // example, class ExponentialSpringForce allocates 4 discrete states, + // not from the default Subsystem, but from the GeneralForceSubsystem. + // To account for the fact that an object might allocate discrete + // variables from a different Subsystem, a pointer was added to the + // DiscreteVariableInfo struct. This pointer is now consulted for any + // non-default Subsystem. If this pointer is nullptr, which is its + // default value, then the default Subsystem is used. + const SimTK::Subsystem* subsystem = it->second.subsystem; + if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); + SimTK::Value::downcast( + subsystem->updDiscreteVariable(s, dvIndex)).upd() = value; + } else { std::stringstream msg; - msg << "Component::getDiscreteVariable: ERR- name '" << name + msg << "Component::setDiscreteVariable: ERR- name '" << name << "' not found.\n " << "for component '"<< getName() << "' of type " << getConcreteClassName(); throw Exception(msg.str(),__FILE__,__LINE__); - return SimTK::NaN; } } -// Set the value of a discrete variable allocated by this Component by name. -void Component:: -setDiscreteVariableValue(SimTK::State& s, const std::string& name, double value) const +//_____________________________________________________________________________ +// F. C. Anderson (May 2023) +// Added so that a discrete variable can be accessed based on a specified path. +const Component* +Component:: +resolveDiscreteVariableNameAndOwner(const std::string& pathName, + std::string& dvName) const +{ + ComponentPath path{pathName}; + size_t nLevels = path.getNumPathLevels(); + dvName = path.getSubcomponentNameAtLevel(nLevels - 1); + const Component* owner = this; + if (nLevels > 1) { + // Need to traverse to the owner of the DV based on the path. + const ComponentPath& ownerPath = path.getParentPath(); + owner = traversePathToComponent(ownerPath); + } + return owner; +} + +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023, May 2023) +// This method was added in order to handle Discrete Variables (DV) that are +// not type double. In addition, a DV can be accessed by specifying its path +// instead of just the name of the DV. +const SimTK::AbstractValue& +Component:: +getDiscreteVariableAbstractValue(const SimTK::State& s, + const std::string& pathName) const { // Must have already called initSystem. OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); + // Resolve the name of the DV and its owner. + std::string dvName{""}; + const Component* owner = + resolveDiscreteVariableNameAndOwner(pathName, dvName); + + // Find the variable. std::map::const_iterator it; - it = _namedDiscreteVariableInfo.find(name); + it = owner->_namedDiscreteVariableInfo.find(dvName); - if(it != _namedDiscreteVariableInfo.end()) { + if (it != owner->_namedDiscreteVariableInfo.end()) { SimTK::DiscreteVariableIndex dvIndex = it->second.index; - SimTK::Value::downcast( - getDefaultSubsystem().updDiscreteVariable(s, dvIndex)).upd() = value; + + // F. C. Anderson (Jan 2023) + // Previously, it was assumed that all discrete states were allocated + // from the default Subsystem. This is likely not the case when + // discrete states are allocated by native Simbody objects. For + // example, class ExponentialSpringForce allocates 4 discrete states, + // not from the default Subsystem, but from the GeneralForceSubsystem. + // To account for the fact that an object might allocate discrete + // variables from a different Subsystem, a pointer was added to the + // DiscreteVariableInfo struct. This pointer is now consulted for any + // non-default Subsystem. If this pointer is nullptr, which is its + // default value, then the default Subsystem is used. + const SimTK::Subsystem* subsystem = it->second.subsystem; + if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); + return subsystem->getDiscreteVariable(s, dvIndex); + } else { std::stringstream msg; - msg << "Component::setDiscreteVariable: ERR- name '" << name + msg << "Component::getDiscreteVariable: ERR- name '" << pathName << "' not found.\n " - << "for component '"<< getName() << "' of type " + << "for component '" << getName() << "' of type " << getConcreteClassName(); - throw Exception(msg.str(),__FILE__,__LINE__); + throw Exception(msg.str(), __FILE__, __LINE__); } } +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023, May 2023) +// This method was added in order to handle Discrete Variables (DV) that are +// not type double and, in addition, to allow a path to be specified instead +// of just the name of the DV. +SimTK::AbstractValue& +Component:: +updDiscreteVariableAbstractValue(SimTK::State& s, + const std::string& pathName) const +{ + // Must have already called initSystem. + OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); + + // Resolve the name of the DV and its owner. + std::string dvName{""}; + const Component* owner = + resolveDiscreteVariableNameAndOwner(pathName, dvName); + + std::map::const_iterator it; + it = owner->_namedDiscreteVariableInfo.find(dvName); + + if (it != owner->_namedDiscreteVariableInfo.end()) { + SimTK::DiscreteVariableIndex dvIndex = it->second.index; + + // F. C. Anderson (Jan 2023) + // Previously, it was assumed that all discrete states were allocated + // from the default Subsystem. This is likely not the case when + // discrete states are allocated by native Simbody objects. For + // example, class ExponentialSpringForce allocates 4 discrete states, + // not from the default Subsystem, but from the GeneralForceSubsystem. + // To account for the fact that an object might allocate discrete + // variables from a different Subsystem, a pointer was added to the + // DiscreteVariableInfo struct. This pointer is now consulted for any + // non-default Subsystem. If this pointer is nullptr, which is its + // default value, then the default Subsystem is used. + const SimTK::Subsystem* subsystem = it->second.subsystem; + if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); + return subsystem->updDiscreteVariable(s, dvIndex); + + } else { + std::stringstream msg; + msg << "Component::updDiscreteVariable: ERR- name '" << pathName + << "' not found.\n " + << "for component '" << getName() << "' of type " + << getConcreteClassName(); + throw Exception(msg.str(), __FILE__, __LINE__); + } +} + + SimTK::CacheEntryIndex Component::getCacheVariableIndex(const std::string& name) const { auto it = this->_namedCacheVariables.find(name); @@ -1393,6 +1590,27 @@ getDiscreteVariableIndex(const std::string& name) const return it->second.index; } +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023) +// This method was added so that a derived Component can properly initialize +// the index and Subsystem of a discrete variable that is allocated outside +// of class Component. +void +Component:: +updDiscreteVariableIndex(const std::string& name, + const SimTK::DiscreteVariableIndex& index, + const SimTK::Subsystem* subsystem) const +{ + std::map::iterator it; + it = _namedDiscreteVariableInfo.find(name); + if (it == _namedDiscreteVariableInfo.end()) { + throw Exception("Component::updDiscreteVariableIndex: " + " discrete variable '" + name + "' not found."); + } + it->second.index = index; + it->second.subsystem = subsystem; +} + Array Component:: getStateVariableNamesAddedByComponent() const { @@ -1449,8 +1667,19 @@ void Component::extendRealizeTopology(SimTK::State& s) const // Allocate Discrete State Variables for (auto& kv : _namedDiscreteVariableInfo) { DiscreteVariableInfo& dvi = kv.second; - dvi.index = subSys.allocateDiscreteVariable( - s, dvi.invalidatesStage, new SimTK::Value(0.0)); + + // F. C. Anderson (Jan 2023) + // Do not allocate if the discrete state is allocated outside of class + // Component. This case is encountered when a native Simbody object, + // wrapped as an OpenSim Component, posseses discrete states of its + // own. In such a case, the derived Component is responsible for + // initializing the discrete state index, as well as its Subsystem. + // See ExponentialContact::extendAddToSystem() and + // ExponentialContact::extendRealizeTopology for an example. + if (!dvi.allocate) continue; + + dvi.index = subSys.allocateDiscreteVariable(s, + dvi.invalidatesStage, new SimTK::Value(0.0)); } // allocate cache entry in the state diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index 3da4c2cd0b..59b89572a7 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -860,6 +860,13 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); */ Array getStateVariableNames() const; + /** + * Get the names of discrete state variables maintained by the Component + * and its subcomponents. + * @throws ComponentHasNoSystem if this Component has not been added to a + * System (i.e., if initSystem has not been called) + */ + Array getDiscreteVariableNames() const; /** @name Component Socket Access methods Access Sockets of this component by name. */ @@ -1369,16 +1376,17 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); const std::string& name) const; /** - * Get the value of a discrete variable allocated by this Component by name. + * Get the value (assumed to be type double) of a discrete variable + * allocated by this Component by name. * * @param state the State from which to get the value * @param name the name of the state variable - * @return value the discrete variable value + * @return value the discrete variable value as a double * @throws ComponentHasNoSystem if this Component has not been added to a * System (i.e., if initSystem has not been called) */ double getDiscreteVariableValue(const SimTK::State& state, - const std::string& name) const; + const std::string& name) const; /** * %Set the value of a discrete variable allocated by this Component by name. @@ -1392,6 +1400,149 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); void setDiscreteVariableValue(SimTK::State& state, const std::string& name, double value) const; + + //------------------------------------------------------------------------- + // F. C. Anderson (January 2023, May 2023) + // Added the ability to get the value of a discrete variable as a + // SimTK::AbstractValue, thereby allowing types like Vec3. + // In addition, getDiscreteVariableAbstractValue() and + // updDiscreteVariableAbstractValue() accept the path of a discrete + // variable, not just its name. + + /** + * Based on a specified path, resolve the name of a discrete variable and + * the component that owns it (i.e., its parent). + * + * @param pathName Specified path of the discrete variable in the Model + * heirarchy. + * @param dvName Returned name of the discrete variable. This string is + * simply the last string in the specified pathName. + * @return Pointer to the Component that owns the discrete variable. + */ + const Component* resolveDiscreteVariableNameAndOwner( + const std::string& pathName, std::string& dvName) const; + + /** + * Retrieve a read-only reference to the abstract value of the discrete + * variable at a specified path. This method provides a more general + * interface that is not limited to values of type double. + * + * To obtain the type-specific value of a discrete variable, perform + * a cast using the template methods provided in class SimTK::Value. + * When the type is unknown, it can be queried using the + * SimTK::Value::isA() method. For example, + * + * ``` + * const SimTK::AbstractValue& valAbstract = + * getDiscreteVariableAbstractValue(state, pathName); + * + * if (SimTK::Value::isA(valAbstract)) { + * const SimTK::Value& valDbl = + * SimTK::Value::downcast(valAbstract); + * double x = valDbl + 0.4; + * + * } else if (SimTK::Value::isA(valAbstract)) { + * const SimTK::Value& valVec3 = + * SimTK::Value::downcast(valAbstract); + * Vec3 x = valDbl + Vec3(0.4); + * } + * ``` + * + * @param state State from which to get the value. + * @param pathName Specified path of the discrete variable in the Model + * heirarchy. + * @return Value of the discrete variable as a reference to an + * AbstractValue. + * @throws ComponentHasNoSystem if this Component has not been added to a + * System (i.e., if initSystem has not been called). + * @throws Exception if the discrete variable is not found. + */ + const SimTK::AbstractValue& getDiscreteVariableAbstractValue( + const SimTK::State& state, const std::string& pathName) const; + + /** + * Retrieve a writable reference to the abstract value of the discrete + * variable at a specified path. This method provides a more general + * interface that is not limited to values of type double. + * + * To obtain the type-specific value of a discrete variable, perform + * a cast using the template methods provided in class SimTK::Value. + * When the type is unknown, it can be queried using the + * SimTK::Value::isA() method. For example, + * + * ``` + * SimTK::AbstractValue& valAbstract = + * updDiscreteVariableAbstractValue(state, pathName); + * + * if (SimTK::Value::isA(valAbstract)) { + * SimTK::Value& valDbl = + * SimTK::Value::updDowncast(valAbstract); + * valDbl = 0.4; + * + * } else if (SimTK::Value::isA(valAbstract)) { + * SimTK::Value& valVec3 = + * SimTK::Value::updDowncast(valAbstract); + * valDbl = Vec3(0.4); + * } + * ``` + * + * @param state State from which to get the value. + * @param pathName Specified path of the discrete variable in the Model + * heirarchy. + * @return Value of the discrete variable as a reference to an + * AbstractValue. + * @throws ComponentHasNoSystem if this Component has not been added to a + * System (i.e., if initSystem has not been called). + * @throws Exception if the discrete variable is not found. + */ + SimTK::AbstractValue& updDiscreteVariableAbstractValue( + SimTK::State& state, const std::string& name) const; + //-------------------------------------------------------------------------- + + + + /** + * %Set the value of a discrete variable allocated by this Component by + * name. This method is a template to account for Discrete Variables that + * are not type double. + * + * @param s the State for which to set the value + * @param name the name of the discrete variable + * @param value the value to set + * @throws ComponentHasNoSystem if this Component has not been added to a + * System (i.e., if initSystem has not been called) + */ + template + void setDiscreteVariableValue(SimTK::State& s, const std::string& name, + const T& value) const + { + // Must have already called initSystem. + OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); + + std::map::const_iterator it; + it = _namedDiscreteVariableInfo.find(name); + + if (it != _namedDiscreteVariableInfo.end()) { + SimTK::DiscreteVariableIndex dvIndex = it->second.index; + + // Account for non-default subsystem + const SimTK::Subsystem* subsystem = it->second.subsystem; + if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); + + // Update the value + SimTK::Value::downcast( + subsystem->updDiscreteVariable(s,dvIndex)) = value; + + } else { + std::stringstream msg; + msg << "Component::setDiscreteVariable: ERR- name '" << name + << "' not found.\n " + << "for component '" << getName() << "' of type " + << getConcreteClassName(); + throw Exception(msg.str(), __FILE__, __LINE__); + } + } + /** * A cache variable containing a value of type T. * @@ -1956,6 +2107,12 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); protected: class StateVariable; + + // F. C. Anderson (May 2023) + // Need declaration up front to support new methods for traversing the + // Component graph to Discrete Variables. + //struct DiscreteVariableInfo; + //template friend class ComponentSet; // Give the ComponentMeasure access to the realize() methods. template friend class ComponentMeasure; @@ -2422,10 +2579,12 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); void addStateVariable(Component::StateVariable* stateVariable) const; /** Add a system discrete variable belonging to this Component, give - it a name by which it can be referenced, and declare the lowest Stage that - should be invalidated if this variable's value is changed. **/ + it a name by which it can be referenced, declare the lowest Stage that + should be invalidated if this variable's value is changed, and specify + whether the discrete state should be allocated by class Component. + */ void addDiscreteVariable(const std::string& discreteVariableName, - SimTK::Stage invalidatesStage) const; + SimTK::Stage invalidatesStage, bool allocate = true) const; /** * Get writable reference to the MultibodySystem that this component is @@ -2455,6 +2614,17 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); const SimTK::DiscreteVariableIndex getDiscreteVariableIndex(const std::string& name) const; + /** + * Update the index of a Component's discrete variable. This method is + * intended for derived Components that may need to initialize the index + * held by OpenSim. Such a situation occurs when an underlying Simbody + * subsystem is responsible for the allocation of a discrete variable + * (not OpenSim), and OpenSim needs to be told the value of that index. + */ + void updDiscreteVariableIndex(const std::string& name, + const SimTK::DiscreteVariableIndex& index, + const SimTK::Subsystem* subsystem = nullptr) const; + // End of System Creation and Access Methods. //@} @@ -2657,6 +2827,8 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); */ const StateVariable* traverseToStateVariable( const ComponentPath& path) const; + + #endif /// @name Access to the owning component (advanced). @@ -2934,6 +3106,18 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); { return (int)_namedStateVariableInfo.size(); } Array getStateVariableNamesAddedByComponent() const; + // F. C. Anderson (Feb 2023) + // Added so that discrete states can be serialized and deserialized. + // + // Get the number of discrete states that the Component added to the + // underlying computational system. It includes the number of built-in + // discrete states exposed by this component. It represents the number of + // discrete variables managed by this Component. + int getNumDiscreteVariablesAddedByComponent() const { + return (int)_namedDiscreteVariableInfo.size(); + } + Array getDiscreteVariableNamesAddedByComponent() const; + const SimTK::DefaultSystemSubsystem& getDefaultSubsystem() const { return getSystem().getDefaultSubsystem(); } SimTK::DefaultSystemSubsystem& updDefaultSubsystem() const @@ -3176,17 +3360,44 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); int order; }; - // Structure to hold related info about discrete variables + // Structure to hold related info about discrete variables. struct DiscreteVariableInfo { DiscreteVariableInfo() {} - explicit DiscreteVariableInfo(SimTK::Stage invalidates) - : invalidatesStage(invalidates) {} + explicit DiscreteVariableInfo(SimTK::Stage invalidates, + bool allocate = true) : + invalidatesStage(invalidates), allocate(allocate) {} + // Model SimTK::Stage invalidatesStage; + // System SimTK::DiscreteVariableIndex index; + + // F. C. Anderson (Jan 2023) + // Introduced two data members: 'subsystem' and 'allocate'. + // These two data members allow OpenSim::Component to expose a + // discrete state that was allocated by a class other than + // OpenSim::Component and as a member of Subsystem other than the + // default SimTK::Subsystem. + + // Subsystem to which the discrete state belongs. + // If 'nullptr' (default), class Component assumes that the discrete + // state was allocated as a member of the default SimTK::Subsystem. + const SimTK::Subsystem* subsystem{nullptr}; + + // Flag to prevent a double allocation. + // If 'true' (default), the discrete variable is allocated normally + // in Component::extendRealizeTopology(). + // If 'false', allocation in Component::extendRealizeTopology() is + // skipped and is assumed to occur elsewhere. In this case, the + // derived Component is responsible for initializing the index of the + // discrete state, as well as its Subsystem. This should be done by + // implementing an overriding extendRealizeTopology() method. + // See ExponentialContact::extendRealizeTopology() for an example. + bool allocate{true}; }; + /** * A cache variable, as stored internally by Component. */ diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp new file mode 100644 index 0000000000..c665aeae5f --- /dev/null +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -0,0 +1,570 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: ExponentialContact.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) 2022-20232 Stanford University and the Authors * + * Author(s): F. C. Anderson * + * * + * 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 "Model.h" +#include "simbody/internal/SimbodyMatterSubsystem.h" +#include "ExponentialContact.h" + +using namespace OpenSim; +using namespace std; +using SimTK::Real; +using SimTK::Vec3; +using SimTK::State; + + +//============================================================================= +// ExponentialContact::Parameters +//============================================================================= +//_____________________________________________________________________________ +ExponentialContact::Parameters:: +Parameters() { + setNull(); + constructProperties(); +} +//_____________________________________________________________________________ +ExponentialContact::Parameters:: +Parameters(const SimTK::ExponentialSpringParameters& params) { + setNull(); + _stkparams = params; + constructProperties(); +} +//_____________________________________________________________________________ +void ExponentialContact::Parameters::setNull() { + setAuthors("F. C. Anderson"); +} +//_____________________________________________________________________________ +void +ExponentialContact::Parameters:: +constructProperties() { + SimTK::Vec3 shape; + _stkparams.getShapeParameters(shape[0], shape[1], shape[2]); + constructProperty_exponential_shape_parameters(shape); + constructProperty_normal_viscosity(_stkparams.getNormalViscosity()); + constructProperty_max_normal_force(_stkparams.getMaxNormalForce()); + constructProperty_friction_elasticity(_stkparams.getFrictionElasticity()); + constructProperty_friction_viscosity(_stkparams.getFrictionViscosity()); + constructProperty_settle_velocity(_stkparams.getSettleVelocity()); + constructProperty_initial_mu_static(_stkparams.getInitialMuStatic()); + constructProperty_initial_mu_kinetic(_stkparams.getInitialMuKinetic()); +} +//_____________________________________________________________________________ +// Update the Properties based on the SimTK::ExponentialSpringParameters. +void +ExponentialContact::Parameters:: +updateProperties() { + SimTK::Vec3 shape; + _stkparams.getShapeParameters(shape[0], shape[1], shape[2]); + set_exponential_shape_parameters(shape); + set_normal_viscosity(_stkparams.getNormalViscosity()); + set_max_normal_force(_stkparams.getMaxNormalForce()); + set_friction_elasticity(_stkparams.getFrictionElasticity()); + set_friction_viscosity(_stkparams.getFrictionViscosity()); + set_settle_velocity(_stkparams.getSettleVelocity()); + set_initial_mu_static(_stkparams.getInitialMuStatic()); + set_initial_mu_kinetic(_stkparams.getInitialMuKinetic()); +} +//_____________________________________________________________________________ +// Update the SimTK::ExponentialSpringParamters based on the Properties. +void +ExponentialContact::Parameters:: +updateParameters() { + const SimTK::Vec3 shape = get_exponential_shape_parameters(); + _stkparams.setShapeParameters(shape[0], shape[1], shape[2]); + _stkparams.setNormalViscosity(get_normal_viscosity()); + _stkparams.setMaxNormalForce(get_max_normal_force()); + _stkparams.setFrictionElasticity(get_friction_elasticity()); + _stkparams.setFrictionViscosity(get_friction_viscosity()); + _stkparams.setSettleVelocity(get_settle_velocity()); + _stkparams.setInitialMuStatic(get_initial_mu_static()); + _stkparams.setInitialMuKinetic(get_initial_mu_kinetic()); +} +//_____________________________________________________________________________ +// This method is needed to ensure that the SimTK::ExponentialSpringParameters +// member variable (_stkparam) is kept consistent with the properties. +// It is necessary to have the _stkparams member variable because there needs +// to be a place to store non-default parameters when the underlying +// SimTK::ExponentialSpringForce hasn't yet been instantiated. +// Having to do a little extra bookkeeping (i.e., storing properties values +// twice [once in the Properties and once in _stkparams]) is justified +// by not having to rewrite a whole bunch of additional accessor methods. +// All parameter set/gets go through the SimTK::ExponentialSpringParameters +// interface (i.e., through _stkparams). +void +ExponentialContact::Parameters:: +updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) { + Super::updateFromXMLNode(node, versionNumber); + updateParameters(); // catching any invalid property values in the process + updateProperties(); // pushes valid parameters back to the properties. +} +//_____________________________________________________________________________ +// Note that the OpenSim Properties are updated as well. +void +ExponentialContact::Parameters:: +setSimTKParameters(const SimTK::ExponentialSpringParameters& params) { + _stkparams = params; + updateProperties(); +} +//_____________________________________________________________________________ +// Get a read-only reference to the SimTK::ExponentialSpringParameters held +// by this instance. +const SimTK::ExponentialSpringParameters& +ExponentialContact::Parameters:: +getSimTKParameters() const { + return _stkparams; +} + + +//============================================================================= +// ExponentialContact +//============================================================================= +//_____________________________________________________________________________ +ExponentialContact::ExponentialContact() { + setNull(); + constructProperties(); +} +//_____________________________________________________________________________ +ExponentialContact:: +ExponentialContact(const SimTK::Transform& contactPlaneXform, + const std::string& bodyName, const SimTK::Vec3& station, + SimTK::ExponentialSpringParameters params) +{ + setNull(); + constructProperties(); + setContactPlaneTransform(contactPlaneXform); + setBodyName(bodyName); + setBodyStation(station); + setParameters(params); +} +//_____________________________________________________________________________ +void +ExponentialContact:: +setNull() { + setAuthors("F. C. Anderson"); + _spr = NULL; +} +//_____________________________________________________________________________ +void +ExponentialContact:: +constructProperties() { + SimTK::Transform X_GP; + SimTK::Vec3 origin(0.0); + Parameters params; + constructProperty_contact_plane_transform(X_GP); + constructProperty_body_name(""); + constructProperty_body_station(origin); + constructProperty_contact_parameters(params); +} +//_____________________________________________________________________________ +void +ExponentialContact:: +updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) { + Super::updateFromXMLNode(node, versionNumber); +} +//_____________________________________________________________________________ +void +ExponentialContact:: +extendConnectToModel(OpenSim::Model& model) { + // Allow based class to connect first + Super::extendConnectToModel(model); + + // Find the OpenSim::Body + const string& bodyName = getBodyName(); + if (getModel().hasComponent(bodyName)) + _body = &(getModel().getComponent(bodyName)); + else + _body = &(getModel().getComponent( + "./bodyset/" + bodyName)); +} +//_____________________________________________________________________________ +void +ExponentialContact:: +extendAddToSystem(SimTK::MultibodySystem& system) const { + // Extend the OpenSim::Force parent + Super::extendAddToSystem(system); + + // Construct the SimTK::ExponentialContact object + SimTK::GeneralForceSubsystem& forces = _model->updForceSubsystem(); + const SimTK::Transform& XContactPlane = get_contact_plane_transform(); + const SimTK::Vec3& station = get_body_station(); + SimTK::ExponentialSpringForce* spr = + new SimTK::ExponentialSpringForce(forces, XContactPlane, + _body->getMobilizedBody(), station, getParameters()); + + // Get the subsystem index so we can access the SimTK::Force later. + ExponentialContact* mutableThis = + const_cast(this); + mutableThis->_spr = spr; + mutableThis->_index = spr->getForceIndex(); + + // Expose the discrete states of ExponentialSpringForce in OpenSim + bool allocate = false; + std::string name = getMuStaticDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); + name = getMuKineticDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); + name = getSlidingDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); + name = getAnchorPointDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); +} + + +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023) +// This method is needed because class OpenSim::ExponentialContact has 4 +// discrete states that are allocated in +// SimTK::ExponentialSpringForce::realizeTopology(), not in +// OpenSim::Component::extendRealizeTopology(). +// These states are added to the OpenSim map of discrete states +// (i.e., Component::_namedDiscreteVariableInfo) so that they are accessible +// in OpenSim. See ExponentialContact::extendAddToSystem(). However, because +// these discrete states are not allocated by OpenSim::Component, OpenSim has +// no knowledge of their indices into the SimTK::State. The purpose of this +// method is to properly initialize those indices. +void +ExponentialContact:: +extendRealizeTopology(SimTK::State& state) const { + + Super::extendRealizeTopology(state); + + // A critical question... + // Can we guarrantee that ExponentialSpringForce::realizeTopology() will + // always be called before this method !?!? + // + // Maybe. Perhaps the ExponentialSpringForce object will always + // be listed in the SimTK::System before the ExponentialContact component? + // If not, then it is possible that getMuStaticStateIndex() will + // return a bad index. + // + // What I can confirm is that, so far, the indices in multiple tests have + // been assigned correctly. + // + // If there are mysterious failures because of a bad State index, the + // source of the issue may be that ExponentialSpringForce::realizeTopology + // was not called prior to ExponentialContact::extendRealizeTopology. + + const SimTK::Subsystem* subsys = getSubsystem(); + SimTK::DiscreteVariableIndex index; + std::string name; + + name = getMuStaticDiscreteStateName(); + index = _spr->getMuStaticStateIndex(); + updDiscreteVariableIndex(name, index, subsys); + + name = getMuKineticDiscreteStateName(); + index = _spr->getMuKineticStateIndex(); + updDiscreteVariableIndex(name, index, subsys); + + name = getSlidingDiscreteStateName(); + index = _spr->getSlidingStateIndex(); + updDiscreteVariableIndex(name, index, subsys); + + name = getAnchorPointDiscreteStateName(); + index = _spr->getAnchorPointStateIndex(); + updDiscreteVariableIndex(name, index, subsys); +} + + + +//----------------------------------------------------------------------------- +// Utility +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +ExponentialContact:: +resetAnchorPoint(SimTK::State& state) const { + _spr->resetAnchorPoint(state); +} +//_____________________________________________________________________________ +void +ExponentialContact:: +resetAnchorPoints(OpenSim::ForceSet& fSet, SimTK::State& state) { + int i; + int n = fSet.getSize(); + for (i = 0; i < n; ++i) { + try { + ExponentialContact& ec = + dynamic_cast(fSet.get(i)); + ec.resetAnchorPoint(state); + } catch (const std::bad_cast&) { + // Nothing should happen here. Execution is just skipping any + // OpenSim::Force that is not an ExponentialContact. + } + } +} + +//----------------------------------------------------------------------------- +// ACCESSORS for properties +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void ExponentialContact:: +setContactPlaneTransform(const SimTK::Transform& XContactPlane) { + set_contact_plane_transform(XContactPlane); +} +//_____________________________________________________________________________ +const SimTK::Transform& +ExponentialContact::getContactPlaneTransform() const { + return get_contact_plane_transform(); +} + +//_____________________________________________________________________________ +void +ExponentialContact:: +setParameters(const SimTK::ExponentialSpringParameters& params) { + ExponentialContact::Parameters& p = upd_contact_parameters(); + p.setSimTKParameters(params); + // The following call will invalidate the System at Stage::Topology + if (_spr != NULL) _spr->setParameters(params); +} +//_____________________________________________________________________________ +const SimTK::ExponentialSpringParameters& +ExponentialContact:: +getParameters() const { + return get_contact_parameters().getSimTKParameters(); +} + +//----------------------------------------------------------------------------- +// ACCESSORS for states +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +ExponentialContact:: +setMuStatic(SimTK::State& state, SimTK::Real mus) { + _spr->setMuStatic(state, mus); +} +//_____________________________________________________________________________ +SimTK::Real +ExponentialContact:: +getMuStatic(const SimTK::State& state) const { + return _spr->getMuStatic(state); +} + +//_____________________________________________________________________________ +void +ExponentialContact:: +setMuKinetic(SimTK::State& state, SimTK::Real mus) { + _spr->setMuKinetic(state, mus); +} +//_____________________________________________________________________________ +SimTK::Real +ExponentialContact:: +getMuKinetic(const SimTK::State& state) const { + return _spr->getMuKinetic(state); +} + +//_____________________________________________________________________________ +SimTK::Real +ExponentialContact:: +getSliding(const SimTK::State& state) const { + return _spr->getSliding(state); +} + +//----------------------------------------------------------------------------- +// ACCESSORS for data cache entries +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getNormalForceElasticPart(const State& state, bool inGround) const { + return _spr->getNormalForceElasticPart(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getNormalForceDampingPart(const State& state, bool inGround) const { + return _spr->getNormalForceDampingPart(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getNormalForce(const State& state, bool inGround) const { + return _spr->getNormalForce(state, inGround); +} +//_____________________________________________________________________________ +Real +ExponentialContact:: +getMu(const State& state) const { + return _spr->getMu(state); +} +//_____________________________________________________________________________ +Real +ExponentialContact:: +getFrictionForceLimit(const SimTK::State& state) const { + return _spr->getFrictionForceLimit(state); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getFrictionForceElasticPart(const State& state, bool inGround) const { + return _spr->getFrictionForceElasticPart(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getFrictionForceDampingPart(const State& state, bool inGround) const { + return _spr->getFrictionForceDampingPart(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getFrictionForce(const State& state, bool inGround) const { + return _spr->getFrictionForce(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getForce(const State& state, bool inGround) const { + return _spr->getForce(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getStationPosition(const State& state, bool inGround) const { + return _spr->getStationPosition(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getStationVelocity(const State& state, bool inGround) const { + return _spr->getStationVelocity(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getAnchorPointPosition(const State& state, bool inGround) const { + return _spr->getAnchorPointPosition(state, inGround); +} + +//----------------------------------------------------------------------------- +// Reporting +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +OpenSim::Array +ExponentialContact:: +getRecordLabels() const { + OpenSim::Array labels(""); + string name = getName(); // Name of this contact instance. + std::string frameName = getBodyName(); + std::string groundName = getModel().getGround().getName(); + + // Record format consistent with HuntCrossleyForce. + // Body + labels.append(name + "." + frameName + ".force.X"); + labels.append(name + "." + frameName + ".force.Y"); + labels.append(name + "." + frameName + ".force.Z"); + labels.append(name + "." + frameName + ".torque.X"); + labels.append(name + "." + frameName + ".torque.Y"); + labels.append(name + "." + frameName + ".torque.Z"); + // Ground + labels.append(name + "." + groundName + ".force.X"); + labels.append(name + "." + groundName + ".force.Y"); + labels.append(name + "." + groundName + ".force.Z"); + labels.append(name + "." + groundName + ".torque.X"); + labels.append(name + "." + groundName + ".torque.Y"); + labels.append(name + "." + groundName + ".torque.Z"); + + return labels; +} +//_____________________________________________________________________________ +OpenSim::Array +ExponentialContact:: +getRecordValues(const SimTK::State& state) const { + OpenSim::Array values(0.0); + + const auto& forceSubsys = getModel().getForceSubsystem(); + const SimTK::Force& abstractForce = forceSubsys.getForce(_index); + const auto& spr = (SimTK::ExponentialSpringForce&)(abstractForce); + + // Get the loads + SimTK::Vector_ bForces(0); // body + SimTK::Vector_ pForces(0); // particle + SimTK::Vector mForces(0); // mobility + spr.calcForceContribution(state, bForces, pForces, mForces); + + // Body + SimTK::Vec3 force; + SimTK::Vec3 torque; + const auto& bodyIndex = _body->getMobilizedBodyIndex(); + SimTK::SpatialVec& bodyForce = bForces(bodyIndex); + force = bodyForce[1]; + double fy = force[1]; + torque = bodyForce[0]; + values.append(3, &force[0]); + values.append(3, &torque[0]); + + // Ground + const SimTK::MultibodySystem& system = _model->getSystem(); + const SimTK::SimbodyMatterSubsystem& matter = system.getMatterSubsystem(); + const SimTK::MobilizedBody& ground = matter.getGround(); + const auto& groundIndex = ground.getMobilizedBodyIndex(); + SimTK::SpatialVec& groundForce = bForces(groundIndex); + force = groundForce[1]; + torque = groundForce[0]; + values.append(3, &force[0]); + values.append(3, &torque[0]); + + return values; +} + +//----------------------------------------------------------------------------- +// Internal Testing +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +ExponentialContact:: +assertPropertiesAndParametersEqual() const { + const ExponentialContact::Parameters& a = get_contact_parameters(); + const SimTK::ExponentialSpringParameters& b = getParameters(); + + const SimTK::Vec3& vecA = a.get_exponential_shape_parameters(); + SimTK::Vec3 vecB; + b.getShapeParameters(vecB[0], vecB[1], vecB[2]); + SimTK_TEST_EQ(vecA[0], vecB[0]); + SimTK_TEST_EQ(vecA[1], vecB[1]); + SimTK_TEST_EQ(vecA[2], vecB[2]); + + double valA, valB; + valA = a.get_normal_viscosity(); + valB = b.getNormalViscosity(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_friction_elasticity(); + valB = b.getFrictionElasticity(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_friction_viscosity(); + valB = b.getFrictionViscosity(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_settle_velocity(); + valB = b.getSettleVelocity(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_initial_mu_static(); + valB = b.getInitialMuStatic(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_initial_mu_kinetic(); + valB = b.getInitialMuKinetic(); + SimTK_TEST_EQ(valA, valB); +} diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h new file mode 100644 index 0000000000..4d88c947ac --- /dev/null +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -0,0 +1,682 @@ +#ifndef OPENSIM_EXPONENTIAL_CONTACT_H_ +#define OPENSIM_EXPONENTIAL_CONTACT_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: ExponentialContactForce.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) 2022 Stanford University and the Authors * + * Author(s): F. C. Anderson * + * * + * 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 "SimTKsimbody.h" +#include "Force.h" +#include "OpenSim/Common/Set.h" +#include "OpenSim/Simulation/Model/ForceSet.h" + +namespace OpenSim { + +//============================================================================= +// ExponentialContact +//============================================================================= +/** Class ExponentialContact uses an "exponential spring" as a means +of modeling contact of a specified point on a Body with a contact +plane that is fixed to Ground. This specified point is referred to in this +documentation as the "body station". Each ExponentialContact instance +acts at only one body station. In practice, you should choose a number of +body stations strategically located across the surface of a Body, +and construct an ExponentialContact instance for each of those body stations. +For example, if the Body were a cube, you would likely choose the body +stations to be the corners of the cube and construct an ExponentialContact +instance for each corner of the cube (so a total of 8 instances). The contact +plane is typically used to model interactions with a floor, but is not +limited to this use case. The contact plane can be rotated and translated +relative to the Ground frame and so can be used to model a wall, ramp, or +some other planar structure. + +Aspects of the exponential contact model are described in the following +publication: + + Anderson F.C. and Pandy M.G. (1999). A dynamics optimization + solution for vertical jumping in three dimensions. Computer Methods + in Biomechanics and Biomedical Engineering 2(3):201-231. + +Under the covers, the OpenSim::ExponentialContact class encapsulates two +SimTK objects: ExponentialSpringForce and ExponentialSpringParameters. +For the details concerning these classes, see the Simbody API +documentation. A condensed version of that documentation is provided here. + +---------------------------------- +Computations and Coordinate Frames +---------------------------------- +The positive z-axis of the contact plane defines its normal. The positive +z-axis is the axis along which the repelling normal force (modeled using an +exponential) is applied. The x-axis and y-axis of the contact plane together +define the tangent plane. Friction forces will always be tangent to the x-y +plane. + +In the equations below, all quantities are expressed in the frame of the +contact plane. A variable with a "z" suffix (e.g., pz, vz, or cz) refers +to a quantity that is normal to the contact plane or that pertains to +calculation of the normal force. A variable with an "xy" suffix +(e.g., pxy, vxy, or cxy) refers to a quantity that lies in or is tangent to +the contact plane or that pertains to calculation of the friction force. + +### Normal Force (positive z-axis) + +The elastic part of the normal force is computed using an exponential +whose shape is a function of three parameters (d₀, d₁, and d₂): + + fzElastic = d₁exp(−d₂(pz−d₀)) + +Note that pz is the displacement of the body station above (pz > 0.0) +or below (pz < 0.0) the contact plane. The default values of the shape +parameters were chosen to maximize integration step size while maintaining a +number of constraints (e.g., the normal force should fall below 0.01 Newtons +when pz > 1.0 cm). The damping part of the normal force is linear in velocity +and scaled by the elastic part: + + fzDamping = −cz vz fzElastic, + +where vz is the normal component of the velocity of the body station and +cz is the damping coefficient for the normal direction. All together, the +spring force in the normal direction is given by + + fz = fzElastic + fzDamping + = d₁exp(d₂(py−d₀)) − cz vz d₁exp(d₂(pz−d₀))) + = d₁exp(d₂(pz−d₀)) (1 − cz vz) + +which has the form of the Hunt & Crossley damping model: + + K. H. Hunt and F. R. E. Crossley (1975). Coefficient of Restitution + Interpreted as Damping in Vibroimpact. ASME Journal of Applied + Mechanics, pp. 440-445. + +### Friction Force (x-y plane) + +The friction force is computed by blending two different friction models. +The blending is performed based on the 'Sliding' State of the +ExponentialSpringForce class. + +#### Friction Model 1 - Pure Damping (Sliding = 1.0) +When the body station is sliding with respect to the contact plane, the +friction force is computed using a simple damping term: + + fricDamp = −cxy vxy + +where cxy is the damping coefficient in the contact plane and vxy is the +velocity of the body station in the contact plane. The magnitude of the +total frictional force is not allowed to exceed the frictional limit: + + fricLimit = μ fz + if (|fricDamp| > fricLimit) + fricDamp = −fricLimit vxy / |vxy| = −μ fz vxy / |vxy| + +where μ is the instantaneous coefficient of friction (more below). Note that +fz is always positive and so fricLimit is a positive scalar. Thus, for +velocities in the contact plane above some threshold velocity, which is +typically small (i.e., less than 0.1 m/s), this model is consistent with a +standard Coulomb Friction model. + +#### Friction Model 2 - Damped Linear Spring (Sliding = 0.0) +When the body station is anchored with respect to the contact plane, the +friction force is represented by a damped linear spring. The viscous term is +given by the same damping expression as above: + + fricDampSpr = −cxy vxy + +and the elastic term is given by + + fricElasSpr = −kxy (pxy−p₀) + +where kxy is the friction spring elasticity, pxy is the position of the body +station projected onto the contact plane, and p₀ is the current spring zero +(i.e., the elastic anchor point of the friction spring). Note that p₀ always +resides in the contact plane. + +The total friction spring force is then given by the sum of the elastic and +viscous terms: + + fricSpr = fricElasSpr + fricDampSpr + +If the magnitude of the fricSpr exceeds the magnitude of the friction limit, +the terms are scaled down: + + if(|fricSpr| > fricLimit) + scaleFactor = fricLimit / |fricSpr| + fricDampSpr = scaleFactor * fricDampSpr + fricElasSpr = scaleFactor * fricElasSpr + fricSpr = fricElasSpr + fricDampSpr + +Scaling down the friction spring force does not alter its direction. + +#### Blending the Friction Models +Blending Model 1 and Model 2 is accomplished using linear expressions of the +Sliding State: + + fricElasBlend = fricElasSpr * (1.0 − Sliding) + fricDampBlend = fricDampSpr + (fricDamp − fricDampSpr)*Sliding + fricBlend = fricElasBlend + fricDampBlend + +Model 1 (Pure Damping) dominates as Sliding → 1.0, and Model 2 +(Damped Linear Spring) dominates as Sliding → 0.0. + +#### Moving the Friction Spring Zero +The friction spring zero (p₀) (the elastic anchor point) is always altered +to be consistent with the final value of the blended elastic force: + + p₀ = pxy + fricElasBlend / kxy; + p₀[2] = 0.0; // ensures that p₀ lies in the contact plane + +#### Coefficients of Friction +Coefficients of kinetic (sliding) and static (fixed) friction can be specified +for the spring, subject to the following constraints: + + 0.0 ≤ μₖ ≤ μₛ + +Note that there is no upper bound on μₛ. The instantaneous coefficient of +friction (μ) is calculated based on the value of the Sliding State: + + μ = μₛ − Sliding*(μₛ − μₖ) + +The value of Sliding is calculated using a continuous function of the +ratio of the speed of the elastic anchor point (ṗ₀) to the settle velocity +(vSettle). vSettle is a customizable topology-stage parameter that represents +the speed at which a body station settles into a static state with respect to +the contact plane. See SimTK::ExponentialSpringParameters::setSettleVelocity() +for details. In particular, + + ṗ₀ = |Δp₀| / Δt + Sliding = SimTK::stepUp( SimTK::clamp(0.0, ṗ₀/vSettle, 1.0) ) + +where Δp₀ is the change in p₀ and Δt is the change in time since the last +successful integration step. When ṗ₀ ≥ vSettle, Sliding = 1.0, and as +ṗ₀ → 0.0, Sliding → 0.0. + +----------------------- +CUSTOMIZABLE PARAMETERS +----------------------- +Customizable Topology-stage parameters specifying the characteristics of the +exponential spring are managed using SimTK::ExponentialSpringParameters. +To customize any of the Topology-stage parameters on an ExponentialContact +instance, you should + +1) Create an ExponentialSpringParameters object. For example, + + ExponentialSpringParameters myParams; + +2) Use the available 'set' methods in ExponentialSpringParamters to change +the parameters of that object. For example, + + myParams.setNormalViscosity(0.25); + +3) Use ExponentialContact::setParameters() to alter the parameters of one +(or many) ExponentialContact instances. For example, + + ExponentialContact spr1, spr2; + spr1.setParameters(myParams); + spr2.setParameters(myParams); + +4) Realize the system to Stage::Topology. When a new set of parameters is +set on an ExponentialContact instance, as above in step 3, the System will +be invalidated at Stage::Topology. The System must therefore be realized at +Stage::Topology (and hence Stage::Model) before a simulation can proceed. + + system.realizeTopology(); + +Note that each ExponentialContact instance owns its own private +ExponentialSpringParameters object. The myParams object is just used to set +the desired parameter values of the privately owned parameters object. It is +fine for objects like myParams to go out of scope or for myParams objects +allocated from the heap to be deleted. + +Therefore, also note that the parameter values possessed by an +ExponentialContact instance do not necessarily correspond to the values +held by a local instance of ExponentialSpringParameters until a call to +ExponentialContact::setParameters() is made. + +The default values of the parameters are expressed in units of Newtons, +meters, seconds, and kilograms; however, you may use an alternate set of +self-consistent units by re-specifying all parameters. + +The default values of the parameters work well for typical contact +interactions, but clearly may not be appropriate for simulating many contact +interactions. For the full descriptions of the contact parameters see the +Simbody API documentation for SimTK::ExponentialSpringParameters. + +@author F. C. Anderson **/ +class OSIMSIMULATION_API ExponentialContact : public Force { + OpenSim_DECLARE_CONCRETE_OBJECT(ExponentialContact, Force); + +public: + class Parameters; + + //------------------------------------------------------------------------- + // PROPERTIES + //------------------------------------------------------------------------- + OpenSim_DECLARE_PROPERTY(contact_plane_transform, SimTK::Transform, + "Orientation and location of the contact plane wrt Ground. The positive z-axis of the contact plane defines the normal."); + OpenSim_DECLARE_PROPERTY(body_name, std::string, + "Name of the Body to which the resultant contact force is applied."); + OpenSim_DECLARE_PROPERTY(body_station, SimTK::Vec3, + "Point on the body, expressed in the Body frame, at which the resultant contact force is applied."); + OpenSim_DECLARE_PROPERTY(contact_parameters, ExponentialContact::Parameters, + "Customizable topology-stage parameters."); + + //------------------------------------------------------------------------- + // Construction + //------------------------------------------------------------------------- + /** Default constructor. */ + ExponentialContact(); + + /** Construct an ExponentialContact instance. + @param X_GP Transform specifying the location and orientation of the + contact plane frame (P) with respect to the Ground frame (G). The positive + z-axis of P defines the normal direction; the x-axis and y-axis of P + together define the tangent (or friction) plane. Note that X_GP is the + operator that transforms a point of P (point_P) to that same point in + space but measured from the Ground origin (G₀) and expressed in G + (i.e., point_G = X_GP * point_P). + @param bodyName Name of the body to which the force is applied. + @param station Point on the body at which the force is applied. + @param params Optional parameters object used to customize the + topology-stage characteristics of the contact model. */ + explicit ExponentialContact(const SimTK::Transform& X_GP, + const std::string& bodyName, const SimTK::Vec3& station, + SimTK::ExponentialSpringParameters params = + SimTK::ExponentialSpringParameters()); + + /** Destructor. */ + ~ExponentialContact() { + if (_spr != NULL) delete _spr; + } + + //------------------------------------------------------------------------- + // Utility + //------------------------------------------------------------------------- + /** Reset the elastic anchor point (friction spring zero) so that it + coincides with the projection of the body station onto the contact + plane. This step is often needed at the beginning of a simulation to + ensure that a simulation does not begin with large friction forces. + After this call, the elastic portion of the friction force should be 0.0 + Calling this method will invalidate the System at Stage::Dynamics. + @param state State object on which to base the reset. */ + void resetAnchorPoint(SimTK::State& state) const; + + /** Reset the elastic anchor points (friction spring zeros) of all + ExponentialContact instances in an OpenSim::ForceSet. This step is often + needed at the beginning of a simulation to ensure that a simulation does + not begin with large friction forces. Calling this method will invalidate + the System at Stage::Dynamics. + @param fSet Force set. + @param state State object on which to base the reset. */ + static void resetAnchorPoints(OpenSim::ForceSet& fSet, SimTK::State& state); + + //------------------------------------------------------------------------- + // Accessors for properties + //------------------------------------------------------------------------- + /** Set the transform that specifies the location and orientation of the + contact plane in the Ground frame. */ + void setContactPlaneTransform(const SimTK::Transform& X_GP); + /** Get the transform that specifies the location and orientation of the + contact plane in the Ground frame. */ + const SimTK::Transform& getContactPlaneTransform() const; + + /** Set the name of the body to which this force is applied. */ + void setBodyName(const std::string& bodyName) { + set_body_name(bodyName); + } + /** Get the name of the body to which this force is applied. */ + const std::string& getBodyName() const { return get_body_name(); } + + /** Set the point on the body at which this force is applied. */ + void setBodyStation(const SimTK::Vec3& station) { + set_body_station(station); + } + /** Get the point on the body at which this force is applied. */ + const SimTK::Vec3& getBodyStation() const { return get_body_station(); } + + /** Set the customizable Topology-stage spring parameters. Calling this + method will invalidate the SimTK::System at Stage::Toplogy. */ + void setParameters(const SimTK::ExponentialSpringParameters& params); + /** Get the customizable Topology-stage spring parameters. Use the copy + constructor on the returned reference to create an object that can + be altered. */ + const SimTK::ExponentialSpringParameters& getParameters() const; + + //------------------------------------------------------------------------- + // Accessors for Discrete States + //------------------------------------------------------------------------- + /** Get a pointer to the SimTK::Subsystem from which this + ExponentialContact instance allocates its discrete states. */ + const SimTK::Subsystem* getSubsystem() const { + return &_spr->getForceSubsystem(); + } + + /** Get the name used for the discrete state representing the static + coefficient of friction (μₛ). This name is used to access informattion + related to μₛ via the OpenSim::Component API. For example, see + Component::getDiscreteVariableValue(). */ + std::string getMuStaticDiscreteStateName() const { return "mu_static"; } + + /** Set the static coefficient of friction (μₛ) for this exponential + spring. μₛ is a discrete state. The value of μₛ is held in the System's + State object. Unlike the parameters managed by ExponentialSpringParameters, + μₛ can be set at any time during a simulation. A change to μₛ will + invalidate the System at Stage::Dynamics, but not at Stage::Topology. + @param state State object that will be modified. + @param mus %Value of the static coefficient of friction. No upper bound. + 0.0 ≤ μₛ. If μₛ < μₖ, μₖ is set equal to μₛ. */ + void setMuStatic(SimTK::State& state, SimTK::Real mus); + + /** Get the static coefficient of friction (μₛ) held by the specified + state for this exponential contact instance. + @param state State object from which to retrieve μₛ. */ + SimTK::Real getMuStatic(const SimTK::State& state) const; + + /** Get the name used for the discrete state representing the kinetic + coefficient of friction (μₖ). This name is the used to access informattion + related to μₖ via the OpenSim::Component API. For example, see + Component::getDiscreteVariableValue(). */ + std::string getMuKineticDiscreteStateName() const { return "mu_kinetic"; } + + /** Set the kinetic coefficient of friction (μₖ) for this exponential + spring. The value of μₖ is held in the System's State object. Unlike the + parameters managed by ExponentialSpringParameters, μₖ can be set at any + time during a simulation. A change to μₖ will invalidate the System at + Stage::Dynamics. + @param state State object that will be modified. + @param muk %Value of the kinetic coefficient of friction. No upper bound. + 0.0 ≤ μₖ. If μₖ > μₛ, μₛ is set equal to μₖ. */ + void setMuKinetic(SimTK::State& state, SimTK::Real muk); + + /** Get the kinetic coefficient of friction (μₖ) held by the specified + state for this exponential contact instance. + @param state State object from which to retrieve μₖ. */ + SimTK::Real getMuKinetic(const SimTK::State& state) const; + + /** Get the name used for the discrete state representing the 'sliding' + state (K) of the elastic anchor point. This name is used to access + informattion related to K via the OpenSim::Component API. For example, see + Component::getDiscreteVariableValue(). */ + std::string getSlidingDiscreteStateName() const { return "sliding"; } + + /** Get the Sliding state of this exponential contact instance after it + has been updated to be consistent with the System State. The System must + be realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. */ + SimTK::Real getSliding(const SimTK::State& state) const; + + /** Get the name used for the discrete state representing the position + of the elastic anchor point (p₀). This name is used to access + informattion related to p₀ via the OpenSim::Component API. For example, + see Component::getDiscreteVariableAbstractValue(). */ + std::string getAnchorPointDiscreteStateName() const { return "anchor"; } + + /** Get the position of the elastic anchor point (p₀) after it has been + updated to be consistent with friction limits. p₀ is the spring zero of + the damped linear spring used in Friction Model 2. The system must be + realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getAnchorPointPosition( + const SimTK::State& state, bool inGround = true) const; + + //------------------------------------------------------------------------- + // Accessors for data cache entries + //------------------------------------------------------------------------- + /** Get the elastic part of the normal force. The system must be realized + to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getNormalForceElasticPart( + const SimTK::State& state, bool inGround = true) const; + + /** Get the damping part of the normal force. The system must be realized + to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getNormalForceDampingPart( + const SimTK::State& state, bool inGround = true) const; + + /** Get the normal force. The system must be realized to Stage::Dynamics + to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getNormalForce( + const SimTK::State& state, bool inGround = true) const; + + /** Get the instantaneous coefficient of friction (μ). The system must be + realized to Stage::Dynamics to access this data. μ is obtained by using + the Sliding state to transition between μₖ and μₛ: + + μ = μₛ - Sliding*(μₛ - μₖ) + + Because 0.0 ≤ Sliding ≤ 1.0, μₖ ≤ μ ≤ μₛ. + @param state State object on which to base the calculations. */ + SimTK::Real getMu(const SimTK::State& state) const; + + /** Get the friction limit. The system must be realized to Stage::Dynamics + to access this data. + @param state State object on which to base the calculations. */ + SimTK::Real getFrictionForceLimit(const SimTK::State& state) const; + + /** Get the elastic part of the friction force. The system must be + realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getFrictionForceElasticPart( + const SimTK::State& state, bool inGround = true) const; + + /** Get the damping part of the friction force. The system must be + realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getFrictionForceDampingPart( + const SimTK::State& state, bool inGround = true) const; + + /** Get the total friction force. The total frictional force is always + just the sum of the elastic part and the damping part of the frictional + force, which may be obtained separately by calling + getFrictionalForceElasticPart() and getFrictionalForceDampingPart(). + The system must be realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getFrictionForce( + const SimTK::State& state, bool inGround = true) const; + + /** Get the total force applied to the body by this + ExponentialSpringForce instance. The total force is the vector sum of the + friction force, which may be obtained by a call to getFrictionForce(), and + the normal force, which may be obtained by a call to getNormalForce(). + The system must be realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getForce( + const SimTK::State& state, bool inGround = true) const; + + /** Get the position of the body station (i.e., the point on the body at + which the force generated by this ExponentialSpringForce is applied). This + method differs from getStation() in terms of the frame in which the + station is expressed. getStation() expresses the point in the frame of + the MobilizedBody. getStationPosition() expresses the point either in the + Ground frame or in the frame of the Contact Plane. The system must be + realized to Stage::Position to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getStationPosition( + const SimTK::State& state, bool inGround = true) const; + + /** Get the velocity of the body station (i.e., the point on the body at + which the force generated by this ExponentialSpringForce is applied). + The system must be realized to Stage::Velocity to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getStationVelocity( + const SimTK::State& state, bool inGround = true) const; + + //------------------------------------------------------------------------- + // Reporting + //------------------------------------------------------------------------- + /** Provide name(s) of the quantities (column labels) of the value(s) + to be reported. */ + OpenSim::Array getRecordLabels() const override; + /** Provide the value(s) to be reported that correspond to the labels. */ + OpenSim::Array getRecordValues( + const SimTK::State& state) const override; + + //------------------------------------------------------------------------- + // Internal Testing + //------------------------------------------------------------------------- + /** Assess consistency between Properties and internal parameters. */ + void assertPropertiesAndParametersEqual() const; + +protected: + /** Connect to the OpenSim Model. */ + void extendConnectToModel(Model& model) override; + + /** Create a SimTK::ExponentialSpringForce object that implements + this Force. */ + void extendAddToSystem(SimTK::MultibodySystem& system) const override; + + /** Initialize discrete variable indices. */ + virtual void extendRealizeTopology(SimTK::State& state) const override; + + /** Update this Object base on an XML node. */ + void updateFromXMLNode(SimTK::Xml::Element& node, + int versionNumber) override; + +private: + void setNull(); + void constructProperties(); + SimTK::ReferencePtr _body; + SimTK::ExponentialSpringForce* _spr{NULL}; + +}; // END of class ExponentialContact + + +//============================================================================= +// ExponentialContact::Parameters +//============================================================================= +/** This subclass helps manage the topology-stage parameters of an +OpenSim::ExponentialContact object. It does not provide the interface for +getting and setting contact parameters (directly anyway) but rather provides +the infrastructure for making the underlying SimTK::ExponentialSpringForce and +SimTK::ExponentialSpringParameters classes available in OpenSim. + +More specifically, this class chiefly does 3 things: +- Implements OpenSim Properties for most of the customizable contact +parameters of class OpenSim::ExponentialContact, enabling those parameters +to be serialized and de-serialized from file. +- Provides a member variable (_stkparams) for storing user-set parameters +prior to the creation of an underlying SimTK::ExponentialSpringForce object. +During model initialization, when the SimTK::ExponetialSpringForce object is +constructed, the user-set parameters are then pushed to that object. +- Ensures that the values held by the OpenSim properties are kept consistent +with the values held by a SimTK::ExponentialSpringParameters object. +Depending on the circumstance, parameters are updated to match properties or +properties are update to match parameters. + +To get or set values of the parameters managed by this class, you should use +ExponentialContact::getParameters() and ExponentialContact::setParameters(). + +@author F. C. Anderson **/ +class ExponentialContact::Parameters : public Object { + OpenSim_DECLARE_CONCRETE_OBJECT(ExponentialContact::Parameters, Object); + +public: + OpenSim_DECLARE_PROPERTY(exponential_shape_parameters, SimTK::Vec3, + "Shape parameters for the exponential that models the normal force: d0 (0.0065905 m), d1 (0.5336 N), d2 (1150.0/m)."); + OpenSim_DECLARE_PROPERTY(normal_viscosity, double, + "Viscosity in the normal direction (0.5 s/m)."); + OpenSim_DECLARE_PROPERTY(max_normal_force, double, + "Maximum allowed normal force (100,000.0 N)."); + OpenSim_DECLARE_PROPERTY(friction_elasticity, double, + "Elasticity of the friction spring (20,000.0 N/m)."); + OpenSim_DECLARE_PROPERTY(friction_viscosity, double, + "Viscosity of the friction spring (282.8427 N*s/m)."); + OpenSim_DECLARE_PROPERTY(settle_velocity, double, + "Velocity below which static friction conditions are triggered (0.01 m/s) ."); + OpenSim_DECLARE_PROPERTY(initial_mu_static, double, + "Initial value of the static coefficient of friction."); + OpenSim_DECLARE_PROPERTY(initial_mu_kinetic, double, + "Initial value of the kinetic coefficient of friction."); + + /** Default constructor. */ + Parameters(); + + /** Construct an instance based on a SimTK::ExponentialSpringParameters + object. */ + Parameters(const SimTK::ExponentialSpringParameters& params); + + /** Set the underlying SimTK parameters. This method is used to maintain + consistency between OpenSim Properties and the underlying parameters. + The typical user of OpenSim::ExponentialContact will not have reason to + call this method. For setting contact parameters, the typical user should + call OpenSim::ExponentialContact::setParameters(). */ + void setSimTKParameters(const SimTK::ExponentialSpringParameters& params); + + /** Get a read-only reference to the underlying SimTK parameters. This + method is used to maintain consistency between OpenSim Properties and the + underlying parameters. The typical user of OpenSim::ExponentialContact will + not have reason to call this method. For getting contact parameters, the + typical user should call OpenSim::ExponentialContact::getParameters() */ + const SimTK::ExponentialSpringParameters& getSimTKParameters() const; + +private: + void setNull(); + void constructProperties(); + void updateParameters(); + void updateProperties(); + void updateFromXMLNode(SimTK::Xml::Element& node, + int versionNumber) override; + SimTK::ExponentialSpringParameters _stkparams; +}; + +} // end of namespace OpenSim + +#endif // OPENSIM_EXPONENTIAL_SPRING_FORCE_H_ diff --git a/OpenSim/Simulation/Model/HuntCrossleyForce.h b/OpenSim/Simulation/Model/HuntCrossleyForce.h index 28142908de..a31a86c512 100644 --- a/OpenSim/Simulation/Model/HuntCrossleyForce.h +++ b/OpenSim/Simulation/Model/HuntCrossleyForce.h @@ -32,7 +32,7 @@ namespace OpenSim { // HUNT CROSSLEY FORCE //============================================================================== /** This force subclass implements a Hunt-Crossley contact model. It uses Hertz -contact theory to model the interactions between a set of ContactSpheres and +contact theory to model the interactions between a set of ContactSpheres and ContactHalfSpaces. @author Peter Eastman **/ @@ -45,7 +45,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce, Force); //============================================================================== // PROPERTIES //============================================================================== - OpenSim_DECLARE_PROPERTY(contact_parameters, + OpenSim_DECLARE_PROPERTY(contact_parameters, HuntCrossleyForce::ContactParametersSet, "Material properties."); OpenSim_DECLARE_PROPERTY(transition_velocity, double, @@ -72,7 +72,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce, Force); * %Set the transition velocity for switching between static and dynamic friction. */ void setTransitionVelocity(double velocity); - + /** * Access to ContactParameters. Methods assume size 1 of ContactParametersSet and add one ContactParameter if needed */ @@ -92,7 +92,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce, Force); //----------------------------------------------------------------------------- // Reporting //----------------------------------------------------------------------------- - /** + /** * Provide name(s) of the quantities (column labels) of the force value(s) to be reported */ OpenSim::Array getRecordLabels() const override ; @@ -129,23 +129,18 @@ OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce::ContactParameters, Object); //============================================================================== OpenSim_DECLARE_LIST_PROPERTY(geometry, std::string, "Names of geometry objects affected by these parameters."); - OpenSim_DECLARE_PROPERTY(stiffness, double, - ""); - OpenSim_DECLARE_PROPERTY(dissipation, double, - ""); - OpenSim_DECLARE_PROPERTY(static_friction, double, - ""); - OpenSim_DECLARE_PROPERTY(dynamic_friction, double, - ""); - OpenSim_DECLARE_PROPERTY(viscous_friction, double, - ""); + OpenSim_DECLARE_PROPERTY(stiffness, double, ""); + OpenSim_DECLARE_PROPERTY(dissipation, double, ""); + OpenSim_DECLARE_PROPERTY(static_friction, double, ""); + OpenSim_DECLARE_PROPERTY(dynamic_friction, double, ""); + OpenSim_DECLARE_PROPERTY(viscous_friction, double, ""); //============================================================================== // PUBLIC METHODS //============================================================================== ContactParameters(); - ContactParameters(double stiffness, double dissipation, - double staticFriction, double dynamicFriction, + ContactParameters(double stiffness, double dissipation, + double staticFriction, double dynamicFriction, double viscousFriction); const Property& getGeometry() const; @@ -170,9 +165,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce::ContactParameters, Object); //============================================================================== // HUNT CROSSLEY FORCE :: CONTACT PARAMETERS SET //============================================================================== -class OSIMSIMULATION_API HuntCrossleyForce::ContactParametersSet +class OSIMSIMULATION_API HuntCrossleyForce::ContactParametersSet : public Set { -OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce::ContactParametersSet, +OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce::ContactParametersSet, Set); public: diff --git a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp index 2cfa3a5285..867da8c5ac 100644 --- a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp +++ b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp @@ -47,6 +47,7 @@ #include "Model/CoordinateLimitForce.h" #include "Model/CoordinateSet.h" #include "Model/ElasticFoundationForce.h" +#include "Model/ExponentialContact.h" #include "Model/HuntCrossleyForce.h" #include "Model/SmoothSphereHalfSpaceForce.h" #include "Model/Ligament.h" @@ -242,6 +243,8 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::registerType( ContactSphere() ); Object::registerType( CoordinateLimitForce() ); Object::registerType( SmoothSphereHalfSpaceForce() ); + Object::registerType( ExponentialContact() ); + Object::registerType( ExponentialContact::Parameters() ); Object::registerType( HuntCrossleyForce() ); Object::registerType( ElasticFoundationForce() ); Object::registerType( HuntCrossleyForce::ContactParameters() ); diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp new file mode 100644 index 0000000000..efcb863ea2 --- /dev/null +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -0,0 +1,925 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: testContactExponentialSpring.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) 2022-2023 Stanford University and the Authors * + * Author(s): F. C. Anderson * + * * + * 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 +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "SimTKsimbody.h" + +using namespace SimTK; +using namespace std; +using namespace OpenSim; + +//============================================================================= +/** Class ExponentialContactTester provides a scope and framework for +evaluating and testing the ExponentialContact class. Using a testing class, as +opposed to just a main() and C-style procedures, gets a lot of variables out +of the global scope and allows for more structured memory management. */ +class ExponentialContactTester +{ +public: + // Contact choices + enum ContactChoice { + Exp = 0, + Hunt, + Both + }; + + // Initial condition choices + enum InitialConditionsChoice{ + Static = 0, + Bounce, + Slide, + Spin, + SpinSlide, + SpinTop, + Tumble + }; + + // Constructor + ExponentialContactTester() { + corner[0] = Vec3( hs, -hs, hs); + corner[1] = Vec3( hs, -hs, -hs); + corner[2] = Vec3(-hs, -hs, -hs); + corner[3] = Vec3(-hs, -hs, hs); + corner[4] = Vec3( hs, hs, hs); + corner[5] = Vec3( hs, hs, -hs); + corner[6] = Vec3(-hs, hs, -hs); + corner[7] = Vec3(-hs, hs, hs); + }; + + // Destructor + ~ExponentialContactTester() { + if (model) { + model->disownAllComponents(); + delete model; + } + if (blockEC) delete blockEC; + if (blockHC) delete blockHC; + for (int i = 0; i < n; i++) { + if (sprEC[i]) delete sprEC[i]; + if (sprHC[i]) delete sprHC[i]; + if (geomHC[i]) delete geomHC[i]; + } + } + + // Command line parsing and usage + int parseCommandLine(int argc, char** argv); + void printUsage(); + void printConditions(); + + // Model Creation + void buildModel(); + OpenSim::Body* addBlock(const std::string& suffix); + void addExponentialContact(OpenSim::Body* body); + void addHuntCrossleyContact(OpenSim::Body* body); + void setForceData( + double t, const SimTK::Vec3& point, const SimTK::Vec3& force); + void setForceDataHeader(); + + // Test stuff not covered elsewhere. + void test(); + void testParameters(); + void testSerialization(); + void printDiscreteVariableAbstractValue(const string& pathName, + const AbstractValue& value) const; + void testDiscreteVariables(State& state, const ForceSet& fSet); + + // Simulation + void setInitialConditions(SimTK::State& state, + const SimTK::MobilizedBody& body, double dz); + void simulate(); + + //------------------------------------------------------------------------- + // Member variables + //------------------------------------------------------------------------- + private: + + // Simulation related + double integ_accuracy{1.0e-5}; + double dt_max{0.03}; + SimTK::Vec3 gravity{SimTK::Vec3(0, -9.8065, 0)}; + double mass{10.0}; + double tf{5.0}; + const static int n{8}; + const double hs{0.10}; // half of a side of a cube (like a radius) + Vec3 corner[n]; + + // Command line options and their defaults + ContactChoice whichContact{Exp}; + InitialConditionsChoice whichInit{Slide}; + bool noDamp{false}; + bool applyFx{false}; + bool showVisuals{false}; + + // Model and parts + Model* model{NULL}; + OpenSim::Body* blockEC{NULL}; + OpenSim::Body* blockHC{NULL}; + OpenSim::ExponentialContact* sprEC[n]{nullptr}; + OpenSim::HuntCrossleyForce* sprHC[n]{nullptr}; + OpenSim::ContactGeometry* geomHC[n]{nullptr}; + Storage fxData; + ExternalForce* fxEC{nullptr}; + ExternalForce* fxHC{nullptr}; + + // Reporters + StatesTrajectoryReporter* statesReporter{nullptr}; + +}; // End class ExponentialContactTester declarations + +//_____________________________________________________________________________ +int +ExponentialContactTester:: +parseCommandLine(int argc, char** argv) +{ + std::string option; + for (int i = 1; i < argc; ++i) { + + option = argv[i]; + + // Contact choice + if (option == "Exp") + whichContact = Exp; + else if (option == "Hunt") + whichContact = Hunt; + else if (option == "Both") + whichContact = Both; + + // Initial condition choice + else if (option == "Static") + whichInit = Static; + else if (option == "Bounce") + whichInit = Bounce; + else if (option == "Slide") + whichInit = Slide; + else if (option == "Spin") + whichInit = Spin; + else if (option == "SpinSlide") + whichInit = SpinSlide; + else if (option == "SpinTop") + whichInit = SpinTop; + else if (option == "Tumble") + whichInit = Tumble; + + // Turn off all dissipative terms + else if (option == "NoDamp") + noDamp = true; + + // Apply a horizontal ramping force + else if (option == "Fx") + applyFx = true; + + // Show the visuals + else if (option == "Vis") + showVisuals = true; + + // Unrecognized + else { + printUsage(); + return -1; + } + } + return 0; +} +//_____________________________________________________________________________ +void +ExponentialContactTester:: +printUsage() +{ + cout << endl << "Usage:" << endl; + cout << "$ testExponetialContact " + << "[InitCond] [Contact] [NoDamp] [Fx] [Vis]" << endl; + cout << "\tInitCond (choose one): Static Bounce Slide Spin "; + cout << "SpinSlide SpinTop Tumble" << endl; + cout << "\t Contact (choose one): Exp Hunt Both" << endl << endl; + + cout << "All arguments are optional. If no arguments are specified, "; + cout << "a 'Slide' will" << endl; + cout << "be simulated with one block that uses "; + cout << "ExponentialContact instances," << endl; + cout << "with typical damping settings, "; + cout << "with no extnerally applied force, " << endl; + cout << "and with no visuals." << endl << endl; + + cout << "Example:" << endl; + cout << "To simulated 2 blocks (one with ExponentialContact and one"; + cout << " with Hunt-Crossley)" << endl; + cout << "that bounce without energy dissipation and with Visuals, "; + cout << "enter the following: " << endl << endl; + + cout << "$ testExponentialContact Bounce Both NoDamp Vis" << endl << endl; +} +//_____________________________________________________________________________ +void +ExponentialContactTester:: +printConditions() { + std::string modelDes; + std::string contactDes; + switch (whichContact) { + case (Exp): + modelDes = "One Block"; + contactDes = "Exponential"; + break; + case (Hunt): + modelDes = "One Block"; + contactDes = "Hunt Crossley"; + break; + case (Both): + modelDes = "Two Blocks"; + contactDes = "1 Block Exponential, 1 Block Hunt Crossley"; + } + + std::string motion; + switch (whichInit) { + case (Static): motion = "Sitting Still"; break; + case (Bounce): motion = "Bouncing"; break; + case (Slide): motion = "Sliding"; break; + case (Spin): motion = "Spinning"; break; + case (SpinSlide): motion = "Spinning & Sliding"; break; + case (SpinTop): motion = "Spinnning like a Top"; break; + case (Tumble): motion = "Tumbling Horizontally"; + } + + std::string appliedForce; + if (applyFx) + appliedForce = "Ramping Fx after 25 sec."; + else + appliedForce = "None"; + + cout << endl << endl; + cout << " model: " << modelDes << " (10 kg, 6 dof, 20x20x20 cm^3)" << endl; + cout << " contact: " << contactDes << endl; + cout << " motion: " << motion << endl; + cout << " applied force: " << appliedForce << endl; + cout << " integ acc: " << integ_accuracy << endl; + cout << " max step size: " << dt_max << " sec" << endl; + cout << " tf: " << tf << " sec" << endl; +} +//_____________________________________________________________________________ +// Build the model +void +ExponentialContactTester:: +buildModel() +{ + // Create the bodies + model = new Model(); + model->setGravity(gravity); + model->setName("BouncingBlock_ExponentialContact"); + switch (whichContact) { + case Exp: + blockEC = addBlock("EC"); + addExponentialContact(blockEC); + break; + case Hunt: + blockHC = addBlock("HC"); + addHuntCrossleyContact(blockHC); + break; + case Both: + blockEC = addBlock("EC"); + addExponentialContact(blockEC); + blockHC = addBlock("HC"); + addHuntCrossleyContact(blockHC); + } + + // Add the external force + if (applyFx) { + setForceDataHeader(); + SimTK::Vec3 point(0.0, -hs, 0.0); + SimTK::Vec3 zero(0.0); + SimTK::Vec3 force(-0.7*gravity[1]*mass, 0.0, 0.0); + setForceData(0.0, point, zero); + setForceData(tf, point, zero); + tf = tf + 25.0; + setForceData(tf, point, force); + if (blockEC) { + cout << "Adding fx for " << blockEC->getName() << endl; + fxEC = new ExternalForce(fxData,"force", "point", "", + blockEC->getName(), "ground", blockEC->getName()); + fxEC->setName("externalforceES"); + model->addForce(fxEC); + } + if (blockHC) { + cout << "Adding fx for " << blockHC->getName() << endl; + fxHC = new ExternalForce(fxData, "force", "point", "", + blockHC->getName(), "ground", blockHC->getName()); + fxHC->setName("externalforceHC"); + model->addForce(fxHC); + } + } + + // Reporters + // StatesTrajectory + statesReporter = new StatesTrajectoryReporter(); + statesReporter->setName("states_reporter"); + statesReporter->set_report_time_interval(0.001); + model->addComponent(statesReporter); + + + // Visuals? + if (showVisuals) { + if (blockEC) { + auto blockESGeometry = new Brick(Vec3(hs)); + blockESGeometry->setColor(Vec3(0.1, 0.1, 0.8)); + blockEC->attachGeometry(blockESGeometry); + } + if (blockHC) { + auto blockHCGeometry = new Brick(Vec3(hs)); + blockHCGeometry->setColor(Vec3(0.8, 0.1, 0.1)); + blockHC->attachGeometry(blockHCGeometry); + } + model->setUseVisualizer(true); + } + + // Build the System + model->buildSystem(); +} +//______________________________________________________________________________ +void +ExponentialContactTester:: +setForceDataHeader() +{ + fxData.setName("fx"); + fxData.setDescription("An external force applied to a block."); + Array lab; // labels + lab.append("time"); + lab.append("point.x"); + lab.append("point.y"); + lab.append("point.z"); + lab.append("force.x"); + lab.append("force.y"); + lab.append("force.z"); + fxData.setColumnLabels(lab); +} +//______________________________________________________________________________ +void +ExponentialContactTester:: +setForceData(double t, const SimTK::Vec3& point, const SimTK::Vec3& force) +{ + SimTK::Vector_ data(6); + for (int i = 0; i < 3; ++i) { + data[i] = point[i]; + data[3 + i] = force[i]; + } + StateVector sv(t, data); + fxData.append(sv); +} +//______________________________________________________________________________ +OpenSim::Body* +ExponentialContactTester:: +addBlock(const std::string& suffix) +{ + Ground& ground = model->updGround(); + + // Body + std::string name = "block" + suffix; + OpenSim::Body* block = new OpenSim::Body(); + block->setName(name); + block->set_mass(mass); + block->set_mass_center(Vec3(0)); + block->setInertia(Inertia(1.0)); + + // Joint + name = "free" + suffix; + FreeJoint *free = new + FreeJoint(name, ground, Vec3(0), Vec3(0), *block, Vec3(0), Vec3(0)); + model->addBody(block); + model->addJoint(free); + + return block; +} +//______________________________________________________________________________ +void +ExponentialContactTester:: +addExponentialContact(OpenSim::Body* block) +{ + Ground& ground = model->updGround(); + + // Contact Plane Transform + Real angle = convertDegreesToRadians(90.0); + Rotation floorRot(-angle, XAxis); + Vec3 floorOrigin(0., -0.004, 0.); + Transform floorXForm(floorRot, floorOrigin); + + // Contact Parameters + SimTK::ExponentialSpringParameters params; // yields default params + if (noDamp) { + params.setNormalViscosity(0.0); + params.setFrictionViscosity(0.0); + params.setInitialMuStatic(0.0); + } + + // Place a spring at each of the 8 corners + std::string name = ""; + for (int i = 0; i < n; ++i) { + name = "Exp" + std::to_string(i); + sprEC[i] = new OpenSim::ExponentialContact(floorXForm, + block->getName(), corner[i], params); + sprEC[i]->setName(name); + model->addForce(sprEC[i]); + } +} +//______________________________________________________________________________ +void +ExponentialContactTester:: +addHuntCrossleyContact(OpenSim::Body* block) +{ + Ground& ground = model->updGround(); + + // Geometry for the floor + ContactHalfSpace* floor = new ContactHalfSpace( + Vec3(0), Vec3(0, 0, -0.5 * SimTK_PI), ground, "floor"); + model->addContactGeometry(floor); + + // Place a contact sphere at each of the 8 corners + std::string name = ""; + for (int i = 0; i < n; ++i) { + // Geometry + name = "sphere_" + std::to_string(i); + geomHC[i] = new ContactSphere(0.005, corner[i], *block, name); + model->addContactGeometry(geomHC[i]); + + // HuntCrossleyForce + double dissipation = 4e-1; + double mus = 0.7; + double muk = 0.5; + if (noDamp) { + dissipation = 0.0; + mus = 0.0; + muk = 0.0; + } + auto* contactParams = new OpenSim::HuntCrossleyForce:: + ContactParameters(1.0e7, dissipation, mus, muk, 0.0); + + contactParams->addGeometry(name); + contactParams->addGeometry("floor"); + sprHC[i] = new OpenSim::HuntCrossleyForce(contactParams); + name = "HuntCrossleyForce_" + std::to_string(i); + sprHC[i]->setName(name); + sprHC[i]->setTransitionVelocity(0.01); + model->addForce(sprHC[i]); + } +} +//_____________________________________________________________________________ +// dz allows for the body to be shifted along the z axis. This is useful for +// displacing the Exp and Hunt models. +void +ExponentialContactTester:: +setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, + double dz) +{ + SimTK::Rotation R; + SimTK::Vec3 pos(0.0, 0.0, dz); + SimTK::Vec3 vel(0.0); + SimTK::Vec3 angvel(0.0); + + switch (whichInit) { + case Static: + pos[0] = 0.0; + pos[1] = hs; + body.setQToFitTranslation(state, pos); + break; + case Bounce: + pos[0] = 0.0; + pos[1] = 1.0; + body.setQToFitTranslation(state, pos); + break; + case Slide: + pos[0] = 2.0; + pos[1] = 2.0 * hs; + vel[0] = -4.0; + body.setQToFitTranslation(state, pos); + body.setUToFitLinearVelocity(state, vel); + break; + case Spin: + pos[0] = 0.0; + pos[1] = hs; + vel[0] = 0.0; + angvel[1] = 8.0 * SimTK::Pi; + body.setQToFitTranslation(state, pos); + body.setUToFitLinearVelocity(state, vel); + body.setUToFitAngularVelocity(state, angvel); + break; + case SpinSlide: + pos[0] = 1.0; + pos[1] = hs; + vel[0] = -3.0; + angvel[1] = 4.0 * SimTK::Pi; + body.setQToFitTranslation(state, pos); + body.setUToFitLinearVelocity(state, vel); + body.setUToFitAngularVelocity(state, angvel); + break; + case SpinTop: + R.setRotationFromAngleAboutNonUnitVector( + convertDegreesToRadians(54.74), Vec3(1, 0, 1)); + pos[0] = 0.0; + pos[1] = 2.0*hs; + vel[0] = 0.0; + angvel[1] = 1.5 * SimTK::Pi; + body.setQToFitRotation(state, R); + body.setQToFitTranslation(state, pos); + body.setUToFitLinearVelocity(state, vel); + body.setUToFitAngularVelocity(state, angvel); + break; + case Tumble: + pos[0] = -1.5; + pos[1] = 2.0 * hs; + vel[0] = -1.0; + angvel[2] = 2.0 * SimTK::Pi; + body.setQToFitTranslation(state, pos); + body.setUToFitLinearVelocity(state, vel); + body.setUToFitAngularVelocity(state, angvel); + break; + default: + cout << "Unrecognized set of initial conditions!" << endl; + } +} + +//----------------------------------------------------------------------------- +// TESTING +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +ExponentialContactTester:: +test() +{ + // Don't run tests if the only contact is Hunt-Crossley + if (whichContact == Hunt) return; + + testParameters(); + testSerialization(); +} +//_____________________________________________________________________________ +void +ExponentialContactTester:: +testParameters() +{ + // Check current properties/parameters for all springs + for (int i = 0; i < n; i++) { + sprEC[i]->assertPropertiesAndParametersEqual(); + } + + // Pick just one contact instance to manipulate. + ExponentialContact& spr = *sprEC[0]; + + // Save the starting parameters + SimTK::ExponentialSpringParameters p0 = spr.getParameters(); + + // Change the properties systematically + SimTK::ExponentialSpringParameters p1 = p0; + + // Shape + double delta = 0.1; + Vec3 d; + p1.getShapeParameters(d[0], d[1], d[2]); + p1.setShapeParameters(d[0] + delta, d[1], d[2]); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + p1.setShapeParameters(d[0], d[1] + delta, d[2]); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + p1.setShapeParameters(d[0], d[1], d[2] + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Normal Viscosity + double value; + value = p1.getNormalViscosity(); + p1.setNormalViscosity(value + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Friction Elasticity + value = p1.getFrictionElasticity(); + p1.setFrictionElasticity(value + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Friction Viscosity + value = p1.getFrictionViscosity(); + p1.setFrictionViscosity(value + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Settle Velocity + value = p1.getSettleVelocity(); + p1.setSettleVelocity(value + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Initial Coefficients of Friction + double mus = p1.getInitialMuStatic(); + double muk = p1.getInitialMuKinetic(); + p1.setInitialMuStatic(muk - delta); // Changes muk also + mus = p1.getInitialMuStatic(); + muk = p1.getInitialMuKinetic(); + SimTK_TEST_EQ(mus, muk); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + p1.setInitialMuKinetic(mus + delta); // Changes mus also + SimTK_TEST_EQ(mus, muk); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Return to the starting parameters + spr.setParameters(p0); + spr.assertPropertiesAndParametersEqual(); +} +//_____________________________________________________________________________ +void +ExponentialContactTester:: +testSerialization() { + // Serialize the current model + std::string fileName = "BouncingBlock_ExponentialContact_Serialized.osim"; + model->print(fileName); + ExponentialSpringParameters p = sprEC[0]->getParameters(); + + // Deserialize the model + Model modelCopy(fileName); + + // Get the 0th contact instance + ExponentialContact* spr = sprEC[0]; + ExponentialContact* sprCopy = dynamic_cast( + &modelCopy.updForceSet().get(spr->getName())); + ExponentialSpringParameters pCopy = sprCopy->getParameters(); + + // Parameters should be equal + SimTK_ASSERT_ALWAYS(pCopy == p, + "Deserialized parameters are not equal to original parameters."); +} +//_____________________________________________________________________________ +void +ExponentialContactTester:: +printDiscreteVariableAbstractValue(const string& pathName, + const AbstractValue& value) const +{ + cout << pathName << " = "; + + // Switch depending on the type + if (SimTK::Value::isA(value)) { + double x = SimTK::Value::downcast(value); + cout << x << endl; + } else if (SimTK::Value::isA(value)) { + Vec3 x = SimTK::Value::downcast(value); + cout << x << endl; + } +} + +//_____________________________________________________________________________ +// The only types that are handled are double and Vec3 at this point. +// The significant changes in how Discrete Variables are handled are: +// 1. Values are now not assumed to be doubles but are AbstractValues. +// 2. Discrete variables outside of OpenSim are permitted. +// 3. Discrete variables may be accessed via the Component API by +// specifying the path (e.g., path = "/forceset/Exp0/anchor"). +void +ExponentialContactTester:: +testDiscreteVariables(State& state, const ForceSet& fSet) { + + // Get the names + OpenSim::Array names = fSet.getDiscreteVariableNames(); + + // Loop + int n = names.size(); + for (int i = 0; i < n; ++i) { + + // Print values for debugging purposes. + AbstractValue& valAbstract = + fSet.updDiscreteVariableAbstractValue(state, names[i]); + printDiscreteVariableAbstractValue(names[i], valAbstract); + + // Declarations + double tol = 1.0e-6; + double deltaDbl = 0.1; + Vec3 deltaVec3(deltaDbl); + double valStartDbl{NaN}; + Vec3 valStartVec3{NaN}; + + // Perturb + if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valDbl = + SimTK::Value::updDowncast(valAbstract); + valStartDbl = valDbl; + valDbl = valStartDbl + deltaDbl; + } else if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valVec3 = + SimTK::Value::updDowncast(valAbstract); + valStartVec3 = valVec3.get(); + valVec3 = valStartVec3 + deltaVec3; + } + printDiscreteVariableAbstractValue(names[i], valAbstract); + + // Check that the value changed correctly + if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valDbl = + SimTK::Value::updDowncast(valAbstract); + ASSERT_EQUAL(valDbl.get(), valStartDbl + deltaDbl, tol); + } else if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valVec3 = + SimTK::Value::updDowncast(valAbstract); + ASSERT_EQUAL(valVec3.get(), valStartVec3 + deltaVec3, tol); + } + + // Restore the starting value + if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valDbl = + SimTK::Value::updDowncast(valAbstract); + valDbl = valStartDbl; + } else if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valVec3 = + SimTK::Value::updDowncast(valAbstract); + valVec3 = valStartVec3; + } + printDiscreteVariableAbstractValue(names[i], valAbstract); + + // Check that the value was correctly restored + if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valDbl = + SimTK::Value::updDowncast(valAbstract); + ASSERT_EQUAL(valDbl.get(), valStartDbl, tol); + } else if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valVec3 = + SimTK::Value::updDowncast(valAbstract); + ASSERT_EQUAL(valVec3.get(), valStartVec3, tol); + } + + } + +} + +//_____________________________________________________________________________ +void +ExponentialContactTester:: +simulate() +{ + // Initialize the state + // Note that model components have already been connected and the + // SimTK::System has already been built. + // See TestExponentialContact::buildModel(). + SimTK::State& state = model->initializeState(); + + // Set initial conditions + double dz = 1.0; + if (blockEC != NULL) + setInitialConditions(state, blockEC->getMobilizedBody(), dz); + if (blockHC != NULL) + setInitialConditions(state, blockHC->getMobilizedBody(), -dz); + + // Reset the elastic anchor point for each ExponentialContact instance + ForceSet& fSet = model->updForceSet(); + ExponentialContact::resetAnchorPoints(fSet, state); + + // Check the Component API for discrete states. + int n = fSet.getSize(); + try { + testDiscreteVariables(state, fSet); + } catch (const std::exception& e) { + cout << e.what() << endl; + } + + // Integrate + Manager manager(*model); + manager.getIntegrator().setMaximumStepSize(dt_max); + manager.setIntegratorAccuracy(integ_accuracy); + state.setTime(0.0); + manager.initialize(state); + manager.setWriteToStorage(true); + std::clock_t startTime = std::clock(); + state = manager.integrate(tf); + auto runTime = 1.e3 * (std::clock() - startTime) / CLOCKS_PER_SEC; + + // Output + int trys = manager.getIntegrator().getNumStepsAttempted(); + int steps = manager.getIntegrator().getNumStepsTaken(); + printConditions(); + cout << " trys: " << trys << endl; + cout << " steps: " << steps << endl; + cout << " cpu time: " << runTime << " msec" << endl; + + // Write the model to file + //model->print("C:\\Users\\fcand\\Documents\\block.osim"); + + // Write recorded states + // From the Storage object maintained by the Manager + Storage& store = manager.getStateStorage(); + store.print("BouncingBlock.states"); + // From the StatesTrajectoryReporter + const StatesTrajectory& statesTrajectory = statesReporter->getStates(); +} + +//_____________________________________________________________________________ +/* Entry Point (i.e., main()) + +The motion of a 10 kg, 6 degree-of-freedom block and its force interaction +with a laboratory floor are simulated. + +Contact with the floor is modeled using either + 1) 8 ExponentialContact instances, one at each corner of the block, or + 2) 8 HuntCrossleyForce instances, one at each corner of the block. + +For a side-by-side comparison of simulated motions, two blocks (one using +the ExponentialContact class for contact and the other using the +HuntCrossleyForce class) can be created and visualized simultaneously. + +For an assessment of computational performance, just one block should be +simulated at a time. Number of integration trys and steps, as well as cpu +time, are reported. + +Choice of initial conditions can be made in order to simulate the following +motions: + 1) Static (y = 0.1 m, sitting at rest on the floor) + 2) Bouncing (y = 1.0 m, dropped) + 3) Sliding (y = 0.2 m, vx = -4.0 m/s) + 4) Spinning (y = 0.1 m, vx = 0.0 m/s, wy = 8.0 pi rad/sec) + 5) Spining and Sliding (y = 0.1 m, vx = -3.0 m/s, wy = 4.0 pi rad/sec) + 6) Spinning like a Top. (y = 0.2 m, vx = 0.0 m/s, wy = 1.5 pi rad/sec) + 7) Tumbling (py = 0.2 m, vx = -1.0 m/s, wz = 2.0 pi rad/sec) + +Additional options allow the following to be specified: + NoDamp Parameters are chosen to eliminate all energy dissipation. + Fx A ramping horizontal force (Fx) is applied. + Vis Open a visualization window. + +If no external force is applied, tf = 5.0 s. + +If a horizontal force is applied, tf = 30.0 s and the force ramps up +linearly from a value of fx = 0.0 at t = 25.0 s to a value of +fx = |mass*g| at t = 30.0 s. The force is not ramped up prior to t = 25.0 s +in order to allow the block an opportunity to come fully to rest. +This ramping profile was done with the "Static" initial condition choice +in mind so that friction models could be evaluated more critically. +In particular, a static block should not start sliding until fx > μₛ Fₙ. + +For ExponentialContact, the following things are tested: + a) instantiation + b) model initialization + c) consistency between OpenSim Properties and SimTK parameters + d) data cache access + e) realization stage invalidation + f) reporting + g) serialization + +The HuntCrossleyForce class is tested elsewhere (e.g., see +testContactGeometry.cpp and testForce.cpp). */ +int main(int argc, char** argv) { + try { + ExponentialContactTester tester; + int status = tester.parseCommandLine(argc, argv); + if (status < 0) { + cout << "Exiting..." << endl; + return 1; + } + tester.buildModel(); + tester.test(); + tester.simulate(); + + } catch (const OpenSim::Exception& e) { + e.print(cerr); + return 1; + } + cout << "Done" << endl; + return 0; +} diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index cdfb53dc0e..7c9e93a300 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -28,12 +28,13 @@ // 2. BushingForce // 3. ElasticFoundationForce // 4. HuntCrossleyForce -// 5. SmoothSphereHalfSpaceForce -// 6. CoordinateLimitForce -// 7. RotationalCoordinateLimitForce -// 8. ExternalForce -// 9. PathSpring -// 10. ExpressionBasedPointToPointForce +// 5. ExponentialContact +// 6. SmoothSphereHalfSpaceForce +// 7. CoordinateLimitForce +// 8. RotationalCoordinateLimitForce +// 9. ExternalForce +// 10. PathSpring +// 11. ExpressionBasedPointToPointForce // 11. Blankevoort1991Ligament // // Add tests here as Forces are added to OpenSim @@ -65,6 +66,7 @@ void testExpressionBasedBushingForceTranslational(); void testExpressionBasedBushingForceRotational(); void testElasticFoundation(); void testHuntCrossleyForce(); +void testExponentialContact(); void testSmoothSphereHalfSpaceForce(); void testCoordinateLimitForce(); void testCoordinateLimitForceRotational(); @@ -127,6 +129,13 @@ int main() { cout << e.what() <getForceStorage().print("elastic_contact_forces.mot"); + reporter->getForceStorage().print("elastic_contact_forces.sto"); // Bouncing ball should have settled to rest on ground due to dissipation // In that case the force generated by contact should be identically body @@ -1263,6 +1271,163 @@ void testElasticFoundation() { ASSERT(isEqual); } + +// Test the wrapping of ExponentialSpringForce in OpenSim +// Simple simulation of bouncing block with dissipation should generate contact +// forces that settle to block weight (mg = 10.0 * g). +void testExponentialContact() { + using namespace SimTK; + + // Construct a model and serialize it ----------------------------- + // This is done so that a model file does not have to be installed. + // Below the temporary scope, checks are done with the deserialized model. + string fileName = "BouncingBlock_ExponentialContact.osim"; + string baseName = "EC"; + const int nEC{8}; + // Start temporary model scope + { + Model* model = new Model(); + model->setGravity(gravity_vec); + model->setName("BouncingBlock_ExponentialContact"); + // Body + OpenSim::Body* block = new OpenSim::Body(); + block->setName("block"); + block->set_mass(10.0); + block->set_mass_center(Vec3(0)); + block->setInertia(Inertia(1.0)); + // Joint + FreeJoint* freeJoint = new FreeJoint("free", model->updGround(), + Vec3(0), Vec3(0), *block, Vec3(0), Vec3(0)); + model->addBody(block); + model->addJoint(freeJoint); + + // ExponentialContact Instances---------------------------------- + // Customizable Parameters + // Default parameters work well, but may be altered. + // Default values are used here. + SimTK::ExponentialSpringParameters params; + // Contact plane transform. + // Need to make the normal of the contact plane, which is along + // along the z-axis, point along the y-axis of the Ground frame, + // which is up in OpenSim. + Real angle = convertDegreesToRadians(90.0); + Rotation floorRot(-angle, XAxis); + Vec3 floorOrigin(0., -0.004, 0.); // Shift to account for penetration. + Transform X_GP(floorRot, floorOrigin); + // Stations + double hs = 0.01; // half side = 10 cm. + Vec3 corner[nEC]; + corner[0] = Vec3(hs, -hs, hs); + corner[1] = Vec3(hs, -hs, -hs); + corner[2] = Vec3(-hs, -hs, -hs); + corner[3] = Vec3(-hs, -hs, hs); + corner[4] = Vec3(hs, hs, hs); + corner[5] = Vec3(hs, hs, -hs); + corner[6] = Vec3(-hs, hs, -hs); + corner[7] = Vec3(-hs, hs, hs); + // Place an exponential contact at each of the 8 stations + ExponentialContact* ec[nEC]{NULL}; + for (int i = 0; i < nEC; ++i) { + string nameEC = baseName + std::to_string(i); + ec[i] = new OpenSim::ExponentialContact( + X_GP, block->getName(), corner[i], params); + ec[i]->setName(nameEC); + model->addForce(ec[i]); + } + + // Build the Sytem + model->buildSystem(); + + // Serialize the model + model->print(fileName); + + // Delete the model + delete model; + + } // End temporary model scope. + + // Deserialize the model + Model model{fileName}; + + // Create the force reporter + ForceReporter* reporter = new ForceReporter(&model); + model.addAnalysis(reporter); + + // Initialize the system + SimTK::State& state = model.initSystem(); + + // Set the starting position + const OpenSim::Body& block = model.getBodySet().get("block"); + const SimTK::MobilizedBody& body = block.getMobilizedBody(); + SimTK::Vec3 pos(0.0); + pos[1] = 0.5; // starts from 0.5 above the ground + body.setQToFitTranslation(state, pos); + model.getMultibodySystem().realize(state, Stage::Position); + + // Reset the anchor points of all ExponentialContact instances. + ForceSet &fSet = model.updForceSet(); + ExponentialContact::resetAnchorPoints(fSet, state); + + // Simulate + Manager manager(model); + manager.setIntegratorAccuracy(1e-6); + manager.setIntegratorMaximumStepSize(0.02); + state.setTime(0.0); + manager.initialize(state); + clock_t startTime = clock(); + state = manager.integrate(10.0); + clock_t stopTime = clock(); + cout << "Exponential Contact simulation execution time = " + << 1.e3 * (stopTime - startTime) / CLOCKS_PER_SEC << "ms" << endl; + + // Print out the states and contact information + manager.getStateStorage().print("BouncingBlock_EC_states.sto"); + reporter->getForceStorage().print("BouncingBlock_EC_forces.sto"); + + // Realize to Stage::Dynamics + // This is necessary to compute elastic anchor points and + // all of the contact forces. + model.getMultibodySystem().realize(state, Stage::Dynamics); + + // The block should have settled on ground due to dissipation. + // The force generated by contact then should sum to body weight + // vertically and zero horizontally when the forces are totaled + // across all contact instances. + Vec3 gfrc = block.getMass() * gravity_vec; + Vec3 tfrc(0.0); + for (int i = 0; i < nEC; ++i) { + string nameEC = baseName + to_string(i); + const OpenSim::ExponentialContact& spr = + (OpenSim::ExponentialContact&)model.getForceSet().get(nameEC); + OpenSim::Array labels = spr.getRecordLabels(); + //cout << endl << labels << endl; + Array spr_values = spr.getRecordValues(state); + //cout << endl << spr_values << endl; + tfrc[0] += spr_values[0]; // x + tfrc[1] += spr_values[1]; // y + tfrc[2] += spr_values[2]; // z + } + //cout << "gfrc = " << gfrc << endl; + //cout << "tfrc = " << tfrc << endl; + ASSERT_EQUAL(tfrc[0], -gfrc[0], 1.0e-4); // x + ASSERT_EQUAL(tfrc[1], -gfrc[1], 1.0e-3); // y + ASSERT_EQUAL(tfrc[2], -gfrc[2], 1.0e-4); // z + + // Before exiting lets see if cloning works + string ecName = baseName + "0"; + const OpenSim::ExponentialContact& spr = + (OpenSim::ExponentialContact&)model.getForceSet().get(ecName); + OpenSim::ExponentialContact* sprCopy = spr.clone(); + bool isEqual = (*sprCopy == spr); + if (!isEqual) { + spr.print("origElasticContact.xml"); + sprCopy->print("cloneElasticContact.xml"); + } + + ASSERT(isEqual); +} + + // Test our wrapping of Hunt-Crossley force in OpenSim // Simple simulation of bouncing ball with dissipation should generate contact // forces that settle to ball weight. @@ -1316,7 +1481,7 @@ void testHuntCrossleyForce() { // In that case the force generated by contact should be identically body // weight in vertical and zero else where. OpenSim::HuntCrossleyForce& contact = - (OpenSim::HuntCrossleyForce&)osimModel.getForceSet().get("contact"); + (OpenSim::HuntCrossleyForce&)osimModel.getForceSet().get("contact"); Array contact_force = contact.getRecordValues(osim_state); ASSERT_EQUAL( @@ -1342,6 +1507,7 @@ void testHuntCrossleyForce() { ASSERT(isEqual); } + // Test our wrapping of SimTK::SmoothSphereHalfSpaceForce. // Simple simulation of bouncing ball with dissipation should generate contact // forces that settle to ball weight. diff --git a/OpenSim/Simulation/osimSimulation.h b/OpenSim/Simulation/osimSimulation.h index 173bd34a95..5894e3ca03 100644 --- a/OpenSim/Simulation/osimSimulation.h +++ b/OpenSim/Simulation/osimSimulation.h @@ -40,6 +40,7 @@ #include "Model/ContactSphere.h" #include "Model/CoordinateSet.h" #include "Model/ElasticFoundationForce.h" +#include "Model/ExponentialContact.h" #include "Model/HuntCrossleyForce.h" #include "Model/SmoothSphereHalfSpaceForce.h" #include "Model/Ligament.h" diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index 312e55e2b6..9a25db1535 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -17,14 +17,14 @@ function(SetDefaultCMakeInstallPrefix) get_filename_component(BASE_DIR ${BASE_DIR} DIRECTORY) # Default install prefix for OpenSim dependencies. If user changes # CMAKE_INSTALL_PREFIX, this directory will be removed. - set(DEFAULT_CMAKE_INSTALL_PREFIX + set(DEFAULT_CMAKE_INSTALL_PREFIX ${BASE_DIR}/opensim_dependencies_install CACHE INTERNAL "Default CMAKE_INSTALL_PREFIX for OpenSim dependencies.") if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) - set(CMAKE_INSTALL_PREFIX + set(CMAKE_INSTALL_PREFIX ${DEFAULT_CMAKE_INSTALL_PREFIX} CACHE PATH @@ -33,7 +33,7 @@ function(SetDefaultCMakeInstallPrefix) endif() endfunction() -# CMake doesn't clear prefix directories when user changes it. +# CMake doesn't clear prefix directories when user changes it. # Remove it to avoid confusion. function(RemoveDefaultInstallDirIfEmpty DIR) file(GLOB CONTENTS ${DIR}/*) @@ -66,7 +66,7 @@ endfunction() # GIT_URL -- (Required) git repository to download the sources from. # GIT_TAG -- (Required) git tag to checkout before commencing build. # DEPENDS -- (Optional) Other projects this project depends on. -# CMAKE_ARGS -- (Optional) A CMake list of arguments to be passed to CMake +# CMAKE_ARGS -- (Optional) A CMake list of arguments to be passed to CMake # while building the project. # You must provide either URL or GIT_URL and GIT_TAG, but not all 3. function(AddDependency) @@ -176,7 +176,7 @@ AddDependency(NAME ezc3d AddDependency(NAME simbody DEFAULT ON GIT_URL https://github.com/simbody/simbody.git - GIT_TAG e855d954a786128c3271a3406d7bda782e7c4c4f + GIT_TAG 462b2a6dbb8794db2922d72f52b29b488a178ebc CMAKE_ARGS -DBUILD_EXAMPLES:BOOL=OFF -DBUILD_TESTING:BOOL=OFF) @@ -251,7 +251,7 @@ if (WIN32) if(SUPERBUILD_ipopt) - + # Ipopt: Download pre-built binaries built by chrisdembia. # This binary distribution comes with MUMPS and OpenBLAS. # Compiled with clang-cl 5.0 and flang, using conda.