Rollout ForceProducer/ForceConsumer to almost all remaining Components

opensim-org · Aug 29, 2024 · f30bfe0 · f30bfe0
1 parent b89b7de
commit f30bfe0
Show file tree

Hide file tree

Showing 20 changed files with 177 additions and 242 deletions.
diff --git a/OpenSim/Actuators/BodyActuator.cpp b/OpenSim/Actuators/BodyActuator.cpp
@@ -21,13 +21,11 @@
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */
 
-//=============================================================================
-// INCLUDES
-//=============================================================================
-#include <OpenSim/Simulation/Model/Model.h>
-
 #include "BodyActuator.h"
 
+#include <OpenSim/Simulation/Model/Model.h>
+#include <OpenSim/Simulation/Model/ForceConsumer.h>
+
 using namespace OpenSim;
 using namespace std;
 using SimTK::Vec3;
@@ -104,21 +102,41 @@ const Body& BodyActuator::getBody() const
     return getConnectee<Body>("body");
 }
 
-//==============================================================================
-// APPLICATION
-//==============================================================================
 //_____________________________________________________________________________
 /**
-* Apply the actuator force/torque to Body.
+* Compute power consumed by moving the body via applied spatial force.
+* Reads the body spatial velocity vector and spatial force vector applied via
+* BodyActuator and computes the power as p = F (dotProdcut) V.
 */
