Merge pull request #138 from prashanthr05/feature/util-definitions

Add CommonConversions and ManifConversions library

CHANGELOG.md

@@ -16,5 +16,6 @@ All notable changes to this project are documented in this file.
 - Implement `Estimators` library (https://github.com/dic-iit/bipedal-locomotion-controllers/pull/23)
 - Renamed from ``bipedal-locomotion-controllers`` to ``bipedal-locomotion-framework`` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/40).
 - Implement `Contact` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/43 and https://github.com/dic-iit/bipedal-locomotion-framework/pull/45)
+- Added `CommonConversions` and `ManifConversions` libraries to handle type conversions.
 
 [Unreleased]: https://github.com/dic-iit/bipedal-locomotion-framework/

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 Eigen3::Eigen)
+
+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,40 @@
+/**
+ * @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 reference of rotation matrix
+ * @param translation reference of translation vector
+ * @return homogeneous transform as a Eigen::Matrix4d object
+ */
+ template <class Scalar>
+ Eigen::Matrix<Scalar, 4, 4> toEigenPose(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();
+ return transform;
+ }
+
+
+} // namespace Conversions
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_COMMON_CONVERSIONS_H

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

@@ -0,0 +1,66 @@
+/**
+ * @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
+ * @return pose as manif SE3 object
+ */
+ template <class Scalar>
+ manif::SE3<Scalar> toManifPose(const Eigen::Matrix<Scalar, 3, 3>& rotation,
+ const Eigen::Matrix<Scalar, 3, 1>& translation)
+ {
+ Eigen::Quaternion<Scalar> quat = Eigen::Quaternion<Scalar>(rotation);
+ quat.normalize(); // SO3 constructor expects normalized quaternion
+ return manif::SE3<Scalar>(translation, quat);
+ }
+
+ /**
+ * @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
+ * @return pose as manif SE3d object
+ */
+ inline manif::SE3d toManifPose(Eigen::Ref<const Eigen::Matrix3d> rotation,
+ Eigen::Ref<const Eigen::Vector3d> translation)
+ {
+ Eigen::Quaterniond quat = Eigen::Quaterniond(rotation);
+ quat.normalize(); // SO3 constructor expects normalized quaternion
+ return manif::SE3d(translation, quat);
+ }
+
+ /**
+ * @brief Convert iDynTree transform object to manif SE3d object
+ *
+ * @param H reference to iDynTree Tranform object
+ * @return pose as manif SE3d object
+ */
+ inline manif::SE3d toManifPose(const iDynTree::Transform& H)
+ {
+ return toManifPose(iDynTree::toEigen(H.getRotation()),
+ iDynTree::toEigen(H.getPosition()));;
+ }
+
+} // 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,35 @@
+/**
+ * @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::Matrix3d R = quat.toRotationMatrix();
+ Eigen::Matrix4d H = BipedalLocomotion::Conversions::toEigenPose(R, pos);
+
+ iDynTree::Transform iDynH;
+ iDynTree::fromEigen(iDynH, H);
+
+ manif::SE3d pose = BipedalLocomotion::Conversions::toManifPose(iDynH);
+
+ constexpr double tolerance = 1e-4;
+ REQUIRE(pose.transform().isApprox(H, tolerance));
+
+ manif::SE3d pose2 = BipedalLocomotion::Conversions::toManifPose(R, pos);
+ REQUIRE(pose2.transform().isApprox(iDynTree::toEigen(iDynH.asHomogeneousTransform())));
+}