Implement the skeleton of the OptimalControlElement class and SE3Task

.github/workflows/ci.yml

@@ -13,13 +13,15 @@ env:
   vcpkg_TAG: 76a7e9248fb3c57350b559966dcaa2d52a5e4458
   YCM_TAG: v0.11.1
   YARP_TAG: 964bb26fa4791d83d72882711ea7509306248106
-  iDynTree_TAG: 7e3b33fed0025e2096d244cc904e3cbbf593faa4
+  iDynTree_TAG: 64cbaf7e35a0d545167cc9819f4d248d08aa5031
   Catch2_TAG: v2.11.3
   Qhull_TAG: f7aff465101711ae4cee3f501a528d6d53e75185
   CasADi_TAG: 3.5.1
   manif_TAG: 44bdfebff0fbc56cb189f680212257dc7f20ea58
   matioCpp_TAG: 6db196335cd35489f2627cfdce67808c16389be9
-  # Overwrite the VCPKG_INSTALLATION_ROOT env variable defined by GitHub Actions to point to our vcpkg  
+  LieGroupControllers_TAG: fdd7bf5a446f1a60ef4d6717a7f7d13805297699
+
+  # Overwrite the VCPKG_INSTALLATION_ROOT env variable defined by GitHub Actions to point to our vcpkg
   VCPKG_INSTALLATION_ROOT: C:\robotology\vcpkg
 
 jobs:
@@ -103,7 +105,7 @@ jobs:
       with:
         path: ${{ github.workspace }}/install/deps
         # Including ${{ runner.temp }} is a workaround taken from https://github.com/robotology/whole-body-estimators/pull/62 to fix macos configuration failure on https://github.com/dic-iit/bipedal-locomotion-framework/pull/45
-        key: source-deps-${{ runner.os }}-${{runner.temp}}-build-type-${{ matrix.build_type }}-vcpkg-${{ env.vcpkg_TAG }}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }}-qhull-${{ env.Qhull_TAG }}-casADi-${{ env.CasADi_TAG }}-manif-${{ env.manif_TAG }}-matioCpp-${{ env.matioCpp_TAG }}
+        key: source-deps-${{ runner.os }}-${{runner.temp}}-build-type-${{ matrix.build_type }}-vcpkg-${{ env.vcpkg_TAG }}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }}-qhull-${{ env.Qhull_TAG }}-casADi-${{ env.CasADi_TAG }}-manif-${{ env.manif_TAG }}-matioCpp-${{ env.matioCpp_TAG }}-LieGroupControllers-${{ env.LieGroupControllers_TAG }}
 
 
     - name: Source-based Dependencies [Windows]
@@ -209,6 +211,18 @@ jobs:
                     -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
         cmake --build . --config ${{ matrix.build_type }} --target install
 
+        # LieGroupControllers
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/dic-iit/lie-group-controllers
+        cd lie-group-controllers
+        git checkout ${LieGroupControllers_TAG}
+        mkdir build
+        cd build
+        cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \
+                     -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \
+                     -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
+                    -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
+        cmake --build . --config ${{ matrix.build_type }} --target install
 
     - name: Source-based Dependencies [Ubuntu/macOS]
       if: steps.cache-source-deps.outputs.cache-hit != 'true' && (matrix.os == 'ubuntu-latest' || matrix.os == 'macOS-latest')
@@ -273,6 +287,16 @@ jobs:
         cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
         cmake --build . --config ${{ matrix.build_type }} --target install
 
+        # LieGroupControllers
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/dic-iit/lie-group-controllers
+        cd lie-group-controllers
+        git checkout ${LieGroupControllers_TAG}
+        mkdir build
+        cd build
+        cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
+        cmake --build . --config ${{ matrix.build_type }} --target install
+
     - name: Source-based Dependencies [Ubuntu]
       if: steps.cache-source-deps.outputs.cache-hit != 'true' && matrix.os == 'ubuntu-latest'
       shell: bash

CHANGELOG.md

