Add fluid added mass to inertial

include/gz/math/Inertial.hh

@@ -17,8 +17,12 @@
 #ifndef GZ_MATH_INERTIAL_HH_
 #define GZ_MATH_INERTIAL_HH_
 
+#include <optional>
+
 #include <gz/math/config.hh>
 #include "gz/math/MassMatrix3.hh"
+#include "gz/math/Matrix3.hh"
+#include "gz/math/Matrix6.hh"
 #include "gz/math/Pose3.hh"
 
 namespace gz
@@ -39,6 +43,9 @@ namespace gz
     /// specifying the pose X_FBi of the inertial frame Bi in the
     /// inertial object frame F.
     ///
+    /// Throughout this documentation, the terms "body", "object" and "link"
+    /// are used interchangeably.
+    ///
     /// For information about the X_FBi notation, see
     /// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html
     template<typename T>
@@ -62,6 +69,23 @@ namespace gz
       : massMatrix(_massMatrix), pose(_pose)
       {}
 
+      /// \brief Constructs an inertial object from the mass matrix for a body
+      /// B, about its center of mass Bcm, and expressed in a frame that we'll
+      /// call the "inertial" frame Bi, i.e. the frame in which the components
+      /// of the mass matrix are specified (see this class's documentation for
+      /// details). The pose object specifies the pose X_FBi of the inertial
+      /// frame Bi in the frame F of this inertial object
+      /// (see class's documentation). The added mass matrix is expressed
+      /// in the link origin frame F.
+      /// \param[in] _massMatrix Mass and inertia matrix.
+      /// \param[in] _pose Pose of center of mass reference frame.
+      /// \param[in] _addedMass Coefficients for fluid added mass.
+      public: Inertial(const MassMatrix3<T> &_massMatrix,
+                       const Pose3<T> &_pose,
+                       const Matrix6<T> &_addedMass)
+      : massMatrix(_massMatrix), pose(_pose), addedMass(_addedMass)
+      {}
+
       /// \brief Copy constructor.
       /// \param[in] _inertial Inertial element to copy
       public: Inertial(const Inertial<T> &_inertial) = default;
@@ -112,6 +136,28 @@ namespace gz
         return this->pose;
       }
 
+      /// \brief Set the matrix representing the inertia of the fluid that is
+      /// dislocated when the body moves. Added mass should be zero if the
+      /// density of the surrounding fluid is negligible with respect to the
+      /// body's density.
+      ///
+      /// \param[in] _m New Matrix6 object, which must be a symmetric matrix.
+      /// \return True if the Matrix6 is symmetric.
+      /// \sa SpatialMatrix
+      public: bool SetFluidAddedMass(const Matrix6<T> &_m)
+      {
+        this->addedMass = _m;
+        return this->addedMass.value() == this->addedMass.value().Transposed();
+      }
+
+      /// \brief Get the fluid added mass matrix.
+      /// \return The added mass matrix. It will be nullopt if the added mass
+      /// was never set.
+      public: const std::optional< Matrix6<T> > &FluidAddedMass() const
+      {
+        return this->addedMass;
+      }
+
       /// \brief Get the moment of inertia matrix computer about the body's
       /// center of mass and expressed in this Inertial object’s frame F.
       /// \return The inertia matrix computed about the body’s center of
@@ -123,6 +169,56 @@ namespace gz
         return R * this->massMatrix.Moi() * R.Transposed();
       }
 
+      /// \brief Spatial mass matrix for body B. It does not include fluid
+      /// added mass. The matrix is expressed in the object's frame F, not to
+      /// be confused with the center of mass frame Bi.
+      ///
+      /// The matrix is arranged as follows:
+      ///
+      /// | m          0          0          0           m * Pz    -m * Py |
+      /// | 0          m          0          -m * Pz    0           m * Px |
+      /// | 0          0          m           m * Py    -m * Px    0       |
+      /// | 0          -m * Pz     m * Py    Ixx        Ixy        Ixz     |
+      /// |  m * Pz    0          -m * Px    Ixy        Iyy        Iyz     |
+      /// | -m * Py     m* Px     0          Ixz        Iyz        Izz     |
+      ///
+      /// \return The body's 6x6 inertial matrix.
+      /// \sa SpatialMatrix
+      public: Matrix6<T> BodyMatrix() const
+      {
+        Matrix6<T> result;
+
+        result.SetSubmatrix(Matrix6<T>::TOP_LEFT,
+            Matrix3<T>::Identity * this->massMatrix.Mass());
+
+        result.SetSubmatrix(Matrix6<T>::BOTTOM_RIGHT, this->Moi());
+
+        auto x = this->pose.Pos().X();
+        auto y = this->pose.Pos().Y();
+        auto z = this->pose.Pos().Z();
+        auto skew = Matrix3<T>(
+            0, -z, y,
+            z, 0, -x,
+            -y, x, 0) * this->massMatrix.Mass();
+        result.SetSubmatrix(Matrix6<T>::BOTTOM_LEFT, skew);
+        result.SetSubmatrix(Matrix6<T>::TOP_RIGHT, skew.Transposed());
+
+        return result;
+      }
+
+      /// \brief Spatial mass matrix, which includes the body's inertia, as well
+      /// as the inertia of the fluid that is dislocated when the body moves.
+      /// The matrix is expressed in the object's frame F, not to be confused
+      /// with the center of mass frame Bi.
+      /// \return The spatial mass matrix.
+      /// \sa BodyMatrix
+      /// \sa FluidAddedMass
+      public: Matrix6<T> SpatialMatrix() const
+      {
+        return this->addedMass.has_value() ?
+            this->BodyMatrix() + this->addedMass.value() : this->BodyMatrix();
+      }
+
       /// \brief Set the inertial pose rotation without affecting the
       /// MOI in the base coordinate frame.
       /// \param[in] _q New rotation for inertial pose.
