From e5acde92be6fc23a2167e92ffe456a3aad0fd546 Mon Sep 17 00:00:00 2001
From: pepbos <pepijnvandenbos@gmail.com>
Date: Thu, 16 Nov 2023 17:06:02 +0100
Subject: [PATCH 01/10] removes speed computation from
 PathActuator::computeForce

---
 OpenSim/Simulation/Model/PathActuator.cpp | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/OpenSim/Simulation/Model/PathActuator.cpp b/OpenSim/Simulation/Model/PathActuator.cpp
index 218e433ea9..59c6b197c1 100644
--- a/OpenSim/Simulation/Model/PathActuator.cpp
+++ b/OpenSim/Simulation/Model/PathActuator.cpp
@@ -175,13 +175,6 @@ void PathActuator::computeForce( const SimTK::State& s,
 
     const auto &path = getPath();
 
-    // compute path's lengthening speed if necessary
-    double speed = path.getLengtheningSpeed(s);
-
-    // the lengthening speed of this actuator is the "speed" of the actuator 
-    // used to compute power
-    setSpeed(s, speed);
-
     double force =0;
     if( isActuationOverridden(s) ) {
         force = computeOverrideActuation(s);

From 55e9ae0c5a94042724ad64206e26b6fbfed545e4 Mon Sep 17 00:00:00 2001
From: pepbos <pepijnvandenbos@gmail.com>
Date: Mon, 20 Nov 2023 17:29:17 +0100
Subject: [PATCH 02/10] in ScalarActuator: removes setSpeed, and removes impl
 from getSpeed

---
 OpenSim/Actuators/PointActuator.cpp           |  71 +++++---
 OpenSim/Actuators/PointActuator.h             |  17 +-
 OpenSim/Actuators/PointToPointActuator.cpp    | 154 ++++++++++++------
 OpenSim/Actuators/PointToPointActuator.h      |  16 ++
 OpenSim/Actuators/TorqueActuator.cpp          |  70 +++++---
 OpenSim/Actuators/TorqueActuator.h            |  14 ++
 .../CustomActuatorExample/PistonActuator.cpp  |  83 +++++++---
 .../CustomActuatorExample/PistonActuator.h    |  10 ++
 OpenSim/Moco/MocoStudyFactory.cpp             |   6 +
 OpenSim/Simulation/Model/Actuator.cpp         |  13 +-
 OpenSim/Simulation/Model/Actuator.h           |   4 +-
 OpenSim/Simulation/Model/PathActuator.h       |   8 +
 12 files changed, 326 insertions(+), 140 deletions(-)

diff --git a/OpenSim/Actuators/PointActuator.cpp b/OpenSim/Actuators/PointActuator.cpp
index 052afbc326..753091ea43 100644
--- 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
 //=============================================================================
@@ -137,6 +145,25 @@ 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);
+    }
+
+    // get the velocity of the actuator in ground
+    Vec3 velocity = _body->findStationVelocityInGround(s, get_point());
+
+    // the speed of the point is the "speed" of the actuator used to compute
+    // power
+    double speed = velocity.norm();
+
+    updCacheVariableValue(s, _speedCV) = speed;
+    markCacheVariableValid(s, _speedCV);
+    return speed;
+}
+
 /**
  * Compute all quantities necessary for applying the actuator force to the
  * model.
@@ -158,37 +185,31 @@ double PointActuator::computeActuation( const SimTK::State& s ) const
 /**
  * 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
index 8401914634..d4f1fc50f4 100644
--- 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,11 @@ class OSIMACTUATORS_API PointActuator : public ScalarActuator {
     void setBody(Body* aBody);
     Body* getBody() const;
 
+    //--------------------------------------------------------------------------
+    // Implement ScalarActuator interface
+    //--------------------------------------------------------------------------
+    double getSpeed( const SimTK::State& s) const override;
+
     //--------------------------------------------------------------------------
     // Implement Force interface
     //--------------------------------------------------------------------------
@@ -112,7 +124,7 @@ class OSIMACTUATORS_API PointActuator : public ScalarActuator {
     //--------------------------------------------------------------------------
     // Setup method to initialize Body reference
     void extendConnectToModel(Model& model) override;
-    
+
     //--------------------------------------------------------------------------
     // Implement Object interface.
     //--------------------------------------------------------------------------
@@ -122,6 +134,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
 
diff --git a/OpenSim/Actuators/PointToPointActuator.cpp b/OpenSim/Actuators/PointToPointActuator.cpp
index 49debe1dda..b53c0bd24d 100644
--- a/OpenSim/Actuators/PointToPointActuator.cpp
+++ b/OpenSim/Actuators/PointToPointActuator.cpp
@@ -71,6 +71,18 @@ void PointToPointActuator::constructProperties()
     constructProperty_optimal_force(1.0);
 }
 
+void PointToPointActuator::extendAddToSystem(
+    SimTK::MultibodySystem& system) const
+{
+    Super::extendAddToSystem(system);
+
+    // Cache the computed speed of the actuator
+    this->_speedCV     = addCacheVariable("speed", 0.0, SimTK::Stage::Velocity);
+    this->_directionCV = addCacheVariable(
+        "direction",
+        SimTK::UnitVec3{1.0, 0., 0.},
+        SimTK::Stage::Position);
+}
 
 //==============================================================================
 // GET AND SET
@@ -140,75 +152,113 @@ double PointToPointActuator::computeActuation( const SimTK::State& s ) const
 // APPLICATION
 //==============================================================================
 //_____________________________________________________________________________
-/**
- * Apply the actuator force to BodyA and BodyB.
- *
- * @param s current SimTK::State
- */
-void PointToPointActuator::computeForce(const SimTK::State& s, 
-                                SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
-                                SimTK::Vector& generalizedForces) const
+
+SimTK::UnitVec3 PointToPointActuator::getDirectionBAInGround(
+    const SimTK::State& s) const
 {
+    if (isCacheVariableValid(s, _directionCV)) {
+        return getCacheVariableValue(s, _directionCV);
+    }
+
+    // Get pointA and pointB positions in the local frame of bodyA and bodyB,
+    // respectively.
+    // Points may have been supplied either global or local frame.
     const bool pointsAreGlobal = getPointsAreGlobal();
-    const SimTK::Vec3& pointA = getPointA();
-    const SimTK::Vec3& pointB = getPointB();
+    const SimTK::Vec3& pointA  = getPointA();
+    const SimTK::Vec3& pointB  = getPointB();
 
-    if(!_model) return;
-    
-    if( !_bodyA || !_bodyB )
-        return;
-    
-    // Get pointA and pointB positions in both the global frame, and in 
-    // the local frame of bodyA and bodyB, respectively. Points may have
-    // been supplied either way.
-
-    SimTK::Vec3 pointA_inGround, pointB_inGround, 
-                pointA_inBodyA, pointB_inBodyB;
-    Ground ground = getModel().getGround();
-
-    if (pointsAreGlobal)
-    {
-        pointA_inGround = pointA;
-        pointB_inGround = pointB;
-        pointA_inBodyA = ground.findStationLocationInAnotherFrame(s, pointA_inGround, *_bodyA);
-        pointB_inBodyB = ground.findStationLocationInAnotherFrame(s, pointB_inGround, *_bodyB);
-    }
-    else
-    {
-        pointA_inBodyA = pointA;
-        pointB_inBodyB = pointB;
-        pointA_inGround = _bodyA->findStationLocationInGround(s, pointA_inBodyA);
-        pointB_inGround = _bodyB->findStationLocationInGround(s, pointB_inBodyB);
-    }
+    SimTK::Vec3 pointA_inGround =
+        pointsAreGlobal ? pointA
+                        : _bodyA->findStationLocationInGround(s, pointA);
+    SimTK::Vec3 pointB_inGround =
+        pointsAreGlobal ? pointB
+                        : _bodyB->findStationLocationInGround(s, pointB);
 
     // Find the direction along which the actuator applies its force.
     // NOTE: this will fail if the points are coincident.
     const SimTK::Vec3 r = pointA_inGround - pointB_inGround;
     const SimTK::UnitVec3 direction(r); // normalize
 
-    // Find the force magnitude and set it. Then form the force vector.
-    double forceMagnitude;
+    updCacheVariableValue(s, _directionCV) = direction;
+    markCacheVariableValid(s, _directionCV);
+    return direction;
+}
 
