Implement toManifTwist in Conversions components and Add default value for the desired spatial and angular velocity to the IK SO3 and SE3 tasks

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)

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
     )

bindings/python/IK/src/SE3Task.cpp

@@ -15,6 +15,8 @@
 
 #include <BipedalLocomotion/bindings/IK/SE3Task.h>
 
+#include <manif/manif.h>
+
 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

bindings/python/IK/src/SO3Task.cpp

@@ -13,6 +13,8 @@
 #include <BipedalLocomotion/IK/SO3Task.h>
 #include <BipedalLocomotion/bindings/IK/SO3Task.h>
 
+#include <manif/manif.h>
+
 namespace BipedalLocomotion
 {
 namespace bindings
@@ -28,7 +30,10 @@ void CreateSO3Task(pybind11::module& module)
     py::class_<SO3Task, std::shared_ptr<SO3Task>, 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

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

@@ -1,103 +1,116 @@
 /**
  * @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.
  */
 
 #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>
+#include <iDynTree/Core/Transform.h>
+#include <iDynTree/Core/Twist.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 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 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()));
+}
 
-    /**
-     * @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 SO3 object
+ *
+ * @param rotation reference to 3x3 Eigen matrix
+ * @return pose as manif SO3 object
+ */
+template <class Scalar> manif::SO3<Scalar> toManifRot(const Eigen::Matrix<Scalar, 3, 3>& rotation)
+{
+    Eigen::Quaternion<Scalar> quat = Eigen::Quaternion<Scalar>(rotation);
+    quat.normalize(); // SO3 constructor expects normalized quaternion
+    return manif::SO3<Scalar>(quat);
+}
 
-    /**
-     * @brief Convert rotation matrix to manif SO3 object
-     *
-     * @param rotation reference to 3x3 Eigen matrix
-     * @return pose as manif SO3 object
-     */
-    template <class Scalar>
-    manif::SO3<Scalar> toManifRot(const Eigen::Matrix<Scalar, 3, 3>& rotation)
-    {
-        Eigen::Quaternion<Scalar> quat = Eigen::Quaternion<Scalar>(rotation);
-        quat.normalize(); // SO3 constructor expects normalized quaternion
-        return manif::SO3<Scalar>(quat);
-    }
+/**
+ * @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<const Eigen::Matrix3d> rotation)
+{
+    Eigen::Quaterniond quat = Eigen::Quaterniond(rotation);
+    quat.normalize(); // SO3 constructor expects normalized quaternion
+    return manif::SO3d(quat);
+}
 
-    /**
-     * @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<const Eigen::Matrix3d> rotation)
-    {
-        Eigen::Quaterniond quat = Eigen::Quaterniond(rotation);
-        quat.normalize(); // SO3 constructor expects normalized quaternion
-        return manif::SO3d(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 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

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)

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)