From f30bfe0694b325c695a12e815e6fe52a59dee39f Mon Sep 17 00:00:00 2001 From: Adam Kewley Date: Thu, 29 Aug 2024 14:36:54 +0200 Subject: [PATCH] Rollout ForceProducer/ForceConsumer to almost all remaining Components --- OpenSim/Actuators/BodyActuator.cpp | 81 ++++++++----------- OpenSim/Actuators/BodyActuator.h | 10 +-- OpenSim/Actuators/CoordinateActuator.cpp | 35 ++++---- OpenSim/Actuators/CoordinateActuator.h | 10 +-- OpenSim/Actuators/McKibbenActuator.cpp | 18 ----- OpenSim/Actuators/McKibbenActuator.h | 8 -- OpenSim/Actuators/PointActuator.cpp | 29 +++---- OpenSim/Actuators/PointActuator.h | 11 ++- OpenSim/Actuators/PointToPointActuator.cpp | 17 ++-- OpenSim/Actuators/PointToPointActuator.h | 10 +-- OpenSim/Actuators/TorqueActuator.cpp | 13 +-- OpenSim/Actuators/TorqueActuator.h | 10 +-- .../ControllableSpring.h | 34 ++++---- .../CustomActuatorExample/PistonActuator.cpp | 16 ++-- .../CustomActuatorExample/PistonActuator.h | 17 ++-- OpenSim/Moco/MocoStudyFactory.cpp | 30 ++++--- OpenSim/Simulation/Model/Actuator.h | 6 +- OpenSim/Simulation/Model/PathActuator.cpp | 46 ++++------- OpenSim/Simulation/Model/PathActuator.h | 13 ++- OpenSim/Simulation/osimSimulation.h | 5 ++ 20 files changed, 177 insertions(+), 242 deletions(-) diff --git a/OpenSim/Actuators/BodyActuator.cpp b/OpenSim/Actuators/BodyActuator.cpp index 54c820979e..17f87cee26 100644 --- a/OpenSim/Actuators/BodyActuator.cpp +++ b/OpenSim/Actuators/BodyActuator.cpp @@ -21,13 +21,11 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -//============================================================================= -// INCLUDES -//============================================================================= -#include - #include "BodyActuator.h" +#include +#include + using namespace OpenSim; using namespace std; using SimTK::Vec3; @@ -104,21 +102,41 @@ const Body& BodyActuator::getBody() const return getConnectee("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_& 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 index dd04ac265e..a69b377be3 100644 --- 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_& 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 index 17c473dfde..6464b49f9b 100644 --- a/OpenSim/Actuators/CoordinateActuator.cpp +++ b/OpenSim/Actuators/CoordinateActuator.cpp @@ -22,16 +22,14 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -//============================================================================== -// INCLUDES -//============================================================================== +#include "CoordinateActuator.h" + #include #include #include +#include #include -#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_& 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 index e0d9b99e5d..59c6ec6a62 100644 --- 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_& 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 index 06719f4c62..3f1a192545 100644 --- 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_& 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 index a4c0ac164c..caa5a15afd 100644 --- 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_& bodyForces, - SimTK::Vector& mobilityForces) const override; - - //-------------------------------------------------------------------------- // Implement ModelComponent interface //-------------------------------------------------------------------------- diff --git a/OpenSim/Actuators/PointActuator.cpp b/OpenSim/Actuators/PointActuator.cpp index 87e6270480..527db2b6b5 100644 --- a/OpenSim/Actuators/PointActuator.cpp +++ b/OpenSim/Actuators/PointActuator.cpp @@ -25,16 +25,13 @@ * Author: Ajay Seth */ +#include "PointActuator.h" -//============================================================================= -// INCLUDES -//============================================================================= #include +#include #include #include -#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_& 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 index 6bfa46a40d..4faa4492be 100644 --- 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_& 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 index b951e542c3..79b6ac60ed 100644 --- a/OpenSim/Actuators/PointToPointActuator.cpp +++ b/OpenSim/Actuators/PointToPointActuator.cpp @@ -25,14 +25,12 @@ * Author: Matt DeMers */ -//============================================================================== -// INCLUDES -//============================================================================== +#include "PointToPointActuator.h" + +#include #include #include -#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_& 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 index 6e3667cab7..9ce28f78c5 100644 --- 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_& 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) diff --git a/OpenSim/Actuators/TorqueActuator.cpp b/OpenSim/Actuators/TorqueActuator.cpp index b7ffef3c16..e2af03d41a 100644 --- a/OpenSim/Actuators/TorqueActuator.cpp +++ b/OpenSim/Actuators/TorqueActuator.cpp @@ -29,6 +29,8 @@ // INCLUDES //============================================================================== #include "TorqueActuator.h" + +#include #include using namespace OpenSim; @@ -146,10 +148,9 @@ double TorqueActuator::computeActuation(const State& s) const /** * Apply the actuator force to BodyA and BodyB. */ -void TorqueActuator::computeForce( - const State& s, - Vector_& bodyForces, - Vector& generalizedForces) const +void TorqueActuator::implProduceForces( + const SimTK::State& s, + ForceConsumer& forceConsumer) const { if (!_model || !_bodyA) { return; @@ -168,11 +169,11 @@ void TorqueActuator::computeForce( torque = _bodyA->expressVectorInGround(s, torque); } - applyTorque(s, *_bodyA, torque, bodyForces); + forceConsumer.consumeTorque(s, *_bodyA, torque); // if bodyB is not specified, use the ground body by default if (_bodyB) { - applyTorque(s, *_bodyB, -torque, bodyForces); + forceConsumer.consumeTorque(s, *_bodyB, -torque); } } diff --git a/OpenSim/Actuators/TorqueActuator.h b/OpenSim/Actuators/TorqueActuator.h index 5bfcbd69ec..db067dc52f 100644 --- a/OpenSim/Actuators/TorqueActuator.h +++ b/OpenSim/Actuators/TorqueActuator.h @@ -137,12 +137,10 @@ class OSIMACTUATORS_API TorqueActuator : public ScalarActuator { private: void constructProperties(); - //-------------------------------------------------------------------------- - // Implement Force interface - //-------------------------------------------------------------------------- - void computeForce(const SimTK::State& state, - SimTK::Vector_& 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) diff --git a/OpenSim/Examples/CustomActuatorExample/ControllableSpring.h b/OpenSim/Examples/CustomActuatorExample/ControllableSpring.h index cf6ef65f6e..7b3f391336 100644 --- a/OpenSim/Examples/CustomActuatorExample/ControllableSpring.h +++ b/OpenSim/Examples/CustomActuatorExample/ControllableSpring.h @@ -25,6 +25,8 @@ #include "PistonActuator.h" +#include + //============================================================================= //============================================================================= /** @@ -79,18 +81,20 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ControllableSpring, PistonActuator); void setRestLength(double aLength) { set_rest_length(aLength); }; double getRestLength() const { return get_rest_length(); }; - //-------------------------------------------------------------------------- - // COMPUTATIONS - //-------------------------------------------------------------------------- - - /* The computeForce method is the meat of this simple actuator example. It - * computes the direction and distance between the two application points. - * It then uses the difference between its current length and rest length - * to determine the force magnitude, then applies the force at the - * application points, in the direction between them. */ - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override +private: + /** + * Implements the `ForceProducer` API, which is the "meat" of this example. + * + * The implementation: + * + * - Computes the direction and distance between the two application points. + * - Then uses the difference between its current length and rest length to + * determine the force magnitude + * - Then emits (produces into the `ForceConsumer`) the force as a point + * force at the application points that points in the direction between + * the two points. + */ + void implProduceForces(const SimTK::State& s, ForceConsumer& forceConsumer) const override { const PhysicalFrame& frameA = getFrameA(); const PhysicalFrame& frameB = getFrameB(); @@ -131,9 +135,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ControllableSpring, PistonActuator); setActuation(s, forceMagnitude); SimTK::Vec3 force = forceMagnitude*direction; - // Apply equal and opposite forces to the bodies. - applyForceToPoint(s, frameA, pointA, force, bodyForces); - applyForceToPoint(s, frameB, pointB, -force, bodyForces); + // Produce equal and opposite point forces + forceConsumer.consumePointForce(s, frameA, pointA, force); + forceConsumer.consumePointForce(s, frameB, pointB, -force); } //============================================================================= diff --git a/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp b/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp index 566d9406ac..2bef3ed9e1 100644 --- a/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp +++ b/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp @@ -21,10 +21,8 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -//============================================================================= -// INCLUDES -//============================================================================= #include "PistonActuator.h" + #include using namespace OpenSim; @@ -103,13 +101,9 @@ double PistonActuator::getStress( const SimTK::State& s) const } -//============================================================================= -// FORCE INTERFACE -//============================================================================= -void PistonActuator::computeForce( +void PistonActuator::implProduceForces( const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const + ForceConsumer& forceConsumer) const { const PhysicalFrame& frameA = getFrameA(); const PhysicalFrame& frameB = getFrameB(); @@ -129,8 +123,8 @@ void PistonActuator::computeForce( 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); + forceConsumer.consumePointForce(s, frameA, pointA, force); + forceConsumer.consumePointForce(s, frameB, pointB, -force); } SimTK::UnitVec3 PistonActuator::calcDirectionBAInGround( diff --git a/OpenSim/Examples/CustomActuatorExample/PistonActuator.h b/OpenSim/Examples/CustomActuatorExample/PistonActuator.h index 1f8401ce24..b090900d83 100644 --- a/OpenSim/Examples/CustomActuatorExample/PistonActuator.h +++ b/OpenSim/Examples/CustomActuatorExample/PistonActuator.h @@ -119,13 +119,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PistonActuator, ScalarActuator); /** This is the absolute value of the force generated by this actuator * divided by its optimal force. */ double getStress(const SimTK::State& s) const override; - //-------------------------------------------------------------------------- - // FORCE INTERFACE - //-------------------------------------------------------------------------- - /** Apply the actuator force to frameA and frameB. */ - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; //-------------------------------------------------------------------------- // ACTUATOR INTERFACE @@ -141,8 +134,14 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PistonActuator, ScalarActuator); 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; + /** + * Implements the `ForceProducer` interface by emitting point forces on + * frameA and frameB. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + + /** 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 fc26f48c0d..198b11818c 100644 --- a/OpenSim/Moco/MocoStudyFactory.cpp +++ b/OpenSim/Moco/MocoStudyFactory.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace OpenSim; @@ -38,22 +39,29 @@ MocoStudy MocoStudyFactory::createLinearTangentSteeringStudy( double computeActuation(const SimTK::State&) const override { return SimTK::NaN; } - void computeForce(const SimTK::State& state, - SimTK::Vector_&, - SimTK::Vector& mobilityForces) const override { - const double angle = getControl(state); - const auto& coordX = getModel().getCoordinateSet().get(0); - const auto& coordY = getModel().getCoordinateSet().get(1); - applyGeneralizedForce(state, coordX, - get_acceleration() * cos(angle), mobilityForces); - applyGeneralizedForce(state, coordY, - get_acceleration() * sin(angle), mobilityForces); - } double getSpeed(const SimTK::State& state) const override { return SimTK::NaN; } + private: + void implProduceForces(const SimTK::State& state, + ForceConsumer& forceConsumer) const override { + const double angle = getControl(state); + const auto& coordX = getModel().getCoordinateSet().get(0); + const auto& coordY = getModel().getCoordinateSet().get(1); + + forceConsumer.consumeGeneralizedForce( + state, + coordX, + get_acceleration() * cos(angle) + ); + forceConsumer.consumeGeneralizedForce( + state, + coordY, + get_acceleration() * sin(angle) + ); + } }; class LinearTangentFinalSpeed : public MocoGoal { diff --git a/OpenSim/Simulation/Model/Actuator.h b/OpenSim/Simulation/Model/Actuator.h index a1c0be7fc7..b14baf285d 100644 --- a/OpenSim/Simulation/Model/Actuator.h +++ b/OpenSim/Simulation/Model/Actuator.h @@ -24,7 +24,7 @@ * -------------------------------------------------------------------------- */ #include -#include "Force.h" +#include #ifdef SWIG @@ -46,8 +46,8 @@ namespace OpenSim { * * @author Ajay Seth */ -class OSIMSIMULATION_API Actuator : public Force { -OpenSim_DECLARE_ABSTRACT_OBJECT(Actuator, Force); +class OSIMSIMULATION_API Actuator : public ForceProducer { +OpenSim_DECLARE_ABSTRACT_OBJECT(Actuator, ForceProducer); //============================================================================= // NO PROPERTIES //============================================================================= diff --git a/OpenSim/Simulation/Model/PathActuator.cpp b/OpenSim/Simulation/Model/PathActuator.cpp index 2689458d0d..ae3bcaa84d 100644 --- a/OpenSim/Simulation/Model/PathActuator.cpp +++ b/OpenSim/Simulation/Model/PathActuator.cpp @@ -159,35 +159,6 @@ double PathActuator::computeActuation(const SimTK::State& s) const return( getControl(s) * get_optimal_force() ); } - -//============================================================================= -// APPLICATION -//============================================================================= -//_____________________________________________________________________________ -/** - * Apply the actuator force along path wrapping over and connecting rigid bodies - */ -void PathActuator::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const -{ - if(!_model) return; - - const auto &path = getPath(); - - double force =0; - if( isActuationOverridden(s) ) { - force = computeOverrideActuation(s); - } else { - force = computeActuation(s); - } - - // the force of this actuator used to compute power - setActuation(s, force); - - path.addInEquivalentForces(s, force, bodyForces, mobilityForces); -} - /** * Compute the moment-arm of this muscle about a coordinate. */ @@ -213,6 +184,23 @@ void PathActuator::extendRealizeDynamics(const SimTK::State& state) const } } +void PathActuator::implProduceForces(const SimTK::State& s, + ForceConsumer& forceConsumer) const +{ + if (!_model) { + return; + } + + const double force = isActuationOverridden(s) ? + computeOverrideActuation(s) : + computeActuation(s); + + // the force of this actuator used to compute power + setActuation(s, force); + + getPath().produceForces(s, force, forceConsumer); +} + //------------------------------------------------------------------------------ // COMPUTE PATH COLOR //------------------------------------------------------------------------------ diff --git a/OpenSim/Simulation/Model/PathActuator.h b/OpenSim/Simulation/Model/PathActuator.h index 3e57e28df9..67fd93958c 100644 --- a/OpenSim/Simulation/Model/PathActuator.h +++ b/OpenSim/Simulation/Model/PathActuator.h @@ -132,13 +132,6 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator { const PhysicalFrame& aBody, const SimTK::Vec3& aPositionOnBody); - //-------------------------------------------------------------------------- - // APPLICATION - //-------------------------------------------------------------------------- - virtual void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; - //-------------------------------------------------------------------------- // COMPUTATIONS //-------------------------------------------------------------------------- @@ -169,6 +162,12 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator { void extendRealizeDynamics(const SimTK::State& state) const override; private: + /** + * Implements the `ForceProducer` API by applying the actuator force along + * the (potentially, wrapping) path connecting rigid bodies. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + void setNull(); void constructProperties(); diff --git a/OpenSim/Simulation/osimSimulation.h b/OpenSim/Simulation/osimSimulation.h index 5672961ccd..8c7e87534b 100644 --- a/OpenSim/Simulation/osimSimulation.h +++ b/OpenSim/Simulation/osimSimulation.h @@ -28,6 +28,11 @@ #include "Model/Bhargava2004SmoothedMuscleMetabolics.h" #include "Model/Model.h" #include "Model/ModelVisualizer.h" +#include "Model/Force.h" +#include "Model/ForceAdapter.h" +#include "Model/ForceApplier.h" +#include "Model/ForceConsumer.h" +#include "Model/ForceProducer.h" #include "Model/ForceSet.h" #include "Model/BodyScale.h" #include "Model/BodyScaleSet.h"