Add Inertia python bindings (#1388)

python/dartpy/dynamics/BodyNode.cpp

@@ -621,6 +621,11 @@ void BodyNode(py::module& m)
             self->setInertia(inertia);
           },
           ::py::arg("inertia"))
+      .def(
+          "getInertia",
+          +[](const dart::dynamics::BodyNode* self)
+              -> const dart::dynamics::Inertia& { return self->getInertia(); },
+          ::py::return_value_policy::reference_internal)
       .def(
           "setLocalCOM",
           +[](dart::dynamics::BodyNode* self, const Eigen::Vector3d& _com) {
@@ -868,7 +873,10 @@ void BodyNode(py::module& m)
           })
       .def(
           "getChildBodyNode",
-          +[](dart::dynamics::BodyNode* self, std::size_t index) -> dart::dynamics::BodyNode* { return self->getChildBodyNode(index); },
+          +[](dart::dynamics::BodyNode* self,
+              std::size_t index) -> dart::dynamics::BodyNode* {
+            return self->getChildBodyNode(index);
+          },
           ::py::arg("index"),
           ::py::return_value_policy::reference_internal)
       .def(
@@ -878,7 +886,8 @@ void BodyNode(py::module& m)
           })
       .def(
           "getChildJoint",
-          +[](dart::dynamics::BodyNode* self, std::size_t index) -> dart::dynamics::Joint* { return self->getChildJoint(index); },
+          +[](dart::dynamics::BodyNode* self, std::size_t index)
+              -> dart::dynamics::Joint* { return self->getChildJoint(index); },
           ::py::arg("index"),
           ::py::return_value_policy::reference_internal)
       .def(

python/dartpy/dynamics/Inertia.cpp

@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2011-2019, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/master/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dart/dart.hpp>
+#include <pybind11/pybind11.h>
+// #include <pybind11/stl.h>
+#include "eigen_geometry_pybind.h"
+#include "eigen_pybind.h"
+
+namespace py = pybind11;
+
+namespace dart {
+namespace python {
+
+void Inertia(py::module& m)
+{
+  ::py::class_<
+      dart::dynamics::Inertia,
+      std::shared_ptr<dart::dynamics::Inertia>>(m, "Inertia")
+      .def(
+          ::py::init<double, const Eigen::Vector3d&, const Eigen::Matrix3d&>(),
+          ::py::arg_v("mass", 1),
+          ::py::arg_v(
+              "com", Eigen::Vector3d::Zero(), "Eigen::Vector3d::Zero()"),
+          ::py::arg_v(
+              "momentOfInertia",
+              Eigen::Matrix3d::Identity(),
+              "Eigen::Matrix3d::Identity()"))
+      .def(
+          ::py::init<const Eigen::Matrix6d&>(),
+          ::py::arg("spatialInertiaTensor"))
+      .def(
+          "setMass",
+          +[](dart::dynamics::Inertia* self, double mass) {
+            self->setMass(mass);
+          },
+          ::py::arg("mass"))
+      .def(
+          "getMass",
+          +[](const dart::dynamics::Inertia* self) -> double {
+            return self->getMass();
+          })
+      .def(
+          "setLocalCOM",
+          +[](dart::dynamics::Inertia* self, const Eigen::Vector3d& com) {
+            self->setLocalCOM(com);
+          },
+          ::py::arg("com"))
+      .def(
+          "getLocalCOM",
+          +[](const dart::dynamics::Inertia* self) -> const Eigen::Vector3d& {
+            return self->getLocalCOM();
+          },
+          ::py::return_value_policy::reference_internal)
+      .def(
+          "setMoment",
+          +[](dart::dynamics::Inertia* self, const Eigen::Matrix3d& moment) {
+            self->setMoment(moment);
+          },
+          ::py::arg("moment"))
+      .def(
+          "getMoment",
+          +[](const dart::dynamics::Inertia* self) -> Eigen::Matrix3d {
+            return self->getMoment();
+          })
+      .def(
+          "setSpatialTensor",
+          +[](dart::dynamics::Inertia* self, const Eigen::Matrix6d& spatial) {
+            self->setSpatialTensor(spatial);
+          },
+          ::py::arg("spatial"))
+      .def(
+          "getSpatialTensor",
+          +[](const dart::dynamics::Inertia* self) -> const Eigen::Matrix6d& {
+            return self->getSpatialTensor();
+          },
+          ::py::return_value_policy::reference_internal)
+      .def(
+          "verify",
+          +[](const dart::dynamics::Inertia* self,
+              bool printWarnings,
+              double tolerance) -> bool {
+            return self->verify(printWarnings, tolerance);
+          },
+          ::py::arg_v("printWarnings", true),
+          ::py::arg_v("tolerance", 1e-8))
+      .def(
+          "__eq__",
+          +[](const dart::dynamics::Inertia* self,
+              const dart::dynamics::Inertia& other) -> bool {
+            return self && *self == other;
+          })
+      .def_static(
+          "verifyMoment",
+          +[](const Eigen::Matrix3d& moment,
+              bool printWarnings,
+              double tolerance) -> bool {
+            return dart::dynamics::Inertia::verifyMoment(
+                moment, printWarnings, tolerance);
+          },
+          ::py::arg("moment"),
+          ::py::arg_v("printWarnings", true),
+          ::py::arg_v("tolerance", 1e-8))
+      .def_static(
+          "verifySpatialTensor",
+          +[](const Eigen::Matrix6d& spatial,
+              bool printWarnings = true,
+              double tolerance = 1e-8) -> bool {
+            return dart::dynamics::Inertia::verifySpatialTensor(
+                spatial, printWarnings, tolerance);
+          },
+          ::py::arg("spatial"),
+          ::py::arg_v("printWarnings", true),
+          ::py::arg_v("tolerance", 1e-8));
+}
+
+} // namespace python
+} // namespace dart

python/dartpy/dynamics/module.cpp

@@ -74,6 +74,7 @@ void Chain(py::module& sm);
 void Skeleton(py::module& sm);
 
 void InverseKinematics(py::module& sm);
+void Inertia(py::module& sm);
 
 void dart_dynamics(py::module& m)
 {
@@ -116,6 +117,8 @@ void dart_dynamics(py::module& m)
   Skeleton(sm);
 
   InverseKinematics(sm);
+
+  Inertia(sm);
 }
 
 } // namespace python

