diff --git a/include/gz/math/Inertial.hh b/include/gz/math/Inertial.hh index 232fb9354..eb24f149f 100644 --- a/include/gz/math/Inertial.hh +++ b/include/gz/math/Inertial.hh @@ -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 &operator-=(const Inertial &_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 ixxyyzz; + Vector3 ixyxzyz; + + // First subtract matrices in base frame + { + auto moi = this->Moi() - _inertial.Moi(); + ixxyyzz = Vector3(moi(0, 0), moi(1, 1), moi(2, 2)); + ixyxzyz = Vector3(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(m1, ixxyyzz, ixyxzyz); + this->pose = Pose3(com1, Quaternion::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. @@ -252,6 +311,16 @@ namespace gz return Inertial(*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 operator-(const Inertial &_inertial) const + { + return Inertial(*this) -= _inertial; + } + /// \brief Mass and inertia matrix of the object expressed in the /// center of mass reference frame. private: MassMatrix3 massMatrix; diff --git a/src/Inertial_TEST.cc b/src/Inertial_TEST.cc index 76a9afbb6..24bbea8c8 100644 --- a/src/Inertial_TEST.cc +++ b/src/Inertial_TEST.cc @@ -366,9 +366,10 @@ 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); @@ -376,11 +377,15 @@ TEST(Inertiald_Test, Addition) 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; @@ -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; @@ -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); @@ -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()); @@ -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 @@ -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) @@ -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()); } } @@ -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()); + } +} diff --git a/src/python_pybind11/src/Inertial.hh b/src/python_pybind11/src/Inertial.hh index ee28f5c1e..7661ce70f 100644 --- a/src/python_pybind11/src/Inertial.hh +++ b/src/python_pybind11/src/Inertial.hh @@ -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(), diff --git a/src/python_pybind11/test/Inertial_TEST.py b/src/python_pybind11/test/Inertial_TEST.py index f63844b3f..b0dfade8e 100644 --- a/src/python_pybind11/test/Inertial_TEST.py +++ b/src/python_pybind11/test/Inertial_TEST.py @@ -278,19 +278,23 @@ def test_diagonalize(self): self.Diagonalize(12, Vector3d(4.125, 5.5, 4.375), Vector3d(-math.sqrt(3), -math.sqrt(3)/2, 3.0)*0.25) - def test_addition(self): - # Add two half-cubes together + def test_addition_subtraction(self): + # Add two half-cubes together and + # Subtract one half-cube from a full cube mass = 12.0 size = Vector3d(1, 1, 1) cubeMM3 = MassMatrix3d() self.assertTrue(cubeMM3.set_from_box(mass, size)) cube = Inertiald(cubeMM3, Pose3d.ZERO) half = MassMatrix3d() - self.assertTrue(half.set_from_box(0.5*mass, Vector3d(0.5, 1, 1))) + half_size = Vector3d(0.5, 1, 1) + self.assertTrue(half.set_from_box(0.5*mass, half_size)) left = Inertiald(half, Pose3d(-0.25, 0, 0, 0, 0, 0)) right = Inertiald(half, Pose3d(0.25, 0, 0, 0, 0, 0)) self.assertEqual(cube, left + right) self.assertEqual(cube, right + left) + self.assertEqual(right, cube - left) + self.assertEqual(left, cube - right) # test += operator tmp = left @@ -303,36 +307,46 @@ def test_addition(self): tmp += left self.assertTrue(cube == tmp) + # test -= operator + tmp = copy.copy(cube) + tmp -= right + self.assertTrue(left == tmp) + + tmp = copy.copy(cube) + tmp -= left + self.assertTrue(right == tmp) + # Test equivalent_box left = Inertiald(half, Pose3d(-0.25, 0, 0, 0, 0, 0)) right = Inertiald(half, Pose3d(0.25, 0, 0, 0, 0, 0)) + size2 = Vector3d() rot2 = Quaterniond() - - # TODO(chapulina) Restore original test after migrating to pybind11 inertial_sum = (left + right) - mm = MassMatrix3d(inertial_sum.mass_matrix().mass(), - inertial_sum.mass_matrix().diagonal_moments(), - inertial_sum.mass_matrix().off_diagonal_moments()) - self.assertTrue(mm.equivalent_box(size2, rot2)) - # self.assertTrue((left + right).mass_matrix().equivalent_box(size2, rot2)) + self.assertTrue((left + right).mass_matrix().equivalent_box(size2, rot2)) self.assertEqual(size, size2) self.assertEqual(rot2, Quaterniond.IDENTITY) size2 = Vector3d() rot2 = Quaterniond() - - # TODO(chapulina) Restore original test after migrating to pybind11 - inertial_sum = (right + left) - mm = MassMatrix3d(inertial_sum.mass_matrix().mass(), - inertial_sum.mass_matrix().diagonal_moments(), - inertial_sum.mass_matrix().off_diagonal_moments()) - self.assertTrue(mm.equivalent_box(size2, rot2)) - # self.assertTrue((right + left).mass_matrix().equivalent_box(size2, rot2)) + self.assertTrue((right + left).mass_matrix().equivalent_box(size2, rot2)) self.assertEqual(size, size2) self.assertEqual(rot2, Quaterniond.IDENTITY) - # Add two rotated half-cubes together + size2 = Vector3d() + rot2 = Quaterniond() + self.assertTrue((cube - right).mass_matrix().equivalent_box(size2, rot2)) + self.assertEqual(half_size, size2) + self.assertEqual(rot2, Quaterniond.IDENTITY) + + size2 = Vector3d() + rot2 = Quaterniond() + self.assertTrue((cube - left).mass_matrix().equivalent_box(size2, rot2)) + self.assertEqual(half_size, size2) + self.assertEqual(rot2, Quaterniond.IDENTITY) + + # Add two rotated half-cubes together and + # Subtract a rotated half-cube from rotated full-cube mass = 12.0 size = Vector3d(1, 1, 1) cubeMM3 = MassMatrix3d() @@ -349,61 +363,67 @@ def test_addition(self): # but mass, center of mass, and base-frame MOI should match self.assertNotEqual(cube, left + right) self.assertNotEqual(cube, right + left) - - # TODO(chapulina) Restore original tests after migrating to pybind11 - inertial_sum = (left + right) - mm = MassMatrix3d(inertial_sum.mass_matrix().mass(), - inertial_sum.mass_matrix().diagonal_moments(), - inertial_sum.mass_matrix().off_diagonal_moments()) - self.assertEqual(cubeMM3.mass(), mm.mass()) - # self.assertEqual(cubeMM3.mass(), (left + right).mass_matrix().mass()) - inertial_sum = (right + left) - mm = MassMatrix3d(inertial_sum.mass_matrix().mass(), - inertial_sum.mass_matrix().diagonal_moments(), - inertial_sum.mass_matrix().off_diagonal_moments()) - self.assertEqual(cubeMM3.mass(), mm.mass()) - # self.assertEqual(cubeMM3.mass(), (right + left).mass_matrix().mass()) + self.assertEqual(cubeMM3.mass(), (left + right).mass_matrix().mass()) + self.assertEqual(cubeMM3.mass(), (right + left).mass_matrix().mass()) self.assertEqual(cube.pose().pos(), (left + right).pose().pos()) self.assertEqual(cube.pose().pos(), (right + left).pose().pos()) self.assertEqual(cube.moi(), (left + right).moi()) self.assertEqual(cube.moi(), (right + left).moi()) - + # -operator + self.assertNotEqual(left, cube - right) + self.assertNotEqual(right, cube - left) + self.assertEqual(left.mass_matrix().mass(), (cube - right).mass_matrix().mass()) + self.assertEqual(right.mass_matrix().mass(), (cube - left).mass_matrix().mass()) + self.assertEqual(left.pose().pos(), (cube - right).pose().pos()) + self.assertEqual(right.pose().pos(), (cube - left).pose().pos()) + self.assertEqual(left.moi(), (cube - right).moi()) + self.assertEqual(right.moi(), (cube - left).moi()) + + # Add eight cubes together into larger cube and + # Subtract seven cubes from larger cube mass = 12.0 size = Vector3d(1, 1, 1) cubeMM3 = MassMatrix3d() self.assertTrue(cubeMM3.set_from_box(mass, size)) - addedCube = Inertiald( + sevenCubes = Inertiald( Inertiald(cubeMM3, Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) + Inertiald(cubeMM3, Pose3d(-0.5, 0.5, -0.5, 0, 0, 0)) + Inertiald(cubeMM3, Pose3d(0.5, -0.5, -0.5, 0, 0, 0)) + Inertiald(cubeMM3, Pose3d(0.5, 0.5, -0.5, 0, 0, 0)) + Inertiald(cubeMM3, Pose3d(-0.5, -0.5, 0.5, 0, 0, 0)) + Inertiald(cubeMM3, Pose3d(-0.5, 0.5, 0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, -0.5, 0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, 0.5, 0.5, 0, 0, 0))) + Inertiald(cubeMM3, Pose3d(0.5, -0.5, 0.5, 0, 0, 0))) + lastCube = Inertiald(cubeMM3, Pose3d(0.5, 0.5, 0.5, 0, 0, 0)) + addedCube = sevenCubes + lastCube trueCubeMM3 = MassMatrix3d() self.assertTrue(trueCubeMM3.set_from_box(mass * 8, size * 2)) self.assertEqual(addedCube, Inertiald(trueCubeMM3, Pose3d.ZERO)) + self.assertEqual(lastCube, addedCube - sevenCubes) + self.assertEqual(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 mass = 12.0 size = Vector3d(1, 1, 1) cubeMM3 = MassMatrix3d() self.assertTrue(cubeMM3.set_from_box(mass, size)) - addedCube = Inertiald( + sevenCubes = Inertiald( Inertiald(cubeMM3, Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) + Inertiald(cubeMM3, Pose3d(-0.5, 0.5, -0.5, math.pi/2, 0, 0)) + Inertiald(cubeMM3, Pose3d(0.5, -0.5, -0.5, 0, math.pi/2, 0)) + Inertiald(cubeMM3, Pose3d(0.5, 0.5, -0.5, 0, 0, math.pi/2)) + Inertiald(cubeMM3, Pose3d(-0.5, -0.5, 0.5, math.pi, 0, 0)) + Inertiald(cubeMM3, Pose3d(-0.5, 0.5, 0.5, 0, math.pi, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, -0.5, 0.5, 0, 0, math.pi)) + - Inertiald(cubeMM3, Pose3d(0.5, 0.5, 0.5, 0, 0, 0))) + Inertiald(cubeMM3, Pose3d(0.5, -0.5, 0.5, 0, 0, math.pi))) + lastCube = Inertiald(cubeMM3, Pose3d(0.5, 0.5, 0.5, 0, 0, 0)) + addedCube = sevenCubes + lastCube trueCubeMM3 = MassMatrix3d() self.assertTrue(trueCubeMM3.set_from_box(mass * 8, size * 2)) self.assertEqual(addedCube, Inertiald(trueCubeMM3, Pose3d.ZERO)) + self.assertEqual(lastCube, addedCube - sevenCubes) + self.assertEqual(sevenCubes, addedCube - lastCube) # Add two cubes with diagonal corners touching at one point # ┌---------┐ @@ -434,9 +454,9 @@ def test_addition(self): Vector3d.ZERO, cubeMM3.off_diagonal_moments()) - diagonalCubes = Inertiald( - Inertiald(cubeMM3, Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, 0.5, 0.5, 0, 0, 0))) + cube1 = Inertiald(cubeMM3, Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) + cube2 = Inertiald(cubeMM3, Pose3d(0.5, 0.5, 0.5, 0, 0, 0)) + diagonalCubes = cube1 + cube2 # lumped mass = 6 + 6 = 12 # lumped center of mass at (0, 0, 0) @@ -467,6 +487,23 @@ def test_addition(self): self.assertEqual( Vector3d(-3, -3, -3), diagonalCubes.mass_matrix().off_diagonal_moments()) + # -operator + self.assertEqual(cube1.pose(), (diagonalCubes - cube2).pose()) + self.assertEqual(cube2.pose(), (diagonalCubes - cube1).pose()) + self.assertEqual(mass, (diagonalCubes - cube2).mass_matrix().mass()) + self.assertEqual(mass, (diagonalCubes - cube1).mass_matrix().mass()) + self.assertEqual( + cubeMM3.diagonal_moments(), + (diagonalCubes - cube2).mass_matrix().diagonal_moments()) + self.assertEqual( + cubeMM3.diagonal_moments(), + (diagonalCubes - cube1).mass_matrix().diagonal_moments()) + self.assertEqual( + cubeMM3.off_diagonal_moments(), + (diagonalCubes - cube2).mass_matrix().off_diagonal_moments()) + self.assertEqual( + cubeMM3.off_diagonal_moments(), + (diagonalCubes - cube1).mass_matrix().off_diagonal_moments()) def test_addition_invalid(self): # inertias all zero @@ -511,12 +548,55 @@ def test_addition_invalid(self): tmp += i self.assertEqual(tmp, i) - # TODO(chapulina) Fix tests after migrating to pybind11 - # self.assertTrue((i + i0).mass_matrix().is_positive()) - # self.assertTrue((i0 + i).mass_matrix().is_positive()) - # self.assertTrue((i + i0).mass_matrix().is_valid()) - # self.assertTrue((i0 + i).mass_matrix().is_valid()) + self.assertTrue((i + i0).mass_matrix().is_positive()) + self.assertTrue((i0 + i).mass_matrix().is_positive()) + self.assertTrue((i + i0).mass_matrix().is_valid()) + self.assertTrue((i0 + i).mass_matrix().is_valid()) + def test_subtraction_invalid(self): + mass = 12.0 + m1 = MassMatrix3d() + m2 = MassMatrix3d() + self.assertTrue(m1.set_from_box(0.5*mass, Vector3d(0.5, 1, 1))) + self.assertTrue(m1.is_positive()) + self.assertTrue(m1.is_valid()) + self.assertTrue(m2.set_from_box(0.5*mass, Vector3d(0.5, 0.25, 0.25))) + self.assertTrue(m2.is_positive()) + self.assertTrue(m2.is_valid()) + + # two inertials with i2 having higher mass than i1 + i1 = Inertiald(m1, Pose3d(-0.25, 0, 0, 0, 0, 0)) + i2 = Inertiald(m2, Pose3d(0.25, 0, 0, 0, 0, 0)) + + # expect subtraction to equal left argument + self.assertEqual(i1, i1 - i2) + tmp = copy.copy(i1) + tmp -= i2 + self.assertEqual(tmp, i1) + + # one inertial with zero inertias should not affect the subtraction + m1 = MassMatrix3d(mass, Vector3d(2, 3, 4), Vector3d(0.1, 0.2, 0.3)) + m2 = MassMatrix3d(0.0, Vector3d.ZERO, Vector3d.ZERO) + self.assertTrue(m1.is_positive()) + self.assertTrue(m1.is_valid()) + self.assertFalse(m2.is_positive()) + self.assertTrue(m2.is_near_positive()) + self.assertTrue(m2.is_valid()) + + # i2 with zero inertia + i1 = Inertiald(m1, Pose3d(-1, 0, 0, 0, 0, 0)) + i2 = Inertiald(m2, Pose3d(1, 0, 0, 0, 0, 0)) + + # expect i2 to not affect the subtraction + self.assertEqual(i1, i1 - i2) + tmp = copy.copy(i1) + tmp -= i2 + self.assertEqual(tmp, i1) + + self.assertTrue((i1 - i2).mass_matrix().is_positive()) + self.assertFalse((i2 - i1).mass_matrix().is_positive()) + self.assertTrue((i1 - i2).mass_matrix().is_valid()) + self.assertTrue((i2 - i1).mass_matrix().is_valid()) if __name__ == '__main__': unittest.main() diff --git a/src/ruby/Inertial.i b/src/ruby/Inertial.i index a514ac4c4..49b4c34d5 100644 --- a/src/ruby/Inertial.i +++ b/src/ruby/Inertial.i @@ -61,7 +61,11 @@ namespace gz public: Inertial &operator+=(const Inertial &_inertial); + public: Inertial &operator-=(const Inertial &_inertial); + public: const Inertial operator+(const Inertial &_inertial) const; + + public: const Inertial operator-(const Inertial &_inertial) const; }; %template(Inertiald) Inertial;