-void BodyActuator::computeForce(const SimTK::State& s,
-                                SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
-                                SimTK::Vector& generalizedForces) const
+double BodyActuator::getPower(const SimTK::State& s) const
+{
+    const Body& body = getBody();
+
+    const SimTK::MobilizedBody& body_mb = body.getMobilizedBody();
+    SimTK::SpatialVec bodySpatialVelocities = body_mb.getBodyVelocity(s);
+
+    SimTK::Vector bodyVelocityVec(6);
+    bodyVelocityVec[0] = bodySpatialVelocities[0][0];
+    bodyVelocityVec[1] = bodySpatialVelocities[0][1];
+    bodyVelocityVec[2] = bodySpatialVelocities[0][2];
+    bodyVelocityVec[3] = bodySpatialVelocities[1][0];
+    bodyVelocityVec[4] = bodySpatialVelocities[1][1];
+    bodyVelocityVec[5] = bodySpatialVelocities[1][2];
+
+    const SimTK::Vector bodyForceVals = getControls(s);
+
+    double power = ~bodyForceVals * bodyVelocityVec;
+
+    return power;
+}
+
+void BodyActuator::implProduceForces(const SimTK::State& s,
+        ForceConsumer& forceConsumer) const
 {
     if (!_model) return;
 
     const bool spatialForceIsGlobal = getSpatialForceIsGlobal();
-    
+
     const Body& body = getBody();
     // const SimTK::MobilizedBody& body_mb = body.getMobilizedBody();
 
@@ -144,39 +162,8 @@ void BodyActuator::computeForce(const SimTK::State& s,
     // case) transform it to body frame
     if (get_point_is_global())
         pointOfApplication = getModel().getGround().
-            findStationLocationInAnotherFrame(s, pointOfApplication, body);
-
-    applyTorque(s, body, torqueVec, bodyForces);
-    applyForceToPoint(s, body, pointOfApplication, forceVec, bodyForces);
+        findStationLocationInAnotherFrame(s, pointOfApplication, body);
 
+    forceConsumer.consumeTorque(s, body, torqueVec);
+    forceConsumer.consumePointForce(s, body, pointOfApplication, forceVec);
 }
-
-//_____________________________________________________________________________
-/**
-* Compute power consumed by moving the body via applied spatial force.
-* Reads the body spatial velocity vector and spatial force vector applied via
-* BodyActuator and computes the power as p = F (dotProdcut) V.
-*/
-double BodyActuator::getPower(const SimTK::State& s) const
-{
-    const Body& body = getBody();
-
-    const SimTK::MobilizedBody& body_mb = body.getMobilizedBody();
-    SimTK::SpatialVec bodySpatialVelocities = body_mb.getBodyVelocity(s);
-
-    SimTK::Vector bodyVelocityVec(6);
-    bodyVelocityVec[0] = bodySpatialVelocities[0][0];
-    bodyVelocityVec[1] = bodySpatialVelocities[0][1];
-    bodyVelocityVec[2] = bodySpatialVelocities[0][2];
-    bodyVelocityVec[3] = bodySpatialVelocities[1][0];
-    bodyVelocityVec[4] = bodySpatialVelocities[1][1];
-    bodyVelocityVec[5] = bodySpatialVelocities[1][2];
-
-    const SimTK::Vector bodyForceVals = getControls(s);
-
-    double power = ~bodyForceVals * bodyVelocityVec;
-
-    return power;
-}
-
-
diff --git a/OpenSim/Actuators/BodyActuator.h b/OpenSim/Actuators/BodyActuator.h
@@ -140,12 +140,10 @@ class OSIMACTUATORS_API BodyActuator : public Actuator {
     Begin with its properties. */
     void constructProperties();
 
-    //--------------------------------------------------------------------------
-    // Implement Force interface
-    //--------------------------------------------------------------------------
-    void computeForce(const SimTK::State& state,
-                    SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
-                    SimTK::Vector& mobilityForces) const override;
+    /**
+     * Implements the `ForceProducer` interface.
+     */
+    void implProduceForces(const SimTK::State&, ForceConsumer&) const override;
 
     //--------------------------------------------------------------------------
     // Implement Actuator interface.

diff --git a/OpenSim/Actuators/CoordinateActuator.cpp b/OpenSim/Actuators/CoordinateActuator.cpp
@@ -22,16 +22,14 @@
  * limitations under the License.                                             *
  * -------------------------------------------------------------------------- */
 
-//==============================================================================
-// INCLUDES
-//==============================================================================
+#include "CoordinateActuator.h"
+
 #include <OpenSim/Common/Assertion.h>
 #include <OpenSim/Simulation/Model/Model.h>
 #include <OpenSim/Simulation/Model/CoordinateSet.h>
+#include <OpenSim/Simulation/Model/ForceConsumer.h>
 #include <OpenSim/Simulation/Model/ForceSet.h>
 
-#include "CoordinateActuator.h"
-
 using namespace OpenSim;
 using namespace std;
 
@@ -183,24 +181,23 @@ CreateForceSetOfCoordinateActuatorsForModel(const SimTK::State& s, Model& aModel
 /**
  * Apply the actuator force to BodyA and BodyB.
  */
-void CoordinateActuator::computeForce( const SimTK::State& s, 
-                               SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
-                               SimTK::Vector& mobilityForces) const
+void CoordinateActuator::implProduceForces(const SimTK::State& s,
+        ForceConsumer& forceConsumer) const
 {
-    if(!_model) return;
-
-   double force;
-   if (isActuationOverridden(s)) {
-       force = computeOverrideActuation(s);
-    } else {
-       force = computeActuation(s);
+    if (!_model) {
+        return;
     }
-   setActuation(s, force);
 
-    if(isCoordinateValid()){
-        applyGeneralizedForce(s, *_coord, getActuation(s), mobilityForces);
+    const double force = isActuationOverridden(s) ?
+        computeOverrideActuation(s) :
+        computeActuation(s);
+
+    setActuation(s, force);
+
+    if (isCoordinateValid()) {
+        forceConsumer.consumeGeneralizedForce(s, *_coord, getActuation(s));
     } else {
-        log_warn("CoordinateActuator::computeForce: Invalid coordinate");
+        log_warn("CoordinateActuator::implProduceForces: Invalid coordinate");
     }
 }
 

diff --git a/OpenSim/Actuators/CoordinateActuator.h b/OpenSim/Actuators/CoordinateActuator.h
@@ -89,12 +89,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(CoordinateActuator, ScalarActuator);
 // PRIVATE
 //==============================================================================
 private:
-    //--------------------------------------------------------------------------
-    // Implement Force interface
-    //--------------------------------------------------------------------------
-    void computeForce(const SimTK::State& state, 
-                      SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
-                      SimTK::Vector& mobilityForces) const override;
+    /**
+     * Implements the `ForceProducer` interface.
+     */
+    void implProduceForces(const SimTK::State&, ForceConsumer&) const override;
 
 
     //--------------------------------------------------------------------------

diff --git a/OpenSim/Actuators/McKibbenActuator.cpp b/OpenSim/Actuators/McKibbenActuator.cpp
@@ -91,24 +91,6 @@ double McKibbenActuator::computeActuation( const SimTK::State& s ) const
     return force;
 }
 
-//==============================================================================
-// APPLICATION
-//==============================================================================
-//_____________________________________________________________________________
-/**
- * Apply the actuator force to the path
- *
- * @param s current SimTK::State
- */
-void McKibbenActuator::computeForce(const SimTK::State& s, 
-                                SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
-                                SimTK::Vector& generalizedForces) const
-{
-
-    double actuation = computeActuation(s);
-
-    getPath().addInEquivalentForces(s, actuation, bodyForces, generalizedForces);
-}
 //_____________________________________________________________________________
 /**
  * Sets the actual Body references _bodyA and _bodyB

diff --git a/OpenSim/Actuators/McKibbenActuator.h b/OpenSim/Actuators/McKibbenActuator.h
@@ -96,14 +96,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(McKibbenActuator, PathActuator);
 private:
     void constructProperties();
 
-    //--------------------------------------------------------------------------
-    // Implement Force interface
-    //--------------------------------------------------------------------------
-    void computeForce(const SimTK::State& state, 
-                      SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
-                      SimTK::Vector& mobilityForces) const override;
-
-
     //--------------------------------------------------------------------------
     // Implement ModelComponent interface
     //--------------------------------------------------------------------------

diff --git a/OpenSim/Actuators/PointActuator.cpp b/OpenSim/Actuators/PointActuator.cpp
@@ -25,16 +25,13 @@
  * Author: Ajay Seth
  */
 
+#include "PointActuator.h"
 
-//=============================================================================
-// INCLUDES
-//=============================================================================
 #include <OpenSim/Common/XMLDocument.h>
+#include <OpenSim/Simulation/Model/ForceConsumer.h>
 #include <OpenSim/Simulation/Model/Model.h>
 #include <OpenSim/Simulation/Model/BodySet.h>
 
-#include "PointActuator.h"
-
 using namespace OpenSim;
 using namespace std;
 using SimTK::Vec3;
@@ -180,38 +177,32 @@ double PointActuator::computeActuation(const SimTK::State& s) const
     return getControl(s) * getOptimalForce();
 }
 
-//=============================================================================
-// APPLICATION
-//=============================================================================
-//_____________________________________________________________________________
-/**
- * Apply the actuator force to BodyA and BodyB.
- */
-void PointActuator::computeForce(
+void PointActuator::implProduceForces(
     const SimTK::State& s,
-    SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
-    SimTK::Vector& generalizedForces) const
+    ForceConsumer& forceConsumer) const
 {
     if (!_model || !_body) {
         return;
     }
 
-    double force = isActuationOverridden(s) ? computeOverrideActuation(s)
-                                            : computeActuation(s);
+    const 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()) {
         forceVec = _body->expressVectorInGround(s, forceVec);
     }
+
+    Vec3 lpoint = get_point();
     if (get_point_is_global()) {
         lpoint = getModel().getGround().findStationLocationInAnotherFrame(
             s,
             lpoint,
             *_body);
     }
-    applyForceToPoint(s, *_body, lpoint, forceVec, bodyForces);
+
+    forceConsumer.consumePointForce(s, *_body, lpoint, forceVec);
 }
 //_____________________________________________________________________________
 /**

diff --git a/OpenSim/Actuators/PointActuator.h b/OpenSim/Actuators/PointActuator.h
@@ -110,12 +110,11 @@ class OSIMACTUATORS_API PointActuator : public ScalarActuator {
     /** Computes speed along force vector. */
     double calcSpeed(const SimTK::State& s) const;
 
-    //--------------------------------------------------------------------------
-    // Implement Force interface
-    //--------------------------------------------------------------------------
-    void computeForce(const SimTK::State& state, 
-                      SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
-                      SimTK::Vector& mobilityForces) const override;
+    /**
+     * Implements the `ForceProducer` interface by applying the actuator force to
+     * BodyA and BodyB.
+     */
+    void implProduceForces(const SimTK::State&, ForceConsumer&) const override;
 
     //--------------------------------------------------------------------------
     // Implement Actuator interface (also see getOptimalForce() above)

diff --git a/OpenSim/Actuators/PointToPointActuator.cpp b/OpenSim/Actuators/PointToPointActuator.cpp
@@ -25,14 +25,12 @@
  * Author: Matt DeMers
  */
 
-//==============================================================================
-// INCLUDES
-//==============================================================================
+#include "PointToPointActuator.h"
+
+#include <OpenSim/Simulation/Model/ForceConsumer.h>
 #include <OpenSim/Simulation/Model/Model.h>
 #include <OpenSim/Simulation/Model/BodySet.h>
 
-#include "PointToPointActuator.h"
-
 using namespace OpenSim;
 using std::string;
 using SimTK::Vec3; using SimTK::Vector_; using SimTK::Vector; 
@@ -226,10 +224,9 @@ double PointToPointActuator::calcSpeed(const SimTK::State& s) const
     return speed;
 }
 
-void PointToPointActuator::computeForce(
+void PointToPointActuator::implProduceForces(
     const SimTK::State& s,
-    SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
-    SimTK::Vector& generalizedForces) const
+    ForceConsumer& forceConsumer) const
 {
     if (!_model || !_bodyA || !_bodyB) {
         return;
@@ -260,8 +257,8 @@ void PointToPointActuator::computeForce(
     const SimTK::Vec3 force = forceMagnitude * getDirectionBAInGround(s);
 
     // Apply equal and opposite forces to the bodies.
-    applyForceToPoint(s, *_bodyA, pointA_inBodyA, force, bodyForces);
-    applyForceToPoint(s, *_bodyB, pointB_inBodyB, -force, bodyForces);
+    forceConsumer.consumePointForce(s, *_bodyA, pointA_inBodyA, force);
+    forceConsumer.consumePointForce(s, *_bodyB, pointB_inBodyB, -force);
 }
 //_____________________________________________________________________________
 /**

diff --git a/OpenSim/Actuators/PointToPointActuator.h b/OpenSim/Actuators/PointToPointActuator.h
@@ -128,12 +128,10 @@ class OSIMACTUATORS_API PointToPointActuator : public ScalarActuator {
     Body* getBodyA() const {return _bodyA.get();}
     Body* getBodyB() const {return _bodyB.get();}    
 
-    //--------------------------------------------------------------------------
-    // Implement Force interface
-    //--------------------------------------------------------------------------
-    void computeForce(const SimTK::State& state, 
-                      SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
-                      SimTK::Vector& mobilityForces) const override;
+    /**
+     * Implements the `ForceProducer` interface.
+     */
+    void implProduceForces(const SimTK::State&, ForceConsumer&) const override;
 
     //--------------------------------------------------------------------------
     // Implement Actuator interface (also see getOptimalForce() above)