opensim-org · nickbianco · Apr 7, 2025 · Apr 5, 2025 · Apr 5, 2025 · Apr 7, 2025
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -89,6 +89,9 @@ v4.6
 - Fixed a bug where `DeGrooteFregly2016Muscle::getBoundsNormalizedFiberLength()` was returning
   tendon force bounds rather than fiber length bounds. (#4040)
 - Fixed bugs in `PolynomialPathFitter` when too few coordinate samples were provided. (#4039)
+- Exposed the "dissipated energy" state variable allocated by the `SimTK::Force::LinearBushing` that is internal to `BushingForce`. 
+  This change fixed a bug in Moco where adding a `BushingForce` led to a segfault due to a mismatch between the size of the 
+  auxiliary state vector reserved by Moco and `SimTK::State::getZ()`. (#4054)
 
 
 v4.5.1

diff --git a/OpenSim/Moco/Test/testMocoInterface.cpp b/OpenSim/Moco/Test/testMocoInterface.cpp
@@ -2198,3 +2198,39 @@ TEST_CASE("Locked coordinates") {
     CHECK_THROWS_WITH(problem.createRep(),
             ContainsSubstring("Coordinate '/slider/position' is locked"));
 }
+
+TEST_CASE("BushingForce does not cause segfault") {
+
+    // Previously, a segfault occurred in Moco when a BushingForce was added to 
+    // a model. This was because the "dissipated energy" state variable
+    // allocated by the internal SimTK::Force::LinearBushing was not exposed by
+    // BushingForce, leading to a mismatch in the size of the auxiliary state
+    // vector between OpenSim and CasADi. This test ensures that this segfault 
+    // no longer occurs, since BushingForce now exposes the dissipated energy
+    // state variable.
+
+    // Create a model with a BushingForce.
+    std::unique_ptr<Model> model = createSlidingMassModel();
+    const auto& body = model->getComponent<Body>("/body");
+    auto* force = new BushingForce("bushing", model->getGround(), body);
+    model->addComponent(force);
+    model->finalizeConnections();
+
+    // Create a "sliding mass" MocoStudy.
+    MocoStudy study;
+    study.setName("sliding_mass");
+    study.set_write_solution("false");
+    MocoProblem& mp = study.updProblem();
+    mp.setModel(std::move(model));
+    mp.setTimeBounds(MocoInitialBounds(0), MocoFinalBounds(0, 10));
+    mp.setStateInfo("/slider/position/value", MocoBounds(0, 1),
+            MocoInitialBounds(0), MocoFinalBounds(1));
+    mp.setStateInfo("/slider/position/speed", {-100, 100}, 0, 0);
+    mp.addGoal<MocoFinalTimeGoal>();
+    auto& ms = study.initSolver<MocoCasADiSolver>();
+    ms.set_num_mesh_intervals(25);
+
+    // Confirm that no segfault occurs (i.e., the energy dissipation state 
+    // variable is acccounted for by Moco).
+    REQUIRE_NOTHROW(study.solve());
+}
diff --git a/OpenSim/Simulation/Model/BushingForce.cpp b/OpenSim/Simulation/Model/BushingForce.cpp
@@ -25,7 +25,6 @@
 // INCLUDES
 //=============================================================================
 #include <OpenSim/Simulation/Model/Model.h>
-#include "simbody/internal/Force_LinearBushing.h"
 
 #include "BushingForce.h"
 
@@ -203,14 +202,18 @@ void BushingForce::
     // SimTK::Force later.
     BushingForce* mutableThis = const_cast<BushingForce *>(this);
     mutableThis->_index = simtkForce.getForceIndex();
+
+    // Add a StateVariable to the system to expose the energy dissipation state
+    // variable allocated internally by Simbody.
+    StateVariable* sv = new DissipatedEnergyStateVariable("dissipated_energy", 
+        *this, _model->getForceSubsystem().getMySubsystemIndex(),
+        mutableThis->_index);
+    addStateVariable(sv);
 }
 
 //=============================================================================
-// SET
+// GET AND SET
 //=============================================================================
-//_____________________________________________________________________________
-// The following methods set properties of the bushing Force.
-
 
 /* Potential energy is computed by underlying SimTK::Force. */
 double BushingForce::computePotentialEnergy(const SimTK::State& s) const
@@ -219,6 +222,23 @@ double BushingForce::computePotentialEnergy(const SimTK::State& s) const
                                       .calcPotentialEnergyContribution(s);
 }
 
+const SimTK::Force::LinearBushing& BushingForce::getSimTKLinearBushing() const {
+    return static_cast<const SimTK::Force::LinearBushing&>
+            (_model->getForceSubsystem().getForce(_index));
+}
+
+double BushingForce::getDissipatedEnergy(const SimTK::State& state) const {
+    return getSimTKLinearBushing().getDissipatedEnergy(state);
+}
+
+void BushingForce::setDissipatedEnergy(SimTK::State& state, double value) const {
+    getSimTKLinearBushing().setDissipatedEnergy(state, value);
+}
+
+double BushingForce::getPowerDissipation(const SimTK::State& state) const {
+    return getSimTKLinearBushing().getPowerDissipation(state);
+}
+
 //=============================================================================
 // Reporting
 //=============================================================================
@@ -261,8 +281,7 @@ getRecordValues(const SimTK::State& state) const
 
     OpenSim::Array<double> values(1);
 
-    const SimTK::Force::LinearBushing &simtkSpring = 
-        (SimTK::Force::LinearBushing &)(_model->getForceSubsystem().getForce(_index));
+    const SimTK::Force::LinearBushing& simtkSpring = getSimTKLinearBushing();
 
     SimTK::Vector_<SimTK::SpatialVec> bodyForces(0);
     SimTK::Vector_<SimTK::Vec3> particleForces(0);
@@ -283,3 +302,34 @@ getRecordValues(const SimTK::State& state) const
 
     return values;
 }
