Added Automatic Moment of Inertia Calculations for Basic Shapes Pytho… (

#1424)

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

python/src/sdf/pyBox.cc

@@ -16,6 +16,7 @@
 #include "pyBox.hh"
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 
 #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);
     })

python/src/sdf/pyCapsule.cc

@@ -16,6 +16,7 @@
 #include "pyCapsule.hh"
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 
 #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_),

python/src/sdf/pyCollision.cc

@@ -17,6 +17,7 @@
 #include "pyCollision.hh"
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 
 #include "sdf/Collision.hh"
 #include "sdf/Geometry.hh"
@@ -37,6 +38,12 @@ void defineCollision(pybind11::object module)
   geometryModule
     .def(pybind11::init<>())
     .def(pybind11::init<sdf::Collision>())
+    .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::Errors &, gz::math::Inertiald &, const ParserConfig &>(
+          &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.")

python/src/sdf/pyCylinder.cc

@@ -16,6 +16,7 @@
 #include "pyCylinder.hh"
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 
 #include "sdf/Cylinder.hh"
 
@@ -33,6 +34,8 @@ void defineCylinder(pybind11::object module)
   pybind11::class_<sdf::Cylinder>(module, "Cylinder")
     .def(pybind11::init<>())
     .def(pybind11::init<sdf::Cylinder>())
+    .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,

python/src/sdf/pyEllipsoid.cc

@@ -16,6 +16,7 @@
 #include "pyEllipsoid.hh"
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 
 #include "sdf/Ellipsoid.hh"
 
@@ -33,6 +34,8 @@ void defineEllipsoid(pybind11::object module)
   pybind11::class_<sdf::Ellipsoid>(module, "Ellipsoid")
     .def(pybind11::init<>())
     .def(pybind11::init<sdf::Ellipsoid>())
+    .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,

python/src/sdf/pyGeometry.cc

@@ -17,6 +17,7 @@
 #include "pyGeometry.hh"
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 
 #include "sdf/ParserConfig.hh"
 
@@ -45,6 +46,8 @@ void defineGeometry(pybind11::object module)
   geometryModule
     .def(pybind11::init<>())
     .def(pybind11::init<sdf::Geometry>())
+    .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,

python/src/sdf/pyLink.cc

@@ -41,6 +41,16 @@ void defineLink(pybind11::object module)
   pybind11::class_<sdf::Link>(module, "Link")
     .def(pybind11::init<>())
     .def(pybind11::init<sdf::Link>())
+    .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,

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,

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_<sdf::ConfigureResolveAutoInertials>(
+    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

python/src/sdf/pyRoot.cc

@@ -38,6 +38,8 @@ void defineRoot(pybind11::object module)
 {
   pybind11::class_<sdf::Root>(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));

python/src/sdf/pySphere.cc

@@ -16,6 +16,7 @@
 #include "pySphere.hh"
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 
 #include "sdf/Sphere.hh"
 
@@ -33,6 +34,8 @@ void defineSphere(pybind11::object module)
   pybind11::class_<sdf::Sphere>(module, "Sphere")
     .def(pybind11::init<>())
     .def(pybind11::init<sdf::Sphere>())
+    .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,

python/src/sdf/pyWorld.cc

@@ -48,6 +48,8 @@ void defineWorld(pybind11::object module)
   geometryModule
     .def(pybind11::init<>())
     .def(pybind11::init<sdf::World>())
+    .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.")

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()

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()