Skip to content

Commit

Permalink
Merge 1a6a3ad into 8194ad6
Browse files Browse the repository at this point in the history
  • Loading branch information
@chapulina
chapulina authored Jul 28, 2022
2 parents 8194ad6 + 1a6a3ad commit dee433e
Show file tree
Hide file tree
Showing 4 changed files with 196 additions and 3 deletions.
1 change: 0 additions & 1 deletion proto/gz/msgs/entity_wrench.proto
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@

/*
* Copyright (C) 2022 Open Source Robotics Foundation
*
Expand Down
31 changes: 30 additions & 1 deletion proto/gz/msgs/inertial.proto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand All @@ -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;
}
82 changes: 81 additions & 1 deletion src/Utility.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

/////////////////////////////////////////////
Expand Down Expand Up @@ -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));
}
}

/////////////////////////////////////////////////
Expand Down
85 changes: 85 additions & 0 deletions src/Utility_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}

/////////////////////////////////////////////////
Expand All @@ -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());
}

/////////////////////////////////////////////////
Expand Down

0 comments on commit dee433e

Please sign in to comment.