diff --git a/proto/gz/msgs/entity_wrench.proto b/proto/gz/msgs/entity_wrench.proto index a631bb1a..9a03eaca 100644 --- a/proto/gz/msgs/entity_wrench.proto +++ b/proto/gz/msgs/entity_wrench.proto @@ -1,4 +1,3 @@ - /* * Copyright (C) 2022 Open Source Robotics Foundation * diff --git a/proto/gz/msgs/inertial.proto b/proto/gz/msgs/inertial.proto index 642fcef1..d88ab0f5 100644 --- a/proto/gz/msgs/inertial.proto +++ b/proto/gz/msgs/inertial.proto @@ -21,7 +21,7 @@ option java_package = "com.gz.msgs"; option java_outer_classname = "InertialProtos"; /// \ingroup gz.msgs -/// \interface Inertial +/// \interface Inertial /// \brief Information about inertia import "gz/msgs/pose.proto"; @@ -32,12 +32,41 @@ message Inertial /// \brief Optional header data Header header = 1; + /// \brief Mass in kg double mass = 2; + + /// \brief CoM pose with respect to the link origin. In meters. Pose pose = 3; + + /// \brief Inertia matrix's XX element, in kg * m^2. double ixx = 4; + + /// \brief Inertia matrix's XY element, in kg * m^2. double ixy = 5; + + /// \brief Inertia matrix's XZ element, in kg * m^2. double ixz = 6; + + /// \brief Inertia matrix's YY element, in kg * m^2. double iyy = 7; + + /// \brief Inertia matrix's YZ element, in kg * m^2. double iyz = 8; + + /// \brief Inertia matrix's ZZ element, in kg * m^2. double izz = 9; + + /// \brief Fluid added mass matrix. The matrix is symmetric, so only the 21 + /// elements of the top-half need to be set, organized as follows: + /// + /// 00, 01, 02, 03, 04, 05, + /// 11, 12, 13, 14, 15, + /// 22, 23, 24, 25, + /// 33, 34, 35, + /// 44, 45, + /// 55, + /// + /// Elements on the top-left 3x3 corner are in kg, the bottom-right ones are + /// in kg * m^2, and the rest are in kg * m. + repeated double fluid_added_mass = 10; } diff --git a/src/Utility.cc b/src/Utility.cc index ee8ee541..dca67138 100644 --- a/src/Utility.cc +++ b/src/Utility.cc @@ -114,12 +114,67 @@ namespace gz math::Inertiald Convert(const msgs::Inertial &_i) { auto pose = msgs::Convert(_i.pose()); + + if (_i.fluid_added_mass().empty()) + { + return math::Inertiald( + math::MassMatrix3d( + _i.mass(), + math::Vector3d(_i.ixx(), _i.iyy(), _i.izz()), + math::Vector3d(_i.ixy(), _i.ixz(), _i.iyz())), + pose); + } + + math::Matrix6d addedMass{ + _i.fluid_added_mass(0), + _i.fluid_added_mass(1), + _i.fluid_added_mass(2), + _i.fluid_added_mass(3), + _i.fluid_added_mass(4), + _i.fluid_added_mass(5), + + _i.fluid_added_mass(1), + _i.fluid_added_mass(6), + _i.fluid_added_mass(7), + _i.fluid_added_mass(8), + _i.fluid_added_mass(9), + _i.fluid_added_mass(10), + + _i.fluid_added_mass(2), + _i.fluid_added_mass(7), + _i.fluid_added_mass(11), + _i.fluid_added_mass(12), + _i.fluid_added_mass(13), + _i.fluid_added_mass(14), + + _i.fluid_added_mass(3), + _i.fluid_added_mass(8), + _i.fluid_added_mass(12), + _i.fluid_added_mass(15), + _i.fluid_added_mass(16), + _i.fluid_added_mass(17), + + _i.fluid_added_mass(4), + _i.fluid_added_mass(9), + _i.fluid_added_mass(13), + _i.fluid_added_mass(16), + _i.fluid_added_mass(18), + _i.fluid_added_mass(19), + + _i.fluid_added_mass(5), + _i.fluid_added_mass(10), + _i.fluid_added_mass(14), + _i.fluid_added_mass(17), + _i.fluid_added_mass(19), + _i.fluid_added_mass(20) + }; + return math::Inertiald( math::MassMatrix3d( _i.mass(), math::Vector3d(_i.ixx(), _i.iyy(), _i.izz()), math::Vector3d(_i.ixy(), _i.ixz(), _i.iyz())), - pose); + pose, addedMass); } ///////////////////////////////////////////// @@ -472,6 +527,31 @@ namespace gz { msgs::Set(_i, _m.MassMatrix()); msgs::Set(_i->mutable_pose(), _m.Pose()); + + if (_m.FluidAddedMass().has_value()) + { + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(0, 0)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(0, 1)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(0, 2)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(0, 3)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(0, 4)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(0, 5)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(1, 1)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(1, 2)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(1, 3)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(1, 4)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(1, 5)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(2, 2)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(2, 3)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(2, 4)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(2, 5)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(3, 3)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(3, 4)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(3, 5)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(4, 4)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(4, 5)); + _i->add_fluid_added_mass(_m.FluidAddedMass().value()(5, 5)); + } } ///////////////////////////////////////////////// diff --git a/src/Utility_TEST.cc b/src/Utility_TEST.cc index 1d22d9a3..ef15aae7 100644 --- a/src/Utility_TEST.cc +++ b/src/Utility_TEST.cc @@ -195,6 +195,7 @@ TEST(MsgsTest, ConvertMathInertialToMsgs) EXPECT_DOUBLE_EQ(0.2, msg.ixz()); EXPECT_DOUBLE_EQ(0.3, msg.iyz()); EXPECT_EQ(pose, msgs::Convert(msg.pose())); + EXPECT_TRUE(msg.fluid_added_mass().empty()); } ///////////////////////////////////////////////// @@ -217,6 +218,90 @@ TEST(MsgsTest, ConvertMsgsInertialToMath) EXPECT_DOUBLE_EQ(0.2, inertial.MassMatrix().Ixz()); EXPECT_DOUBLE_EQ(0.3, inertial.MassMatrix().Iyz()); EXPECT_EQ(pose, inertial.Pose()); + EXPECT_FALSE(inertial.FluidAddedMass().has_value()); +} + +///////////////////////////////////////////////// +TEST(MsgsTest, ConvertMathInertialAddedMassToMsgs) +{ + const auto pose = math::Pose3d(5, 6, 7, 0.4, 0.5, 0.6); + 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}; + + auto msg = msgs::Convert( + math::Inertiald( + math::MassMatrix3d(12.0, + math::Vector3d(2, 3, 4), + math::Vector3d(0.1, 0.2, 0.3)), + pose, addedMass)); + + EXPECT_DOUBLE_EQ(12.0, msg.mass()); + EXPECT_DOUBLE_EQ(2.0, msg.ixx()); + EXPECT_DOUBLE_EQ(3.0, msg.iyy()); + EXPECT_DOUBLE_EQ(4.0, msg.izz()); + EXPECT_DOUBLE_EQ(0.1, msg.ixy()); + EXPECT_DOUBLE_EQ(0.2, msg.ixz()); + EXPECT_DOUBLE_EQ(0.3, msg.iyz()); + EXPECT_EQ(pose, msgs::Convert(msg.pose())); + ASSERT_EQ(21, msg.fluid_added_mass().size()); + EXPECT_DOUBLE_EQ(0.1, msg.fluid_added_mass(0)); + EXPECT_DOUBLE_EQ(0.2, msg.fluid_added_mass(1)); + EXPECT_DOUBLE_EQ(0.3, msg.fluid_added_mass(2)); + EXPECT_DOUBLE_EQ(0.4, msg.fluid_added_mass(3)); + EXPECT_DOUBLE_EQ(0.5, msg.fluid_added_mass(4)); + EXPECT_DOUBLE_EQ(0.6, msg.fluid_added_mass(5)); + EXPECT_DOUBLE_EQ(0.7, msg.fluid_added_mass(6)); + EXPECT_DOUBLE_EQ(0.8, msg.fluid_added_mass(7)); + EXPECT_DOUBLE_EQ(0.9, msg.fluid_added_mass(8)); + EXPECT_DOUBLE_EQ(1.0, msg.fluid_added_mass(9)); + EXPECT_DOUBLE_EQ(1.1, msg.fluid_added_mass(10)); + EXPECT_DOUBLE_EQ(1.2, msg.fluid_added_mass(11)); + EXPECT_DOUBLE_EQ(1.3, msg.fluid_added_mass(12)); + EXPECT_DOUBLE_EQ(1.4, msg.fluid_added_mass(13)); + EXPECT_DOUBLE_EQ(1.5, msg.fluid_added_mass(14)); + EXPECT_DOUBLE_EQ(1.6, msg.fluid_added_mass(15)); + EXPECT_DOUBLE_EQ(1.7, msg.fluid_added_mass(16)); + EXPECT_DOUBLE_EQ(1.8, msg.fluid_added_mass(17)); + EXPECT_DOUBLE_EQ(1.9, msg.fluid_added_mass(18)); + EXPECT_DOUBLE_EQ(2.0, msg.fluid_added_mass(19)); + EXPECT_DOUBLE_EQ(2.1, msg.fluid_added_mass(20)); +} + +///////////////////////////////////////////////// +TEST(MsgsTest, ConvertMsgsInertialAddedMassToMath) +{ + const auto pose = math::Pose3d(5, 6, 7, 0.4, 0.5, 0.6); + 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}; + + auto msg = msgs::Convert( + math::Inertiald( + math::MassMatrix3d(12.0, + math::Vector3d(2, 3, 4), + math::Vector3d(0.1, 0.2, 0.3)), + pose, addedMass)); + auto inertial = msgs::Convert(msg); + + EXPECT_DOUBLE_EQ(12.0, inertial.MassMatrix().Mass()); + EXPECT_DOUBLE_EQ(2.0, inertial.MassMatrix().Ixx()); + EXPECT_DOUBLE_EQ(3.0, inertial.MassMatrix().Iyy()); + EXPECT_DOUBLE_EQ(4.0, inertial.MassMatrix().Izz()); + EXPECT_DOUBLE_EQ(0.1, inertial.MassMatrix().Ixy()); + EXPECT_DOUBLE_EQ(0.2, inertial.MassMatrix().Ixz()); + EXPECT_DOUBLE_EQ(0.3, inertial.MassMatrix().Iyz()); + EXPECT_EQ(pose, inertial.Pose()); + ASSERT_TRUE(inertial.FluidAddedMass().has_value()); + EXPECT_EQ(addedMass, inertial.FluidAddedMass().value()); } /////////////////////////////////////////////////