Merge pull request #3962 from opensim-org/body_momentum_wrappers

Add methods and `Output`s for calculating the momentum of a `Body`

CHANGELOG.md

@@ -43,7 +43,11 @@ v4.6
   a lossless process. (#3902)
 - Improved `OpenSim::IO::stod` string-to-decimal parsing function by making it not-locale-dependant (#3943, #3924; thanks @alexbeattie42)
 - Improved the performance of `ComponentPath` traversal (e.g. as used by `Component::getComponent`, `Component::getStateVariableValue`)
+- Added Python and Java (Matlab) scripting support for `TimeSeriesTable_<SimTK::Rotation>`. (#3940)
+- Added the templatized `MocoStudy::analyze<T>()` and equivalent scripting counterparts: `analyzeVec3`, `analyzeSpatialVec`, `analyzeRotation`. (#3940)
 - Added `ConstantCurvatureJoint` to the SWIG bindings; it is now available in Matlab and Python (#3957). 
+- Added methods and `Output`s for calculating the angular momentum of a `Body`. (#3962)
+
 
 v4.5.1
 ======
@@ -108,8 +112,7 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781).
 - Fixed `MocoOrientationTrackingGoal::initializeOnModelImpl` to check for missing kinematic states, but allow other missing columns. (#3830)
 - Improved exception handling for internal errors in `MocoCasADiSolver`. Problems will now abort and print a descriptive error message (rather than fail due to an empty trajectory). (#3834)
 - Upgraded the Ipopt dependency Metis to version 5.1.0 on Unix and macOS to enable building on `osx-arm64` (#3874).
-- Added Python and Java (Matlab) scripting support for `TimeSeriesTable_<SimTK::Rotation>`. (#3940)
-- Added the templatized `MocoStudy::analyze<T>()` and equivalent scripting counterparts: `analyzeVec3`, `analyzeSpatialVec`, `analyzeRotation`. (#3940)
+
 
 v4.5
 ====

OpenSim/Simulation/SimbodyEngine/Body.cpp

@@ -26,16 +26,15 @@
 //=============================================================================
 #include "Body.h"
 #include <OpenSim/Common/ScaleSet.h>
+#include "simbody/internal/MobilizedBody.h"
 
 //=============================================================================
 // STATICS
 //=============================================================================
 using namespace std;
-//using namespace SimTK;
 using namespace OpenSim;
 using SimTK::Mat33;
 using SimTK::Vec3;
-using SimTK::DecorativeGeometry;
 
 //=============================================================================
 // CONSTRUCTOR(S)
@@ -182,6 +181,35 @@ void Body::setInertia(const SimTK::Inertia& inertia)
     upd_inertia()[5] = I[1][2];
 }
 
+SimTK::SpatialVec Body::calcMomentumAboutOrigin(const SimTK::State& s) const {
+    const SimTK::MobilizedBody& mobod = getMobilizedBody();
+    return mobod.calcBodyMomentumAboutBodyOriginInGround(s);
+}
+
+SimTK::Vec3 Body::calcAngularMomentumAboutOrigin(const SimTK::State& s) const {
+    return calcMomentumAboutOrigin(s)[0];
+}
+
+SimTK::Vec3 Body::calcLinearMomentumAboutOrigin(const SimTK::State& s) const {
+    return calcMomentumAboutOrigin(s)[1];
+}
+
+SimTK::SpatialVec Body::calcMomentumAboutMassCenter(const SimTK::State& s) const 
+{
+    const SimTK::MobilizedBody& mobod = getMobilizedBody();
+    return mobod.calcBodyMomentumAboutBodyMassCenterInGround(s);
+}
+
+SimTK::Vec3 Body::calcAngularMomentumAboutMassCenter(const SimTK::State& s) const 
+{
+    return calcMomentumAboutMassCenter(s)[0];
+}
+
+SimTK::Vec3 Body::calcLinearMomentumAboutMassCenter(const SimTK::State& s) const 
+{
+    return calcMomentumAboutMassCenter(s)[1];
+}
+
 //==============================================================================
 // SCALING
 //==============================================================================

OpenSim/Simulation/SimbodyEngine/Body.h

@@ -56,6 +56,27 @@ class OSIMSIMULATION_API Body : public PhysicalFrame {
         "The elements of the inertia tensor (Vec6) as [Ixx Iyy Izz Ixy Ixz Iyz] "
         "measured about the mass_center and not the body origin.");
 
+//==============================================================================
+// OUTPUTS
+//==============================================================================
+    OpenSim_DECLARE_OUTPUT(momentum_about_origin, SimTK::SpatialVec,
+            calcMomentumAboutOrigin, SimTK::Stage::Velocity);
+
+    OpenSim_DECLARE_OUTPUT(angular_momentum_about_origin, SimTK::Vec3,
+            calcAngularMomentumAboutOrigin, SimTK::Stage::Velocity);
+
+    OpenSim_DECLARE_OUTPUT(linear_momentum_about_origin, SimTK::Vec3,
+            calcLinearMomentumAboutOrigin, SimTK::Stage::Velocity);
+
+    OpenSim_DECLARE_OUTPUT(momentum_about_mass_center, SimTK::SpatialVec,
+            calcMomentumAboutMassCenter, SimTK::Stage::Velocity);
+
+    OpenSim_DECLARE_OUTPUT(angular_momentum_about_mass_center, SimTK::Vec3,
+            calcAngularMomentumAboutMassCenter, SimTK::Stage::Velocity);
+
+    OpenSim_DECLARE_OUTPUT(linear_momentum_about_mass_center, SimTK::Vec3,
+            calcLinearMomentumAboutMassCenter, SimTK::Stage::Velocity);
+
 //=============================================================================
 // PUBLIC METHODS
 //=============================================================================
@@ -105,6 +126,31 @@ class OSIMSIMULATION_API Body : public PhysicalFrame {
     void scaleInertialProperties(const SimTK::Vec3& scaleFactors, bool scaleMass = true);
 
     void scaleMass(double aScaleFactor);
+
+    /** Calculate the Body's spatial momentum (angular, linear) measured and 
+    expressed in Ground, but taken about the Body origin. */
+    SimTK::SpatialVec calcMomentumAboutOrigin(const SimTK::State& s) const;
+
+    /** Calculate the Body's angular momentum measured and expressed in Ground,
+    but taken about the Body origin. */
+    SimTK::Vec3 calcAngularMomentumAboutOrigin(const SimTK::State& s) const;
+
+    /** Calculate the Body's linear momentum measured and expressed in Ground,
+    but taken about the Body origin. */
+    SimTK::Vec3 calcLinearMomentumAboutOrigin(const SimTK::State& s) const;
+
+    /** Calculate the Body's spatial momentum (angular, linear) measured and
+    expressed in Ground, but taken about the Body mass center. */
+    SimTK::SpatialVec calcMomentumAboutMassCenter(const SimTK::State& s) const;
+
+    /** Calculate the Body's angular momentum measured and expressed in Ground,
+    but taken about the Body mass center. */
+    SimTK::Vec3 calcAngularMomentumAboutMassCenter(const SimTK::State& s) const;
+
+    /** Calculate the Body's linear momentum measured and expressed in Ground,
+    but taken about the Body mass center. */
+    SimTK::Vec3 calcLinearMomentumAboutMassCenter(const SimTK::State& s) const;
+
  protected:
 
     // Model component interface.

dependencies/CMakeLists.txt

@@ -185,7 +185,7 @@ AddDependency(NAME       ezc3d
 AddDependency(NAME       simbody
               DEFAULT    ON
               GIT_URL    https://github.com/simbody/simbody.git
-              GIT_TAG    f853397efad00798a169ff3dd1c52f5e20a24ba9
+              GIT_TAG    7265c6e77bc1879e7b9602c0134ee93aa81b6900
               CMAKE_ARGS -DBUILD_EXAMPLES:BOOL=OFF
                          -DBUILD_TESTING:BOOL=OFF
                          ${SIMBODY_EXTRA_CMAKE_ARGS})