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

Jump to bottom

Subtraction operator for Inertial class #432

Merged
merged 14 commits into from
Jun 2, 2022
Merged

Subtraction operator for Inertial class #432

Show file tree
Hide file tree
Changes from 13 commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
f8e9375
Added initial work on subtraction operator
deepanshubansal01 May 24, 2022
e36c314
add + operator inside ruby file
deepanshubansal01 May 24, 2022
a52d5bb
unit test subtract operator with two half cubes
deepanshubansal01 May 25, 2022
5d978f6
unit test two rotated half cubes
deepanshubansal01 May 25, 2022
fc2b01d
piggy bank on +operator for -operator
deepanshubansal01 May 25, 2022
0ed01be
implement -=operator separately instead of piggybanking to avoid conf…
deepanshubansal01 May 25, 2022
8508889
add -operator unit test along with +operator
deepanshubansal01 May 25, 2022
286086c
add remaining unit tests for -operator
deepanshubansal01 May 25, 2022
7689ede
make codecheck
deepanshubansal01 May 25, 2022
48cc689
correct unit test bug
deepanshubansal01 May 26, 2022
9b7f131
added python binding and unit test
deepanshubansal01 May 26, 2022
3ffe8eb
Merge branch 'main' into deepanshu/inertial-subtract-operator
adityapande-1995 May 31, 2022
fb94579
code review changes
deepanshubansal01 Jun 1, 2022
0142b93
added subtraction invalid unit test
deepanshubansal01 Jun 1, 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
69 changes: 69 additions & 0 deletions include/gz/math/Inertial.hh
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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