Implement Conversions::toiDynTreeRot function (#842)

CHANGELOG.md

@@ -18,6 +18,7 @@ All notable changes to this project are documented in this file.
 - Add the possibility to get only position or position/velocity from the spline (https://github.com/ami-iit/bipedal-locomotion-framework/pull/834)
 - Add the possibility to set the number of threads used by onnxruntime in `MANN` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/836)
 - Implement `ButterworthLowPassFilter` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/838)
+- Implement `Conversions::toiDynTreeRot` function (https://github.com/ami-iit/bipedal-locomotion-framework/pull/842)
 
 ### Changed
 - 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810)

src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h

@@ -123,6 +123,17 @@ inline iDynTree::Transform toiDynTreePose(const manif::SE3d& se3)
     return iDynTree::Transform(iDynTree::make_matrix_view(se3.transform()));
 }
 
+/**
+ * @brief Convert a manif SO3 object into and iDynTree::Rotation
+ *
+ * @param so3 a manif SO3 object
+ * @return rotation as iDynTree::Rotation
+ */
+inline iDynTree::Rotation toiDynTreeRot(const manif::SO3d& so3)
+{
+    return iDynTree::Rotation(iDynTree::make_matrix_view(so3.rotation()));
+}
+
 } // namespace Conversions
 } // namespace BipedalLocomotion
 

src/Conversions/tests/ManifConversionsTest.cpp

@@ -32,4 +32,8 @@ TEST_CASE("Manif Conversions")
 
     manif::SE3d pose2 = BipedalLocomotion::Conversions::toManifPose(R, pos);
     REQUIRE(pose2.transform().isApprox(iDynTree::toEigen(iDynH.asHomogeneousTransform())));
+
+    manif::SO3d rot = quat;
+    iDynTree::Rotation idynRot = BipedalLocomotion::Conversions::toiDynTreeRot(rot);
+    REQUIRE(iDynTree::toEigen(idynRot).isApprox(R, tolerance));
 }