Implement the python bindings for the iDynTree to manif conversions

CHANGELOG.md

@@ -11,6 +11,7 @@ All notable changes to this project are documented in this file.
 - Add functions to split a model in a set of submodels in the Estimator component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/604)
 - Add the possibity to call the advanceable capabilities of the `QuinticSpline` from the python (https://github.com/ami-iit/bipedal-locomotion-framework/pull/609)
 - Implement the `CubicSpline` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/609)
+- Implement the python bindings for the iDynTree to manif conversions (https://github.com/ami-iit/bipedal-locomotion-framework/pull/610)
 
 ### Changed
 - Ask for `toml++ v3.0.1` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/581)

bindings/python/Conversions/CMakeLists.txt

@@ -10,6 +10,7 @@ if(FRAMEWORK_COMPILE_ManifConversions)
     NAME ManifConversionsBindings
     SOURCES src/ManifConversions.cpp src/Module.cpp
     HEADERS ${H_PREFIX}/ManifConversions.h ${H_PREFIX}/Module.h
-    LINK_LIBRARIES BipedalLocomotion::ManifConversions)
+    LINK_LIBRARIES BipedalLocomotion::ManifConversions
+    TESTS tests/test_manif_conversions.py)
 
 endif()

bindings/python/Conversions/src/ManifConversions.cpp

@@ -10,7 +10,11 @@
 
 #include <Eigen/Dense>
 
+#include <iDynTree/Core/Rotation.h>
+#include <iDynTree/Core/Transform.h>
+
 #include <BipedalLocomotion/Conversions/ManifConversions.h>
+#include <BipedalLocomotion/bindings/type_caster/swig.h>
 
 namespace BipedalLocomotion
 {
@@ -19,6 +23,34 @@ namespace bindings
 namespace Conversions
 {
 
+manif::SE3d toManifPose(::pybind11::object& obj)
+{
+    const iDynTree::Transform* const cls
+        = pybind11::detail::swig_wrapped_pointer_to_pybind<iDynTree::Transform>(obj);
+
+    if (cls == nullptr)
+    {
+        throw ::pybind11::value_error("Invalid input for the function. Please provide "
+                                      "an iDynTree::Transform object.");
+    }
+
+    return BipedalLocomotion::Conversions::toManifPose(*cls);
+}
+
+manif::SO3d toManifRot(::pybind11::object& obj)
+{
+    const iDynTree::Rotation* const cls
+        = pybind11::detail::swig_wrapped_pointer_to_pybind<iDynTree::Rotation>(obj);
+
+    if (cls == nullptr)
+    {
+        throw ::pybind11::value_error("Invalid input for the function. Please provide "
+                                      "an iDynTree::Rotation object.");
+    }
+
+    return BipedalLocomotion::Conversions::toManifRot(*cls);
+}
+
 void CreateManifConversions(pybind11::module& module)
 {
     namespace py = ::pybind11;
@@ -30,9 +62,17 @@ void CreateManifConversions(pybind11::module& module)
                  &::BipedalLocomotion::Conversions::toManifPose<double>),
              py::arg("rotation"),
              py::arg("translation"))
+        .def("to_manif_pose",
+             py::overload_cast<::pybind11::object&>(
+                 &::BipedalLocomotion::bindings::Conversions::toManifPose),
+             py::arg("transform"))
         .def("to_manif_rot",
              py::overload_cast<const Eigen::Matrix<double, 3, 3>&>(
                  &::BipedalLocomotion::Conversions::toManifRot<double>),
+             py::arg("rotation"))
+        .def("to_manif_rot",
+             py::overload_cast<::pybind11::object&>(
+                 &::BipedalLocomotion::bindings::Conversions::toManifRot),
              py::arg("rotation"));
 }
 

bindings/python/Conversions/tests/test_manif_conversions.py

@@ -0,0 +1,31 @@
+import pytest
+pytestmark = pytest.mark.conversions
+
+import bipedal_locomotion_framework.bindings as blf
+import manifpy as manif
+import numpy as np
+
+import idyntree.swig as idyn
+
+def test_manif_conversions_from_numpy():
+    idyntree_so3 = idyn.Rotation.RPY(0.1, 0.2, -1.32)
+    numpy_so3 = idyntree_so3.toNumPy()
+    manif_so3 = blf.conversions.to_manif_rot(numpy_so3)
+    assert numpy_so3 == pytest.approx(manif_so3.rotation())
+
+    traslation = [9.1, -1.2, 3.1]
+    manif_se3 = blf.conversions.to_manif_pose(numpy_so3, traslation)
+    assert numpy_so3 == pytest.approx(manif_se3.rotation())
+    assert traslation == pytest.approx(manif_se3.translation())
+
+
+def test_manif_conversions_from_idyntree():
+    idyntree_so3 = idyn.Rotation.RPY(0.1, 0.2, -1.32)
+    manif_so3 = blf.conversions.to_manif_rot(idyntree_so3)
+    assert idyntree_so3.toNumPy() == pytest.approx(manif_so3.rotation())
+
+    traslation = [9.1, -1.2, 3.1]
+    idyntree_se3 = idyn.Transform(idyntree_so3, traslation)
+    manif_se3 = blf.conversions.to_manif_pose(idyntree_se3)
+    assert idyntree_se3.getRotation().toNumPy() == pytest.approx(manif_se3.rotation())
+    assert idyntree_se3.getPosition().toNumPy() == pytest.approx(manif_se3.translation())