diff --git a/README.md b/README.md index 4b9fbc6767..dd31dec2e1 100644 --- a/README.md +++ b/README.md @@ -18,6 +18,7 @@ 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::Common**](./src/Common): Library for common definitions and conversions 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 +42,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) +- `Common` requires: + - 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) diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index 952c62fddf..a4d9beda8b 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/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) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 52379a7080..bf05b16f77 100644 --- a/src/CMakeLists.txt +++ b/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) diff --git a/src/Conversions/CMakeLists.txt b/src/Conversions/CMakeLists.txt new file mode 100644 index 0000000000..84a05d24b2 --- /dev/null +++ b/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() + + diff --git a/src/Conversions/include/BipedalLocomotion/Conversions/CommonConversions.h b/src/Conversions/include/BipedalLocomotion/Conversions/CommonConversions.h new file mode 100644 index 0000000000..ddf8cdc2c6 --- /dev/null +++ b/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 + +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 + void eigenRotTrans2HomogeneousTransform(const Eigen::Matrix& rotation, + const Eigen::Matrix& translation, + Eigen::Matrix& 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(const Eigen::Matrix&, + const Eigen::Matrix&, + Eigen::Matrix&); + + template void eigenRotTrans2HomogeneousTransform(const Eigen::Matrix&, + const Eigen::Matrix&, + Eigen::Matrix&); + + +} // namespace Conversions +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_COMMON_CONVERSIONS_H diff --git a/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h b/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h new file mode 100644 index 0000000000..a4be3f0219 --- /dev/null +++ b/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 +#include +#include + + +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 + void eigenRotTrans2ManifPose(const Eigen::Matrix& rotation, + const Eigen::Matrix& translation, + manif::SE3& pose) + { + Eigen::Quaternion quat = Eigen::Quaternion(rotation); + quat.normalize(); // SO3 constructor expects normalized quaternion + pose = manif::SE3(translation, quat); + } + + template void eigenRotTrans2ManifPose(const Eigen::Matrix& rotation, + const Eigen::Matrix& translation, + manif::SE3& pose); + template void eigenRotTrans2ManifPose(const Eigen::Matrix& rotation, + const Eigen::Matrix& translation, + manif::SE3& 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 rotation, + Eigen::Ref 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 diff --git a/src/Conversions/tests/CMakeLists.txt b/src/Conversions/tests/CMakeLists.txt new file mode 100644 index 0000000000..c781dcf815 --- /dev/null +++ b/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) diff --git a/src/Conversions/tests/ManifConversionsTest.cpp b/src/Conversions/tests/ManifConversionsTest.cpp new file mode 100644 index 0000000000..4bc2e8e238 --- /dev/null +++ b/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 + +#include +#include +#include + +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)); +}