@@ -173,7 +269,8 @@ namespace gz
       public: bool operator==(const Inertial<T> &_inertial) const
       {
         return (this->pose == _inertial.Pose()) &&
-               (this->massMatrix == _inertial.MassMatrix());
+               (this->massMatrix == _inertial.MassMatrix()) &&
+               (this->addedMass == _inertial.FluidAddedMass());
       }
 
       /// \brief Inequality test operator
@@ -328,6 +425,9 @@ namespace gz
       /// \brief Pose offset of center of mass reference frame relative
       /// to a base frame.
       private: Pose3<T> pose;
+
+      /// \brief Fluid added mass.
+      private: std::optional<Matrix6<T>> addedMass;
     };
 
     typedef Inertial<double> Inertiald;

src/Inertial_TEST.cc

@@ -42,6 +42,10 @@ TEST(Inertiald_Test, Constructor)
   EXPECT_EQ(inertial.Pose(), math::Pose3d::Zero);
   EXPECT_EQ(inertial.MassMatrix(), math::MassMatrix3d());
   EXPECT_EQ(inertial.Moi(), math::Matrix3d::Zero);
+  EXPECT_FALSE(inertial.FluidAddedMass().has_value());
+  EXPECT_EQ(inertial.BodyMatrix(), math::Matrix6d::Zero);
+  EXPECT_EQ(inertial.SpatialMatrix(), math::Matrix6d::Zero);
+  EXPECT_EQ(inertial.BodyMatrix(), inertial.SpatialMatrix());
 }
 
 /////////////////////////////////////////////////
@@ -678,6 +682,7 @@ TEST(Inertiald_Test, AdditionInvalid)
   }
 }
 
+/////////////////////////////////////////////////
 TEST(Inertiald_Test, SubtractionInvalid)
 {
   const double mass = 12.0;
@@ -735,3 +740,139 @@ TEST(Inertiald_Test, SubtractionInvalid)
     EXPECT_TRUE((i2 - i1).MassMatrix().IsValid());
   }
 }
