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.

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

Subtraction operator for Inertial class #432

Merged
merged 14 commits into from
Jun 2, 2022
Merged
Show file tree
Hide file tree
Changes from 13 commits
Commits
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
69 changes: 69 additions & 0 deletions include/gz/math/Inertial.hh
Original file line number Diff line number Diff line change
Expand Up @@ -242,6 +242,65 @@ namespace gz
return *this;
}

/// \brief Subtracts inertial properties from current object.
/// The mass, center of mass location, and inertia matrix are updated
/// as long as the remaining mass is positive.
/// \param[in] _inertial Inertial to subtract.
/// \return Reference to this object.
public: Inertial<T> &operator-=(const Inertial<T> &_inertial)
{
T m = this->massMatrix.Mass();
T m2 = _inertial.MassMatrix().Mass();

// Remaining mass
T m1 = m - m2;

// Only continue if remaining mass is positive
if (m1 <= 0)
{
return *this;
Copy link
Member

Choose a reason for hiding this comment

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

we should also add a test case that exercises this line of code. It can be in a separate test case than AdditionSubtraction, like InvalidSubtraction or something like that

Copy link
Contributor Author

Choose a reason for hiding this comment

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

On it!

}

auto com = this->Pose().Pos();
auto com2 = _inertial.Pose().Pos();
// Remaining center of mass location in base frame
auto com1 = (m*com - m2*com2)/m1;

// Components of new moment of inertia matrix
Vector3<T> ixxyyzz;
Vector3<T> ixyxzyz;

// First subtract matrices in base frame
{
auto moi = this->Moi() - _inertial.Moi();
ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
}
// Then account for parallel axis theorem
{
auto dc = com1 - com;
ixxyyzz.X() -= m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
ixxyyzz.Y() -= m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
ixxyyzz.Z() -= m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
ixyxzyz.X() += m1 * dc[0] * dc[1];
ixyxzyz.Y() += m1 * dc[0] * dc[2];
ixyxzyz.Z() += m1 * dc[1] * dc[2];
}
{
auto dc = com2 - com;
ixxyyzz.X() -= m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
ixxyyzz.Y() -= m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
ixxyyzz.Z() -= m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
ixyxzyz.X() += m2 * dc[0] * dc[1];
ixyxzyz.Y() += m2 * dc[0] * dc[2];
ixyxzyz.Z() += m2 * dc[1] * dc[2];
}
this->massMatrix = MassMatrix3<T>(m1, ixxyyzz, ixyxzyz);
this->pose = Pose3<T>(com1, Quaternion<T>::Identity);

return *this;
}

/// \brief Adds inertial properties to current object.
/// The mass, center of mass location, and inertia matrix are updated
/// as long as the total mass is positive.
Expand All @@ -252,6 +311,16 @@ namespace gz
return Inertial<T>(*this) += _inertial;
}

/// \brief Subtracts inertial properties from current object.
/// The mass, center of mass location, and inertia matrix are updated
/// as long as the remaining mass is positive.
/// \param[in] _inertial Inertial to subtract.
/// \return Reference to this object.
public: const Inertial<T> operator-(const Inertial<T> &_inertial) const
{
return Inertial<T>(*this) -= _inertial;
}