-    if (isActuationOverridden(s)) {
-        forceMagnitude = computeOverrideActuation(s);
-    } else {
-       forceMagnitude = computeActuation(s);
+double PointToPointActuator::getSpeed(const SimTK::State& s) const
+{
+    if (!_model || !_bodyA || !_bodyB) {
+        return 0.;
     }
+
+    // Speed is zero for constant points defined in the same frame.
+    const bool pointsAreGlobal = getPointsAreGlobal();
+    if (pointsAreGlobal) {
+        return 0.;
+    }
+
+    if (isCacheVariableValid(s, _speedCV)) {
+        return getCacheVariableValue(s, _speedCV);
+    }
+
+    const SimTK::Vec3& pointA_inBodyA = getPointA();
+    const SimTK::Vec3& pointB_inBodyB = getPointB();
+
+    // Get the relative velocity of the points in ground.
+    SimTK::Vec3 velA_G
+        = _bodyA->findStationVelocityInGround(s, pointA_inBodyA);
+    SimTK::Vec3 velB_G
+        = _bodyB->findStationVelocityInGround(s, pointB_inBodyB);
+    SimTK::Vec3 velAB_G = velA_G - velB_G;
+    // Speed used to compute power is the speed along the line connecting
+    // the two bodies.
+    double speed = ~velAB_G * getDirectionBAInGround(s);
+
+    updCacheVariableValue(s, _speedCV) = speed;
+    markCacheVariableValid(s, _speedCV);
+    return speed;
+}
+
+/**
+ * Apply the actuator force to BodyA and BodyB.
+ *
+ * @param s current SimTK::State
+ */
+void PointToPointActuator::computeForce(
+    const SimTK::State& s,
+    SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
+    SimTK::Vector& generalizedForces) const
+{
+    if (!_model || !_bodyA || !_bodyB) {
+        return;
+    }
+
+    // Get pointA and pointB positions in the global frame.
+    // Points may have been supplied either global or local frame.
+    const bool pointsAreGlobal = getPointsAreGlobal();
+    const SimTK::Vec3& pointA  = getPointA();
+    const SimTK::Vec3& pointB  = getPointB();
+    Ground ground              = getModel().getGround();
+
+    SimTK::Vec3 pointA_inBodyA =
+        pointsAreGlobal
+            ? ground.findStationLocationInAnotherFrame(s, pointA, *_bodyA)
+            : pointA;
+    SimTK::Vec3 pointB_inBodyB =
+        pointsAreGlobal
+            ? ground.findStationLocationInAnotherFrame(s, pointB, *_bodyB)
+            : pointB;
+
+    // Find the force magnitude and set it. Then form the force vector.
+    double forceMagnitude = isActuationOverridden(s)
+                                ? computeOverrideActuation(s)
+                                : computeActuation(s);
     setActuation(s, forceMagnitude);
 
-    const SimTK::Vec3 force = forceMagnitude*direction;
+    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);
-
-    // Get the relative velocity of the points in ground.
-    SimTK::Vec3 velA_G =  _bodyA->findStationVelocityInGround(s, pointA_inBodyA);
-    SimTK::Vec3 velB_G = _bodyB->findStationVelocityInGround(s, pointB_inBodyB);
-    SimTK::Vec3 velAB_G = velA_G-velB_G;
-    // Speed used to compute power is the speed along the line connecting 
-    // the two bodies.
-    setSpeed(s, ~velAB_G*direction);
 }
 //_____________________________________________________________________________
 /**
diff --git a/OpenSim/Actuators/PointToPointActuator.h b/OpenSim/Actuators/PointToPointActuator.h
index 5b3b7631c9..6e5f8d73e2 100644
--- a/OpenSim/Actuators/PointToPointActuator.h
+++ b/OpenSim/Actuators/PointToPointActuator.h
@@ -112,6 +112,17 @@ class OSIMACTUATORS_API PointToPointActuator : public ScalarActuator {
 
     // default destructor, copy constructor, copy assignment
 
+    //--------------------------------------------------------------------------
+    // Implement ScalarActuator interface
+    //--------------------------------------------------------------------------
+    double getSpeed( const SimTK::State& s) const override;
+
+protected:
+    //--------------------------------------------------------------------------
+    // Implement ModelComponent Interface
+    //--------------------------------------------------------------------------
+    void extendAddToSystem(SimTK::MultibodySystem& system) const override;
+
 private:
     void constructProperties();
 
@@ -151,6 +162,11 @@ class OSIMACTUATORS_API PointToPointActuator : public ScalarActuator {
     // The bodies on which this point-to-point actuator acts.
     SimTK::ReferencePtr<Body> _bodyA, _bodyB;
 
+    // CachedVariables: Speed- and direction along force, used to compute power
+    mutable CacheVariable<double> _speedCV;
+    mutable CacheVariable<SimTK::UnitVec3> _directionCV;
+
+    SimTK::UnitVec3 getDirectionBAInGround(const SimTK::State& s) const;
 //=============================================================================
 };  // END of class PointToPointActuator
 
diff --git a/OpenSim/Actuators/TorqueActuator.cpp b/OpenSim/Actuators/TorqueActuator.cpp
index b5f46c96cb..6f8df0a353 100644
--- a/OpenSim/Actuators/TorqueActuator.cpp
+++ b/OpenSim/Actuators/TorqueActuator.cpp
@@ -73,6 +73,13 @@ void TorqueActuator::constructProperties()
     constructProperty_optimal_force(1.0);
 }
 
+void TorqueActuator::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
@@ -137,47 +144,64 @@ double TorqueActuator::computeActuation(const State& s) const
 /**
  * Apply the actuator force to BodyA and BodyB.
  */
