Skip to content

Commit

Permalink
Subtraction operator for Inertial class (#432)
Browse files Browse the repository at this point in the history 
* Added subtraction operator for inertial class
Signed-off-by: deepanshu <[email protected]>

* unit tests subtract operator

Signed-off-by: deepanshu <[email protected]>

* add -operator unit test along with +operator

Signed-off-by: deepanshu <[email protected]>

* add remaining unit tests for -operator

Signed-off-by: deepanshu <[email protected]>

* make codecheck

Signed-off-by: deepanshu <[email protected]>

* added python binding and unit test

Signed-off-by: deepanshu <[email protected]>

* InvalidSubtraction unit test

Signed-off-by: deepanshu <[email protected]>

Co-authored-by: Aditya Pande <[email protected]>
  • Loading branch information
@deepanshubansal01 @adityapande-1995
deepanshubansal01 and adityapande-1995 authored Jun 2, 2022
1 parent d6284a5 commit b250500
Show file tree
Hide file tree
Showing 5 changed files with 349 additions and 62 deletions.
69 changes: 69 additions & 0 deletions include/gz/math/Inertial.hh
View file
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;
}

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
158 changes: 145 additions & 13 deletions src/Inertial_TEST.cc
View file
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);
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);
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 Expand Up @@ -603,3 +677,61 @@ TEST(Inertiald_Test, AdditionInvalid)
EXPECT_TRUE((i0 + i).MassMatrix().IsValid());
}
}

TEST(Inertiald_Test, SubtractionInvalid)
{
const double mass = 12.0;
{
math::MassMatrix3d m1, m2;
EXPECT_TRUE(m1.SetFromBox(0.5*mass, math::Vector3d(0.5, 1, 1)));
EXPECT_TRUE(m1.IsPositive());
EXPECT_TRUE(m1.IsValid());
EXPECT_TRUE(m2.SetFromBox(mass, math::Vector3d(0.5, 0.25, 0.25)));
EXPECT_TRUE(m2.IsValid());
EXPECT_TRUE(m2.IsPositive());

// two inertials with i2 having higher mass than i1
math::Inertiald i1(m1, math::Pose3d(-0.25, 0, 0, 0, 0, 0));
math::Inertiald i2(m2, math::Pose3d(0.25, 0, 0, 0, 0, 0));

// expect subtraction to equal left argument
EXPECT_EQ(i1, i1 - i2);
{
math::Inertiald tmp = i1;
tmp -= i2;
EXPECT_EQ(tmp, i1);
}
}

// one inertial with zero inertias should not affect the subtraction
{
const math::MassMatrix3d m1(mass,
math::Vector3d(2, 3, 4),
math::Vector3d(0.1, 0.2, 0.3));
EXPECT_TRUE(m1.IsPositive());
EXPECT_TRUE(m1.IsValid());

const math::MassMatrix3d m2(0.0,
math::Vector3d::Zero, math::Vector3d::Zero);
EXPECT_FALSE(m2.IsPositive());
EXPECT_TRUE(m2.IsNearPositive());
EXPECT_TRUE(m2.IsValid());

// i2 with zero inertia
math::Inertiald i1(m1, math::Pose3d(-1, 0, 0, 0, 0, 0));
math::Inertiald i2(m2, math::Pose3d(1, 0, 0, 0, 0, 0));

// expect i2 to not affect the subtraction
EXPECT_EQ(i1, i1 - i2);
{
math::Inertiald tmp = i1;
tmp -= i2;
EXPECT_EQ(tmp, i1);
}

EXPECT_TRUE((i1 - i2).MassMatrix().IsPositive());
EXPECT_FALSE((i2 - i1).MassMatrix().IsPositive());
EXPECT_TRUE((i1 - i2).MassMatrix().IsValid());
EXPECT_TRUE((i2 - i1).MassMatrix().IsValid());
}
}
2 changes: 2 additions & 0 deletions src/python_pybind11/src/Inertial.hh
View file
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

0 comments on commit b250500

Please sign in to comment.