Merge pull request #3872 from opensim-org/expression_based_outputs

Add force Outputs to `ExpressionBasedCoordinateForce` and `ExpressionBasedPointToPointForce`

Author: nickbianco

CHANGELOG.md

@@ -15,6 +15,8 @@ v4.6
 - Fixed bugs in `convertToMocoTrajectory()` and `MocoTrajectory::resampleWithFrequency()` by updating `interpolate()` to 
   allow extrapolation using the `extrapolate` flag. Combined with the `ignoreNaNs` flag, this prevents NaNs from 
   occurring in the output. (#3867)
+- Added `Output`s to `ExpressionBasedCoordinateForce`, `ExpressionBasedPointToPointForce`, and `ExpressionBasedBushingForce` for accessing force values. (#3872)
+
 
 v4.5.1
 ======
@@ -79,6 +81,7 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781).
 - Fixed `MocoOrientationTrackingGoal::initializeOnModelImpl` to check for missing kinematic states, but allow other missing columns. (#3830)
 - Improved exception handling for internal errors in `MocoCasADiSolver`. Problems will now abort and print a descriptive error message (rather than fail due to an empty trajectory). (#3834)
 
+  
 v4.5
 ====
 - Added `AbstractGeometryPath` which is a base class for `GeometryPath` and other path types (#3388). All path-based

OpenSim/Auxiliary/auxiliaryTestFunctions.h

@@ -268,14 +268,18 @@ OpenSim::Object* randomize(OpenSim::Object* obj)
             double Ixy = 0.01*Ixx;
             prop = SimTK::Vec6(Ixx, Ixx, Ixx, Ixy, Ixy, Ixy);
         } else if (ts == "string") {
-            string base("ABCXYZ");
-            if (isList){
-                stringstream val;
-                val << base << "_" << ap.size();
-                ap.appendValue<string>(val.str());
-            }
-            else{
-                ap.updValue<string>() = base;
+            // We cannot use an arbitrary string for ExpressionBasedBushingForce 
+            // properties since they must contain specific variable names (e.g.,
+            // "theta_x", "delta_x", etc.). 
+            if (obj->getConcreteClassName() != "ExpressionBasedBushingForce") {
+                string base("ABCXYZ");
+                if (isList) {
+                    stringstream val;
+                    val << base << "_" << ap.size();
+                    ap.appendValue<string>(val.str());
+                } else {
+                    ap.updValue<string>() = base;
+                }
             }
         } else if (ts == "double" && isList && ap.getMaxListSize() < 20) {
             for (int i=0; i< ap.getMaxListSize(); ++i)

OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp

@@ -269,6 +269,7 @@ void ExpressionBasedBushingForce::setFzExpression(std::string expression)
     set_Fz_expression(expression);
     FzProg = Lepton::Parser::parse(expression).optimize().createProgram();
 }
+
 //=============================================================================
 // COMPUTATION
 //=============================================================================
@@ -309,24 +310,21 @@ SimTK::Vec6 ExpressionBasedBushingForce::
     return -_dampingMatrix * dqdot;
 }
 
+SimTK::Vec6 ExpressionBasedBushingForce::calcBushingForce(
+        const SimTK::State& s) const {
+    return calcStiffnessForce(s) + calcDampingForce(s);
+}
+
 
 /* Compute the force contribution to the system and add in to appropriate
 * bodyForce and/or system generalizedForce. */
 void ExpressionBasedBushingForce::computeForce(const SimTK::State& s, 
                               SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
                               SimTK::Vector& generalizedForces) const
 {
-    // stiffness force
-    Vec6 fk = calcStiffnessForce(s);
-    // damping force
-    Vec6 fv = calcDampingForce(s);
-
-    // total bushing force in the internal basis of the deflection (dq) 
-    Vec6 f = fk + fv;
-
     // convert internal forces to spatial and add then add to system
     // physical (body) forces
-    addInPhysicalForcesFromInternal(s, f, bodyForces);
+    addInPhysicalForcesFromInternal(s, calcBushingForce(s), bodyForces);
 }
 
 //=============================================================================
@@ -367,7 +365,7 @@ OpenSim::Array<double> ExpressionBasedBushingForce::
     SpatialVec F_GF( Vec3(0.0),Vec3(0.0) );
 
     // total bushing force in the internal basis of the deflection (dq) 
-    Vec6 f = calcStiffnessForce(s) + calcDampingForce(s);
+    Vec6 f = calcBushingForce(s);
 
     convertInternalForceToForcesOnFrames(s, f, F_GF, F_GM);
 
@@ -426,7 +424,7 @@ void ExpressionBasedBushingForce::generateDecorations
             SpatialVec F_GF(Vec3(0.0), Vec3(0.0));
 
             // total bushing force in the internal basis of the deflection (dq) 
-            Vec6 f = calcStiffnessForce(s) + calcDampingForce(s);
+            Vec6 f = calcBushingForce(s);
 
             convertInternalForceToForcesOnFrames(s, f, F_GF, F_GM);
 

OpenSim/Simulation/Model/ExpressionBasedBushingForce.h

@@ -93,6 +93,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker);
     OpenSim_DECLARE_PROPERTY(translational_damping, SimTK::Vec3,
         "Damping parameters resisting translational deflection (delta_dot) .");
 
+//==============================================================================
+// OUTPUTS
+//==============================================================================
+    OpenSim_DECLARE_OUTPUT(bushing_force, SimTK::Vec6, calcBushingForce,
+            SimTK::Stage::Dynamics);
+
 //==============================================================================
 // PUBLIC METHODS
 //==============================================================================
@@ -226,7 +232,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker);
         function of the deflection rate between the bushing frames. It is the 
         force on frame2 from frame1 in the basis of the deflection rate (dqdot).*/
     SimTK::Vec6 calcDampingForce(const SimTK::State& state) const;
-  
+
+    /** Calculate the total bushing force. This is the sum of the stiffness and
+        damping force contributions. */
+    SimTK::Vec6 calcBushingForce(const SimTK::State& state) const;
 
     //--------------------------------------------------------------------------
     // Reporting

OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp

@@ -53,8 +53,8 @@ ExpressionBasedCoordinateForce::ExpressionBasedCoordinateForce(
     setExpression(expression);
 }
 
-// Set the expression for the force function and create it's lepton program 
-void ExpressionBasedCoordinateForce::setExpression(const string& expression) 
+// Set the expression for the force function and create its lepton program
+void ExpressionBasedCoordinateForce::setExpression(const string& expression)
 {
     set_expression(expression);
 }
@@ -68,7 +68,7 @@ void ExpressionBasedCoordinateForce::setExpression(const string& expression)
  */
 void ExpressionBasedCoordinateForce::setNull()
 {
-    setAuthors("Nabeel Allana"); 
+    setAuthors("Nabeel Allana");
 }
 
 //_____________________________________________________________________________
@@ -94,9 +94,9 @@ void ExpressionBasedCoordinateForce::extendConnectToModel(Model& aModel)
 
     string& expression = upd_expression();
     expression.erase(
-            remove_if(expression.begin(), expression.end(), ::isspace), 
+            remove_if(expression.begin(), expression.end(), ::isspace),
                       expression.end() );
-    
+
     _forceProg = Lepton::Parser::parse(expression).optimize().createProgram();
 
     // Look up the coordinate
@@ -105,7 +105,7 @@ void ExpressionBasedCoordinateForce::extendConnectToModel(Model& aModel)
         throw (Exception(errorMessage.c_str()));
     }
     _coord = &_model->updCoordinateSet().get(coordName);
