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
@@ -62,6 +66,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 also expressed
+      /// in frame Bi.
+      /// \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 +133,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 nullpot of the added mass
+      /// was never set.
+      public: 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 +166,52 @@ 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 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->massMatrix.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.
+      /// \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 +262,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 +418,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;

include/gz/math/Matrix6.hh

@@ -317,6 +317,34 @@ namespace gz
             el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
       }
 
+      /// \brief Addition assignment operator. This matrix will
+      /// become equal to this + _m2.
+      /// \param[in] _m2 Incoming matrix.
+      /// \return This matrix + _m2.
+      public: Matrix6<T> operator+=(const Matrix6<T> &_m2)
+      {
+        (*this) = (*this) + _m2;
+        return *this;
+      }
+
+      /// \brief Addition operator
+      /// \param[in] _m2 Incoming matrix
+      /// \return This matrix + _m2
+      public: Matrix6<T> operator+(const Matrix6<T> &_m2) const
+      {
+        auto el = [&](size_t _row, size_t _col) -> T
+        {
+          return this->data[_row][_col] + _m2(_row, _col);
+        };
+        return Matrix6<T>(
+            el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
+            el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
+            el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
+            el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
+            el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
+            el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
+      }
+
      /// \brief Get the value at the specified row, column index
      /// \param[in] _col The column index. Index values are clamped to a
      /// range of [0, 5].

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());
 }
 
 /////////////////////////////////////////////////
@@ -735,3 +739,124 @@ TEST(Inertiald_Test, SubtractionInvalid)
     EXPECT_TRUE((i2 - i1).MassMatrix().IsValid());
   }
 }
+
+TEST(Inertiald_Test, BodyMatrix)
+{
+  math::MassMatrix3d massMatrix(100, {1, 2, 3}, {4, 5, 6});
+  math::Pose3d com{7, 8, 9, 0, 0, 0};
+
+  math::Inertiald inertial(massMatrix, com);
+
+  auto bodyMatrix = inertial.BodyMatrix();
+
+  EXPECT_EQ(bodyMatrix, inertial.SpatialMatrix());
+
+  EXPECT_EQ(math::Matrix3d(
+      100,   0, 0,
+      0,   100, 0,
+      0,     0, 100),
+      bodyMatrix.Submatrix(math::Matrix6d::TOP_LEFT));
+
+  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));
+
+  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(math::Matrix3d(
+      1, 4, 5,
+      4, 2, 6,
+      5, 6, 3),
+      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));
+}

src/Matrix6_TEST.cc

@@ -106,7 +106,7 @@ TEST(Matrix6dTest, CoverageExtra)
 }
 
 /////////////////////////////////////////////////
-TEST(Matrix6dTest, Multiply4)
+TEST(Matrix6dTest, Multiply)
 {
   Matrix6d mat, mat1;
 
@@ -137,6 +137,36 @@ TEST(Matrix6dTest, Multiply4)
   EXPECT_EQ(mat2, mat4);
 }
 
+/////////////////////////////////////////////////
+TEST(Matrix6dTest, Add)
+{
+  Matrix6d mat, mat1;
+
+  for (int i = 0; i < 6; ++i)
+  {
+    for (int j = 0; j < 6; ++j)
+    {
+      mat(i, j) = i - j;
+      mat1(j, i) = i + j;
+    }
+  }
+
+  Matrix6d mat3(
+      0, 0, 0, 0, 0, 0,
+      2, 2, 2, 2, 2, 2,
+      4, 4, 4, 4, 4, 4,
+      6, 6, 6, 6, 6, 6,
+      8, 8, 8, 8, 8, 8,
+      10, 10, 10, 10, 10, 10);
+
+  auto mat2 = mat + mat1;
+  EXPECT_EQ(mat2, mat3);
+
+  auto mat4 = mat;
+  mat4 += mat1;
+  EXPECT_EQ(mat2, mat4);
+}
+
 /////////////////////////////////////////////////
 TEST(Matrix6dTest, NoIndexException)
 {