diff --git a/include/gz/math/Inertial.hh b/include/gz/math/Inertial.hh index 9b2b5f3b3..5aad35f3c 100644 --- a/include/gz/math/Inertial.hh +++ b/include/gz/math/Inertial.hh @@ -17,8 +17,12 @@ #ifndef GZ_MATH_INERTIAL_HH_ #define GZ_MATH_INERTIAL_HH_ +#include + #include #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 @@ -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 &_massMatrix, + const Pose3 &_pose, + const Matrix6 &_addedMass) + : massMatrix(_massMatrix), pose(_pose), addedMass(_addedMass) + {} + /// \brief Copy constructor. /// \param[in] _inertial Inertial element to copy public: Inertial(const Inertial &_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 &_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 > &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 BodyMatrix() const + { + Matrix6 result; + + result.SetSubmatrix(Matrix6::TOP_LEFT, + Matrix3::Identity * this->massMatrix.Mass()); + + result.SetSubmatrix(Matrix6::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( + 0, -z, y, + z, 0, -x, + -y, x, 0) * this->massMatrix.Mass(); + result.SetSubmatrix(Matrix6::BOTTOM_LEFT, skew); + result.SetSubmatrix(Matrix6::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 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 &_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 pose; + + /// \brief Fluid added mass. + private: std::optional> addedMass; }; typedef Inertial Inertiald; diff --git a/src/Inertial_TEST.cc b/src/Inertial_TEST.cc index f47d29c88..446904f12 100644 --- a/src/Inertial_TEST.cc +++ b/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)); +}