-void TorqueActuator::computeForce(const State& s, 
-                                  Vector_<SpatialVec>& bodyForces, 
-                                  Vector& generalizedForces) const
+void TorqueActuator::computeForce(
+    const State& s,
+    Vector_<SpatialVec>& bodyForces,
+    Vector& generalizedForces) const
 {
-    if(!_model) return;
+    if (!_model || !_bodyA) {
+        return;
+    }
 
     const bool torqueIsGlobal = getTorqueIsGlobal();
-    const Vec3& axis = getAxis();
-    
-    double actuation = 0;
-
-    if (isActuationOverridden(s)) {
-        actuation = computeOverrideActuation(s);
-    } else {
-        actuation = computeActuation(s);
-    }
-    setActuation(s, actuation);
+    const Vec3& axis          = getAxis();
 
-    if(!_bodyA)
-        return;
-    
+    double actuation = isActuationOverridden(s) ? computeOverrideActuation(s)
+                                                : computeActuation(s);
     setActuation(s, actuation);
+
     Vec3 torque = actuation * UnitVec3(axis);
-    
-    if (!torqueIsGlobal)
+
+    if (!torqueIsGlobal) {
         torque = _bodyA->expressVectorInGround(s, torque);
-    
+    }
+
     applyTorque(s, *_bodyA, torque, bodyForces);
 
     // if bodyB is not specified, use the ground body by default
-    if(_bodyB)
+    if (_bodyB) {
         applyTorque(s, *_bodyB, -torque, bodyForces);
+    }
+}
+
+double TorqueActuator::getSpeed(const SimTK::State& s) const
+{
+    if (!_model || !_bodyA) {
+        return 0.;
+    }
+
+    if (isCacheVariableValid(s, _speedCV)) {
+        return getCacheVariableValue(s, _speedCV);
+    }
+
+    const bool torqueIsGlobal = getTorqueIsGlobal();
+    const Vec3& axis          = SimTK::UnitVec3(getAxis());
 
     // get the angular velocity of the body in ground
     Vec3 omegaA = _bodyA->getVelocityInGround(s)[0];
-    Vec3 omegaB = _bodyB->getVelocityInGround(s)[0];
+    // if bodyB is not specified, use the ground body by default
+    Vec3 omegaB =
+        _bodyB ? _bodyB->getVelocityInGround(s)[0] : SimTK::Vec3{0., 0., 0.};
 
     // the "speed" is the relative angular velocity of the bodies
     // projected onto the torque axis.
-    setSpeed(s, ~(omegaA-omegaB)*axis);
+    double speed = ~(omegaA - omegaB) * axis;
+
+    updCacheVariableValue(s, _speedCV) = speed;
+    markCacheVariableValid(s, _speedCV);
+    return speed;
 }
+
 //_____________________________________________________________________________
 /**
  * Sets the actual Body references _bodyA and _bodyB
diff --git a/OpenSim/Actuators/TorqueActuator.h b/OpenSim/Actuators/TorqueActuator.h
index c4f97234f9..9ecd6ca561 100644
--- a/OpenSim/Actuators/TorqueActuator.h
+++ b/OpenSim/Actuators/TorqueActuator.h
@@ -125,6 +125,17 @@ class OSIMACTUATORS_API TorqueActuator : public ScalarActuator {
     /** Get the second body (bodyB) to which this actuator applies torque. */
     const PhysicalFrame& getBodyB() const {return *_bodyB;}
 
+    //--------------------------------------------------------------------------
+    // Implement ScalarActuator interface
+    //--------------------------------------------------------------------------
+    double getSpeed( const SimTK::State& s) const override;
+
+protected:
+    //--------------------------------------------------------------------------
+    // Implement ModelComponent Interface
+    //--------------------------------------------------------------------------
+    void extendAddToSystem(SimTK::MultibodySystem& system) const override;
+
 //==============================================================================
 // PRIVATE
 //==============================================================================
@@ -169,6 +180,9 @@ class OSIMACTUATORS_API TorqueActuator : public ScalarActuator {
     // Corresponding Body to which the equal and opposite torque is applied.
     SimTK::ReferencePtr<const PhysicalFrame> _bodyB;
 
+    // CachedVariable: Speed used to compute power.
+    mutable CacheVariable<double> _speedCV;
+
 //==============================================================================
 };  // END of class TorqueActuator
 
diff --git a/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp b/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp
index ef6e02548b..2ba2fd4f50 100644
--- a/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp
+++ b/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp
@@ -106,48 +106,83 @@ double PistonActuator::getStress( const SimTK::State& s) const
 //=============================================================================
 // FORCE INTERFACE
 //=============================================================================
-void PistonActuator::computeForce(const SimTK::State& s, 
-        SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
-        SimTK::Vector& generalizedForces) const
+void PistonActuator::computeForce(
+    const SimTK::State& s,
+    SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
+    SimTK::Vector& generalizedForces) const
 {
     const PhysicalFrame& frameA = getFrameA();
     const PhysicalFrame& frameB = getFrameB();
 
-    // We need points A and B expressed both in their frame and expressed in
-    // ground.
-    SimTK::Vec3 pointA_inGround;
-    SimTK::Vec3 pointB_inGround;
-
-    SimTK::Vec3 pointA = get_pointA();
-    SimTK::Vec3 pointB = get_pointB();
+    // We need points A and B expressed in local A and B frame respectively.
+    SimTK::Vec3 pointA   = get_pointA();
+    SimTK::Vec3 pointB   = get_pointB();
     const Ground& ground = getModel().getGround();
-    if (get_points_are_global())
-    {
-        pointA_inGround = pointA;
-        pointB_inGround = pointB;
+    if (get_points_are_global()) {
         pointA = ground.findStationLocationInAnotherFrame(s, pointA, frameA);
         pointB = ground.findStationLocationInAnotherFrame(s, pointB, frameB);
     }
-    else
-    {
-        pointA_inGround = frameA.findStationLocationInGround(s, pointA);
-        pointB_inGround = frameB.findStationLocationInGround(s, pointB);
-    }
-
-    // Find the direction along which the actuator applies its force.
-    SimTK::Vec3 r = pointA_inGround - pointB_inGround;
-    SimTK::UnitVec3 direction(r);
 
     // Calculate the force magnitude and the force vector.
     double forceMagnitude = computeActuation(s);
     setActuation(s, forceMagnitude);
-    SimTK::Vec3 force = forceMagnitude * direction;
+    SimTK::Vec3 force = forceMagnitude * calcDirectionBAInGround(s);
 
     // Apply equal and opposite forces to the bodies.
     applyForceToPoint(s, frameA, pointA, force, bodyForces);
     applyForceToPoint(s, frameB, pointB, -force, bodyForces);
 }
 
+SimTK::UnitVec3 PistonActuator::calcDirectionBAInGround(
+    const SimTK::State& s) const
+{
+    // Get pointA and pointB positions in the local frame of bodyA and bodyB,
+    // respectively.
+    // Points may have been supplied either global or local frame.
+    const bool pointsAreGlobal = get_points_are_global();
+    const SimTK::Vec3& pointA  = getPointA();
+    const SimTK::Vec3& pointB  = getPointB();
+
+    SimTK::Vec3 pointA_inGround =
+        pointsAreGlobal ? pointA
+                        : getFrameA().findStationLocationInGround(s, pointA);
+    SimTK::Vec3 pointB_inGround =
+        pointsAreGlobal ? pointB
+                        : getFrameB().findStationLocationInGround(s, pointB);
+
+    // Find the direction along which the actuator applies its force.
+    const SimTK::Vec3 r = pointA_inGround - pointB_inGround;
+    const SimTK::UnitVec3 direction(r); // normalize
+
+    return direction;
+}
+
+//=============================================================================
+// SCALAR ACTUATOR INTERFACE
+//=============================================================================
+double PistonActuator::getSpeed(const SimTK::State& s) const
+{
+    // Speed is zero for constant points defined in the same frame.
+    const bool pointsAreGlobal = get_points_are_global();
+    if (pointsAreGlobal) {
+        return 0.;
+    }
+
+    const SimTK::Vec3& pointA_inBodyA = getPointA();
+    const SimTK::Vec3& pointB_inBodyB = getPointB();
+
+    // Get the relative velocity of the points in ground.
+    SimTK::Vec3 velA_G =
+        getFrameA().findStationVelocityInGround(s, pointA_inBodyA);
+    SimTK::Vec3 velB_G =
+        getFrameB().findStationVelocityInGround(s, pointB_inBodyB);
+    SimTK::Vec3 velAB_G = velA_G - velB_G;
+    // Speed used to compute power is the speed along the line connecting
+    // the two bodies.
+    double speed = ~velAB_G * calcDirectionBAInGround(s);
+    return speed;
+}
+
 //=============================================================================
 // ACTUATOR INTERFACE
 //=============================================================================
diff --git a/OpenSim/Examples/CustomActuatorExample/PistonActuator.h b/OpenSim/Examples/CustomActuatorExample/PistonActuator.h
index 6026f2385c..1f8401ce24 100644
--- a/OpenSim/Examples/CustomActuatorExample/PistonActuator.h
+++ b/OpenSim/Examples/CustomActuatorExample/PistonActuator.h
@@ -134,6 +134,16 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PistonActuator, ScalarActuator);
      */
     double computeActuation(const SimTK::State& s) const override;
 
