From 54364e714155aa60d0d2e8c12ffa8086bfdbc3a9 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 6 Sep 2022 16:35:07 +0200 Subject: [PATCH 1/5] Implement toManifTwist in Conversions components --- .../Conversions/ManifConversions.h | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h b/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h index 934cdb5969..dd99a9773b 100644 --- a/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h +++ b/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h @@ -1,6 +1,6 @@ /** * @file ManifConversions.h - * @authors Prashanth Ramadoss + * @authors Prashanth Ramadoss Giulio Romualdi * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and * distributed under the terms of the BSD-3-Clause license. */ @@ -10,6 +10,8 @@ #include #include +#include + #include @@ -99,6 +101,21 @@ namespace Conversions return toManifRot(iDynTree::toEigen(R)); } + /** + * @brief Convert iDynTree twist object to manif SE3Tangentd object + * + * @param twist reference to iDynTree twist object + * @return a twist as manif SE3Tangentd object + */ + inline manif::SE3Tangentd toManifTwist(const iDynTree::Twist& twist) + { + manif::SE3Tangentd manifTwist; + manifTwist.lin() = iDynTree::toEigen(twist.getLinearVec3()); + manifTwist.ang() = iDynTree::toEigen(twist.getAngularVec3()); + return manifTwist; + } + + } // namespace Conversions } // namespace BipedalLocomotion From e9af57052f74ed4af37e9480a7d0e91c85e2f625 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 6 Sep 2022 16:36:21 +0200 Subject: [PATCH 2/5] Run clang-format on ManifConversion.h --- .../Conversions/ManifConversions.h | 180 +++++++++--------- 1 file changed, 88 insertions(+), 92 deletions(-) diff --git a/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h b/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h index dd99a9773b..655cb5bbed 100644 --- a/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h +++ b/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h @@ -8,113 +8,109 @@ #ifndef BIPEDAL_LOCOMOTION_MANIF_CONVERSIONS_H #define BIPEDAL_LOCOMOTION_MANIF_CONVERSIONS_H -#include #include +#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 - * @return pose as manif SE3 object - */ - template - manif::SE3 toManifPose(const Eigen::Matrix& rotation, - const Eigen::Matrix& translation) - { - Eigen::Quaternion quat = Eigen::Quaternion(rotation); - quat.normalize(); // SO3 constructor expects normalized quaternion - return manif::SE3(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 rotation, - Eigen::Ref translation) - { - Eigen::Quaterniond quat = Eigen::Quaterniond(rotation); - quat.normalize(); // SO3 constructor expects normalized quaternion - return manif::SE3d(translation, quat); - } +/** + * @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 +manif::SE3 toManifPose(const Eigen::Matrix& rotation, + const Eigen::Matrix& translation) +{ + Eigen::Quaternion quat = Eigen::Quaternion(rotation); + quat.normalize(); // SO3 constructor expects normalized quaternion + return manif::SE3(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())); - } +/** + * @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 rotation, + Eigen::Ref translation) +{ + Eigen::Quaterniond quat = Eigen::Quaterniond(rotation); + quat.normalize(); // SO3 constructor expects normalized quaternion + return manif::SE3d(translation, quat); +} - /** - * @brief Convert rotation matrix to manif SO3 object - * - * @param rotation reference to 3x3 Eigen matrix - * @return pose as manif SO3 object - */ - template - manif::SO3 toManifRot(const Eigen::Matrix& rotation) - { - Eigen::Quaternion quat = Eigen::Quaternion(rotation); - quat.normalize(); // SO3 constructor expects normalized quaternion - return manif::SO3(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())); +} - /** - * @brief Convert rotation matrix to manif SO3d object - * - * @param rotation Eigen ref of 3x3 rotation matrix - * @param translation Eigen ref of 3x1 translation vector - * @return pose as manif SO3d object - */ - inline manif::SO3d toManifRot(Eigen::Ref rotation) - { - Eigen::Quaterniond quat = Eigen::Quaterniond(rotation); - quat.normalize(); // SO3 constructor expects normalized quaternion - return manif::SO3d(quat); - } +/** + * @brief Convert rotation matrix to manif SO3 object + * + * @param rotation reference to 3x3 Eigen matrix + * @return pose as manif SO3 object + */ +template manif::SO3 toManifRot(const Eigen::Matrix& rotation) +{ + Eigen::Quaternion quat = Eigen::Quaternion(rotation); + quat.normalize(); // SO3 constructor expects normalized quaternion + return manif::SO3(quat); +} - /** - * @brief Convert iDynTree rotation object to manif SE3d object - * - * @param R reference to iDynTree rotation object - * @return pose as manif SO3d object - */ - inline manif::SO3d toManifRot(const iDynTree::Rotation& R) - { - return toManifRot(iDynTree::toEigen(R)); - } +/** + * @brief Convert rotation matrix to manif SO3d object + * + * @param rotation Eigen ref of 3x3 rotation matrix + * @param translation Eigen ref of 3x1 translation vector + * @return pose as manif SO3d object + */ +inline manif::SO3d toManifRot(Eigen::Ref rotation) +{ + Eigen::Quaterniond quat = Eigen::Quaterniond(rotation); + quat.normalize(); // SO3 constructor expects normalized quaternion + return manif::SO3d(quat); +} - /** - * @brief Convert iDynTree twist object to manif SE3Tangentd object - * - * @param twist reference to iDynTree twist object - * @return a twist as manif SE3Tangentd object - */ - inline manif::SE3Tangentd toManifTwist(const iDynTree::Twist& twist) - { - manif::SE3Tangentd manifTwist; - manifTwist.lin() = iDynTree::toEigen(twist.getLinearVec3()); - manifTwist.ang() = iDynTree::toEigen(twist.getAngularVec3()); - return manifTwist; - } +/** + * @brief Convert iDynTree rotation object to manif SE3d object + * + * @param R reference to iDynTree rotation object + * @return pose as manif SO3d object + */ +inline manif::SO3d toManifRot(const iDynTree::Rotation& R) +{ + return toManifRot(iDynTree::toEigen(R)); +} +/** + * @brief Convert iDynTree twist object to manif SE3Tangentd object + * + * @param twist reference to iDynTree twist object + * @return a twist as manif SE3Tangentd object + */ +inline manif::SE3Tangentd toManifTwist(const iDynTree::Twist& twist) +{ + manif::SE3Tangentd manifTwist; + manifTwist.lin() = iDynTree::toEigen(twist.getLinearVec3()); + manifTwist.ang() = iDynTree::toEigen(twist.getAngularVec3()); + return manifTwist; +} } // namespace Conversions } // namespace BipedalLocomotion From 56eefaf7df1e01aea095bbae79c1af4a63e67010 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 7 Sep 2022 21:07:21 +0200 Subject: [PATCH 3/5] Add default value for the desired spatial and angular velocity to the IK SO3 and SE3 tasks --- src/IK/include/BipedalLocomotion/IK/SE3Task.h | 5 +++-- src/IK/include/BipedalLocomotion/IK/SO3Task.h | 8 ++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/IK/include/BipedalLocomotion/IK/SE3Task.h b/src/IK/include/BipedalLocomotion/IK/SE3Task.h index e7453d6cd9..ee5e952a09 100644 --- a/src/IK/include/BipedalLocomotion/IK/SE3Task.h +++ b/src/IK/include/BipedalLocomotion/IK/SE3Task.h @@ -131,10 +131,11 @@ class SE3Task : public IKLinearTask, public BipedalLocomotion::System::ITaskCont /** * Set the desired set-point of the trajectory. * @param I_H_F Homogeneous transform between the link and the inertial frame. - * @param mixedVelocity 6D-velocity written in mixed representation.g + * @param mixedVelocity 6D-velocity written in mixed representation. The default value is zero. * @return True in case of success, false otherwise. */ - bool setSetPoint(const manif::SE3d& I_H_F, const manif::SE3d::Tangent& mixedVelocity); + bool setSetPoint(const manif::SE3d& I_H_F, + const manif::SE3d::Tangent& mixedVelocity = manif::SE3d::Tangent::Zero()); /** * Get the size of the task. (I.e the number of rows of the vector b) diff --git a/src/IK/include/BipedalLocomotion/IK/SO3Task.h b/src/IK/include/BipedalLocomotion/IK/SO3Task.h index 957d90871c..a3ae64aef6 100644 --- a/src/IK/include/BipedalLocomotion/IK/SO3Task.h +++ b/src/IK/include/BipedalLocomotion/IK/SO3Task.h @@ -98,8 +98,6 @@ class SO3Task : public IKLinearTask */ bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override; - - /** * Update the content of the element. * @return True in case of success, false otherwise. @@ -109,10 +107,12 @@ class SO3Task : public IKLinearTask /** * Set the desired reference trajectory. * @param I_R_F Rotation between the link and the inertial frame. - * @param angularVelocity angular velocity written in mixed inertial frame. + * @param angularVelocity angular velocity written in mixed inertial frame. The default value is + * zero. * @return True in case of success, false otherwise. */ - bool setSetPoint(const manif::SO3d& I_R_F, const manif::SO3d::Tangent& angularVelocity); + bool setSetPoint(const manif::SO3d& I_R_F, + const manif::SO3d::Tangent& angularVelocity = manif::SO3d::Tangent::Zero()); /** * Get the size of the task. (I.e the number of rows of the vector b) From e7e178b13e7313c17a22a8ec668ef7ca6569b0db Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 8 Sep 2022 09:56:33 +0200 Subject: [PATCH 4/5] Update the python bindings accordingly to 56eefaf7df1e01aea095bbae79c1af4a63e67010 --- bindings/python/IK/CMakeLists.txt | 2 +- bindings/python/IK/src/SE3Task.cpp | 7 ++++++- bindings/python/IK/src/SO3Task.cpp | 7 ++++++- 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/bindings/python/IK/CMakeLists.txt b/bindings/python/IK/CMakeLists.txt index 337a4cd094..71eff72f6b 100644 --- a/bindings/python/IK/CMakeLists.txt +++ b/bindings/python/IK/CMakeLists.txt @@ -10,7 +10,7 @@ if(FRAMEWORK_COMPILE_IK) NAME IKBindings SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/AngularMomentumTask.cpp src/Module.cpp src/IKLinearTask.cpp HEADERS ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/IKLinearTask.h - LINK_LIBRARIES BipedalLocomotion::IK + LINK_LIBRARIES BipedalLocomotion::IK MANIF::manif TESTS tests/test_QP_inverse_kinematics.py TESTS_RUNTIME_CONDITIONS FRAMEWORK_USE_icub-models ) diff --git a/bindings/python/IK/src/SE3Task.cpp b/bindings/python/IK/src/SE3Task.cpp index 119767258c..f67e97abf1 100644 --- a/bindings/python/IK/src/SE3Task.cpp +++ b/bindings/python/IK/src/SE3Task.cpp @@ -15,6 +15,8 @@ #include +#include + namespace BipedalLocomotion { namespace bindings @@ -33,7 +35,10 @@ void CreateSE3Task(pybind11::module& module) BipedalLocomotion::System::ITaskControllerManager>(module, "SE3Task") .def(py::init()) .def("set_kin_dyn", &SE3Task::setKinDyn, py::arg("kin_dyn")) - .def("set_set_point", &SE3Task::setSetPoint, py::arg("I_H_F"), py::arg("mixed_velocity")); + .def("set_set_point", + &SE3Task::setSetPoint, + py::arg("I_H_F"), + py::arg("mixed_velocity") = manif::SE3d::Tangent::Zero()); } } // namespace IK diff --git a/bindings/python/IK/src/SO3Task.cpp b/bindings/python/IK/src/SO3Task.cpp index ed7f0ce6db..c13748744b 100644 --- a/bindings/python/IK/src/SO3Task.cpp +++ b/bindings/python/IK/src/SO3Task.cpp @@ -13,6 +13,8 @@ #include #include +#include + namespace BipedalLocomotion { namespace bindings @@ -28,7 +30,10 @@ void CreateSO3Task(pybind11::module& module) py::class_, IKLinearTask>(module, "SO3Task") .def(py::init()) .def("set_kin_dyn", &SO3Task::setKinDyn, py::arg("kin_dyn")) - .def("set_set_point", &SO3Task::setSetPoint, py::arg("I_R_F"), py::arg("angular_velocity")); + .def("set_set_point", + &SO3Task::setSetPoint, + py::arg("I_R_F"), + py::arg("angular_velocity") = manif::SO3d::Tangent::Zero()); } } // namespace IK From 84394e6a16064d30d61a59345986704fddff3ac2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 8 Sep 2022 10:06:25 +0200 Subject: [PATCH 5/5] Update the CHANGELOG --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c3a178d839..689020d4f5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,8 @@ All notable changes to this project are documented in this file. ### Added - Implement the `MultiStateWeightProvider` in `ContinuousDynamicalSystem` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/555) - Implement `PortInput` and `PortOutput`in `System` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/555) +- Implement `toManifTwist` in `Conversions` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/557) +- Add the default value for the desired spatial and angular velocity to the `IK::SO3Task` and `IK::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/557) ### Changed - Now `Advanceable` inherits from `PortInput` and `PortOutput` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/555)