+
+/////////////////////////////////////////////////
+TEST(Inertiald_Test, BodyMatrix)
+{
+  math::MassMatrix3d massMatrix(100, {2.0, 3.0, 4.0}, {0.2, 0.3, 0.4});
+  math::Pose3d com{7, 8, 9, 0, 0, GZ_PI * 0.5};
+
+  math::Inertiald inertial(massMatrix, com);
+
+  auto bodyMatrix = inertial.BodyMatrix();
+
+  EXPECT_EQ(bodyMatrix, inertial.SpatialMatrix());
+
+  // Mass diagonal
+  EXPECT_EQ(math::Matrix3d(
+      100,   0, 0,
+      0,   100, 0,
+      0,     0, 100),
+      bodyMatrix.Submatrix(math::Matrix6d::TOP_LEFT));
+
+  // CoM translational offset
+  EXPECT_EQ(math::Matrix3d(
+      0,        100 * 9,  -100 * 8,
+      -100 * 9, 0,        100 * 7,
+      100 * 8,  -100 * 7, 0),
+      bodyMatrix.Submatrix(math::Matrix6d::TOP_RIGHT));
+
+  // Transpose of TOP_RIGHT
+  EXPECT_EQ(math::Matrix3d(
+      0,        -100 * 9, 100 * 8,
+      100 * 9,  0,        -100 * 7,
+      -100 * 8, 100 * 7,  0),
+      bodyMatrix.Submatrix(math::Matrix6d::BOTTOM_LEFT));
+  EXPECT_EQ(bodyMatrix.Submatrix(math::Matrix6d::BOTTOM_LEFT).Transposed(),
+      bodyMatrix.Submatrix(math::Matrix6d::TOP_RIGHT));
+
+  // Moments of inertia with CoM rotational offset
+  // 90 deg yaw:
+  // * xx <- (-1)*(-1)*yy
+  // * xy <- (-1)*xy
+  // * xz <- (-1)*yz
+  // * yy <- xx
+  // * yz <- xz
+  // * zz <- zz
+  EXPECT_EQ(math::Matrix3d(
+      3.0, -0.2, -0.4,
+      -0.2, 2.0, 0.3,
+      -0.4, 0.3, 4.0),
+      bodyMatrix.Submatrix(math::Matrix6d::BOTTOM_RIGHT));
+}
+
+/////////////////////////////////////////////////
+TEST(Inertiald_Test, FluidAddedMass)
+{
+  math::MassMatrix3d massMatrix(100, {1, 2, 3}, {4, 5, 6});
+  math::Pose3d com{7, 8, 9, 0, 0, 0};
+  math::Matrix6d addedMass{
+      0.1, 0.2, 0.3, 0.4, 0.5, 0.6,
+      0.2, 0.7, 0.8, 0.9, 1.0, 1.1,
+      0.3, 0.8, 1.2, 1.3, 1.4, 1.5,
+      0.4, 0.9, 1.3, 1.6, 1.7, 1.8,
+      0.5, 1.0, 1.4, 1.7, 1.9, 2.0,
+      0.6, 1.1, 1.5, 1.8, 2.0, 2.1};
+
+  math::Inertiald inertial(massMatrix, com, addedMass);
+  EXPECT_TRUE(inertial.FluidAddedMass().has_value());
+  EXPECT_EQ(addedMass, inertial.FluidAddedMass());
+
+  auto spatialMatrix = inertial.SpatialMatrix();
+
+  EXPECT_EQ(math::Matrix3d(
+      100 + 0.1, 0.2,       0.3,
+      0.2,       100 + 0.7, 0.8,
+      0.3,       0.8,       100 + 1.2),
+      spatialMatrix.Submatrix(math::Matrix6d::TOP_LEFT));
+
+  EXPECT_EQ(math::Matrix3d(
+      0.4,           0.5 + 100 * 9, 0.6 - 100 * 8,
+      0.9 - 100 * 9, 1.0,           1.1 + 100 * 7,
+      1.3 + 100 * 8, 1.4 - 100 * 7, 1.5),
+      spatialMatrix.Submatrix(math::Matrix6d::TOP_RIGHT));
+
+  EXPECT_EQ(math::Matrix3d(
+      0.4,           0.9 - 100 * 9, 1.3 + 100 * 8,
+      0.5 + 100 * 9, 1.0,           1.4 - 100 * 7,
+      0.6 - 100 * 8, 1.1 + 100 * 7, 1.5),
+      spatialMatrix.Submatrix(math::Matrix6d::BOTTOM_LEFT));
+
+  EXPECT_EQ(math::Matrix3d(
+      1.6 + 1, 1.7 + 4, 1.8 + 5,
+      1.7 + 4, 1.9 + 2, 2.0 + 6,
+      1.8 + 5, 2.0 + 6, 2.1 +3),
+      spatialMatrix.Submatrix(math::Matrix6d::BOTTOM_RIGHT));
+
+  // Set new added mass
+  math::Matrix6d notSymmetric;
+  notSymmetric(1, 2) = 100;
+  EXPECT_FALSE(inertial.SetFluidAddedMass(notSymmetric));
+
+  math::Matrix6d newAddedMass{
+      0.01, 0.02, 0.03, 0.04, 0.05, 0.06,
+      0.02, 0.07, 0.08, 0.09, 1.00, 1.01,
+      0.03, 0.08, 1.02, 1.03, 1.04, 1.05,
+      0.04, 0.09, 1.03, 1.06, 1.07, 1.08,
+      0.05, 1.00, 1.04, 1.07, 1.09, 2.00,
+      0.06, 1.01, 1.05, 1.08, 2.00, 2.01};
+  EXPECT_TRUE(inertial.SetFluidAddedMass(newAddedMass));
+  EXPECT_EQ(newAddedMass, inertial.FluidAddedMass());
+
+  auto newSpatialMatrix = inertial.SpatialMatrix();
+  EXPECT_NE(newSpatialMatrix, spatialMatrix);
+
+  EXPECT_EQ(math::Matrix3d(
+      100 + 0.01, 0.02,       0.03,
+      0.02,       100 + 0.07, 0.08,
+      0.03,       0.08,       100 + 1.02),
+      newSpatialMatrix.Submatrix(math::Matrix6d::TOP_LEFT));
+
+  EXPECT_EQ(math::Matrix3d(
+      0.04,           0.05 + 100 * 9, 0.06 - 100 * 8,
+      0.09 - 100 * 9, 1.00,           1.01 + 100 * 7,
+      1.03 + 100 * 8, 1.04 - 100 * 7, 1.05),
+      newSpatialMatrix.Submatrix(math::Matrix6d::TOP_RIGHT));
+
+  EXPECT_EQ(math::Matrix3d(
+      0.04,           0.09 - 100 * 9, 1.03 + 100 * 8,
+      0.05 + 100 * 9, 1.00,           1.04 - 100 * 7,
+      0.06 - 100 * 8, 1.01 + 100 * 7, 1.05),
+      newSpatialMatrix.Submatrix(math::Matrix6d::BOTTOM_LEFT));
+
+  EXPECT_EQ(math::Matrix3d(
+      1.06 + 1, 1.07 + 4, 1.08 + 5,
+      1.07 + 4, 1.09 + 2, 2.00 + 6,
+      1.08 + 5, 2.00 + 6, 2.01 +3),
+      newSpatialMatrix.Submatrix(math::Matrix6d::BOTTOM_RIGHT));
+}