Merge pull request #3622 from ComputationalBiomechanicsLab/RemoveSpee…

…dComputationFromForceComputation Remove speed computation from force computation for ScalarActuator
opensim-org · Nov 26, 2023 · efcdfd3 · efcdfd3
2 parents 2c39267 + a5947aa
commit efcdfd3
Show file tree

Hide file tree

Showing 14 changed files with 368 additions and 174 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -40,6 +40,7 @@ and `Blankevoort1991Ligament`, usages of `get_GeometryPath`, `upd_GeometryPath`,
 - Updated scripting method addTableMetaDataString to support overwriting metadata value for an existing key (#3589)
 - Exposed simbody methods to obtain GravityForces, MobilityForces and BodyForces (#3490)
 - Simbody was updated such that the headers it transitively exposes to downstream projects are compatible with C++20 (#3619)
+- Moved speed computation from `computeForce` in children of `ScalarActuator` to dedicated `getSpeed` function.
 
 
 v4.4.1

diff --git a/OpenSim/Actuators/PointActuator.cpp b/OpenSim/Actuators/PointActuator.cpp
@@ -86,6 +86,14 @@ void PointActuator::constructProperties()
     constructProperty_optimal_force(1.0);
 }
 
+void PointActuator::extendAddToSystem(SimTK::MultibodySystem& system) const
+{
+    Super::extendAddToSystem(system);
+
+    // Cache the computed speed of the actuator
+    this->_speedCV = addCacheVariable("speed", 0.0, SimTK::Stage::Velocity);
+}
+
 //=============================================================================
 // GET AND SET
 //=============================================================================
@@ -127,7 +135,7 @@ double PointActuator::getOptimalForce() const
 //_____________________________________________________________________________
 // Get the stress of the force. This would be the force or torque provided by 
 // this actuator divided by its optimal force.
-double PointActuator::getStress( const SimTK::State& s) const
+double PointActuator::getStress(const SimTK::State& s) const
 {
     return std::abs(getActuation(s) / getOptimalForce()); 
 }
@@ -137,58 +145,73 @@ double PointActuator::getStress( const SimTK::State& s) const
 // COMPUTATIONS
 //=============================================================================
 //_____________________________________________________________________________
+
+double PointActuator::getSpeed(const SimTK::State& s) const
+{
+    if (isCacheVariableValid(s, _speedCV)) {
+        return getCacheVariableValue(s, _speedCV);
+    }
+
+    double speed = calcSpeed(s);
+
+    updCacheVariableValue(s, _speedCV) = speed;
+    markCacheVariableValid(s, _speedCV);
+    return speed;
+}
+
+double PointActuator::calcSpeed(const SimTK::State& s) const
+{
+    // get the velocity of the actuator in ground
+    Vec3 velocity = _body->findStationVelocityInGround(s, get_point());
+    return velocity.norm();
+}
+
 /**
  * Compute all quantities necessary for applying the actuator force to the
  * model.
  */
-double PointActuator::computeActuation( const SimTK::State& s ) const
+double PointActuator::computeActuation(const SimTK::State& s) const
 {
-    if(!_model) return 0;
+    if (!_model) {
+        return SimTK::NaN;
+    }
 
     // FORCE
     return getControl(s) * getOptimalForce();
 }
 
-
-
 //=============================================================================
 // APPLICATION
 //=============================================================================
 //_____________________________________________________________________________
 /**
  * Apply the actuator force to BodyA and BodyB.
  */
-void PointActuator::computeForce(const SimTK::State& s, 
-                                 SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
-                                 SimTK::Vector& generalizedForces) const
+void PointActuator::computeForce(
+    const SimTK::State& s,
+    SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
+    SimTK::Vector& generalizedForces) const
 {
-    if( !_model || !_body ) return;
-
-    double force;
-
-    if (isActuationOverridden(s)) {
-        force = computeOverrideActuation(s);
-    } else {
-       force = computeActuation(s);
+    if (!_model || !_body) {
+        return;
     }
+
+    double force = isActuationOverridden(s) ? computeOverrideActuation(s)
+                                            : computeActuation(s);
     setActuation(s, force);
 
-
-    Vec3 forceVec = force*SimTK::UnitVec3(get_direction());
-    Vec3 lpoint = get_point();
-    if (!get_force_is_global())
+    Vec3 forceVec = force * SimTK::UnitVec3(get_direction());
+    Vec3 lpoint   = get_point();
+    if (!get_force_is_global()) {
         forceVec = _body->expressVectorInGround(s, forceVec);
-    if (get_point_is_global())
-        lpoint = getModel().getGround().
-            findStationLocationInAnotherFrame(s, lpoint, *_body);
+    }
+    if (get_point_is_global()) {
+        lpoint = getModel().getGround().findStationLocationInAnotherFrame(
+            s,
+            lpoint,
+            *_body);
+    }
     applyForceToPoint(s, *_body, lpoint, forceVec, bodyForces);
-
-    // get the velocity of the actuator in ground
-    Vec3 velocity = _body->findStationVelocityInGround(s, lpoint);
-
-    // the speed of the point is the "speed" of the actuator used to compute 
-    // power
-    setSpeed(s, velocity.norm());
 }
 //_____________________________________________________________________________
 /**

diff --git a/OpenSim/Actuators/PointActuator.h b/OpenSim/Actuators/PointActuator.h
@@ -85,6 +85,13 @@ class OSIMACTUATORS_API PointActuator : public ScalarActuator {
     /** Get the current value of the 'optimal_force' property. **/
     double getOptimalForce() const override; // Part of Actuator interface.
 
+protected:
+
+    //--------------------------------------------------------------------------
+    // Implement ModelComponent Interface
+    //--------------------------------------------------------------------------
+    void extendAddToSystem(SimTK::MultibodySystem& system) const override;
+
 private:
     void setNull();
     void constructProperties();
@@ -94,6 +101,15 @@ class OSIMACTUATORS_API PointActuator : public ScalarActuator {
     void setBody(Body* aBody);
     Body* getBody() const;
 
+    //--------------------------------------------------------------------------
+    // Implement ScalarActuator interface
+    //--------------------------------------------------------------------------
+    /** Get speed along force vector. */
+    double getSpeed(const SimTK::State& s) const override;
+
+    /** Computes speed along force vector. */
+    double calcSpeed(const SimTK::State& s) const;
+
     //--------------------------------------------------------------------------
     // Implement Force interface
     //--------------------------------------------------------------------------
@@ -104,15 +120,15 @@ class OSIMACTUATORS_API PointActuator : public ScalarActuator {
     //--------------------------------------------------------------------------
     // Implement Actuator interface (also see getOptimalForce() above)
     //--------------------------------------------------------------------------
-    double computeActuation( const SimTK::State& s) const override;
-    double getStress( const SimTK::State& s ) const override;
+    double computeActuation(const SimTK::State& s) const override;
+    double getStress(const SimTK::State& s) const override;
 
     //--------------------------------------------------------------------------
     // Implement ModelComponent interface
     //--------------------------------------------------------------------------
     // Setup method to initialize Body reference
     void extendConnectToModel(Model& model) override;
-    
+
     //--------------------------------------------------------------------------
     // Implement Object interface.
     //--------------------------------------------------------------------------
@@ -122,6 +138,9 @@ class OSIMACTUATORS_API PointActuator : public ScalarActuator {
     // Corresponding Body to which the point actuator is applied.
     SimTK::ReferencePtr<Body> _body;
 
+    // CacheVariable for storing computed speed along force direction.
+    mutable CacheVariable<double> _speedCV;
+
 //=============================================================================
 };  // END of class PointActuator