@@ -29,5 +29,7 @@ All notable changes to this project are documented in this file.
 - Implement Continuous algebraic Riccati equation function (https://github.com/dic-iit/bipedal-locomotion-framework/pull/157)
 - Implement YARP based `ROSPublisher` in the `YarpUtilities` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/156)
 - Implement example YARP device `ROSPublisherTestDevice` for understanding the usage of `ROSPublisher`. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/160)
+- Implement `TSID` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/167)
+
 
 [Unreleased]: https://github.com/dic-iit/bipedal-locomotion-framework/

CMakeLists.txt

@@ -111,6 +111,10 @@ if (FRAMEWORK_USE_matioCpp)
   list(APPEND FRAMEWORK_PUBLIC_DEPENDENCIES matioCpp)
 endif()
 
+if (FRAMEWORK_USE_LieGroupControllers)
+  list(APPEND FRAMEWORK_PUBLIC_DEPENDENCIES LieGroupControllers)
+endif()
+
 include(InstallBasicPackageFiles)
 install_basic_package_files(${PROJECT_NAME}
                             NAMESPACE BipedalLocomotion::

cmake/BipedalLocomotionFrameworkFindDependencies.cmake

@@ -162,6 +162,9 @@ checkandset_dependency(pytest)
 find_package(matioCpp QUIET)
 checkandset_dependency(matioCpp)
 
+find_package(LieGroupControllers QUIET)
+checkandset_dependency(LieGroupControllers)
+
 framework_dependent_option(FRAMEWORK_COMPILE_YarpUtilities
   "Compile YarpHelper library?" ON
   "FRAMEWORK_USE_YARP" OFF)
@@ -210,6 +213,10 @@ framework_dependent_option(FRAMEWORK_COMPILE_matioCppConversions
   "Compile matioCpp Conversions libraries?" ON
   "FRAMEWORK_USE_matioCpp" OFF)
 
+framework_dependent_option(FRAMEWORK_COMPILE_TSID
+  "Compile TSID library?" ON
+  "FRAMEWORK_COMPILE_System;FRAMEWORK_USE_LieGroupControllers;FRAMEWORK_COMPILE_ManifConversions;FRAMEWORK_USE_manif" OFF)
+
 framework_dependent_option(FRAMEWORK_COMPILE_JointPositionTrackingApplication
   "Compile joint-position-tracking application?" ON
   "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_Planners;FRAMEWORK_COMPILE_RobotInterface" OFF)

src/CMakeLists.txt

@@ -13,3 +13,4 @@ add_subdirectory(ContactModels)
 add_subdirectory(AutoDiff)
 add_subdirectory(RobotInterface)
 add_subdirectory(Math)
+add_subdirectory(TSID)

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

@@ -57,7 +57,46 @@ namespace Conversions
     inline manif::SE3d toManifPose(const iDynTree::Transform& H)
     {
         return toManifPose(iDynTree::toEigen(H.getRotation()),
-                           iDynTree::toEigen(H.getPosition()));;
+                           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 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));
     }
 
 } // namespace Conversions

src/System/include/BipedalLocomotion/System/VariablesHandler.h

@@ -8,25 +8,34 @@
 #include <string>
 #include <unordered_map>
 