/// \brief Mass and inertia matrix of the object expressed in the
/// center of mass reference frame.
private: MassMatrix3<T> massMatrix;
Expand Down
100 changes: 87 additions & 13 deletions src/Inertial_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -366,21 +366,26 @@ TEST(Inertiald_Test, Diagonalize)
0.25*math::Vector3d(-sqrt(3), -sqrt(3)/2, 3.0));
}
/////////////////////////////////////////////////
TEST(Inertiald_Test, Addition)
TEST(Inertiald_Test, AdditionSubtraction)
{
// Add two half-cubes together
// Add two half-cubes together and
// Subtract one half-cube from a full cube
{
const double mass = 12.0;
const math::Vector3d size(1, 1, 1);
math::MassMatrix3d cubeMM3;
EXPECT_TRUE(cubeMM3.SetFromBox(mass, size));
const math::Inertiald cube(cubeMM3, math::Pose3d::Zero);
math::MassMatrix3d half;
EXPECT_TRUE(half.SetFromBox(0.5*mass, math::Vector3d(0.5, 1, 1)));
const math::Vector3d half_size(0.5, 1, 1);
EXPECT_TRUE(half.SetFromBox(0.5*mass, half_size));
math::Inertiald left(half, math::Pose3d(-0.25, 0, 0, 0, 0, 0));
math::Inertiald right(half, math::Pose3d(0.25, 0, 0, 0, 0, 0));
EXPECT_EQ(cube, left + right);
EXPECT_EQ(cube, right + left);
EXPECT_EQ(right, cube - left);
EXPECT_EQ(left, cube - right);

// test += operator
{
math::Inertiald tmp = left;
Expand All @@ -392,6 +397,18 @@ TEST(Inertiald_Test, Addition)
tmp += left;
EXPECT_EQ(cube, tmp);
}
// test -= operator
{
math::Inertiald tmp = cube;
tmp -= right;
EXPECT_EQ(left, tmp);
}
{
math::Inertiald tmp = cube;
tmp -= left;
EXPECT_EQ(right, tmp);
}

// Test EquivalentBox
{
math::Vector3d size2;
Expand All @@ -407,9 +424,24 @@ TEST(Inertiald_Test, Addition)
EXPECT_EQ(size, size2);
EXPECT_EQ(rot2, math::Quaterniond::Identity);
}
{
math::Vector3d size2;
math::Quaterniond rot2;
EXPECT_TRUE((cube - right).MassMatrix().EquivalentBox(size2, rot2));
EXPECT_EQ(half_size, size2);
EXPECT_EQ(rot2, math::Quaterniond::Identity);
}
{
math::Vector3d size2;
math::Quaterniond rot2;
EXPECT_TRUE((cube - left).MassMatrix().EquivalentBox(size2, rot2));
EXPECT_EQ(half_size, size2);
EXPECT_EQ(rot2, math::Quaterniond::Identity);
}
}

// Add two rotated half-cubes together
// Add two rotated half-cubes together and
// Subtract a rotated half-cube from rotated full-cube
{
const double mass = 12.0;
const math::Vector3d size(1, 1, 1);
Expand All @@ -425,6 +457,7 @@ TEST(Inertiald_Test, Addition)
// objects won't match exactly
// since inertia matrices will all be in base frame
// but mass, center of mass, and base-frame MOI should match
// +operator
EXPECT_NE(cube, left + right);
EXPECT_NE(cube, right + left);
EXPECT_DOUBLE_EQ(cubeMM3.Mass(), (left + right).MassMatrix().Mass());
Expand All @@ -433,48 +466,69 @@ TEST(Inertiald_Test, Addition)
EXPECT_EQ(cube.Pose().Pos(), (right + left).Pose().Pos());
EXPECT_EQ(cube.Moi(), (left + right).Moi());
EXPECT_EQ(cube.Moi(), (right + left).Moi());
// -operator
EXPECT_NE(left, cube - right);
EXPECT_NE(right, cube - left);
EXPECT_DOUBLE_EQ(left.MassMatrix().Mass(),
(cube - right).MassMatrix().Mass());
EXPECT_DOUBLE_EQ(right.MassMatrix().Mass(),
(cube - left).MassMatrix().Mass());
EXPECT_EQ(left.Pose().Pos(), (cube - right).Pose().Pos());
EXPECT_EQ(right.Pose().Pos(), (cube - left).Pose().Pos());
EXPECT_EQ(left.Moi(), (cube - right).Moi());
EXPECT_EQ(right.Moi(), (cube - left).Moi());
}

// Add eight cubes together into larger cube
// Add eight cubes together into larger cube and
// Subtract seven cubes from larger cube
{
const double mass = 12.0;
const math::Vector3d size(1, 1, 1);
math::MassMatrix3d cubeMM3;
EXPECT_TRUE(cubeMM3.SetFromBox(mass, size));
const math::Inertiald addedCube =
const math::Inertiald sevenCubes =
math::Inertiald(cubeMM3, math::Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(-0.5, 0.5, -0.5, 0, 0, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(0.5, -0.5, -0.5, 0, 0, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(0.5, 0.5, -0.5, 0, 0, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(-0.5, -0.5, 0.5, 0, 0, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(-0.5, 0.5, 0.5, 0, 0, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(0.5, -0.5, 0.5, 0, 0, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(0.5, -0.5, 0.5, 0, 0, 0));
const math::Inertiald lastCube =
math::Inertiald(cubeMM3, math::Pose3d(0.5, 0.5, 0.5, 0, 0, 0));
const math::Inertiald addedCube = sevenCubes + lastCube;

math::MassMatrix3d trueCubeMM3;
EXPECT_TRUE(trueCubeMM3.SetFromBox(8*mass, 2*size));
EXPECT_EQ(addedCube, math::Inertiald(trueCubeMM3, math::Pose3d::Zero));
EXPECT_EQ(lastCube, addedCube - sevenCubes);
Copy link
Member

Choose a reason for hiding this comment

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

nit: let's add EXPECT_EQ(sevenCubes, addedCube - lastCube); as well

EXPECT_EQ(sevenCubes, addedCube - lastCube);
}

// Add eight rotated cubes together into larger cube
// Add eight rotated cubes together into larger cube and
// Subtract seven rotated cubes from larger cube
{
const double mass = 12.0;
const math::Vector3d size(1, 1, 1);
math::MassMatrix3d cubeMM3;
EXPECT_TRUE(cubeMM3.SetFromBox(mass, size));
const math::Inertiald addedCube =
const math::Inertiald sevenCubes =
math::Inertiald(cubeMM3, math::Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(-0.5, 0.5, -0.5, IGN_PI_2, 0, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(0.5, -0.5, -0.5, 0, IGN_PI_2, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(0.5, 0.5, -0.5, 0, 0, IGN_PI_2)) +
math::Inertiald(cubeMM3, math::Pose3d(-0.5, -0.5, 0.5, IGN_PI, 0, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(-0.5, 0.5, 0.5, 0, IGN_PI, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(0.5, -0.5, 0.5, 0, 0, IGN_PI)) +
math::Inertiald(cubeMM3, math::Pose3d(0.5, -0.5, 0.5, 0, 0, IGN_PI));
const math::Inertiald lastCube =
math::Inertiald(cubeMM3, math::Pose3d(0.5, 0.5, 0.5, 0, 0, 0));
const math::Inertiald addedCube = sevenCubes + lastCube;

math::MassMatrix3d trueCubeMM3;
EXPECT_TRUE(trueCubeMM3.SetFromBox(8*mass, 2*size));
EXPECT_EQ(addedCube, math::Inertiald(trueCubeMM3, math::Pose3d::Zero));
EXPECT_EQ(lastCube, addedCube - sevenCubes);
Copy link
Member

Choose a reason for hiding this comment

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

add EXPECT_EQ(sevenCubes, addedCube - lastCube); as well

EXPECT_EQ(sevenCubes, addedCube - lastCube);
}

// Add two cubes with diagonal corners touching at one point
Expand Down Expand Up @@ -506,9 +560,11 @@ TEST(Inertiald_Test, Addition)
gz::math::Vector3d::Zero,
cubeMM3.OffDiagonalMoments());

const math::Inertiald diagonalCubes =
math::Inertiald(cubeMM3, math::Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) +
math::Inertiald(cubeMM3, math::Pose3d(0.5, 0.5, 0.5, 0, 0, 0));
const math::Inertiald cube1 =
math::Inertiald(cubeMM3, math::Pose3d(-0.5, -0.5, -0.5, 0, 0, 0));
const math::Inertiald cube2 =
math::Inertiald(cubeMM3, math::Pose3d(0.5, 0.5, 0.5, 0, 0, 0));
const math::Inertiald diagonalCubes = cube1 + cube2;

// lumped mass = 6 + 6 = 12
// lumped center of mass at (0, 0, 0)
Expand Down Expand Up @@ -539,6 +595,24 @@ TEST(Inertiald_Test, Addition)
EXPECT_EQ(
gz::math::Vector3d(-3, -3, -3),
diagonalCubes.MassMatrix().OffDiagonalMoments());

// -operator
EXPECT_EQ(cube1.Pose(), (diagonalCubes - cube2).Pose());
EXPECT_EQ(cube2.Pose(), (diagonalCubes - cube1).Pose());
EXPECT_DOUBLE_EQ(mass, (diagonalCubes - cube2).MassMatrix().Mass());
EXPECT_DOUBLE_EQ(mass, (diagonalCubes - cube1).MassMatrix().Mass());
EXPECT_EQ(
cubeMM3.DiagonalMoments(),
(diagonalCubes - cube2).MassMatrix().DiagonalMoments());
EXPECT_EQ(
cubeMM3.DiagonalMoments(),
(diagonalCubes - cube1).MassMatrix().DiagonalMoments());
EXPECT_EQ(
cubeMM3.OffDiagonalMoments(),
(diagonalCubes - cube2).MassMatrix().OffDiagonalMoments());
EXPECT_EQ(
cubeMM3.OffDiagonalMoments(),
(diagonalCubes - cube1).MassMatrix().OffDiagonalMoments());
}
}

Expand Down
2 changes: 2 additions & 0 deletions src/python_pybind11/src/Inertial.hh
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,8 @@ void defineMathInertial(py::module &m, const std::string &typestr)
.def(py::self != py::self)
.def(py::self += py::self)
.def(py::self + py::self)
.def(py::self -= py::self)
.def(py::self - py::self)
.def("set_mass_matrix",
&Class::SetMassMatrix,
py::arg("_m") = gz::math::MassMatrix3<T>(),
Expand Down
Loading