python/tests/unit/dynamics/test_body_node.py

@@ -25,7 +25,7 @@ def test_basic():
                 dynamics = shape_node.getDynamicsAspect()
 
 
-def testGetChildPMethods():
+def test_get_child_methods():
     urdfParser = dart.utils.DartLoader()
     kr5 = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf")
     assert kr5 is not None
@@ -45,5 +45,15 @@ def testGetChildPMethods():
         currentBodyNode = childBodyNode
 
 
+def test_get_inertia():
+    urdfParser = dart.utils.DartLoader()
+    kr5 = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf")
+    assert kr5 is not None
+
+    inertias = [kr5.getBodyNode(i).getInertia()
+                for i in range(1, kr5.getNumBodyNodes())]
+    assert all([inertia is not None for inertia in inertias])
+
+
 if __name__ == "__main__":
     pytest.main()

python/tests/unit/dynamics/test_inertia.py

@@ -0,0 +1,78 @@
+import platform
+import pytest
+import math
+import numpy as np
+import dartpy as dart
+
+
+def test_inertia_init():
+    ''' 
+    Test basic functionality for the `dartpy.dynamics.Inertia` class.
+    '''
+    # test default values
+    i1 = dart.dynamics.Inertia()
+    assert i1 is not None
+
+    # initialize with parameters
+    i2 = dart.dynamics.Inertia(0.1, [0, 0, 0], 1.3*np.eye(3))
+    assert i1 is not None
+
+    newMass = 1.5
+    i2.setMass(newMass)
+    assert i2.getMass() == newMass
+
+    newCOM = np.array((0.1, 0, 0))
+    i2.setLocalCOM(newCOM)
+    assert np.allclose(i2.getLocalCOM(), newCOM)
+
+    newMoment = 0.4*newMass*0.1**2*np.eye(3)
+    i2.setMoment(newMoment)
+    assert np.allclose(i2.getMoment(), newMoment)
+
+    i2.setSpatialTensor(0.3*i2.getSpatialTensor())
+
+    assert i2.verify()
+
+    for i in range(10):  # based on the C++ tests
+        mass = np.random.uniform(0.1, 10.0)
+        com = np.random.uniform(-5, 5, 3)
+        I = np.random.rand(3, 3) - 0.5 + \
+            np.diag(np.random.uniform(0.6, 1, 3), 0)
+        I = (I + I.T)/2
+
+        inertia = dart.dynamics.Inertia(mass, com, I)
+        assert inertia.verify()
+
+
+def test_inertia_static_methods():
+    '''
+    Test the class methods `verifyMoment`and `verifySpatialTensor`.
+    '''
+    assert dart.dynamics.Inertia.verifyMoment(np.eye(3), printWarnings=False)
+    for i in range(10):
+        I = np.random.rand(3, 3) - 0.5 + \
+            np.diag(np.random.uniform(1, 10, 3), 0)
+        I = (I + I.T)/2
+        assert dart.dynamics.Inertia.verifyMoment(I)
+
+    assert dart.dynamics.Inertia.verifySpatialTensor(
+        np.eye(6), printWarnings=False)
+
+
+def test_failing_moment_and_spatial():
+    '''
+    Test some failure cases of the verify methods.
+    '''
+
+    for i in range(10):
+        I = np.random.rand(3, 3) - 0.5 - \
+            np.diag(np.random.uniform(1, 10, 3), 0)
+        assert not dart.dynamics.Inertia.verifyMoment(I, printWarnings=False)
+
+    # fails e.g. due to off diagonal values in translational part.
+    assert not dart.dynamics.Inertia.verifySpatialTensor(
+        np.random.rand(6, 6), printWarnings=False)
+
+
+if __name__ == "__main__":
+    pytest.main()