[WIP] Exponential Contact

OpenSim/Common/Component.cpp

@@ -812,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<std::string> Component::getStateVariableNames() const
@@ -1090,13 +1091,12 @@ getDiscreteVariableValue(const SimTK::State& s, const std::string& name) const
 }
 
 //_____________________________________________________________________________
-// F. C. Anderson (Jan 2023)
-// This method was added in order to handle Discrete Variables that are not
-// type double.
-const SimTK::AbstractValue&
+// Set the value (assumed to be double) of a discrete variable allocated by
+// this Component by name.
+void
 Component::
-getDiscreteVariableAbstractValue(const SimTK::State& s,
-    const std::string& name) const
+setDiscreteVariableValue(SimTK::State& s, const std::string& name,
+    double value) const
 {
     // Must have already called initSystem.
     OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem);
@@ -1120,35 +1120,62 @@ getDiscreteVariableAbstractValue(const SimTK::State& s,
         // 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);
+        SimTK::Value<double>::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__);
     }
 }
 
+//_____________________________________________________________________________
+// 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<Component>(ownerPath);
+    }
+    return owner;
+}
 
 //_____________________________________________________________________________
-// F. C. Anderson (Jan 2023)
-// This method was added in order to handle Discrete Variables that
-// are not type double.
-SimTK::AbstractValue&
+// 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::
-updDiscreteVariableAbstractValue(SimTK::State& s,
-    const std::string& name) const
+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<std::string, DiscreteVariableInfo>::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;
 
         // F. C. Anderson (Jan 2023)
@@ -1164,34 +1191,40 @@ updDiscreteVariableAbstractValue(SimTK::State& s,
         // 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);
+        return subsystem->getDiscreteVariable(s, dvIndex);
 
     } else {
         std::stringstream msg;
-        msg << "Component::getDiscreteVariable: ERR- name '" << name
+        msg << "Component::getDiscreteVariable: ERR- name '" << pathName
             << "' not found.\n "
             << "for component '" << getName() << "' of type "
             << getConcreteClassName();
         throw Exception(msg.str(), __FILE__, __LINE__);
     }
 }
 
-
 //_____________________________________________________________________________
-// Set the value (assumed to be double) of a discrete variable allocated by
-// this Component by name.
-void
+// 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::
-setDiscreteVariableValue(SimTK::State& s, const std::string& name,
-    double value) const
+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<std::string, DiscreteVariableInfo>::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;
 
         // F. C. Anderson (Jan 2023)
@@ -1207,22 +1240,19 @@ setDiscreteVariableValue(SimTK::State& s, const std::string& name,
         // default value, then the default Subsystem is used.
         const SimTK::Subsystem* subsystem = it->second.subsystem;
         if (subsystem == nullptr) subsystem = &getDefaultSubsystem();
-        SimTK::Value<double>::downcast(
-            subsystem->updDiscreteVariable(s, dvIndex)).upd() = value;
+        return subsystem->updDiscreteVariable(s, dvIndex);
 
     } else {
         std::stringstream msg;
-        msg << "Component::setDiscreteVariable: ERR- name '" << name
+        msg << "Component::updDiscreteVariable: 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__);
     }
 }
 
 
-
-
 SimTK::CacheEntryIndex Component::getCacheVariableIndex(const std::string& name) const
 {
     auto it = this->_namedCacheVariables.find(name);

OpenSim/Common/Component.h

@@ -1389,44 +1389,117 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object);
         const std::string& name) const;
 
     /**
-     * Retrieve a read-only reference to the abstract value of the discrete
-     * variable of a specified name. This method provides a more general
-     * interface to handle discrete variables that may not be type double.
+     * %Set the value 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 discrete state variable
-     * @return value  the discrete variable value as a SimTK::AbstractValue
+     * @param state  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)
      */
+    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<T>.
+     * When the type is unknow, it can be querried using the
+     * SimTK::Value<T>::isA() method. For example,
+     *
+     * ```
+     *      const SimTK::AbstractValue& valAbstract =
+     *          getDiscreteVariableAbstractValue(state, pathName);
+     *
+     *      if (SimTK::Value<double>::isA(valAbstract)) {
+     *          const SimTK::Value<double>& valDbl =
+     *              SimTK::Value<double>::downcast(valAbstract);
+     *          double x = valDbl + 0.4;
+     *
+     *      } else if (SimTK::Value<Vec3>::isA(valAbstract)) {
+     *          const SimTK::Value<Vec3>& valVec3 =
+     *              SimTK::Value<Vec3>::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& name) const;
+            const SimTK::State& state, const std::string& pathName) const;
 
     /**
      * Retrieve a writable reference to the abstract value of the discrete
-     * variable of a specified name. This method provides a more general
-     * interface to handle discrete variables that may not be type double.
-     *
-     * @param state   the State from which to get the value
-     * @param name    the name of the discrete state variable
-     * @return value  the discrete variable value as a SimTK::AbstractValue
+     * 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<T>.
+     * When the type is unknow, it can be querried using the
+     * SimTK::Value<T>::isA() method. For example,
+     *
+     * ```
+     *      SimTK::AbstractValue& valAbstract =
+     *          updDiscreteVariableAbstractValue(state, pathName);
+     *
+     *      if (SimTK::Value<double>::isA(valAbstract)) {
+     *          SimTK::Value<double>& valDbl =
+     *              SimTK::Value<double>::updDowncast(valAbstract);
+     *          valDbl = 0.4;
+     *
+     *      } else if (SimTK::Value<Vec3>::isA(valAbstract)) {
+     *          SimTK::Value<Vec3>& valVec3 =
+     *              SimTK::Value<Vec3>::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)
+     *         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.
-     *
-     * @param state  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)
-     */
-    void setDiscreteVariableValue(SimTK::State& state, const std::string& name,
-                                  double value) const;
 
     /**
      * %Set the value of a discrete variable allocated by this Component by
@@ -2034,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 <class T> friend class ComponentSet;
     // Give the ComponentMeasure access to the realize() methods.
     template <class T> friend class ComponentMeasure;
@@ -2748,6 +2827,8 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object);
      */
     const StateVariable* traverseToStateVariable(
             const ComponentPath& path) const;
+
+
 #endif
 
     /// @name Access to the owning component (advanced).
@@ -3316,6 +3397,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object);
         bool allocate{true};
     };
 
+
     /**
     * A cache variable, as stored internally by Component.
     */