+    //--------------------------------------------------------------------------
+    // SCALAR ACTUATOR INTERFACE
+    //--------------------------------------------------------------------------
+    /** Compute the speed along the force direction. */
+    double getSpeed(const SimTK::State& s) const override;
+
+    private:
+    /** Compute the direction of the actuator force in ground frame. */
+    SimTK::UnitVec3 calcDirectionBAInGround(const SimTK::State& s) const;
+
 //=============================================================================
 }; // END of class PistonActuator
 
diff --git a/OpenSim/Moco/MocoStudyFactory.cpp b/OpenSim/Moco/MocoStudyFactory.cpp
index 8caafb1b4b..fc26f48c0d 100644
--- a/OpenSim/Moco/MocoStudyFactory.cpp
+++ b/OpenSim/Moco/MocoStudyFactory.cpp
@@ -19,6 +19,7 @@
 #include "MocoStudyFactory.h"
 
 #include "MocoProblem.h"
+#include <SimTKcommon/Scalar.h>
 
 #include <OpenSim/Actuators/ModelFactory.h>
 
@@ -48,6 +49,11 @@ MocoStudy MocoStudyFactory::createLinearTangentSteeringStudy(
             applyGeneralizedForce(state, coordY,
                     get_acceleration() * sin(angle), mobilityForces);
         }
+
+        double getSpeed(const SimTK::State& state) const override
+        {
+            return SimTK::NaN;
+        }
     };
 
     class LinearTangentFinalSpeed : public MocoGoal {
diff --git a/OpenSim/Simulation/Model/Actuator.cpp b/OpenSim/Simulation/Model/Actuator.cpp
index ef9d2ce00e..b228b8e62c 100644
--- a/OpenSim/Simulation/Model/Actuator.cpp
+++ b/OpenSim/Simulation/Model/Actuator.cpp
@@ -182,9 +182,8 @@ void ScalarActuator::extendAddToSystem(SimTK::MultibodySystem& system) const
     // override actuation provided
     addModelingOption(overrideActuationKey, 1);
 
-    // Cache the computed actuation and speed of the scalar valued actuator
+    // Cache the computed actuation of the scalar valued actuator
     _actuationCV = addCacheVariable("actuation", 0.0, Stage::Velocity);
-    _speedCV = addCacheVariable("speed", 0.0, Stage::Velocity);
 
     // Discrete state variable is the override actuation value if in override mode
     addDiscreteVariable(overrideActuationKey, Stage::Time);
@@ -219,16 +218,6 @@ void ScalarActuator::setActuation(const State& s, double aActuation) const
     setCacheVariableValue(s, _actuationCV, aActuation);
 }
 