+
+//-----------------------------------------------------------------------------
+// BushingForce::DissipatedEnergyStateVariable
+//-----------------------------------------------------------------------------
+const BushingForce& 
+BushingForce::DissipatedEnergyStateVariable::getBushingForce() const {
+    return static_cast<const BushingForce&>(getOwner());
+}
+
+double BushingForce::DissipatedEnergyStateVariable::getValue(
+        const SimTK::State& state) const {
+    return getBushingForce().getDissipatedEnergy(state);
+}
+
+void BushingForce::DissipatedEnergyStateVariable::setValue(
+        SimTK::State& state, double value) const {
+    getBushingForce().setDissipatedEnergy(state, value);
+}
+
+double BushingForce::DissipatedEnergyStateVariable::getDerivative(
+        const SimTK::State& state) const {
+    return getBushingForce().getPowerDissipation(state);
+}
+
+void BushingForce::DissipatedEnergyStateVariable::setDerivative(
+        const SimTK::State& state, double deriv) const {
+    OPENSIM_THROW(Exception,
+        "BushingForce::DissipatedEnergyStateVariable::setDerivative(): "
+        "The derivative of dissipated energy (power dissipation) can only be "
+        "computed by the ForceSubsystem.");
+}
diff --git a/OpenSim/Simulation/Model/BushingForce.h b/OpenSim/Simulation/Model/BushingForce.h
@@ -29,22 +29,28 @@
 #include <OpenSim/Simulation/Model/TwoFrameLinker.h>
 #include <OpenSim/Simulation/Model/PhysicalFrame.h>
 
+#include "simbody/internal/Force_LinearBushing.h"
+
 namespace OpenSim {
 
 //==============================================================================
 //                             BUSHING FORCE
 //==============================================================================
 /**
  * A class implementing a Bushing Force.
+ *
  * A Bushing Force is the force proportional to the deviation of two frames.
  * One can think of the Bushing as being composed of 3 linear and 3 torsional
  * spring-dampers, which act along or about the bushing frames. Orientations
  * are measured as x-y-z body-fixed Euler rotations, which are treated as
  * though they were uncoupled. Damping is proportional to the deflection rate of 
- * change (e.g. Euler angle derivatives) which is NOT the angular velocity between
- * the two frames. That makes this bushing model suitable only for relatively
- * small relative orientation deviations between the frames.
- * The underlying Force in Simbody is a SimtK::Force::LinearBushing.
+ * change (e.g. Euler angle derivatives) which is NOT the angular velocity 
+ * between the two frames. That makes this bushing model suitable only for 
+ * relatively small relative orientation deviations between the frames.
+ *
+ * The underlying Force in Simbody is a SimtK::Force::LinearBushing. This 
+ * implementation exposes the state variable for the dissipated energy of the 
+ * bushing force allocated internally by Simbody.
  *
  * @author Ajay Seth
  */
@@ -64,6 +70,13 @@ OpenSim_DECLARE_CONCRETE_OBJECT(BushingForce, TwoFrameLinker);
         "Damping parameters resisting angular deviation rate. (Nm/(rad/s))");
     OpenSim_DECLARE_PROPERTY(translational_damping, SimTK::Vec3,
         "Damping parameters resisting relative translational velocity. (N/(m/s))");
+
+//==============================================================================
+// OUTPUTS
+//==============================================================================
+    OpenSim_DECLARE_OUTPUT(statebounds_dissipated_energy, SimTK::Vec2,
+        getBoundsDissipatedEnergy, SimTK::Stage::Model);   
+
 //==============================================================================
 // PUBLIC METHODS
 //==============================================================================
@@ -195,6 +208,43 @@ OpenSim_DECLARE_CONCRETE_OBJECT(BushingForce, TwoFrameLinker);
     /** Potential energy is the elastic energy stored in the bushing. */
     double computePotentialEnergy(const SimTK::State& s) const final override;
 
