Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add fluid added mass to inertial #271

Merged
merged 9 commits into from
Jul 29, 2022
Merged

Add fluid added mass to inertial #271

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
6667dbd
Add fluid added mass to inertial
chapulina Jul 13, 2022
10493ca
Add entity wrench message (#274)
chapulina Jul 18, 2022
367e376
Add entity wrench message (#274)
chapulina Jul 18, 2022
93822fc
Merge branch 'main' into chapulina/9/added_mass
chapulina Jul 18, 2022
5a63351
Conversion
chapulina Jul 19, 2022
b50a526
merged from main
chapulina Jul 19, 2022
576a773
Merge branch 'main' into chapulina/9/added_mass
chapulina Jul 19, 2022
1d9937d
units
chapulina Jul 28, 2022
1a6a3ad
Merge branch 'main' into chapulina/9/added_mass
chapulina Jul 28, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion proto/gz/msgs/entity_wrench.proto
View file
Open in desktop
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
Open in desktop
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,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this doesn't explicitly say how these values are packed into the repeated double vector. From looking at the test, I believe they are packed from left to right starting with the top row, then working down, like [00, ... 05, 11, ... 15, 22, ... 25, 33, ... 35, 44, 45, 55]

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What's throwing you off, the spacing? The elements are listed in that order exactly

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

it looks like you are labeling the elements according to their place in the matrix, not listing the order in which they are placed in the vector

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

it looks like the upper triangle of a matrix to me, not a vector

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you suggest the wording that would work for you? Maybe mentioning that the indices are row-first and instead of organized as follows say ordered as follows? Or something completely different?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

it looks like the upper triangle of a matrix to me, not a vector

It's both! That's what I was going for at least...

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I guess it looks like a matrix to me and the indices are matrix indices, whereas vector indices would just be integers from [0..20]

 0,  1,  2,  3,  4,  5,
     6,  7,  8,  9, 10,
        11, 12, 13, 14,
            15, 16, 17,
                18, 19,
                    20,

///
/// 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
Open in desktop
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
Open in desktop
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