-double ScalarActuator::getSpeed(const State& s) const
-{
-    return getCacheVariableValue(s, _speedCV);
-}
-
-void ScalarActuator::setSpeed(const State &s, double speed) const
-{
-    setCacheVariableValue(s, _speedCV, speed);
-}
-
 void ScalarActuator::overrideActuation(SimTK::State& s, bool flag) const
 {
     setModelingOption(s, overrideActuationKey, int(flag));
diff --git a/OpenSim/Simulation/Model/Actuator.h b/OpenSim/Simulation/Model/Actuator.h
index 8829bdcb51..a1c0be7fc7 100644
--- a/OpenSim/Simulation/Model/Actuator.h
+++ b/OpenSim/Simulation/Model/Actuator.h
@@ -166,8 +166,7 @@ class OSIMSIMULATION_API ScalarActuator : public Actuator {
     // Accessing actuation, speed, and power of a scalar valued actuator
     virtual void setActuation(const SimTK::State& s, double aActuation) const;
     virtual double getActuation(const SimTK::State& s) const;
-    virtual void setSpeed( const SimTK::State& s, double aspeed) const;
-    virtual double getSpeed( const SimTK::State& s) const;
+    virtual double getSpeed( const SimTK::State& s) const = 0;
     double getPower(const SimTK::State& s) const override { return getActuation(s)*getSpeed(s); }
     virtual double getStress(const SimTK::State& s) const;
     virtual double getOptimalForce() const;
@@ -250,7 +249,6 @@ class OSIMSIMULATION_API ScalarActuator : public Actuator {
     void constructProperties();
 
     mutable CacheVariable<double> _actuationCV;
-    mutable CacheVariable<double> _speedCV;
 
 //=============================================================================
 };  // END of class ScalarActuator
diff --git a/OpenSim/Simulation/Model/PathActuator.h b/OpenSim/Simulation/Model/PathActuator.h
index b39865bad5..52dd6f9122 100644
--- a/OpenSim/Simulation/Model/PathActuator.h
+++ b/OpenSim/Simulation/Model/PathActuator.h
@@ -106,6 +106,14 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator {
     virtual double getLength(const SimTK::State& s) const;
     virtual double getLengtheningSpeed(const SimTK::State& s) const;
 
+    //--------------------------------------------------------------------------
+    // Implement ScalarActuator Interface
+    //--------------------------------------------------------------------------
+    double getSpeed( const SimTK::State& s) const final
+    {
+        return getLengtheningSpeed(s);
+    }
+
     // Power: Since lengthening is positive and tension always shortens, positive power
     // is when muscle is shortening under tension.
     double getPower(const SimTK::State& s) const override 

From 9b46f274efe81d53a1ab793ee744976141e59ee8 Mon Sep 17 00:00:00 2001
From: pepbos <pepijnvandenbos@gmail.com>
Date: Tue, 21 Nov 2023 14:42:51 +0100
Subject: [PATCH 03/10] adds calcSpeed for isolating compuation in getSpeed For
 implementing ScalarActuator interface

---
 OpenSim/Actuators/PointActuator.cpp        | 12 +++++++++---
 OpenSim/Actuators/PointActuator.h          |  4 ++++
 OpenSim/Actuators/PointToPointActuator.cpp | 20 +++++++++++++-------
 OpenSim/Actuators/PointToPointActuator.h   |  9 ++++-----
 OpenSim/Actuators/TorqueActuator.cpp       | 20 +++++++++++++-------
 OpenSim/Actuators/TorqueActuator.h         |  9 ++++-----
 6 files changed, 47 insertions(+), 27 deletions(-)

diff --git a/OpenSim/Actuators/PointActuator.cpp b/OpenSim/Actuators/PointActuator.cpp
index 753091ea43..ebadc8aa5c 100644
--- a/OpenSim/Actuators/PointActuator.cpp
+++ b/OpenSim/Actuators/PointActuator.cpp
@@ -152,15 +152,21 @@ double PointActuator::getSpeed(const SimTK::State& s) const
         return getCacheVariableValue(s, _speedCV);
     }
 
+    double speed = computeSpeed(s);
+
+    updCacheVariableValue(s, _speedCV) = speed;
+    markCacheVariableValid(s, _speedCV);
+    return speed;
+}
+
+double PointActuator::computeSpeed(const SimTK::State& s) const
+{
     // get the velocity of the actuator in ground
     Vec3 velocity = _body->findStationVelocityInGround(s, get_point());
 
     // the speed of the point is the "speed" of the actuator used to compute
     // power
     double speed = velocity.norm();
-
-    updCacheVariableValue(s, _speedCV) = speed;
-    markCacheVariableValid(s, _speedCV);
     return speed;
 }
 
diff --git a/OpenSim/Actuators/PointActuator.h b/OpenSim/Actuators/PointActuator.h
index d4f1fc50f4..f43ab80eea 100644
--- a/OpenSim/Actuators/PointActuator.h
+++ b/OpenSim/Actuators/PointActuator.h
@@ -104,8 +104,12 @@ class OSIMACTUATORS_API PointActuator : public ScalarActuator {
     //--------------------------------------------------------------------------
     // Implement ScalarActuator interface
     //--------------------------------------------------------------------------
+    /** Get speed along force vector. */
     double getSpeed( const SimTK::State& s) const override;
 
+    /** Computes speed along force vector. */
+    double computeSpeed( const SimTK::State& s) const;
+
     //--------------------------------------------------------------------------
     // Implement Force interface
     //--------------------------------------------------------------------------
diff --git a/OpenSim/Actuators/PointToPointActuator.cpp b/OpenSim/Actuators/PointToPointActuator.cpp
index b53c0bd24d..c4ea1d1ce0 100644
--- a/OpenSim/Actuators/PointToPointActuator.cpp
+++ b/OpenSim/Actuators/PointToPointActuator.cpp
@@ -185,6 +185,19 @@ SimTK::UnitVec3 PointToPointActuator::getDirectionBAInGround(
 }
 
 double PointToPointActuator::getSpeed(const SimTK::State& s) const
+{
+    if (isCacheVariableValid(s, _speedCV)) {
+        return getCacheVariableValue(s, _speedCV);
+    }
+
+    double speed = computeSpeed(s);
+
+    updCacheVariableValue(s, _speedCV) = speed;
+    markCacheVariableValid(s, _speedCV);
+    return speed;
+}
+
+double PointToPointActuator::computeSpeed(const SimTK::State& s) const
 {
     if (!_model || !_bodyA || !_bodyB) {
         return 0.;
@@ -196,10 +209,6 @@ double PointToPointActuator::getSpeed(const SimTK::State& s) const
         return 0.;
     }
 
-    if (isCacheVariableValid(s, _speedCV)) {
-        return getCacheVariableValue(s, _speedCV);
-    }
-
     const SimTK::Vec3& pointA_inBodyA = getPointA();
     const SimTK::Vec3& pointB_inBodyB = getPointB();
 
@@ -212,9 +221,6 @@ double PointToPointActuator::getSpeed(const SimTK::State& s) const
     // Speed used to compute power is the speed along the line connecting
     // the two bodies.
     double speed = ~velAB_G * getDirectionBAInGround(s);
-
-    updCacheVariableValue(s, _speedCV) = speed;
-    markCacheVariableValid(s, _speedCV);
     return speed;
 }
 
diff --git a/OpenSim/Actuators/PointToPointActuator.h b/OpenSim/Actuators/PointToPointActuator.h
index 6e5f8d73e2..911340ce6e 100644
--- a/OpenSim/Actuators/PointToPointActuator.h
+++ b/OpenSim/Actuators/PointToPointActuator.h
@@ -112,11 +112,6 @@ class OSIMACTUATORS_API PointToPointActuator : public ScalarActuator {
 
     // default destructor, copy constructor, copy assignment
 
-    //--------------------------------------------------------------------------
-    // Implement ScalarActuator interface
-    //--------------------------------------------------------------------------
-    double getSpeed( const SimTK::State& s) const override;
-
 protected:
     //--------------------------------------------------------------------------
     // Implement ModelComponent Interface
@@ -146,6 +141,10 @@ class OSIMACTUATORS_API PointToPointActuator : public ScalarActuator {
     double computeActuation( const SimTK::State& s) const override;
     // Return the stress, defined as abs(force/optimal_force).
     double getStress( const SimTK::State& s ) const override;
+    /** Get speed along force vector. */
+    double getSpeed( const SimTK::State& s) const override;
+    /** Computes speed along force vector. */
+    double computeSpeed( const SimTK::State& s) const;
 
     //--------------------------------------------------------------------------
     // Implement ModelComponent interface
diff --git a/OpenSim/Actuators/TorqueActuator.cpp b/OpenSim/Actuators/TorqueActuator.cpp
index 6f8df0a353..dd3e73e618 100644
--- a/OpenSim/Actuators/TorqueActuator.cpp
+++ b/OpenSim/Actuators/TorqueActuator.cpp
@@ -176,14 +176,23 @@ void TorqueActuator::computeForce(
 
 double TorqueActuator::getSpeed(const SimTK::State& s) const
 {
-    if (!_model || !_bodyA) {
-        return 0.;
-    }
-
     if (isCacheVariableValid(s, _speedCV)) {
         return getCacheVariableValue(s, _speedCV);
     }
 
+    double speed = computeSpeed(s);
+
+    updCacheVariableValue(s, _speedCV) = speed;
+    markCacheVariableValid(s, _speedCV);
+    return speed;
+}
+
+double TorqueActuator::computeSpeed(const SimTK::State& s) const
+{
+    if (!_model || !_bodyA) {
+        return 0.;
+    }
+
     const bool torqueIsGlobal = getTorqueIsGlobal();
     const Vec3& axis          = SimTK::UnitVec3(getAxis());
 
@@ -196,9 +205,6 @@ double TorqueActuator::getSpeed(const SimTK::State& s) const
     // the "speed" is the relative angular velocity of the bodies
     // projected onto the torque axis.
     double speed = ~(omegaA - omegaB) * axis;
-
-    updCacheVariableValue(s, _speedCV) = speed;
-    markCacheVariableValid(s, _speedCV);
     return speed;
 }
 
diff --git a/OpenSim/Actuators/TorqueActuator.h b/OpenSim/Actuators/TorqueActuator.h
index 9ecd6ca561..909f6e3ca7 100644
--- a/OpenSim/Actuators/TorqueActuator.h
+++ b/OpenSim/Actuators/TorqueActuator.h
@@ -125,11 +125,6 @@ class OSIMACTUATORS_API TorqueActuator : public ScalarActuator {
     /** Get the second body (bodyB) to which this actuator applies torque. */
     const PhysicalFrame& getBodyB() const {return *_bodyB;}
 
-    //--------------------------------------------------------------------------
-    // Implement ScalarActuator interface
-    //--------------------------------------------------------------------------
-    double getSpeed( const SimTK::State& s) const override;
-
 protected:
     //--------------------------------------------------------------------------
     // Implement ModelComponent Interface
@@ -155,6 +150,10 @@ class OSIMACTUATORS_API TorqueActuator : public ScalarActuator {
     double computeActuation(const SimTK::State& s) const override;
     // Return the stress, defined as abs(force/optimal_force).
     double getStress(const SimTK::State& state) const override; 
+    //* Get speed along force vector. */
+    double getSpeed( const SimTK::State& s) const override;
+    //* Compute speed along force vector. */
+    double computeSpeed( const SimTK::State& s) const;
 
     //--------------------------------------------------------------------------
     // Implement ModelComponent interface

From 0454536d935409794b927b92e555d2d3995acc45 Mon Sep 17 00:00:00 2001
From: pepbos <pepijnvandenbos@gmail.com>
Date: Tue, 21 Nov 2023 15:42:10 +0100
Subject: [PATCH 04/10] updates CHANGELOG

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 094bdfe9c6..b50a8529b3 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -37,6 +37,7 @@ and `Blankevoort1991Ligament`, usages of `get_GeometryPath`, `upd_GeometryPath`,
 - Added `examplePolynomialPathFitter.py`, a scripting example that demonstrates how to use `PolynomialPathFitter` (#3607)
 - Fixed a bug where using `to_numpy()` to convert `RowVectorView`s to Python arrays returned incorrect data (#3613)
 - Bumped the version of `ezc3d` which can now Read Kistler files
+- Moved speed computation from `computeForce` in children of `ScalarActuator` to dedicated `getSpeed` function.
 
 v4.4.1
 ======

From 30d5ae1a8e7729ebb007927e2a4e813017440d7b Mon Sep 17 00:00:00 2001
From: pepbos <pepijnvandenbos@gmail.com>
Date: Thu, 23 Nov 2023 16:06:17 +0100
Subject: [PATCH 05/10] fmt: removes spaces before const, and "compute" ->
 "calc"

---
 OpenSim/Actuators/PointActuator.cpp        |  8 ++++----
 OpenSim/Actuators/PointActuator.h          |  8 ++++----
 OpenSim/Actuators/PointToPointActuator.cpp |  8 ++++----
 OpenSim/Actuators/PointToPointActuator.h   | 10 +++++-----
 OpenSim/Actuators/TorqueActuator.cpp       |  4 ++--
 OpenSim/Actuators/TorqueActuator.h         |  4 ++--
 OpenSim/Simulation/Model/PathActuator.cpp  |  6 +++---
 OpenSim/Simulation/Model/PathActuator.h    | 10 +++++-----
 8 files changed, 29 insertions(+), 29 deletions(-)

diff --git a/OpenSim/Actuators/PointActuator.cpp b/OpenSim/Actuators/PointActuator.cpp
index ebadc8aa5c..1cba567f2a 100644
--- a/OpenSim/Actuators/PointActuator.cpp
+++ b/OpenSim/Actuators/PointActuator.cpp
@@ -135,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()); 
 }
@@ -152,14 +152,14 @@ double PointActuator::getSpeed(const SimTK::State& s) const
         return getCacheVariableValue(s, _speedCV);
     }
 
-    double speed = computeSpeed(s);
+    double speed = calcSpeed(s);
 
     updCacheVariableValue(s, _speedCV) = speed;
     markCacheVariableValid(s, _speedCV);
     return speed;
 }
 
-double PointActuator::computeSpeed(const SimTK::State& s) const
+double PointActuator::calcSpeed(const SimTK::State& s) const
 {
     // get the velocity of the actuator in ground
     Vec3 velocity = _body->findStationVelocityInGround(s, get_point());
@@ -174,7 +174,7 @@ double PointActuator::computeSpeed(const SimTK::State& s) const
  * 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;
 
diff --git a/OpenSim/Actuators/PointActuator.h b/OpenSim/Actuators/PointActuator.h
index f43ab80eea..6bfa46a40d 100644
--- a/OpenSim/Actuators/PointActuator.h
+++ b/OpenSim/Actuators/PointActuator.h
@@ -105,10 +105,10 @@ class OSIMACTUATORS_API PointActuator : public ScalarActuator {
     // Implement ScalarActuator interface
     //--------------------------------------------------------------------------
     /** Get speed along force vector. */
-    double getSpeed( const SimTK::State& s) const override;
+    double getSpeed(const SimTK::State& s) const override;
 
     /** Computes speed along force vector. */
-    double computeSpeed( const SimTK::State& s) const;
+    double calcSpeed(const SimTK::State& s) const;
 
     //--------------------------------------------------------------------------
     // Implement Force interface
@@ -120,8 +120,8 @@ 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
diff --git a/OpenSim/Actuators/PointToPointActuator.cpp b/OpenSim/Actuators/PointToPointActuator.cpp
index c4ea1d1ce0..80749c7f43 100644
--- a/OpenSim/Actuators/PointToPointActuator.cpp
+++ b/OpenSim/Actuators/PointToPointActuator.cpp
@@ -126,7 +126,7 @@ void PointToPointActuator::setBodyB(Body* aBody)
  * this actuator divided by its optimal force.
  * @return Stress.
  */
-double PointToPointActuator::getStress( const SimTK::State& s) const
+double PointToPointActuator::getStress(const SimTK::State& s) const
 {
     return std::abs(getActuation(s) / getOptimalForce()); 
 }
@@ -138,7 +138,7 @@ double PointToPointActuator::getStress( const SimTK::State& s) const
  * @param s current SimTK::State 
  */
 
-double PointToPointActuator::computeActuation( const SimTK::State& s ) const
+double PointToPointActuator::computeActuation(const SimTK::State& s) const
 {
     if(!_model) return 0;
 
@@ -190,14 +190,14 @@ double PointToPointActuator::getSpeed(const SimTK::State& s) const
         return getCacheVariableValue(s, _speedCV);
     }
 
-    double speed = computeSpeed(s);
+    double speed = calcSpeed(s);
 
     updCacheVariableValue(s, _speedCV) = speed;
     markCacheVariableValid(s, _speedCV);
     return speed;
 }
 
-double PointToPointActuator::computeSpeed(const SimTK::State& s) const
+double PointToPointActuator::calcSpeed(const SimTK::State& s) const
 {
     if (!_model || !_bodyA || !_bodyB) {
         return 0.;
diff --git a/OpenSim/Actuators/PointToPointActuator.h b/OpenSim/Actuators/PointToPointActuator.h
index 911340ce6e..6e3667cab7 100644
--- a/OpenSim/Actuators/PointToPointActuator.h
+++ b/OpenSim/Actuators/PointToPointActuator.h
@@ -137,14 +137,14 @@ class OSIMACTUATORS_API PointToPointActuator : public ScalarActuator {
 
     //--------------------------------------------------------------------------
     // Implement Actuator interface (also see getOptimalForce() above)
-    //--------------------------------------------------------------------------    
-    double computeActuation( const SimTK::State& s) const override;
+    //--------------------------------------------------------------------------
+    double computeActuation(const SimTK::State& s) const override;
     // Return the stress, defined as abs(force/optimal_force).
-    double getStress( const SimTK::State& s ) const override;
+    double getStress(const SimTK::State& s) const override;
     /** Get speed along force vector. */
-    double getSpeed( const SimTK::State& s) const override;
+    double getSpeed(const SimTK::State& s) const override;
     /** Computes speed along force vector. */
-    double computeSpeed( const SimTK::State& s) const;
+    double calcSpeed(const SimTK::State& s) const;
 
     //--------------------------------------------------------------------------
     // Implement ModelComponent interface
diff --git a/OpenSim/Actuators/TorqueActuator.cpp b/OpenSim/Actuators/TorqueActuator.cpp
index dd3e73e618..6dce6d896e 100644
--- a/OpenSim/Actuators/TorqueActuator.cpp
+++ b/OpenSim/Actuators/TorqueActuator.cpp
@@ -180,14 +180,14 @@ double TorqueActuator::getSpeed(const SimTK::State& s) const
         return getCacheVariableValue(s, _speedCV);
     }
 
-    double speed = computeSpeed(s);
+    double speed = calcSpeed(s);
 
     updCacheVariableValue(s, _speedCV) = speed;
     markCacheVariableValid(s, _speedCV);
     return speed;
 }
 
-double TorqueActuator::computeSpeed(const SimTK::State& s) const
+double TorqueActuator::calcSpeed(const SimTK::State& s) const
 {
     if (!_model || !_bodyA) {
         return 0.;
diff --git a/OpenSim/Actuators/TorqueActuator.h b/OpenSim/Actuators/TorqueActuator.h
index 909f6e3ca7..5bfcbd69ec 100644
--- a/OpenSim/Actuators/TorqueActuator.h
+++ b/OpenSim/Actuators/TorqueActuator.h
@@ -151,9 +151,9 @@ class OSIMACTUATORS_API TorqueActuator : public ScalarActuator {
     // Return the stress, defined as abs(force/optimal_force).
     double getStress(const SimTK::State& state) const override; 
     //* Get speed along force vector. */
-    double getSpeed( const SimTK::State& s) const override;
+    double getSpeed(const SimTK::State& s) const override;
     //* Compute speed along force vector. */
-    double computeSpeed( const SimTK::State& s) const;
+    double calcSpeed(const SimTK::State& s) const;
 
     //--------------------------------------------------------------------------
     // Implement ModelComponent interface
diff --git a/OpenSim/Simulation/Model/PathActuator.cpp b/OpenSim/Simulation/Model/PathActuator.cpp
index 59c6b197c1..612c895eef 100644
--- a/OpenSim/Simulation/Model/PathActuator.cpp
+++ b/OpenSim/Simulation/Model/PathActuator.cpp
@@ -125,7 +125,7 @@ double PathActuator::getLengtheningSpeed(const SimTK::State& s) const
 * this actuator divided by its optimal force.
 * @return Stress.
 */
-double PathActuator::getStress( const SimTK::State& s) const
+double PathActuator::getStress(const SimTK::State& s) const
 {
     return fabs(getActuation(s)/get_optimal_force()); 
 }
@@ -153,7 +153,7 @@ void PathActuator::addNewPathPoint(
  * Compute all quantities necessary for applying the actuator force to the
  * model.
  */
-double PathActuator::computeActuation( const SimTK::State& s ) const
+double PathActuator::computeActuation(const SimTK::State& s ) const
 {
     // FORCE
     return( getControl(s) * get_optimal_force() );
@@ -167,7 +167,7 @@ double PathActuator::computeActuation( const SimTK::State& s ) const
 /**
  * Apply the actuator force along path wrapping over and connecting rigid bodies
  */
-void PathActuator::computeForce( const SimTK::State& s, 
+void PathActuator::computeForce(const SimTK::State& s,
                                SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
                                SimTK::Vector& mobilityForces) const
 {
diff --git a/OpenSim/Simulation/Model/PathActuator.h b/OpenSim/Simulation/Model/PathActuator.h
index 52dd6f9122..a0b79e9b9e 100644
--- a/OpenSim/Simulation/Model/PathActuator.h
+++ b/OpenSim/Simulation/Model/PathActuator.h
@@ -109,7 +109,7 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator {
     //--------------------------------------------------------------------------
     // Implement ScalarActuator Interface
     //--------------------------------------------------------------------------
-    double getSpeed( const SimTK::State& s) const final
+    double getSpeed(const SimTK::State& s) const final
     {
         return getLengtheningSpeed(s);
     }
@@ -121,7 +121,7 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator {
 
 
     // STRESS
-    double getStress( const SimTK::State& s ) const override;
+    double getStress(const SimTK::State& s ) const override;
 
     // Convenience method to add PathPoints
     /** @note This function does not maintain the State and so should be used
@@ -135,15 +135,15 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator {
     //--------------------------------------------------------------------------
     // APPLICATION
     //--------------------------------------------------------------------------
-    virtual void computeForce( const SimTK::State& state, 
+    virtual void computeForce(const SimTK::State& state,
                                SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
                                SimTK::Vector& mobilityForces) const override;
 
     //--------------------------------------------------------------------------
     // COMPUTATIONS
     //--------------------------------------------------------------------------
-    double computeActuation( const SimTK::State& s) const override;
-    virtual double computeMomentArm( const SimTK::State& s, Coordinate& aCoord) const;
+    double computeActuation(const SimTK::State& s) const override;
+    virtual double computeMomentArm(const SimTK::State& s, Coordinate& aCoord) const;
 
 protected:
     /** Override this method if you would like to calculate a color for use when

From a8bd4396a88a39608734df4e4f71fbdf75dc590f Mon Sep 17 00:00:00 2001
From: pepbos <pepijnvandenbos@gmail.com>
Date: Fri, 24 Nov 2023 13:57:16 +0100
Subject: [PATCH 06/10] remove doxygen-like documentation from cpp file

---
 OpenSim/Actuators/PointToPointActuator.cpp | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/OpenSim/Actuators/PointToPointActuator.cpp b/OpenSim/Actuators/PointToPointActuator.cpp
index 80749c7f43..4160631b40 100644
--- a/OpenSim/Actuators/PointToPointActuator.cpp
+++ b/OpenSim/Actuators/PointToPointActuator.cpp
@@ -224,11 +224,6 @@ double PointToPointActuator::calcSpeed(const SimTK::State& s) const
     return speed;
 }
 
-/**
- * Apply the actuator force to BodyA and BodyB.
- *
- * @param s current SimTK::State
- */
 void PointToPointActuator::computeForce(
     const SimTK::State& s,
     SimTK::Vector_<SimTK::SpatialVec>& bodyForces,

From 6bcfed24f64d25d000566f88f151a6fd8a28e35d Mon Sep 17 00:00:00 2001
From: pepbos <pepijnvandenbos@gmail.com>
Date: Fri, 24 Nov 2023 13:58:35 +0100
Subject: [PATCH 07/10] simplify: rewrite for less code

---
 OpenSim/Actuators/PointActuator.cpp                       | 6 +-----
 OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp | 3 +--
 2 files changed, 2 insertions(+), 7 deletions(-)

diff --git a/OpenSim/Actuators/PointActuator.cpp b/OpenSim/Actuators/PointActuator.cpp
index 1cba567f2a..ca10f1dfdb 100644
--- a/OpenSim/Actuators/PointActuator.cpp
+++ b/OpenSim/Actuators/PointActuator.cpp
@@ -163,11 +163,7 @@ double PointActuator::calcSpeed(const SimTK::State& s) const
 {
     // get the velocity of the actuator in ground
     Vec3 velocity = _body->findStationVelocityInGround(s, get_point());
-
-    // the speed of the point is the "speed" of the actuator used to compute
-    // power
-    double speed = velocity.norm();
-    return speed;
+    return velocity.norm();
 }
 
 /**
diff --git a/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp b/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp
index 2ba2fd4f50..566d9406ac 100644
--- a/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp
+++ b/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp
@@ -163,8 +163,7 @@ SimTK::UnitVec3 PistonActuator::calcDirectionBAInGround(
 double PistonActuator::getSpeed(const SimTK::State& s) const
 {
     // Speed is zero for constant points defined in the same frame.
-    const bool pointsAreGlobal = get_points_are_global();
-    if (pointsAreGlobal) {
+    if (get_points_are_global()) {
         return 0.;
     }
 

From d73039f79f08a8c96019317e86c043bee0a5d0cc Mon Sep 17 00:00:00 2001
From: pepbos <pepijnvandenbos@gmail.com>
Date: Fri, 24 Nov 2023 13:58:57 +0100
Subject: [PATCH 08/10] take ground by reference, not value

---
 OpenSim/Actuators/PointToPointActuator.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/OpenSim/Actuators/PointToPointActuator.cpp b/OpenSim/Actuators/PointToPointActuator.cpp
index 4160631b40..bcb8a27a2b 100644
--- a/OpenSim/Actuators/PointToPointActuator.cpp
+++ b/OpenSim/Actuators/PointToPointActuator.cpp
@@ -238,7 +238,7 @@ void PointToPointActuator::computeForce(
     const bool pointsAreGlobal = getPointsAreGlobal();
     const SimTK::Vec3& pointA  = getPointA();
     const SimTK::Vec3& pointB  = getPointB();
-    Ground ground              = getModel().getGround();
+    const Ground& ground       = getModel().getGround();
 
     SimTK::Vec3 pointA_inBodyA =
         pointsAreGlobal

From 59d5b7631494a3418636d41faead46be3c0879b2 Mon Sep 17 00:00:00 2001
From: pepbos <pepijnvandenbos@gmail.com>
Date: Fri, 24 Nov 2023 13:59:19 +0100
Subject: [PATCH 09/10] fmt: remove spaces

---
 OpenSim/Simulation/Model/PathActuator.cpp | 10 +++++-----
 OpenSim/Simulation/Model/PathActuator.h   |  6 +++---
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/OpenSim/Simulation/Model/PathActuator.cpp b/OpenSim/Simulation/Model/PathActuator.cpp
index 612c895eef..2689458d0d 100644
--- a/OpenSim/Simulation/Model/PathActuator.cpp
+++ b/OpenSim/Simulation/Model/PathActuator.cpp
@@ -127,7 +127,7 @@ double PathActuator::getLengtheningSpeed(const SimTK::State& s) const
 */
 double PathActuator::getStress(const SimTK::State& s) const
 {
-    return fabs(getActuation(s)/get_optimal_force()); 
+    return fabs(getActuation(s)/get_optimal_force());
 }
 
 
@@ -138,8 +138,8 @@ double PathActuator::getStress(const SimTK::State& s) const
  *
  */
 void PathActuator::addNewPathPoint(
-         const std::string& proposedName, 
-         const PhysicalFrame& aBody, 
+         const std::string& proposedName,
+         const PhysicalFrame& aBody,
          const SimTK::Vec3& aPositionOnBody) {
     // Create new PathPoint already appended to the PathPointSet for the path
     updGeometryPath().appendNewPathPoint(proposedName, aBody, aPositionOnBody);
@@ -153,7 +153,7 @@ void PathActuator::addNewPathPoint(
  * Compute all quantities necessary for applying the actuator force to the
  * model.
  */
-double PathActuator::computeActuation(const SimTK::State& s ) const
+double PathActuator::computeActuation(const SimTK::State& s) const
 {
     // FORCE
     return( getControl(s) * get_optimal_force() );
@@ -168,7 +168,7 @@ double PathActuator::computeActuation(const SimTK::State& s ) const
  * Apply the actuator force along path wrapping over and connecting rigid bodies
  */
 void PathActuator::computeForce(const SimTK::State& s,
-                               SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
+                               SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
                                SimTK::Vector& mobilityForces) const
 {
     if(!_model) return;
diff --git a/OpenSim/Simulation/Model/PathActuator.h b/OpenSim/Simulation/Model/PathActuator.h
index a0b79e9b9e..3e57e28df9 100644
--- a/OpenSim/Simulation/Model/PathActuator.h
+++ b/OpenSim/Simulation/Model/PathActuator.h
@@ -116,12 +116,12 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator {
 
     // Power: Since lengthening is positive and tension always shortens, positive power
     // is when muscle is shortening under tension.
-    double getPower(const SimTK::State& s) const override 
+    double getPower(const SimTK::State& s) const override
     {   return -getActuation(s)*getSpeed(s); }
 
 
     // STRESS
-    double getStress(const SimTK::State& s ) const override;
+    double getStress(const SimTK::State& s) const override;
 
     // Convenience method to add PathPoints
     /** @note This function does not maintain the State and so should be used
@@ -136,7 +136,7 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator {
     // APPLICATION
     //--------------------------------------------------------------------------
     virtual void computeForce(const SimTK::State& state,
-                               SimTK::Vector_<SimTK::SpatialVec>& bodyForces, 
+                               SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
                                SimTK::Vector& mobilityForces) const override;
 
     //--------------------------------------------------------------------------

From a5947aab2d2a614d90b89becb2e1c0fee182b241 Mon Sep 17 00:00:00 2001
From: pepbos <pepijnvandenbos@gmail.com>
Date: Fri, 24 Nov 2023 14:59:40 +0100
Subject: [PATCH 10/10] returns speed=NaN if no model/bodies present

---
 OpenSim/Actuators/PointActuator.cpp        | 6 +++---
 OpenSim/Actuators/PointToPointActuator.cpp | 6 ++++--
 OpenSim/Actuators/TorqueActuator.cpp       | 6 ++++--
 3 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/OpenSim/Actuators/PointActuator.cpp b/OpenSim/Actuators/PointActuator.cpp
index ca10f1dfdb..87e6270480 100644
--- a/OpenSim/Actuators/PointActuator.cpp
+++ b/OpenSim/Actuators/PointActuator.cpp
@@ -172,14 +172,14 @@ double PointActuator::calcSpeed(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
 //=============================================================================
diff --git a/OpenSim/Actuators/PointToPointActuator.cpp b/OpenSim/Actuators/PointToPointActuator.cpp
index bcb8a27a2b..b951e542c3 100644
--- a/OpenSim/Actuators/PointToPointActuator.cpp
+++ b/OpenSim/Actuators/PointToPointActuator.cpp
@@ -140,7 +140,9 @@ double PointToPointActuator::getStress(const SimTK::State& s) const
 
 double PointToPointActuator::computeActuation(const SimTK::State& s) const
 {
-    if(!_model) return 0;
+    if (!_model) {
+        return SimTK::NaN;
+    }
 
     // FORCE
     return getControl(s) * getOptimalForce();
@@ -200,7 +202,7 @@ double PointToPointActuator::getSpeed(const SimTK::State& s) const
 double PointToPointActuator::calcSpeed(const SimTK::State& s) const
 {
     if (!_model || !_bodyA || !_bodyB) {
-        return 0.;
+        return SimTK::NaN;
     }
 
     // Speed is zero for constant points defined in the same frame.
diff --git a/OpenSim/Actuators/TorqueActuator.cpp b/OpenSim/Actuators/TorqueActuator.cpp
index 6dce6d896e..b7ffef3c16 100644
--- a/OpenSim/Actuators/TorqueActuator.cpp
+++ b/OpenSim/Actuators/TorqueActuator.cpp
@@ -129,7 +129,9 @@ double TorqueActuator::getStress(const State& s) const
  */
 double TorqueActuator::computeActuation(const State& s) const
 {
-    if(!_model) return 0;
+    if (!_model) {
+        return SimTK::NaN;
+    }
 
     // FORCE
     return getControl(s) * getOptimalForce();
@@ -190,7 +192,7 @@ double TorqueActuator::getSpeed(const SimTK::State& s) const
 double TorqueActuator::calcSpeed(const SimTK::State& s) const
 {
     if (!_model || !_bodyA) {
-        return 0.;
+        return SimTK::NaN;
     }
 
     const bool torqueIsGlobal = getTorqueIsGlobal();