-    
+
     if(getName() == "")
         setName("expressionCoordForce_"+coordName);
 }
@@ -124,8 +124,8 @@ void ExpressionBasedCoordinateForce::
 // Computing
 //=============================================================================
 // Compute and apply the force
-void ExpressionBasedCoordinateForce::computeForce(const SimTK::State& s, 
-                              SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
+void ExpressionBasedCoordinateForce::computeForce(const SimTK::State& s,
+                              SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
                               SimTK::Vector& generalizedForces) const
 {
     applyGeneralizedForce(s, *_coord, calcExpressionForce(s), generalizedForces);
@@ -147,16 +147,15 @@ double ExpressionBasedCoordinateForce::calcExpressionForce(const SimTK::State& s
 
 // get the force magnitude that has already been computed
 const double& ExpressionBasedCoordinateForce::
-    getForceMagnitude(const SimTK::State& s)
-{
+        getForceMagnitude(const SimTK::State& s) const {
     return getCacheVariableValue(s, _forceMagnitudeCV);
 }
 
 
 //=============================================================================
 // Reporting
 //=============================================================================
-// Provide names of the quantities (column labels) of the force value(s) 
+// Provide names of the quantities (column labels) of the force value(s)
 // reported.
 Array<std::string> ExpressionBasedCoordinateForce::getRecordLabels() const {
     OpenSim::Array<std::string> labels("");

OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h

@@ -42,6 +42,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force);
         "and its time derivative (qdot). Note, expression cannot have any whitespace"
         "separating characters");
 //==============================================================================
+// OUTPUTS
+//==============================================================================
+    OpenSim_DECLARE_OUTPUT(force_magnitude, double, getForceMagnitude,
+            SimTK::Stage::Dynamics);
+//==============================================================================
 // PUBLIC METHODS
 //==============================================================================
     /** Default constructor. **/
@@ -58,40 +63,40 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force);
     /**
     * Coordinate
     */
-    void setCoordinateName(const std::string& coord) 
+    void setCoordinateName(const std::string& coord)
     {   set_coordinate(coord); }
     const std::string& getCoordinateName() const {return get_coordinate();}
 
     /**
     * %Set the mathematical expression that defines the force magnitude of this
     * coordinate force in terms of the coordinate value (q) and its
     * time derivative (qdot). Expressions with C-mathematical operations
-    * such as +,-,*,/ and common functions: exp, pow, sqrt, sin, cos, tan, 
+    * such as +,-,*,/ and common functions: exp, pow, sqrt, sin, cos, tan,
     * and so on are acceptable.
     * NOTE: a limitation is that the expression may not contain whitespace
     * @param expression    string containing the mathematical expression that
-    *                      defines the coordinate force 
+    *                      defines the coordinate force
     */
     void setExpression(const std::string& expression);
 
 
-    /** 
-    * Get the computed Force magnitude determined by evaluating the 
+    /**
+    * Get the computed Force magnitude determined by evaluating the
     * expression above. Note, computeForce must be evaluated first,
     * and this is done automatically if the system is realized to Dynamics
     * @param state    const state (reference) for the model
     * @return         const double ref to the force magnitude
     */
-    const double& getForceMagnitude(const SimTK::State& state);
+    const double& getForceMagnitude(const SimTK::State& state) const;
 
 
 //==============================================================================
 // COMPUTATION
 //==============================================================================
-    /** Compute the coordinate force based on the user-defined expression 
+    /** Compute the coordinate force based on the user-defined expression
         and apply it to the model */
-    void computeForce(const SimTK::State& state, 
-                              SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
+    void computeForce(const SimTK::State& state,
+                              SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
                               SimTK::Vector& generalizedForces) const override;
 
     //--------------------------------------------------------------------------
@@ -103,7 +108,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force);
 //==============================================================================
 // Reporting
 //==============================================================================
-    /** 
+    /**
      * Provide name(s) of the quantities (column labels) of the force value(s) to be reported
      */
     OpenSim::Array<std::string> getRecordLabels() const override;
@@ -112,7 +117,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force);
     */
     OpenSim::Array<double> getRecordValues(const SimTK::State& state) const override;
 
-    
+
 
 protected:
 //==============================================================================

OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp

@@ -202,8 +202,7 @@ void ExpressionBasedPointToPointForce::computeForce(const SimTK::State& s,
 
 // get the force magnitude that has already been computed
 const double& ExpressionBasedPointToPointForce::
-    getForceMagnitude(const SimTK::State& s)
-{
+        getForceMagnitude(const SimTK::State& s) const {
     return getCacheVariableValue(s, _forceMagnitudeCV);
 }
 

OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h

@@ -71,6 +71,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force);
         "the distance (d) between the points and its time derivative (ddot). "
         "Note, expression cannot have any whitespace separating characters.");
 
+//==============================================================================
+// OUTPUTS
+//==============================================================================
+    OpenSim_DECLARE_OUTPUT(force_magnitude, double, getForceMagnitude,
+            SimTK::Stage::Dynamics);
 
 //==============================================================================
 // PUBLIC METHODS
@@ -132,7 +137,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force);
     * @param state    const state (reference) for the model
     * @return         const double ref to the force magnitude
     */
-    const double& getForceMagnitude(const SimTK::State& state);
+    const double& getForceMagnitude(const SimTK::State& state) const;
 
 
     //--------------------------------------------------------------------------