diff --git a/python/dartpy/dynamics/BodyNode.cpp b/python/dartpy/dynamics/BodyNode.cpp index dba03af740f66..dc1c27dd211c7 100644 --- a/python/dartpy/dynamics/BodyNode.cpp +++ b/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( diff --git a/python/dartpy/dynamics/Inertia.cpp b/python/dartpy/dynamics/Inertia.cpp new file mode 100644 index 0000000000000..0fc69cc6ae064 --- /dev/null +++ b/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 +#include +// #include +#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>(m, "Inertia") + .def( + ::py::init(), + ::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(), + ::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 diff --git a/python/dartpy/dynamics/module.cpp b/python/dartpy/dynamics/module.cpp index 10f568162789c..1a6ffada94786 100644 --- a/python/dartpy/dynamics/module.cpp +++ b/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 diff --git a/python/tests/unit/dynamics/test_body_node.py b/python/tests/unit/dynamics/test_body_node.py index 0616896c4d5b8..9719400032e53 100644 --- a/python/tests/unit/dynamics/test_body_node.py +++ b/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() diff --git a/python/tests/unit/dynamics/test_inertia.py b/python/tests/unit/dynamics/test_inertia.py new file mode 100644 index 0000000000000..4738463051c9a --- /dev/null +++ b/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()