From 3691e7aee77f64a1a2d0e08eee1d40e3d370da0a Mon Sep 17 00:00:00 2001 From: Francesco Romano Date: Mon, 27 Jun 2022 19:21:02 +0200 Subject: [PATCH 1/4] Expose pybind11 for ModelLoader. --- bindings/pybind11/idyntree_modelio_urdf.cpp | 16 +++++ .../tests/test_idyntree_modelio_urdf.py | 68 +++++++++++++++++++ 2 files changed, 84 insertions(+) diff --git a/bindings/pybind11/idyntree_modelio_urdf.cpp b/bindings/pybind11/idyntree_modelio_urdf.cpp index e5d99bd1488..f4fe94257fa 100644 --- a/bindings/pybind11/idyntree_modelio_urdf.cpp +++ b/bindings/pybind11/idyntree_modelio_urdf.cpp @@ -3,7 +3,10 @@ #include "error_utilities.h" #include +#include +#include #include +#include #include #include @@ -58,6 +61,19 @@ void iDynTreeModelIoUrdfBindings(pybind11::module& module) { .def_property_readonly("sensors", &ModelExporter::sensors) .def_property_readonly("options", &ModelExporter::exportingOptions) .def("export_model_to_string", &ExporterHelper::exportAsString); + + py::class_(module, "ModelLoader") + .def(py::init()) + .def("load_model_from_file", + [](ModelLoader* this_, const std::string& filename) { + return this_->loadModelFromFile(filename); + }) + .def("load_reduced_model_from_full_model", + [](ModelLoader* this_, const Model& model, + const std::vector& joints) { + return this_->loadReducedModelFromFullModel(model, joints); + }) + .def_property_readonly("model", &ModelLoader::model); } } // namespace bindings diff --git a/bindings/pybind11/tests/test_idyntree_modelio_urdf.py b/bindings/pybind11/tests/test_idyntree_modelio_urdf.py index 1f97d416c7e..89bdc9dfcbf 100644 --- a/bindings/pybind11/tests/test_idyntree_modelio_urdf.py +++ b/bindings/pybind11/tests/test_idyntree_modelio_urdf.py @@ -1,4 +1,6 @@ """Tests for idyntree-model-io-urdf Python bindings.""" +import os +import tempfile import unittest import idyntree.pybind as iDynTree @@ -52,6 +54,72 @@ def test_custom_sensors(self): self.assertEqual( exporter.sensors.get_nr_of_sensors(iDynTree.ACCELEROMETER), 1) + def test_importer(self): + model = (r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'') + + tmpdir = tempfile.TemporaryDirectory() + urdf_filename = os.path.join(tmpdir.name, 'model.urdf') + with open(urdf_filename, 'w') as f: + f.write(model) + + importer = iDynTree.ModelLoader() + importer.load_model_from_file(urdf_filename) + self.assertEqual(4, importer.model.get_nr_of_links()) + self.assertEqual(3, importer.model.get_nr_of_joints()) + + def test_importer_reduced_model(self): + model = (r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'' + r'') + + tmpdir = tempfile.TemporaryDirectory() + urdf_filename = os.path.join(tmpdir.name, 'model.urdf') + with open(urdf_filename, 'w') as f: + f.write(model) + + importer = iDynTree.ModelLoader() + importer.load_model_from_file(urdf_filename) + importer.load_reduced_model_from_full_model(importer.model, + ('joint1', 'joint3')) + # The reduced model will have the 2 joints we specified, and one less body + # as the missing joint made two bodies being merged together. + self.assertEqual(3, importer.model.get_nr_of_links()) + self.assertEqual(2, importer.model.get_nr_of_joints()) + self.assertEqual('joint1', importer.model.get_joint_name(0)) + self.assertEqual('joint3', importer.model.get_joint_name(1)) + if __name__ == "__main__": unittest.main() From 519461dd2a2d5ad1d11d52122ae52e3fc483554c Mon Sep 17 00:00:00 2001 From: Francesco Romano Date: Tue, 28 Jun 2022 11:10:03 +0200 Subject: [PATCH 2/4] Add Twist object to pybind bindings. --- bindings/pybind11/idyntree_core.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/bindings/pybind11/idyntree_core.cpp b/bindings/pybind11/idyntree_core.cpp index 985f3a70462..aeb18f5de39 100644 --- a/bindings/pybind11/idyntree_core.cpp +++ b/bindings/pybind11/idyntree_core.cpp @@ -11,12 +11,14 @@ #include #include #include +#include #include #include #include #include #include +#include #include namespace iDynTree { @@ -269,6 +271,15 @@ void iDynTreeCoreBindings(pybind11::module& module) { .def("__repr__", [](const SpatialInertia& inertia) { return inertia.asMatrix().toString(); }); + + // Basic twist interface. + py::class_(module, "Twist") + .def(py::init([](const std::array& lin_velocity, + const std::array& ang_velocity) { + LinVelocity lin_vel(lin_velocity.data(), 3); + AngVelocity ang_vel(ang_velocity.data(), 3); + return Twist(lin_vel, ang_vel); + })); } } // namespace bindings } // namespace iDynTree From e66ea60667028917aaf8c82ca68055436ca85f88 Mon Sep 17 00:00:00 2001 From: Francesco Romano Date: Tue, 28 Jun 2022 10:50:16 +0200 Subject: [PATCH 3/4] Add kinematics support in pybind bindings. --- bindings/pybind11/CMakeLists.txt | 3 +- bindings/pybind11/idyntree.cpp | 2 + bindings/pybind11/idyntree_high_level.cpp | 63 +++++++ bindings/pybind11/idyntree_high_level.h | 14 ++ bindings/pybind11/tests/CMakeLists.txt | 2 +- .../tests/test_idyntree_high_level.py | 174 ++++++++++++++++++ .../include/iDynTree/KinDynComputations.h | 2 +- 7 files changed, 257 insertions(+), 3 deletions(-) create mode 100644 bindings/pybind11/idyntree_high_level.cpp create mode 100644 bindings/pybind11/idyntree_high_level.h create mode 100644 bindings/pybind11/tests/test_idyntree_high_level.py diff --git a/bindings/pybind11/CMakeLists.txt b/bindings/pybind11/CMakeLists.txt index 5697f68fea3..3b2a5f09a74 100644 --- a/bindings/pybind11/CMakeLists.txt +++ b/bindings/pybind11/CMakeLists.txt @@ -4,7 +4,8 @@ pybind11_add_module(pybind11_idyntree idyntree.cpp error_utilities.h error_utilities.cpp idyntree_model.h idyntree_model.cpp idyntree_sensors.h idyntree_sensors.cpp - idyntree_modelio_urdf.h idyntree_modelio_urdf.cpp) + idyntree_modelio_urdf.h idyntree_modelio_urdf.cpp + idyntree_high_level.h idyntree_high_level.cpp) target_link_libraries(pybind11_idyntree PUBLIC idyntree-core idyntree-model diff --git a/bindings/pybind11/idyntree.cpp b/bindings/pybind11/idyntree.cpp index fe133219b90..e8f0b4069fd 100644 --- a/bindings/pybind11/idyntree.cpp +++ b/bindings/pybind11/idyntree.cpp @@ -1,6 +1,7 @@ #include #include "idyntree_core.h" +#include "idyntree_high_level.h" #include "idyntree_model.h" #include "idyntree_modelio_urdf.h" #include "idyntree_sensors.h" @@ -17,6 +18,7 @@ PYBIND11_MODULE(pybind, m) { iDynTree::bindings::iDynTreeModelBindings(m); iDynTree::bindings::iDynTreeSensorsBindings(m); iDynTree::bindings::iDynTreeModelIoUrdfBindings(m); + iDynTree::bindings::iDynTreeHighLevelBindings(m); } } // namespace diff --git a/bindings/pybind11/idyntree_high_level.cpp b/bindings/pybind11/idyntree_high_level.cpp new file mode 100644 index 00000000000..c6abbfe0119 --- /dev/null +++ b/bindings/pybind11/idyntree_high_level.cpp @@ -0,0 +1,63 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace iDynTree { +namespace bindings { +namespace { +namespace py = ::pybind11; + +} // namespace + +void iDynTreeHighLevelBindings(pybind11::module& module) { + py::class_(module, "KinDynComputations") + .def(py::init()) + .def("load_robot_model", &KinDynComputations::loadRobotModel) + // Model inspection functions. + .def("get_nr_of_degrees_of_freedom", + &KinDynComputations::getNrOfDegreesOfFreedom) + .def("get_nr_of_links", &KinDynComputations::getNrOfLinks) + .def("get_nr_of_frames", &KinDynComputations::getNrOfFrames) + .def("set_floating_base", &KinDynComputations::setFloatingBase) + .def("get_floating_base", &KinDynComputations::getFloatingBase) + // Model state functions. + .def("set_robot_state", + py::overload_cast< + const iDynTree::Transform&, const iDynTree::VectorDynSize&, + const iDynTree::Twist&, const iDynTree::VectorDynSize&, + const iDynTree::Vector3&>(&KinDynComputations::setRobotState)) + .def("set_fixed_base_robot_state", + py::overload_cast( + &KinDynComputations::setRobotState)) + .def("set_joint_positions", + py::overload_cast( + &KinDynComputations::setJointPos)) + // + .def("get_frame_index", &KinDynComputations::getFrameIndex) + .def("get_frame_name", &KinDynComputations::getFrameName) + .def("get_world_transform", py::overload_cast( + &KinDynComputations::getWorldTransform)) + .def("get_world_transform_for_frame_named", + py::overload_cast( + &KinDynComputations::getWorldTransform)) + .def("get_relative_transform", + py::overload_cast( + &KinDynComputations::getRelativeTransform)) + .def("get_relative_transform_for_frames_named", + py::overload_cast( + &KinDynComputations::getRelativeTransform)) + .def("get_relative_transform_explicit", + py::overload_cast( + &KinDynComputations::getRelativeTransformExplicit)); +} +} // namespace bindings +} // namespace iDynTree diff --git a/bindings/pybind11/idyntree_high_level.h b/bindings/pybind11/idyntree_high_level.h new file mode 100644 index 00000000000..261d3a82a74 --- /dev/null +++ b/bindings/pybind11/idyntree_high_level.h @@ -0,0 +1,14 @@ +#ifndef IDYNTREE_PYBIND11_HIGH_LEVEL_H +#define IDYNTREE_PYBIND11_HIGH_LEVEL_H + +#include + + +namespace iDynTree { +namespace bindings { +void iDynTreeHighLevelBindings(pybind11::module& module); + +} // namespace bindings +} // namespace iDynTree + +#endif /* end of include guard: IDYNTREE_PYBIND11_HIGH_LEVEL_H */ diff --git a/bindings/pybind11/tests/CMakeLists.txt b/bindings/pybind11/tests/CMakeLists.txt index dee072d2765..e258699ce61 100644 --- a/bindings/pybind11/tests/CMakeLists.txt +++ b/bindings/pybind11/tests/CMakeLists.txt @@ -1,4 +1,4 @@ add_test (NAME pybind11_idyntree_test - COMMAND ${Python3_EXECUTABLE} -m unittest discover -s ${CMAKE_CURRENT_SOURCE_DIR} + COMMAND ${Python3_EXECUTABLE} -B -m unittest discover -s ${CMAKE_CURRENT_SOURCE_DIR} WORKING_DIRECTORY ${PROJECT_BINARY_DIR}/pybind11 ) diff --git a/bindings/pybind11/tests/test_idyntree_high_level.py b/bindings/pybind11/tests/test_idyntree_high_level.py new file mode 100644 index 00000000000..508851aa4cc --- /dev/null +++ b/bindings/pybind11/tests/test_idyntree_high_level.py @@ -0,0 +1,174 @@ +"""Tests for idyntree-sensors Python bindings.""" +import itertools +import math +import unittest + +import idyntree.pybind as iDynTree +import numpy as np + + +class IDynTreeHighLevelTest(unittest.TestCase): + + def _transform_equality_func(self, + expected: iDynTree.Transform, + other: iDynTree.Transform, + msg: str = None): + equals = True + error_msg = [msg] if msg else [""] + for r, c in itertools.product(range(3), range(3)): + check = math.isclose(expected.rotation[r, c], other.rotation[r, c]) + if not check: + equals = False + error_msg.append( + f"Rotation [{r},{c}]: expected {expected.rotation[r, c]} " + f"actual {other.rotation[r, c]}") + for i in range(3): + check = math.isclose(expected.position[i], other.position[i]) + if not check: + equals = False + error_msg.append(f"Position [{i}]: expected {expected.position[i]} " + f"actual {other.position[i]}") + if not equals: + raise self.failureException("\n".join(error_msg)) + + def setUp(self): + super().setUp() + self.addTypeEqualityFunc(iDynTree.Transform, self._transform_equality_func) + + model = iDynTree.Model() + # 3 links. + link1 = iDynTree.Link() + link2 = iDynTree.Link() + link3 = iDynTree.Link() + link1_idx = model.add_link("link1", link1) + link2_idx = model.add_link("link2", link2) + link3_idx = model.add_link("link3", link3) + self.assertGreaterEqual(link1_idx, 0) + self.assertGreaterEqual(link2_idx, 0) + self.assertGreaterEqual(link3_idx, 0) + + # 2 joints. Axis is always Z. + direction = iDynTree.Direction(0, 0, 1) + axis = iDynTree.Axis(direction, iDynTree.Position.Zero()) + + joint = iDynTree.RevoluteJoint() + joint.set_attached_links(link1_idx, link2_idx) + position = iDynTree.Position(0, 1, 0) + rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0) + self._joint1_transform = iDynTree.Transform(rotation, position) + joint.set_rest_transform(self._joint1_transform) + joint.set_axis(axis, link2_idx, link1_idx) + self._joint1_idx = model.add_joint("joint1", joint) + self.assertGreaterEqual(self._joint1_idx, 0) + + joint = iDynTree.RevoluteJoint() + joint.set_attached_links(link2_idx, link3_idx) + position = iDynTree.Position(0, 1, 0) + rotation = iDynTree.Rotation(0, -1, 0, 1, 0, 0, 0, 0, 1) + self._joint2_transform = iDynTree.Transform(rotation, position) + joint.set_rest_transform(self._joint2_transform) + joint.set_axis(axis, link3_idx, link2_idx) + self._joint2_idx = model.add_joint("joint2", joint) + self.assertGreaterEqual(self._joint2_idx, 0) + + # 2 frames. + position = iDynTree.Position(1, 0.2, -1.5) + rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0) + self._base_frame_transform = iDynTree.Transform(rotation, position) + model.add_additional_frame_to_link("link1", "base_frame", + self._base_frame_transform) + + position = iDynTree.Position(-0.1, -0.5, 1.1) + rotation = iDynTree.Rotation(1, 0, 0, 0, -1, 0, 0, 0, -1) + self._ee_frame_transform = iDynTree.Transform(rotation, position) + model.add_additional_frame_to_link("link3", "ee_frame", + self._ee_frame_transform) + + self._kin_dyn = iDynTree.KinDynComputations() + self.assertTrue(self._kin_dyn.load_robot_model(model)) + + def test_model_info(self): + self.assertEqual(2, self._kin_dyn.get_nr_of_degrees_of_freedom()) + self.assertEqual(3, self._kin_dyn.get_nr_of_links()) + # 3 links + 2 explicit frames. + self.assertEqual(5, self._kin_dyn.get_nr_of_frames()) + + def test_floating_base(self): + # The default should be the first link as there are no reason to change it + # while generating the traversal. + self.assertEqual("link1", self._kin_dyn.get_floating_base()) + + # Now set a new floating base. + self.assertTrue(self._kin_dyn.set_floating_base("link3")) + self.assertEqual("link3", self._kin_dyn.get_floating_base()) + + def test_relative_transform(self): + positions = iDynTree.VectorDynSize( + self._kin_dyn.get_nr_of_degrees_of_freedom()) + positions.set_zero() + + velocities = iDynTree.VectorDynSize( + self._kin_dyn.get_nr_of_degrees_of_freedom()) + velocities.set_zero() + gravity = iDynTree.Vector3() + gravity.set_zero() + gravity[2] = -9.81 + + # Set a robot state. Use a fixed base state. + # Set an arbitrary configuration for joint2 + positions[self._joint2_idx] = np.deg2rad(45) + self._kin_dyn.set_fixed_base_robot_state(positions, velocities, gravity) + transform = self._kin_dyn.get_relative_transform_for_frames_named( + "ee_frame", "link2") + # Construct the expected transform. + # ee_frame_H_link2 = (link2_H_link3^- * H_joint2 * link3_H_ee_frame) ^-1. + joint2_var_transform = iDynTree.Transform( + iDynTree.Rotation.RotZ(positions[self._joint2_idx]), + iDynTree.Position.Zero()) + expected_transform = (self._joint2_transform * joint2_var_transform * + self._ee_frame_transform).inverse() + + self.assertEqual(expected_transform, transform) + + def test_absolute_transform(self): + positions = iDynTree.VectorDynSize( + self._kin_dyn.get_nr_of_degrees_of_freedom()) + positions.set_zero() + + velocities = iDynTree.VectorDynSize( + self._kin_dyn.get_nr_of_degrees_of_freedom()) + velocities.set_zero() + gravity = iDynTree.Vector3() + gravity.set_zero() + gravity[2] = -9.81 + + # Arbitrary floating base world transform. + floating_base_transform = iDynTree.Transform( + iDynTree.Rotation(0.3224500, -0.9092497, 0.2632318, -0.9453749, + -0.3234002, 0.0409703, 0.0478770, -0.2620636, + -0.9638622), iDynTree.Position(1, 2, 3)) + + base_twist = iDynTree.Twist([0, 0, 0], [0, 0, 0]) + + # Set a robot state. + # Set an arbitrary configuration for joint2 + positions[self._joint2_idx] = np.deg2rad(45) + self._kin_dyn.set_robot_state(floating_base_transform, positions, + base_twist, velocities, gravity) + transform = self._kin_dyn.get_world_transform_for_frame_named("ee_frame") + # Construct the expected transform. + # world_H_ee_frame = (floating_base_transform * link1_H_link2 * + # link2_H_link3^- * H_joint2 * link3_H_ee_frame). + joint2_var_transform = iDynTree.Transform( + iDynTree.Rotation.RotZ(positions[self._joint2_idx]), + iDynTree.Position.Zero()) + expected_transform = ( + floating_base_transform * self._joint1_transform * + self._joint2_transform * joint2_var_transform * + self._ee_frame_transform) + + self.assertEqual(expected_transform, transform) + + +if __name__ == "__main__": + unittest.main() diff --git a/src/high-level/include/iDynTree/KinDynComputations.h b/src/high-level/include/iDynTree/KinDynComputations.h index ab7ca0db085..7cae97f7529 100644 --- a/src/high-level/include/iDynTree/KinDynComputations.h +++ b/src/high-level/include/iDynTree/KinDynComputations.h @@ -195,7 +195,7 @@ class KinDynComputations { /** * Set the link that is used as the floating base link. * - * + * Currently supports only links. See https://github.com/robotology/idyntree/issues/422. * @return true if all went well, false otherwise (for example if the link name was not found). */ bool setFloatingBase(const std::string & floatingBaseName); From 1f0d800f28bf201c948a6c2592df7434c5a82ef5 Mon Sep 17 00:00:00 2001 From: Francesco Romano Date: Tue, 28 Jun 2022 11:33:58 +0200 Subject: [PATCH 4/4] Updated changelog --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6343c58dc0a..49c38a17a5c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,12 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased Major] +### Added +- Additional functionalities for pybind11 bindings (https://github.com/robotology/idyntree/pull/1001): + - `Twist` class + - `ModelLoader` class + - Basic kinematics support in `KinDynComputations` + ## [5.2.0] - 2022-05-09 ### Added