Add CommonConversions and ManifConversions library

README.md

@@ -18,6 +18,8 @@ The **BipedalLocomotionFramework** project is a _suite_ of libraries for achievi
  data and from YARP structures
 - [**BipedalLocomotion::ParametersHandler**](./src/ParametersHandler): Library for
  retrieving parameters from configuration files and not only
+- [**BipedalLocomotion::CommonConversions**](./src/Conversions): Library for common conversion utilities used in the framework
+- [**BipedalLocomotion::ManifConversions**](./src/Conversions): Library for manif library related conversion utilities used in the framework
 - [**BipedalLocomotion::Estimators**](./src/Estimators): Library containing observers
 - [**BipedalLocomotion::FloatingBaseEstimators**](./src/Estimators): Library containing floating base estimators
 - [**BipedalLocomotion::Planner**](./src/Planner): Library containing planner useful for locomotion
@@ -41,6 +43,12 @@ file. Please note that the indicated version is the the minimum required version
  - [`YARP`](https://github.com/robotology/YARP)
  - For testing:
  - [`Catch2`](https://github.com/catchorg/Catch2)
+- `ManifConversions` require:
+ - For using it:
+ - [`iDynTree`](https://github.com/robotology/idyntree) (version 0.11.105) 
+ - [`manif`](https://github.com/artivis/manif)
+ - For testing:
+ - [`Catch2`](https://github.com/catchorg/Catch2) 
 - `Estimators` require:
  - For using it:
  - [`iDynTree`](https://github.com/robotology/idyntree) (version 0.11.105)

cmake/BipedalLocomotionFrameworkFindDependencies.cmake

@@ -186,3 +186,6 @@ framework_dependent_option(FRAMEWORK_COMPILE_FloatingBaseEstimators
  "Compile FloatingBaseEstimators libraries?" ON
  "FRAMEWORK_USE_manif" OFF)
 
+framework_dependent_option(FRAMEWORK_COMPILE_ManifConversions
+ "Compile manif Conversions libraries?" ON
+ "FRAMEWORK_USE_manif" OFF)

src/CMakeLists.txt

@@ -5,6 +5,7 @@
 add_subdirectory(GenericContainer)
 add_subdirectory(YarpUtilities)
 add_subdirectory(ParametersHandler)
+add_subdirectory(Conversions)
 add_subdirectory(Estimators)
 add_subdirectory(System)
 add_subdirectory(Planners)

src/Conversions/CMakeLists.txt

@@ -0,0 +1,20 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+add_bipedal_locomotion_library(
+ NAME CommonConversions
+ IS_INTERFACE
+ PUBLIC_HEADERS include/BipedalLocomotion/Conversions/CommonConversions.h
+ PUBLIC_LINK_LIBRARIES MANIF::manif iDynTree::idyntree-core)
+
+if (FRAMEWORK_COMPILE_ManifConversions)
+ add_bipedal_locomotion_library(
+ NAME ManifConversions
+ IS_INTERFACE
+ PUBLIC_HEADERS include/BipedalLocomotion/Conversions/ManifConversions.h
+ PUBLIC_LINK_LIBRARIES MANIF::manif iDynTree::idyntree-core BipedalLocomotion::CommonConversions
+ SUBDIRECTORIES tests)
+endif()
+
+

src/Conversions/include/BipedalLocomotion/Conversions/CommonConversions.h

@@ -0,0 +1,48 @@
+/**
+ * @file Conversions.h
+ * @authors Prashanth Ramadoss
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_COMMON_CONVERSIONS_H
+#define BIPEDAL_LOCOMOTION_COMMON_CONVERSIONS_H
+
+#include <Eigen/Dense>
+
+namespace BipedalLocomotion
+{
+namespace Conversions
+{
+
+ /**
+ * @brief Construct homogeneous transformation matrix from rotation matrix and translation vector
+ *
+ * @param rotation Eigen ref of rotation matrix
+ * @param translation Eigen ref of translation vector
+ * @param transform Eigen ref of Eigen::Matrix4d object
+ */
+ template <class Scalar>
+ void eigenRotTrans2HomogeneousTransform(const Eigen::Matrix<Scalar, 3, 3>& rotation,
+ const Eigen::Matrix<Scalar, 3, 1>& translation,
+ Eigen::Matrix<Scalar, 4, 4>& transform)
+ {
+ transform.template topLeftCorner<3,3>() = rotation;
+ transform.template topRightCorner<3,1>() = translation;
+ transform(3,3) = 1;
+ transform.template bottomLeftCorner<1,3>().setZero();
+ }
+
+ template void eigenRotTrans2HomogeneousTransform<double>(const Eigen::Matrix<double, 3, 3>&,
+ const Eigen::Matrix<double, 3, 1>&,
+ Eigen::Matrix<double, 4, 4>&);
+
+ template void eigenRotTrans2HomogeneousTransform<float>(const Eigen::Matrix<float, 3, 3>&,
+ const Eigen::Matrix<float, 3, 1>&,
+ Eigen::Matrix<float, 4, 4>&);
+
+
+} // namespace Conversions
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_COMMON_CONVERSIONS_H

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

@@ -0,0 +1,76 @@
+/**
+ * @file Conversions.h
+ * @authors Prashanth Ramadoss
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_MANIF_CONVERSIONS_H
+#define BIPEDAL_LOCOMOTION_MANIF_CONVERSIONS_H
+
+#include <iDynTree/Core/Transform.h>
+#include <iDynTree/Core/EigenHelpers.h>
+#include <manif/manif.h>
+
+
+namespace BipedalLocomotion
+{
+namespace Conversions
+{
+ /**
+ * @brief Convert rotation matrix and translation vector to manif SE3 object
+ *
+ * @param rotation reference to 3x3 Eigen matrix
+ * @param translation reference of 3x1 Eigen matrix
+ * @param pose reference to manif SE3 object
+ */
+ template <class Scalar>
+ void eigenRotTrans2ManifPose(const Eigen::Matrix<Scalar, 3, 3>& rotation,
+ const Eigen::Matrix<Scalar, 3, 1>& translation,
+ manif::SE3<Scalar>& pose)
+ {
+ Eigen::Quaternion<Scalar> quat = Eigen::Quaternion<Scalar>(rotation);
+ quat.normalize(); // SO3 constructor expects normalized quaternion
+ pose = manif::SE3<Scalar>(translation, quat);
+ }
+
+ template void eigenRotTrans2ManifPose<double>(const Eigen::Matrix<double, 3, 3>& rotation,
+ const Eigen::Matrix<double, 3, 1>& translation,
+ manif::SE3<double>& pose);
+ template void eigenRotTrans2ManifPose<float>(const Eigen::Matrix<float, 3, 3>& rotation,
+ const Eigen::Matrix<float, 3, 1>& translation,
+ manif::SE3<float>& pose);
+
+ /**
+ * @brief Convert rotation matrix and translation vector to manif SE3d object
+ *
+ * @param rotation Eigen ref of 3x3 rotation matrix
+ * @param translation Eigen ref of 3x1 translation vector
+ * @param pose reference to manif SE3d object
+ */
+ inline void eigenRotTrans2ManifPose(Eigen::Ref<const Eigen::Matrix3d> rotation,
+ Eigen::Ref<const Eigen::Vector3d> translation,
+ manif::SE3d& pose)
+ {
+ Eigen::Quaterniond quat = Eigen::Quaterniond(rotation);
+ quat.normalize(); // SO3 constructor expects normalized quaternion
+ pose = manif::SE3d(translation, quat);
+ }
+
+ /**
+ * @brief convert iDynTree transform object to manif SE3d object
+ *
+ * @param H reference to iDynTree Tranform object
+ * @param pose reference to manif SE3 object
+ */
+ inline void iDynTransform2ManifPose(const iDynTree::Transform& H, manif::SE3d& pose)
+ {
+ BipedalLocomotion::Conversions::eigenRotTrans2ManifPose(iDynTree::toEigen(H.getRotation()),
+ iDynTree::toEigen(H.getPosition()),
+ pose);
+ }
+
+} // namespace Conversions
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_MANIF_CONVERSIONS_H

src/Conversions/tests/CMakeLists.txt

@@ -0,0 +1,8 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+add_bipedal_test(
+ NAME ManifConversionsTest
+ SOURCES ManifConversionsTest.cpp
+ LINKS BipedalLocomotion::ManifConversions BipedalLocomotion::CommonConversions)

src/Conversions/tests/ManifConversionsTest.cpp

@@ -0,0 +1,36 @@
+/**
+ * @file ManifConversionsTest.cpp
+ * @authors Prashanth Ramadoss
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+// Catch2
+#include <catch2/catch.hpp>
+
+#include <BipedalLocomotion/Conversions/CommonConversions.h>
+#include <BipedalLocomotion/Conversions/ManifConversions.h>
+#include <iDynTree/Core/EigenHelpers.h>
+
+TEST_CASE("Manif Conversions")
+{
+ Eigen::Vector3d pos;
+ pos << 0.0296, -0.1439, 0.4915;
+ Eigen::Quaterniond quat = Eigen::Quaterniond(0.3218, -0.6304, -0.6292, 0.3212);
+ quat.normalize();
+
+ Eigen::Matrix4d H;
+ Eigen::Matrix3d R = quat.toRotationMatrix();
+ BipedalLocomotion::Conversions::eigenRotTrans2HomogeneousTransform(R, pos, H);
+
+ iDynTree::Transform iDynH;
+ iDynTree::Rotation iDynR;
+ iDynTree::Position iDynp;
+ iDynTree::fromEigen(iDynH, H);
+
+ manif::SE3d pose;
+ BipedalLocomotion::Conversions::iDynTransform2ManifPose(iDynH, pose);
+
+ constexpr double tolerance = 1e-4;
+ REQUIRE(pose.transform().isApprox(H, tolerance));
+}