From e341b70f188562b5efdc717a2bdcfde8eb92aa77 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 30 Apr 2024 14:39:21 +0200 Subject: [PATCH] Implement Conversions::toiDynTreeRot function (#842) --- CHANGELOG.md | 1 + .../BipedalLocomotion/Conversions/ManifConversions.h | 11 +++++++++++ src/Conversions/tests/ManifConversionsTest.cpp | 4 ++++ 3 files changed, 16 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index fbc554e15a..6f9509cfe9 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h b/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h index cfeb820cc2..432022057f 100644 --- a/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h +++ b/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 diff --git a/src/Conversions/tests/ManifConversionsTest.cpp b/src/Conversions/tests/ManifConversionsTest.cpp index 4ccd13f1d9..2141d426d1 100644 --- a/src/Conversions/tests/ManifConversionsTest.cpp +++ b/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)); }