-#include <iDynTree/Core/Utils.h>
-
 #ifndef BIPEDAL_LOCOMOTION_SYSTEM_VARIABLES_HANDLER_H
 #define BIPEDAL_LOCOMOTION_SYSTEM_VARIABLES_HANDLER_H
 
 namespace BipedalLocomotion
 {
 namespace System
 {
+
 /**
  * VariableHandler is useful to handle variables in an optimization problem, Their name, dimension
  * and position
  */
 class VariablesHandler
 {
+public:
+    struct VariableDescription
+    {
+        std::ptrdiff_t offset;
+        std::ptrdiff_t size;
 
-    std::unordered_map<std::string, iDynTree::IndexRange> m_variables; /**< Map containing the name
-                                                                          of a variable and its
-                                                                          index range */
+        bool isValid() const;
+        static VariableDescription InvalidVariable();
+    };
+
+private:
+    std::unordered_map<std::string, VariableDescription> m_variables; /**< Map containing the name
+                                                                         of a variable and its
+                                                                         index range */
     std::size_t m_numberOfVariables{0}; /**< Total number of Variable seen as scalar */
 
 public:
@@ -43,7 +52,15 @@ class VariablesHandler
      * @param name of the variable
      * @return the index range associated to the variable
      */
-    iDynTree::IndexRange getVariable(const std::string& name) const noexcept;
+    VariableDescription getVariable(const std::string& name) const noexcept;
+
+    /**
+     * Get a variable from the list
+     * @param name of the variable
+     * @param[out] description the description of the variable
+     * @return true/false in case of success/failure
+     */
+    bool getVariable(const std::string& name, VariableDescription& description) const noexcept;
 
     /**
      * Get the number of variables

src/System/src/VariablesHandler.cpp

@@ -10,6 +10,18 @@
 
 using namespace BipedalLocomotion::System;
 
+bool VariablesHandler::VariableDescription::isValid() const
+{
+    return (offset >= 0) && (size >= 0);
+}
+
+VariablesHandler::VariableDescription VariablesHandler::VariableDescription::InvalidVariable()
+{
+    VariablesHandler::VariableDescription tmp;
+    tmp.offset = tmp.size = -1;
+    return tmp;
+}
+
 bool VariablesHandler::addVariable(const std::string& name, const std::size_t& size) noexcept
 {
     // if the variable already exist cannot be added again.
@@ -20,7 +32,7 @@ bool VariablesHandler::addVariable(const std::string& name, const std::size_t& s
         return false;
     }
 
-    iDynTree::IndexRange indexRange;
+    VariablesHandler::VariableDescription indexRange;
     indexRange.size = size;
     indexRange.offset = m_numberOfVariables;
     m_variables.emplace(name, indexRange);
@@ -29,7 +41,8 @@ bool VariablesHandler::addVariable(const std::string& name, const std::size_t& s
     return true;
 }
 
-iDynTree::IndexRange VariablesHandler::getVariable(const std::string& name) const noexcept
+VariablesHandler::VariableDescription VariablesHandler::getVariable(const std::string& name) const
+    noexcept
 {
     auto variable = m_variables.find(name);
 
@@ -39,7 +52,14 @@ iDynTree::IndexRange VariablesHandler::getVariable(const std::string& name) cons
     if (variable != m_variables.end())
         return variable->second;
     else
-        return iDynTree::IndexRange::InvalidRange();
+        return VariablesHandler::VariableDescription::InvalidVariable();
+}
+
+bool VariablesHandler::getVariable(const std::string& name,
+                                   VariablesHandler::VariableDescription& description) const
+    noexcept
+{
+    return (description = this->getVariable(name)).isValid();
 }
 
 const std::size_t& VariablesHandler::getNumberOfVariables() const noexcept

src/TSID/CMakeLists.txt

@@ -0,0 +1,17 @@
+# 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.
+
+if(FRAMEWORK_COMPILE_TSID)
+
+  set(H_PREFIX include/BipedalLocomotion/TSID)
+
+  add_bipedal_locomotion_library(
+    NAME                  TSID
+    PUBLIC_HEADERS        ${H_PREFIX}/Task.h ${H_PREFIX}/SE3Task.h
+    SOURCES               src/Task.cpp src/SE3Task.cpp
+    PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System LieGroupControllers::LieGroupControllers MANIF::manif iDynTree::idyntree-high-level iDynTree::idyntree-model
+    PRIVATE_LINK_LIBRARIES BipedalLocomotion::ManifConversions
+    SUBDIRECTORIES         tests)
+
+endif()

src/TSID/include/BipedalLocomotion/TSID/SE3Task.h

@@ -0,0 +1,100 @@
+/**
+ * @file SE3Task.h
+ * @authors Giulio Romualdi
+ * @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_TSID_SE3_TASK_H
+#define BIPEDAL_LOCOMOTION_TSID_SE3_TASK_H
+
+#include <manif/manif.h>
+
+#include <BipedalLocomotion/TSID/Task.h>
+
+#include <LieGroupControllers/ProportionalDerivativeController.h>
+
+namespace BipedalLocomotion
+{
+namespace TSID
+{
+
+/**
+ * SE3Task is a concrete implementation of the OptimalControlElement. Please use this element if you
+ * want to control the position and the orientation of a given frame rigidly attached to the robot.
+ * The task assumes perfect control of the robot acceleration \f$\dot{\nu}\f$ that contains the base
+ * linear and angular acceleration expressed in mixed representation and the joints acceleration.
+ * The task represents the following equation
+ * \f[
+ * J \dot{\nu} + \dot{J} \nu = \dot{\mathrm{v}} ^ *
+ * \f]
+ * where \f$J\f$ is the robot jacobian and \f$\dot{\mathrm{v}} ^ *\f$ is the desired acceleration.
+ * The desired acceleration is chosen such that the frame will asymptotically converge to the
+ * desired trajectory. The linear component of \f$\dot{\mathrm{v}} ^ *\f$ is computed with a
+ * standard PD controller in \f$R^3\f$ while the angular acceleration is computed by a PD controller
+ * in \f$SO(3)\f$.
+ * @note Please refer to https://github.com/dic-iit/lie-group-controllers if you are interested in
+ * the implementation of the PD controllers.
+ * @note The SE3Task is technically not a \f$SE(3)\f$ space defined task, instead is a \f$SO(3)
+ * \times \mathbb{R}^3\f$ task. Theoretically, there are differences between the two due to the
+ * different definitions of exponential maps and logarithm maps. Please consider that here the MIXED
+ * representation is used to define the 6d-velocity. You can find further details in Section 2.3.4
+ * of https://traversaro.github.io/phd-thesis/traversaro-phd-thesis.pdf.
+ */
+class SE3Task : public Task
+{
+    LieGroupControllers::ProportionalDerivativeControllerSO3d m_SO3Controller; /**< PD Controller in
+                                                                                  SO(3) */
+    LieGroupControllers::ProportionalDerivativeControllerR3d m_R3Controller; /**< PD Controller in
+                                                                                  R3 */
+
+    System::VariablesHandler::VariableDescription m_robotAccelerationVariable; /**< Variable
+                                                                                  describing the
+                                                                                  robot acceleration
+                                                                                  (base + joint) */
+
+    iDynTree::FrameIndex m_frameIndex; /**< Frame controlled by the OptimalControlElement */
+
+    static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. */
+
+public:
+    /**
+     * Initialize the planner.
+     * @param paramHandler pointer to the parameters handler.
+     * @param variablesHandler reference to a variables handler.
+     * @note the following parameters are required by the class
+     * |           Parameter Name           |   Type   |                                       Description                                      | Mandatory |
+     * |:----------------------------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:|
+     * | `robot_acceleration_variable_name` | `string` | Name of the variable contained in `VariablesHandler` describing the robot acceleration |    Yes    |
+     * |            `frame_name`            | `string` |                       Name of the frame controlled by the SE3Task                      |    Yes    |
+     * |             `kp_linear`            | `double` |                             Gain of the position controller                            |    Yes    |
+     * |             `kd_linear`            | `double` |                         Gain of the linear velocity controller                         |    Yes    |
+     * |            `kp_angular`            | `double` |                           Gain of the orientation controller                           |    Yes    |
+     * |            `kd_angular`            | `double` |                         Gain of the angular velocity controller                        |    Yes    |
+     * @return True in case of success, false otherwise.
+     */
+    bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> paramHandler,
+                    const System::VariablesHandler& variablesHandler) override;
+
+    /**
+     * Update the content of the element.
+     * @return True in case of success, false otherwise.
+     */
+    bool update() override;
+
+    /**
+     * Set the desired reference trajectory.
+     * @param I_H_F Homogeneous transform between the link and the inertial frame.
+     * @param mixedVelocity 6D-velocity written in mixed representation.
+     * @param mixedAcceleration 6D-acceleration written in mixed representation.
+     * @return True in case of success, false otherwise.
+     */
+    bool setReferenceTrajectory(const manif::SE3d& I_H_F,
+                                const manif::SE3d::Tangent& mixedVelocity,
+                                const manif::SE3d::Tangent& mixedAcceleration);
+};
+
+} // namespace TSID
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_TSID_SE3_TASK_H