From 0127bb8ba61afe5ae7e9cc991c5a140c2a614c49 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 30 May 2024 13:15:05 +0200 Subject: [PATCH] =?UTF-8?q?Added=20Automatic=20Moment=20of=20Inertia=20Cal?= =?UTF-8?q?culations=20for=20Basic=20Shapes=20Pytho=E2=80=A6=20(#1424)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- python/src/sdf/pyBox.cc | 3 + python/src/sdf/pyCapsule.cc | 3 + python/src/sdf/pyCollision.cc | 7 ++ python/src/sdf/pyCylinder.cc | 3 + python/src/sdf/pyEllipsoid.cc | 3 + python/src/sdf/pyGeometry.cc | 3 + python/src/sdf/pyLink.cc | 10 ++ python/src/sdf/pyModel.cc | 2 + python/src/sdf/pyParserConfig.cc | 10 ++ python/src/sdf/pyRoot.cc | 4 +- python/src/sdf/pySphere.cc | 3 + python/src/sdf/pyWorld.cc | 2 + python/test/pyBox_TEST.py | 38 ++++++- python/test/pyCapsule_TEST.py | 54 +++++++-- python/test/pyCollision_TEST.py | 140 ++++++++++++++++++++++- python/test/pyCylinder_TEST.py | 47 +++++++- python/test/pyEllipsoid_TEST.py | 46 +++++++- python/test/pyGeometry_TEST.py | 174 ++++++++++++++++++++++++++++- python/test/pyLink_TEST.py | 172 ++++++++++++++++++++++++++-- python/test/pyParserConfig_TEST.py | 6 +- python/test/pyRoot_TEST.py | 37 +++++- python/test/pySphere_TEST.py | 97 +++++++++++----- python/test/pyWorld_TEST.py | 56 +++++++++- 23 files changed, 846 insertions(+), 74 deletions(-) diff --git a/python/src/sdf/pyBox.cc b/python/src/sdf/pyBox.cc index 31d9d8f08..453438119 100644 --- a/python/src/sdf/pyBox.cc +++ b/python/src/sdf/pyBox.cc @@ -16,6 +16,7 @@ #include "pyBox.hh" #include +#include #include "sdf/Box.hh" @@ -42,6 +43,8 @@ void defineBox(pybind11::object module) pybind11::overload_cast<>(&sdf::Box::Shape, pybind11::const_), pybind11::return_value_policy::reference, "Get a mutable Gazebo Math representation of this Box.") + .def("calculate_inertial", &sdf::Box::CalculateInertial, + "Calculate and return the Inertial values for the Box.") .def("__copy__", [](const sdf::Box &self) { return sdf::Box(self); }) diff --git a/python/src/sdf/pyCapsule.cc b/python/src/sdf/pyCapsule.cc index ac25cd917..50ca4337d 100644 --- a/python/src/sdf/pyCapsule.cc +++ b/python/src/sdf/pyCapsule.cc @@ -16,6 +16,7 @@ #include "pyCapsule.hh" #include +#include #include "sdf/Capsule.hh" @@ -41,6 +42,8 @@ void defineCapsule(pybind11::object module) "Get the capsule's length in meters.") .def("set_length", &sdf::Capsule::SetLength, "Set the capsule's length in meters.") + .def("calculate_inertial", &sdf::Capsule::CalculateInertial, + "Calculate and return the Inertial values for the Capsule.") .def( "shape", pybind11::overload_cast<>(&sdf::Capsule::Shape, pybind11::const_), diff --git a/python/src/sdf/pyCollision.cc b/python/src/sdf/pyCollision.cc index c7305063e..d01f6fba3 100644 --- a/python/src/sdf/pyCollision.cc +++ b/python/src/sdf/pyCollision.cc @@ -17,6 +17,7 @@ #include "pyCollision.hh" #include +#include #include "sdf/Collision.hh" #include "sdf/Geometry.hh" @@ -37,6 +38,12 @@ void defineCollision(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("set_density", &sdf::Collision::SetDensity, "Set the density of the collision.") + .def("density", &sdf::Collision::Density, "Get the density of the collision.") + .def("calculate_inertial", + pybind11::overload_cast( + &sdf::Collision::CalculateInertial), + "Calculate and return the MassMatrix for the collision.") .def("name", &sdf::Collision::Name, "Get the name of the collision. " "The name of the collision must be unique within the scope of a Link.") diff --git a/python/src/sdf/pyCylinder.cc b/python/src/sdf/pyCylinder.cc index 83af7dda4..d8515ddc6 100644 --- a/python/src/sdf/pyCylinder.cc +++ b/python/src/sdf/pyCylinder.cc @@ -16,6 +16,7 @@ #include "pyCylinder.hh" #include +#include #include "sdf/Cylinder.hh" @@ -33,6 +34,8 @@ void defineCylinder(pybind11::object module) pybind11::class_(module, "Cylinder") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Cylinder::CalculateInertial, + "Calculate and return the Inertial values for the Cylinder.") .def("radius", &sdf::Cylinder::Radius, "Get the cylinder's radius in meters.") .def("set_radius", &sdf::Cylinder::SetRadius, diff --git a/python/src/sdf/pyEllipsoid.cc b/python/src/sdf/pyEllipsoid.cc index 89fdb1454..47ea60c96 100644 --- a/python/src/sdf/pyEllipsoid.cc +++ b/python/src/sdf/pyEllipsoid.cc @@ -16,6 +16,7 @@ #include "pyEllipsoid.hh" #include +#include #include "sdf/Ellipsoid.hh" @@ -33,6 +34,8 @@ void defineEllipsoid(pybind11::object module) pybind11::class_(module, "Ellipsoid") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Ellipsoid::CalculateInertial, + "Calculate and return the Inertial values for the Ellipsoid.") .def("radii", &sdf::Ellipsoid::Radii, "Get the ellipsoid's radii in meters.") .def("set_radii", &sdf::Ellipsoid::SetRadii, diff --git a/python/src/sdf/pyGeometry.cc b/python/src/sdf/pyGeometry.cc index 84a146c07..ffcbabbe5 100644 --- a/python/src/sdf/pyGeometry.cc +++ b/python/src/sdf/pyGeometry.cc @@ -17,6 +17,7 @@ #include "pyGeometry.hh" #include +#include #include "sdf/ParserConfig.hh" @@ -45,6 +46,8 @@ void defineGeometry(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Geometry::CalculateInertial, + "Calculate and return the Mass Matrix values for the Geometry") .def("type", &sdf::Geometry::Type, "Get the type of geometry.") .def("set_type", &sdf::Geometry::SetType, diff --git a/python/src/sdf/pyLink.cc b/python/src/sdf/pyLink.cc index 3f77912fa..a101d5512 100644 --- a/python/src/sdf/pyLink.cc +++ b/python/src/sdf/pyLink.cc @@ -41,6 +41,16 @@ void defineLink(pybind11::object module) pybind11::class_(module, "Link") .def(pybind11::init<>()) .def(pybind11::init()) + .def("resolve_auto_inertials", &sdf::Link::ResolveAutoInertials, + "Calculate & set inertial values for the link") + .def("auto_inertia", &sdf::Link::AutoInertia, + "Check if the automatic calculation for the link inertial is enabled or not.") + .def("set_auto_inertia", &sdf::Link::SetAutoInertia, + "Enable automatic inertial calculations by setting autoInertia to true") + .def("auto_inertia_saved", &sdf::Link::AutoInertiaSaved, + "Check if the inertial values for this link were saved.") + .def("set_auto_inertia_saved", &sdf::Link::SetAutoInertiaSaved, + "Set the autoInertiaSaved() values") .def("name", &sdf::Link::Name, "Get the name of the link.") .def("set_name", &sdf::Link::SetName, diff --git a/python/src/sdf/pyModel.cc b/python/src/sdf/pyModel.cc index fce94d0b9..1127945de 100644 --- a/python/src/sdf/pyModel.cc +++ b/python/src/sdf/pyModel.cc @@ -46,6 +46,8 @@ void defineModel(pybind11::object module) }, "Check that the FrameAttachedToGraph and PoseRelativeToGraph " "are valid.") + .def("resolve_auto_inertials", &sdf::Model::ResolveAutoInertials, + "Calculate and set the inertials for all the links belonging to the model object") .def("name", &sdf::Model::Name, "Get the name of model.") .def("set_name", &sdf::Model::SetName, diff --git a/python/src/sdf/pyParserConfig.cc b/python/src/sdf/pyParserConfig.cc index 73d8b1990..2e58422fe 100644 --- a/python/src/sdf/pyParserConfig.cc +++ b/python/src/sdf/pyParserConfig.cc @@ -46,6 +46,12 @@ void defineParserConfig(pybind11::object module) .def("find_file_callback", &sdf::ParserConfig::FindFileCallback, "Get the find file callback function") + .def("calculate_inertial_configuration", + &sdf::ParserConfig::CalculateInertialConfiguration, + "Get the current configuration for the CalculateInertial() function") + .def("set_calculate_inertial_configuration", + &sdf::ParserConfig::SetCalculateInertialConfiguration, + "Set the configuration for the CalculateInertial() function") .def("set_find_callback", &sdf::ParserConfig::SetFindCallback, "Set the callback to use when libsdformat can't find a file.") @@ -96,6 +102,10 @@ void defineParserConfig(pybind11::object module) .value("WARN", sdf::EnforcementPolicy::WARN) .value("LOG", sdf::EnforcementPolicy::LOG); + pybind11::enum_( + module, "ConfigureResolveAutoInertials") + .value("SKIP_CALCULATION_IN_LOAD", sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD) + .value("SAVE_CALCULATION", sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION); } } // namespace python } // namespace SDF_VERSION_NAMESPACE diff --git a/python/src/sdf/pyRoot.cc b/python/src/sdf/pyRoot.cc index 42c21907a..2da83830a 100644 --- a/python/src/sdf/pyRoot.cc +++ b/python/src/sdf/pyRoot.cc @@ -38,6 +38,8 @@ void defineRoot(pybind11::object module) { pybind11::class_(module, "Root") .def(pybind11::init<>()) + .def("resolve_auto_inertials", &sdf::Root::ResolveAutoInertials, + "Calculate and set the inertial properties") .def("load", [](Root &self, const std::string &_filename) { @@ -46,7 +48,7 @@ void defineRoot(pybind11::object module) "Parse the given SDF file, and generate objects based on types " "specified in the SDF file.") .def("load", - [](Root &self, const std::string &_filename, + [](Root &self, const std::string &_filename, const ParserConfig &_config) { ThrowIfErrors(self.Load(_filename, _config)); diff --git a/python/src/sdf/pySphere.cc b/python/src/sdf/pySphere.cc index 06e991df7..ee6197b38 100644 --- a/python/src/sdf/pySphere.cc +++ b/python/src/sdf/pySphere.cc @@ -16,6 +16,7 @@ #include "pySphere.hh" #include +#include #include "sdf/Sphere.hh" @@ -33,6 +34,8 @@ void defineSphere(pybind11::object module) pybind11::class_(module, "Sphere") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Sphere::CalculateInertial, + "Calculate and return the Inertial values for the Sphere.") .def("radius", &sdf::Sphere::Radius, "Get the sphere's radius in meters.") .def("set_radius", &sdf::Sphere::SetRadius, diff --git a/python/src/sdf/pyWorld.cc b/python/src/sdf/pyWorld.cc index 41b930fc4..108708ae4 100644 --- a/python/src/sdf/pyWorld.cc +++ b/python/src/sdf/pyWorld.cc @@ -48,6 +48,8 @@ void defineWorld(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("resolve_auto_inertials", &sdf::World::ResolveAutoInertials, + "Calculate and set the inertials for all the models in the world object") .def("validate_graphs", &sdf::World::ValidateGraphs, "Check that the FrameAttachedToGraph and PoseRelativeToGraph " "are valid.") diff --git a/python/test/pyBox_TEST.py b/python/test/pyBox_TEST.py index d98440de5..84ce40bee 100644 --- a/python/test/pyBox_TEST.py +++ b/python/test/pyBox_TEST.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz_test_deps.math import Vector3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Box import unittest @@ -33,7 +33,7 @@ def test_assignment(self): box = Box() box.set_size(size) - box2 = box; + box2 = box self.assertEqual(size, box2.size()) self.assertEqual(1 * 2 * 3, box2.shape().volume()) @@ -55,5 +55,39 @@ def test_shape(self): box.shape().set_size(Vector3d(1, 2, 3)) self.assertEqual(Vector3d(1, 2, 3), box.size()) + def test_calculate_inertial(self): + box = Box() + density = 2710 + + box.set_size(Vector3d(-1, 1, 0)) + invalidBoxInertial = box.calculate_inertial(density) + self.assertEqual(None, invalidBoxInertial) + + l = 2.0 + w = 2.0 + h = 2.0 + box.set_size(Vector3d(l, w, h)) + + expectedMass = box.shape().volume() * density + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + boxInertial = box.calculate_inertial(density) + self.assertEqual(box.shape().material().density(), density) + self.assertNotEqual(None, boxInertial) + self.assertEqual(expectedInertial, boxInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + boxInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + boxInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), boxInertial.pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py index 52472ed91..976e53c7b 100644 --- a/python/test/pyCapsule_TEST.py +++ b/python/test/pyCapsule_TEST.py @@ -16,6 +16,7 @@ import math +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Capsule import unittest @@ -56,7 +57,7 @@ def test_move_construction(self): def test_copy_construction(self): - capsule = Capsule(); + capsule = Capsule() capsule.set_radius(0.2) capsule.set_length(3.0) @@ -65,29 +66,29 @@ def test_copy_construction(self): self.assertEqual(3.0, capsule2.length()) def test_copy_construction(self): - capsule = Capsule(); + capsule = Capsule() capsule.set_radius(0.2) capsule.set_length(3.0) - capsule2 = copy.deepcopy(capsule); + capsule2 = copy.deepcopy(capsule) self.assertEqual(0.2, capsule2.radius()) self.assertEqual(3.0, capsule2.length()) def test_assignment_after_move(self): - capsule1 = Capsule(); + capsule1 = Capsule() capsule1.set_radius(0.2) capsule1.set_length(3.0) - capsule2 = Capsule(); + capsule2 = Capsule() capsule2.set_radius(2) capsule2.set_length(30.0) # This is similar to what std::swap does except it uses std::move for each # assignment tmp = capsule1 - capsule1 = copy.deepcopy(capsule2); - capsule2 = tmp; + capsule1 = copy.deepcopy(capsule2) + capsule2 = tmp self.assertEqual(2, capsule1.radius()) self.assertEqual(30, capsule1.length()) @@ -97,7 +98,7 @@ def test_assignment_after_move(self): def test_shape(self): - capsule = Capsule(); + capsule = Capsule() self.assertEqual(0.5, capsule.radius()) self.assertEqual(1.0, capsule.length()) @@ -106,6 +107,43 @@ def test_shape(self): self.assertEqual(0.123, capsule.radius()) self.assertEqual(0.456, capsule.length()) + def test_calculate_inertial(self): + capsule = Capsule() + + # density of aluminium + density = 2710 + l = 2.0 + r = 0.1 + + capsule.set_length(l) + capsule.set_radius(r) + + expectedMass = capsule.shape().volume() * density + cylinderVolume = math.pi * r * r * l + sphereVolume = math.pi * 4. / 3. * r * r * r + volume = cylinderVolume + sphereVolume + cylinderMass = expectedMass * cylinderVolume / volume + sphereMass = expectedMass * sphereVolume / volume + + ixxIyy = (1 / 12.0) * cylinderMass * (3 * r * r + l * l) + sphereMass * ( + 0.4 * r * r + 0.375 * r * l + 0.25 * l * l) + izz = r*r * (0.5 * cylinderMass + 0.4 * sphereMass) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixxIyy, ixxIyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + capsuleInertial = capsule.calculate_inertial(density) + self.assertEqual(capsule.shape().material().density(), density) + self.assertNotEqual(None, capsuleInertial) + self.assertEqual(expectedInertial, capsuleInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + capsuleInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + capsuleInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), capsuleInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py index 9e92f521b..8d8d96735 100644 --- a/python/test/pyCollision_TEST.py +++ b/python/test/pyCollision_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz_test_deps.math import Pose3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import (Box, Collision, Contact, Cylinder, Error, - Geometry, Plane, Surface, Sphere, + Geometry, ParserConfig, Plane, Root, Surface, Sphere, SDFErrorsException) import gz_test_deps.sdformat as sdf import unittest @@ -26,6 +26,7 @@ class CollisionTEST(unittest.TestCase): def test_default_construction(self): collision = Collision() self.assertTrue(not collision.name()) + self.assertEqual(collision.density(), 1000.0) collision.set_name("test_collison") self.assertEqual(collision.name(), "test_collison") @@ -40,6 +41,9 @@ def test_default_construction(self): with self.assertRaises(SDFErrorsException): semanticPose.resolve() + collision.set_density(1240.0) + self.assertAlmostEqual(collision.density(), 1240.0) + collision.set_raw_pose(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi)) self.assertEqual(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi), collision.raw_pose()) @@ -130,5 +134,137 @@ def test_set_surface(self): self.assertEqual(collision.surface().contact().collide_bitmask(), 0x2) + def test_incorrect_box_collision_calculate_inertial(self): + collision = Collision() + self.assertAlmostEqual(1000.0, collision.density()) + + # sdf::ElementPtr sdf(new sdf::Element()) + # collision.Load(sdf) + + collisionInertial = Inertiald() + sdfParserConfig = ParserConfig() + geom = Geometry() + box = Box() + + # Invalid Inertial test + box.set_size(Vector3d(-1, 1, 0)) + geom.set_type(sdf.GeometryType.BOX) + geom.set_box_shape(box) + collision.set_geometry(geom) + + errors = [] + + collision.calculate_inertial(errors, collisionInertial, sdfParserConfig) + self.assertFalse(len(errors)) + + + def test_correct_box_collision_calculate_inertial(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + model = root.model() + link = model.link_by_index(0) + collision = link.collision_by_index(0) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * collision.density() + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(len(inertialErr), 0) + self.assertAlmostEqual(1240.0, collision.density()) + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(expectedInertial.mass_matrix(), link.inertial().mass_matrix()) + self.assertEqual(expectedInertial.pose(), link.inertial().pose()) + + def test_calculate_inertial_pose_not_relative_to_link(self): + + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " 0 0 1 0 0 0" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 0 0 -1 0 0 0" + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + # self.assertNotEqual(None, root.Element()) + + model = root.model() + link = model.link_by_index(0) + collision = link.collision_by_index(0) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * collision.density() + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(len(inertialErr), 0) + self.assertAlmostEqual(1240.0, collision.density()) + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(expectedInertial.mass_matrix(), link.inertial().mass_matrix()) + self.assertEqual(expectedInertial.pose(), link.inertial().pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py index 99bcf7823..b5387dd0b 100644 --- a/python/test/pyCylinder_TEST.py +++ b/python/test/pyCylinder_TEST.py @@ -16,6 +16,7 @@ import math +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Cylinder import unittest @@ -64,7 +65,7 @@ def test_assignment(self): def test_copy_construction(self): - cylinder = Cylinder(); + cylinder = Cylinder() cylinder.set_radius(0.2) cylinder.set_length(3.0) @@ -81,11 +82,11 @@ def test_copy_construction(self): self.assertEqual(3.0, cylinder2.length()) def test_deepcopy(self): - cylinder = Cylinder(); + cylinder = Cylinder() cylinder.set_radius(0.2) cylinder.set_length(3.0) - cylinder2 = copy.deepcopy(cylinder); + cylinder2 = copy.deepcopy(cylinder) self.assertEqual(0.2, cylinder2.radius()) self.assertEqual(3.0, cylinder2.length()) @@ -98,7 +99,7 @@ def test_deepcopy(self): self.assertEqual(3.0, cylinder2.length()) def test_shape(self): - cylinder = Cylinder(); + cylinder = Cylinder() self.assertEqual(0.5, cylinder.radius()) self.assertEqual(1.0, cylinder.length()) @@ -107,6 +108,44 @@ def test_shape(self): self.assertEqual(0.123, cylinder.radius()) self.assertEqual(0.456, cylinder.length()) + def test_calculate_interial(self): + cylinder = Cylinder() + + # density of aluminium + density = 2170 + + # Invalid dimensions leading to None return in + # CalculateInertial() + cylinder.set_length(-1) + cylinder.set_radius(0) + invalidCylinderInertial = cylinder.calculate_inertial(density) + self.assertEqual(None, invalidCylinderInertial) + + l = 2.0 + r = 0.1 + + cylinder.set_length(l) + cylinder.set_radius(r) + + expectedMass = cylinder.shape().volume() * density + ixxIyy = (1 / 12.0) * expectedMass * (3 * r * r + l * l) + izz = 0.5 * expectedMass * r * r + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixxIyy, ixxIyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + cylinderInertial = cylinder.calculate_inertial(density) + self.assertEqual(cylinder.shape().mat().density(), density) + self.assertNotEqual(None, cylinderInertial) + self.assertEqual(expectedInertial, cylinderInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + cylinderInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + cylinderInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), cylinderInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py index a102a42ad..c12690ffe 100644 --- a/python/test/pyEllipsoid_TEST.py +++ b/python/test/pyEllipsoid_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz_test_deps.math import Vector3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d import math from gz_test_deps.sdformat import Ellipsoid import unittest @@ -51,7 +51,7 @@ def test_deepcopy(self): def test_deepcopy_after_assignment(self): - ellipsoid1 = Ellipsoid(); + ellipsoid1 = Ellipsoid() expectedradii1 = Vector3d(1.0, 2.0, 3.0) ellipsoid1.set_radii(expectedradii1) @@ -62,8 +62,8 @@ def test_deepcopy_after_assignment(self): # This is similar to what std::swap does except it uses std::move for each # assignment tmp = copy.deepcopy(ellipsoid1) - ellipsoid1 = ellipsoid2; - ellipsoid2 = tmp; + ellipsoid1 = ellipsoid2 + ellipsoid2 = tmp self.assertEqual(expectedradii1, ellipsoid2.shape().radii()) self.assertEqual(expectedradii2, ellipsoid1.shape().radii()) @@ -77,6 +77,44 @@ def test_shape(self): ellipsoid.shape().set_radii(expectedradii) self.assertEqual(expectedradii, ellipsoid.radii()) + def test_calculate_inertial(self): + ellipsoid = Ellipsoid() + + # density of aluminium + density = 2170 + + # Invalid dimensions leading to std::nullopt return in + # CalculateInertial() + ellipsoid.set_radii(Vector3d(-1, 2, 0)) + invalidEllipsoidInertial = ellipsoid.calculate_inertial(density) + self.assertEqual(None, invalidEllipsoidInertial) + + a = 1.0 + b = 10.0 + c = 100.0 + + ellipsoid.set_radii(Vector3d(a, b, c)) + + expectedMass = ellipsoid.shape().volume() * density + ixx = (expectedMass / 5.0) * (b * b + c * c) + iyy = (expectedMass / 5.0) * (a * a + c * c) + izz = (expectedMass / 5.0) * (a * a + b * b) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + ellipsoidInertial = ellipsoid.calculate_inertial(density) + self.assertEqual(ellipsoid.shape().material().density(), density) + self.assertNotEqual(None, ellipsoidInertial) + self.assertEqual(expectedInertial, ellipsoidInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + ellipsoidInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + ellipsoidInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), ellipsoidInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index a1f4ec65b..c4cd9c511 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -13,10 +13,11 @@ # limitations under the License. import copy -from gz_test_deps.sdformat import (Geometry, Box, Capsule, Cylinder, Ellipsoid, - Mesh, Plane, Sphere) -from gz_test_deps.math import Vector3d, Vector2d +from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cylinder, Ellipsoid, + Mesh, ParserConfig, Plane, Sphere) +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d import gz_test_deps.sdformat as sdf +import math import unittest @@ -185,5 +186,172 @@ def test_plane(self): self.assertEqual(Vector2d(9, 8), geom.plane_shape().size()) + def test_calculate_inertial(self): + geom = Geometry() + + # Density of Aluminimum + density = 2170.0 + expectedMass = 0 + expectedMassMat = MassMatrix3d() + expectedInertial = Inertiald() + sdfParserConfig = ParserConfig() + errors = [] + + # Not supported geom type + geom.set_type(sdf.GeometryType.EMPTY) + element = Element() + notSupportedInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + self.assertEqual(notSupportedInertial, None) + + box = Box() + l = 2.0 + w = 2.0 + h = 2.0 + box.set_size(Vector3d(l, w, h)) + + expectedMass = box.shape().volume() * density + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixx, iyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.BOX) + geom.set_box_shape(box) + boxInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, boxInertial) + self.assertEqual(expectedInertial, boxInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), boxInertial.pose()) + + # Capsule + capsule = Capsule() + l = 2.0 + r = 0.1 + capsule.set_length(l) + capsule.set_radius(r) + + expectedMass = capsule.shape().volume() * density + cylinderVolume = math.pi * r * r * l + sphereVolume = math.pi * 4. / 3. * r * r * r + volume = cylinderVolume + sphereVolume + cylinderMass = expectedMass * cylinderVolume / volume + sphereMass = expectedMass * sphereVolume / volume + ixxIyy = (1 / 12.0) * cylinderMass * (3 * r *r + l * l) + sphereMass * ( + 0.4 * r * r + 0.375 * r * l + 0.25 * l * l) + izz = r * r * (0.5 * cylinderMass + 0.4 * sphereMass) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixxIyy, ixxIyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.CAPSULE) + geom.set_capsule_shape(capsule) + capsuleInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, capsuleInertial) + self.assertEqual(expectedInertial, capsuleInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), capsuleInertial.pose()) + + # Cylinder + cylinder = Cylinder() + l = 2.0 + r = 0.1 + + cylinder.set_length(l) + cylinder.set_radius(r) + + expectedMass = cylinder.shape().volume() * density + ixxIyy = (1/12.0) * expectedMass * (3*r*r + l*l) + izz = 0.5 * expectedMass * r * r + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixxIyy, ixxIyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.CYLINDER) + geom.set_cylinder_shape(cylinder) + cylinderInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, cylinderInertial) + self.assertEqual(expectedInertial, cylinderInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), cylinderInertial.pose()) + + # Ellipsoid + ellipsoid = Ellipsoid() + + a = 1.0 + b = 10.0 + c = 100.0 + + ellipsoid.set_radii(Vector3d(a, b, c)) + + expectedMass = ellipsoid.shape().volume() * density + ixx = (expectedMass / 5.0) * (b*b + c*c) + iyy = (expectedMass / 5.0) * (a*a + c*c) + izz = (expectedMass / 5.0) * (a*a + b*b) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixx, iyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.ELLIPSOID) + geom.set_ellipsoid_shape(ellipsoid) + ellipsoidInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, ellipsoidInertial) + self.assertEqual(expectedInertial, ellipsoidInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), ellipsoidInertial.pose()) + + # Sphere + sphere = Sphere() + r = 0.1 + + sphere.set_radius(r) + + expectedMass = sphere.shape().volume() * density + ixxIyyIzz = 0.4 * expectedMass * r * r + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments( + Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.SPHERE) + geom.set_sphere_shape(sphere) + sphereInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, sphereInertial) + self.assertEqual(expectedInertial, sphereInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py index e4ad505a4..0f2c8615f 100644 --- a/python/test/pyLink_TEST.py +++ b/python/test/pyLink_TEST.py @@ -14,8 +14,8 @@ import copy from gz_test_deps.math import Pose3d, Inertiald, MassMatrix3d, Vector3d -from gz_test_deps.sdformat import (Collision, Light, Link, Projector, Sensor, - Visual, SDFErrorsException) +from gz_test_deps.sdformat import (Collision, Light, Link, ParserConfig, Projector, Sensor, + Visual, Root, SDFErrorsException) import unittest import math @@ -61,6 +61,14 @@ def test_default_construction(self): link.set_enable_wind(True) self.assertTrue(link.enable_wind()) + self.assertFalse(link.auto_inertia_saved()) + link.set_auto_inertia_saved(True) + self.assertTrue(link.auto_inertia_saved()) + + self.assertFalse(link.auto_inertia()) + link.set_auto_inertia(True) + self.assertTrue(link.auto_inertia()) + self.assertEqual(0, link.sensor_count()) self.assertEqual(None, link.sensor_by_index(0)) self.assertEqual(None, link.sensor_by_index(1)) @@ -313,8 +321,8 @@ def test_mutable_by_index(self): pr = link.projector_by_index(0) self.assertNotEqual(None, pr) self.assertEqual("projector1", pr.name()) - pr.set_name("projector2"); - self.assertEqual("projector2", link.projector_by_index(0).name()); + pr.set_name("projector2") + self.assertEqual("projector2", link.projector_by_index(0).name()) def test_mutable_by_name(self): link = Link() @@ -376,7 +384,7 @@ def test_mutable_by_name(self): self.assertFalse(link.sensor_name_exists("sensor1")) self.assertTrue(link.sensor_name_exists("sensor2")) - # // Modify the particle emitter + # # Modify the particle emitter # sdf::ParticleEmitter *p = link.ParticleEmitterByName("pe1") # self.assertNotEqual(None, p) # self.assertEqual("pe1", p.name()) @@ -385,12 +393,154 @@ def test_mutable_by_name(self): # self.assertTrue(link.particle_emitter_name_exists("pe2")) # Modify the projector - pr = link.projector_by_name("projector1"); - self.assertNotEqual(None, pr); - self.assertEqual("projector1", pr.name()); - pr.set_name("projector2"); - self.assertFalse(link.projector_name_exists("projector1")); - self.assertTrue(link.projector_name_exists("projector2")); + pr = link.projector_by_name("projector1") + self.assertNotEqual(None, pr) + self.assertEqual("projector1", pr.name()) + pr.set_name("projector2") + self.assertFalse(link.projector_name_exists("projector1")) + self.assertTrue(link.projector_name_exists("projector2")) + + def test_resolveauto_inertialsWithNoCollisionsInLink(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + with self.assertRaises(SDFErrorsException): + errors = root.load_sdf_string(sdf, sdfParserConfig) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Default Inertial values set during load + self.assertEqual(1.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d.ONE, + link.inertial().mass_matrix().diagonal_moments()) + + def test_resolveauto_inertialsWithMultipleCollisions(self): + sdf = "" + \ + "" + \ + " " + \ + " 0 0 1.0 0 0 0" + \ + " " + \ + " " + \ + " " + \ + " 0 0 -0.5 0 0 0" + \ + " 2.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 0 0 0.5 0 0 0" + \ + " 4" + \ + " " + \ + " " + \ + " 0.5" + \ + " 1.0" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Mass of cube(volume * density) + mass of cylinder(volume * density) + expectedMass = 1.0 * 2.0 + math.pi * 0.5 * 0.5 * 1 * 4.0 + + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d(2.013513, 2.013513, 0.72603), + link.inertial().mass_matrix().diagonal_moments()) + + def test_inertial_values_given_with_auto_set_to_true(self): + sdf = "" + \ + "" + \ + " " + \ + " " + \ + " " + \ + " 4.0" + \ + " 1 1 1 2 2 2" + \ + " " + \ + " 1" + \ + " 1" + \ + " 1" + \ + " " + \ + " " + \ + " " + \ + " 2.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + self.assertNotEqual(4.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Pose3d.ZERO, link.inertial().pose()) + self.assertEqual(Vector3d(0.33333, 0.33333, 0.33333), + link.inertial().mass_matrix().diagonal_moments()) + + def test_resolveauto_inertialsCalledWithAutoFalse(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Default Inertial values set during load + self.assertEqual(1.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d.ONE, + link.inertial().mass_matrix().diagonal_moments()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyParserConfig_TEST.py b/python/test/pyParserConfig_TEST.py index a56bfc405..52917dd30 100644 --- a/python/test/pyParserConfig_TEST.py +++ b/python/test/pyParserConfig_TEST.py @@ -26,7 +26,7 @@ def test_construction(self): self.assertFalse(config.uri_path_map()) self.assertFalse(config.find_file_callback()) - testDir = source_file(); + testDir = source_file() config.add_uri_path("file://", testDir) self.assertTrue(config.uri_path_map()) @@ -35,7 +35,7 @@ def test_construction(self): self.assertEqual(it[0], testDir) def testFunc(argument): - return "test/dir2"; + return "test/dir2" config.set_find_callback(testFunc) self.assertTrue(config.find_file_callback()) @@ -102,7 +102,7 @@ def test_copy(self): # so we'll use the source path testDir1 = source_file() - config1 = ParserConfig(); + config1 = ParserConfig() config1.add_uri_path("file://", testDir1) it = config1.uri_path_map().get("file://") diff --git a/python/test/pyRoot_TEST.py b/python/test/pyRoot_TEST.py index 0df1d3023..0a41bf667 100644 --- a/python/test/pyRoot_TEST.py +++ b/python/test/pyRoot_TEST.py @@ -14,7 +14,7 @@ import copy from gz_test_deps.math import Pose3d -from gz_test_deps.sdformat import (Error, Model, Light, Root, SDF_VERSION, +from gz_test_deps.sdformat import (ConfigureResolveAutoInertials, Error, Model, ParserConfig, Light, Root, SDF_VERSION, SDFErrorsException, SDF_PROTOCOL_VERSION, World) import gz_test_deps.sdformat as sdf @@ -121,7 +121,7 @@ def test_string_light_sdf_parse(self): # ///////////////////////////////////////////////// # TEST(DOMRoot, StringActorSdfParse) # { - # std::string sdf = "" + # sdf = "" # " " # " " # " 0 0 1.0 0 0 0" @@ -324,6 +324,39 @@ def test_element_to_light(self): # ASSERT_EQ(None, root2.Actor()) self.assertEqual(0, root2.world_count()) + def test_resolve_auto_inertials_with_save_calculation_configuration(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + model = root.model() + link = model.link_by_index(0) + + sdfParserConfig.set_calculate_inertial_configuration( + ConfigureResolveAutoInertials.SAVE_CALCULATION) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + self.assertEqual(len(inertialErr), 0) + self.assertTrue(link.auto_inertia_saved()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py index 0636d602b..f687f6815 100644 --- a/python/test/pySphere_TEST.py +++ b/python/test/pySphere_TEST.py @@ -15,66 +15,101 @@ import copy import math from gz_test_deps.sdformat import Sphere +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d import unittest class SphereTEST(unittest.TestCase): def test_default_construction(self): - sphere = Sphere(); - self.assertEqual(1.0, sphere.radius()); - sphere.set_radius(0.5); - self.assertEqual(0.5, sphere.radius()); + sphere = Sphere() + self.assertEqual(1.0, sphere.radius()) + sphere.set_radius(0.5) + self.assertEqual(0.5, sphere.radius()) def test_assignment(self): - sphere = Sphere(); - sphere.set_radius(0.2); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = sphere; - self.assertEqual(0.2, sphere2.radius()); + sphere2 = sphere + self.assertEqual(0.2, sphere2.radius()) def test_copy_construction(self): - sphere = Sphere(); - sphere.set_radius(0.2); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = Sphere(sphere); - self.assertEqual(0.2, sphere2.radius()); + sphere2 = Sphere(sphere) + self.assertEqual(0.2, sphere2.radius()) - self.assertEqual(4.0/3.0*math.pi*math.pow(0.2, 3), sphere2.shape().volume()); - self.assertEqual(0.2, sphere2.shape().radius()); + self.assertEqual(4.0/3.0*math.pi*math.pow(0.2, 3), sphere2.shape().volume()) + self.assertEqual(0.2, sphere2.shape().radius()) def test_deepcopy_construction(self): - sphere = Sphere(); - sphere.set_radius(0.2); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = copy.deepcopy(sphere); - self.assertEqual(0.2, sphere2.radius()); + sphere2 = copy.deepcopy(sphere) + self.assertEqual(0.2, sphere2.radius()) def test_deepcopy_after_assignment(self): - sphere1 = Sphere(); - sphere1.set_radius(0.1); + sphere1 = Sphere() + sphere1.set_radius(0.1) - sphere2 = Sphere(); - sphere2.set_radius(0.2); + sphere2 = Sphere() + sphere2.set_radius(0.2) # This is similar to what swap does - tmp = copy.deepcopy(sphere1); - sphere1 = sphere2; - sphere2 = tmp; + tmp = copy.deepcopy(sphere1) + sphere1 = sphere2 + sphere2 = tmp - self.assertEqual(0.2, sphere1.radius()); - self.assertEqual(0.1, sphere2.radius()); + self.assertEqual(0.2, sphere1.radius()) + self.assertEqual(0.1, sphere2.radius()) def test_shape(self): - sphere = Sphere(); - self.assertEqual(1.0, sphere.radius()); + sphere = Sphere() + self.assertEqual(1.0, sphere.radius()) - sphere.shape().set_radius(0.123); - self.assertEqual(0.123, sphere.radius()); + sphere.shape().set_radius(0.123) + self.assertEqual(0.123, sphere.radius()) + + def test_calculate_inertial(self): + sphere = Sphere() + + # density of aluminium + density = 2170 + + sphere.set_radius(-2) + invalidSphereInertial = sphere.calculate_inertial(density) + self.assertEqual(None, invalidSphereInertial) + + r = 0.1 + + sphere.set_radius(r) + + expectedMass = sphere.shape().volume() * density + ixxIyyIzz = 0.4 * expectedMass * r * r + + expectedMassMat = MassMatrix3d( + expectedMass, Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + sphereInertial = sphere.calculate_inertial(density) + self.assertEqual(sphere.shape().material().density(), density) + self.assertNotEqual(None, sphereInertial) + self.assertEqual(expectedInertial, sphereInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + sphereInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + sphereInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) if __name__ == '__main__': diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index f77fcdc8e..f8f5f9d0c 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -14,8 +14,9 @@ import copy from gz_test_deps.math import Color, Vector3d, SphericalCoordinates +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import (Atmosphere, Gui, Physics, Plugin, Error, - Frame, Joint, Light, Model, Scene, World) + Frame, Joint, Light, Model, ParserConfig, Root, Scene, World) import gz_test_deps.sdformat as sdf import unittest import math @@ -87,8 +88,8 @@ def test_default_construction(self): "PoseRelativeToGraph error: scope does not point to a valid graph.") # world doesn't have graphs, so no names should exist in graphs - self.assertFalse(world.name_exists_in_frame_attached_to_graph("")); - self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")); + self.assertFalse(world.name_exists_in_frame_attached_to_graph("")) + self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")) def test_copy_construction(self): @@ -443,6 +444,55 @@ def test_plugins(self): world.clear_plugins() self.assertEqual(0, len(world.plugins())) + def test_resolve_auto_inertials(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + world = root.world_by_index(0) + model = world.model_by_index(0) + link = model.link_by_index(0) + + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * 1240.0 + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(0, len(errors)) + self.assertEqual(expectedInertial, link.inertial()) if __name__ == '__main__': unittest.main()