+    /** 
+     * Obtain the total amount of energy dissipated by this BushingForce since 
+     * some arbitrary starting point, in joules.
+     * 
+     * This is the time integral of the power dissipation. For a system whose 
+     * only non-conservative forces are Bushings, the sum of potential, kinetic, 
+     * and dissipated energies should be conserved.
+     */
+    double getDissipatedEnergy(const SimTK::State& state) const;
+
+    /** 
+     * Set the accumulated dissipated energy to an arbitrary value, in joules.
+     * 
+     * Typically this is used only to reset the dissipated energy to zero, but 
+     * non-zero values can be useful if you are trying to match some existing 
+     * data or continuing a simulation.
+     */
+    void setDissipatedEnergy(SimTK::State& state, double value) const;
+
+    /** 
+     * Obtain the rate at which energy is being dissipated by this BushingForce, 
+     * that is, the power being lost, in watts.
+     */
+    double getPowerDissipation(const SimTK::State& state) const;
+
+    /** 
+     * The first element of the Vec2 is the lower bound, and the second is the
+     * upper bound.
+     * 
+     * This function is intended primarily for the model Output 
+     * 'statebounds_dissipated_energy'. We don't need the state, but the state 
+     * parameter is a requirement of Output functions.
+     */
+    SimTK::Vec2 getBoundsDissipatedEnergy(const SimTK::State&) const {
+         return {0, SimTK::Infinity}; 
+    }
+
     //--------------------------------------------------------------------------
     // Reporting
     //--------------------------------------------------------------------------
@@ -211,8 +261,34 @@ OpenSim_DECLARE_CONCRETE_OBJECT(BushingForce, TwoFrameLinker);
     // Implement ModelComponent interface.
     //--------------------------------------------------------------------------
     // Create a SimTK::Force::LinearBushing which implements this BushingForce.
-    void extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) 
-                                                                    const override;
+    void extendAddToSystemAfterSubcomponents(
+            SimTK::MultibodySystem& system) const override;
+
+    /** 
+     * Get the underlying SimTK::Force::LinearBushing force.
+     */
+    const SimTK::Force::LinearBushing& getSimTKLinearBushing() const;
+
+    /** 
+     * Concrete implementation of `StateVariable` to expose the "dissipated 
+     * energy" state variable allocated internally by the 
+     * SimTK::Force::LinearBushing.
+     */
+    class DissipatedEnergyStateVariable : public StateVariable {
+    public:
+        explicit DissipatedEnergyStateVariable(const std::string& name,
+                    const Component& owner, 
+                    SimTK::SubsystemIndex subSysIndex,
+                    int index) : 
+                StateVariable(name, owner, subSysIndex, index, false) {}
+
+        const BushingForce& getBushingForce() const;
+        double getValue(const SimTK::State& state) const override;
+        void setValue(SimTK::State& state, double value) const override;
+        double getDerivative(const SimTK::State& state) const override;
+        void setDerivative(const SimTK::State& state, 
+                double deriv) const override;
+    };
 
     void setNull();
     void constructProperties();

diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp
@@ -637,7 +637,7 @@ TEST_CASE("testBushingForce") {
     osimModel.setName("BushingTest");
     // OpenSim bodies
     const Ground& ground = osimModel.getGround();
-    ;
+
     auto* ball = new OpenSim::Body(
             "ball", mass, Vec3(0), mass * SimTK::Inertia::sphere(0.1));
     ball->attachGeometry(new Sphere{0.1});
@@ -731,6 +731,25 @@ TEST_CASE("testBushingForce") {
     // Save the forces
     reporter->getForceStorage().print("bushing_forces.mot");
 
+    // Check that the energy dissipation is zero (i.e., no damping).
+    CHECK(osim_state.getNZ() == 1);
+    CHECK_THAT(osim_state.getZ()[0], Catch::Matchers::WithinAbs(0.0, 1e-9));
+    CHECK_THAT(bushingForce.getDissipatedEnergy(osim_state), 
+            Catch::Matchers::WithinAbs(0.0, 1e-9));
+    CHECK_THAT(bushingForce.getPowerDissipation(osim_state), 
+            Catch::Matchers::WithinAbs(0.0, 1e-9));
+
+    // Update the energy dissipation to be non-zero.
+    Random::Uniform rand(0, 1);
+    Real energyDissipation = rand.getValue();
+    osim_state.updZ()[0] = energyDissipation;
+    CHECK_THAT(bushingForce.getDissipatedEnergy(osim_state), 
+            Catch::Matchers::WithinAbs(energyDissipation, 1e-9));
+    energyDissipation = rand.getValue();
+    bushingForce.setDissipatedEnergy(osim_state, energyDissipation);
+    CHECK_THAT(bushingForce.getDissipatedEnergy(osim_state), 
+            Catch::Matchers::WithinAbs(energyDissipation, 1e-9));
+
     // Before exiting lets see if copying the spring works
     BushingForce* copyOfSpring = spring->clone();