Implement the Inverse kinematics in the repository

.github/workflows/ci.yml

@@ -16,10 +16,12 @@ env:
   iDynTree_TAG: v3.0.0
   Catch2_TAG: v2.13.4
   Qhull_TAG: 2020.2
-  CasADi_TAG: 3.5.1
+  CasADi_TAG: 3.5.5.2
   manif_TAG: 0.0.3
   matioCpp_TAG: v0.1.0
   LieGroupControllers_TAG: v0.0.1
+  osqp_TAG: v0.6.2
+  OsqpEigen_TAG: 9b0091038cf0fb9099a5d6c55820d6e5846034a7
 
   # Overwrite the VCPKG_INSTALLATION_ROOT env variable defined by GitHub Actions to point to our vcpkg
   VCPKG_INSTALLATION_ROOT: C:\robotology\vcpkg
@@ -111,7 +113,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}}-os-${{ matrix.os }}-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 }}
+        key: source-deps-${{ runner.os }}-${{runner.temp}}-os-${{ matrix.os }}-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 }}-osqp-${{ env.osqp_TAG }}-osqp-eigen-${{ env.OsqpEigen_TAG }}
 
 
     - name: Source-based Dependencies [Windows]
@@ -170,12 +172,38 @@ jobs:
                      -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
         cmake --build . --config ${{ matrix.build_type }} --target install
 
+        # osqp
+        cd ${GITHUB_WORKSPACE}
+        git clone --recursive -b ${osqp_TAG} https://github.com/oxfordcontrol/osqp
+        cd osqp
+        mkdir -p 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
+
+        # osqp-eigen
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/osqp-eigen
+        cd osqp-eigen
+        git checkout ${OsqpEigen_TAG}
+        mkdir -p 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
+
+
         # CasADi
         # We override the casadi installation structure to be compliant with the folder structure used for
         # the other dependencies. Please check:
         # https://github.com/casadi/casadi/blob/6f122ca22e2a869903628c5738f154c8ac0f7455/CMakeLists.txt#L317
         cd ${GITHUB_WORKSPACE}
-        git clone https://github.com/casadi/casadi.git -b ${CasADi_TAG} casadi
+        git clone https://github.com/dic-iit/casadi.git -b ${CasADi_TAG} casadi
         cd casadi
         mkdir build
         cd build
@@ -187,10 +215,10 @@ jobs:
                      -DCMAKE_PREFIX:PATH="lib/cmake/casadi" \
                      -DLIB_PREFIX:PATH="lib" \
                      -DBIN_PREFIX:PATH="bin" \
-                     -DWITH_IPOPT=BOOL:ON ..
+                     -DWITH_IPOPT=BOOL:ON \
+                     -DWITH_OSQP:BOOL=ON -DUSE_SYSTEM_WISE_OSQP=BOOL:ON ..
         cmake --build . --config ${{ matrix.build_type }} --target install
 
-
         # manif
         cd ${GITHUB_WORKSPACE}
         git clone https://github.com/artivis/manif.git
@@ -264,13 +292,35 @@ jobs:
               -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
         cmake --build . --config ${{ matrix.build_type }} --target install
 
+        # osqp
+        cd ${GITHUB_WORKSPACE}
+        git clone --recursive -b ${osqp_TAG} https://github.com/oxfordcontrol/osqp
+        cd osqp
+        mkdir -p build
+        cd build
+        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
+
+        # osqp-eigen
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/osqp-eigen
+        cd osqp-eigen
+        git checkout ${OsqpEigen_TAG}
+        mkdir -p build
+        cd build
+        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
+
         # CasADi
         cd ${GITHUB_WORKSPACE}
-        git clone https://github.com/casadi/casadi.git -b ${CasADi_TAG} casadi
+        git clone https://github.com/dic-iit/casadi.git -b ${CasADi_TAG} casadi
         cd casadi
         mkdir build
         cd build
-        cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps -DWITH_IPOPT=BOOL:ON ..
+        cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps -DWITH_IPOPT=BOOL:ON \
+              -DWITH_OSQP:BOOL=ON -DUSE_SYSTEM_WISE_OSQP=BOOL:ON ..
         cmake --build . --config ${{ matrix.build_type }} --target install
 
         # manif

.github/workflows/conda-forge-ci.yml

@@ -5,7 +5,7 @@ on:
   pull_request:
   schedule:
   # * is a special character in YAML so you have to quote this string
-  # Execute a "nightly" build at 2 AM UTC 
+  # Execute a "nightly" build at 2 AM UTC
   - cron:  '0 2 * * *'
 
 jobs:
@@ -30,33 +30,35 @@ jobs:
     - name: Dependencies
       shell: bash -l {0}
       run: |
-        # Compilation related dependencies 
+        # Compilation related dependencies
         mamba install cmake compilers make ninja pkg-config
         # Actual dependencies
         mamba install -c robotology-staging idyntree yarp matio-cpp lie-group-controllers eigen qhull casadi cppad manif spdlog catch2
-        
+
     - name: Configure [Linux&macOS]
       if: contains(matrix.os, 'macos') || contains(matrix.os, 'ubuntu')
       shell: bash -l {0}
       run: |
         mkdir -p build
         cd build
-        cmake -GNinja -DBUILD_TESTING:BOOL=ON -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} ..
-
+        cmake -GNinja -DBUILD_TESTING:BOOL=ON -DFRAMEWORK_COMPILE_IK:BOOL=OFF \
+              -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} ..
+
     - name: Configure [Windows]
       if: contains(matrix.os, 'windows')
       shell: bash -l {0}
       run: |
         mkdir -p build
         cd build
-        cmake -G"Visual Studio 16 2019" -DBUILD_TESTING:BOOL=ON -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} ..
+        cmake -G"Visual Studio 16 2019" -DBUILD_TESTING:BOOL=ON -DFRAMEWORK_COMPILE_IK:BOOL=OFF \
+              -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} ..
 
-    - name: Build 
+    - name: Build
       shell: bash -l {0}
       run: |
         cd build
         cmake --build . --config ${{ matrix.build_type }}
-        
+
     - name: Test
       shell: bash -l {0}
       run: |

CHANGELOG.md

@@ -14,6 +14,7 @@ All notable changes to this project are documented in this file.
 - Implement `ICameraBridge` and `IPointCloudBridge` interface classes as a part of `PerceptionInterface` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/165)
 - Implement `RealSense` driver class as a part of `PerceptionCapture` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/165)
 - Implement `realsense-test` utility application. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/165)
+- Implement the inverse kinematics component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/229)
 
 ### Changed
 - Move all the Contacts related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/204)

README.md

@@ -54,6 +54,7 @@ The **bipedal-locomotion-framework** project consists of several components. The
 |        [Estimators](./src/Estimators)        |                 Library containing observers                 |                              -                               |
 |  [FloatingBaseEstimator](./src/Estimators)   |         Library containing floating base estimators          |         [`manif`](https://github.com/artivis/manif)          |
 |  [GenericContainer](./src/GenericContainer)  |      Data structure similar to ``span`` but resizable.       |                              -                               |
+|               [IK](./src/IK)                 |                      Inverse kinematics                      | [`manif`](https://github.com/artivis/manif) [`osqp-eigen`](https://github.com/robotology/osqp-eigen) |
 |              [Math](./src/Math)              |          Library containing mathematical algorithms          |                              -                               |
 | [ParametersHandler](./src/ParametersHandler) |  Library for retrieving parameters from configuration files  | [`YARP`](https://www.yarp.it/git-master/) (only if you want the `YARP` implementation) |
 |          [Planners](./src/Planners)          |       Library containing planner useful for locomotion       | [`manif`](https://github.com/artivis/manif) [`CasADi`](https://web.casadi.org/) [`qhull`](http://www.qhull.org/) |

cmake/BipedalLocomotionFrameworkDependencies.cmake

@@ -29,6 +29,9 @@ checkandset_dependency(cppad)
 find_package(manif QUIET)
 checkandset_dependency(manif)
 
+find_package(OsqpEigen QUIET)
+checkandset_dependency(OsqpEigen)
+
 find_package(Python3 3.6 COMPONENTS Interpreter Development QUIET)
 checkandset_dependency(Python3 MINIMUM_VERSION 3.6 COMPONENTS Interpreter Development)
 
@@ -116,6 +119,10 @@ 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_IK
+  "Compile IK library?" ON
+  "FRAMEWORK_COMPILE_System;FRAMEWORK_USE_LieGroupControllers;FRAMEWORK_COMPILE_ManifConversions;FRAMEWORK_USE_manif;FRAMEWORK_USE_OsqpEigen" 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

@@ -17,3 +17,4 @@ add_subdirectory(RobotInterface)
 add_subdirectory(Math)
 add_subdirectory(TSID)
 add_subdirectory(Perception)
+add_subdirectory(IK)

src/IK/CMakeLists.txt

@@ -0,0 +1,21 @@
+# 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_IK)
+
+  set(H_PREFIX include/BipedalLocomotion/IK)
+
+  add_bipedal_locomotion_library(
+    NAME                   IK
+    PUBLIC_HEADERS         ${H_PREFIX}/LinearTask.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h
+    SOURCES                src/LinearTask.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/CoMTask.cpp src/IntegrationBasedIK.cpp src/QPInverseKinematics.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::TextLogging OsqpEigen::OsqpEigen BipedalLocomotion::ManifConversions
+    SUBDIRECTORIES         tests)
+
+endif()

src/IK/include/BipedalLocomotion/IK/CoMTask.h

@@ -0,0 +1,120 @@
+/**
+ * @file CoMTask.h
+ * @authors Giulio Romualdi
+ * @copyright 2021 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_IK_COM_TASK_H
+#define BIPEDAL_LOCOMOTION_IK_COM_TASK_H
+
+#include <memory>
+
+#include <manif/manif.h>
+
+#include <BipedalLocomotion/IK/LinearTask.h>
+
+#include <LieGroupControllers/ProportionalController.h>
+
+namespace BipedalLocomotion
+{
+namespace IK
+{
+
+/**
+ * SE3Task is a concrete implementation of the Task. 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 velocity \f$\nu\f$ that contains the base
+ * linear and angular velocity expressed in mixed representation and the joint velocities.
+ * The task represents the following equation
+ * \f[
+ * J \nu = \mathrm{v} ^ *
+ * \f]
+ * where \f$J\f$ is the robot jacobian and \f$\mathrm{v} ^ *\f$ is the desired velocity.
+ * The desired velocity is chosen such that the frame will asymptotically converge to the
+ * desired trajectory. The linear component of \f$\mathrm{v} ^ *\f$ is computed with a
+ * standard Proportional controller in \f$R^3\f$ while the angular velocity is computed
+ * by a Proportional 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 CoMTask : public LinearTask
+{
+    LieGroupControllers::ProportionalControllerR3d m_R3Controller; /**< P Controller in R3 */
+
+    System::VariablesHandler::VariableDescription m_robotVelocityVariable; /**< Variable
+                                                                              describing the robot
+                                                                              velocity (base +
+                                                                              joint) */
+
+    static constexpr std::size_t m_linearVelocitySize{3}; /**< Size of the linear velocity vector. */
+    static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. */
+
+    bool m_isInitialized{false}; /**< True if the task has been initialized. */
+
+public:
+
+    /**
+     * Initialize the task.
+     * @param paramHandler pointer to the parameters handler.
+     * @note the following parameters are required by the class
+     * |           Parameter Name           |   Type   |                                       Description                                      | Mandatory |
+     * |:----------------------------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:|
+     * |   `robot_velocity_variable_name`   | `string` |   Name of the variable contained in `VariablesHandler` describing the robot velocity   |    Yes    |
+     * |             `kp_linear`            | `double` |                             Gain of the position controller                            |    Yes    |
+     * @return True in case of success, false otherwise.
+     * Where the generalized robot velocity is a vector containing the base spatial-velocity
+     * (expressed in mixed representation) and the joint velocities.
+     * @return True in case of success, false otherwise.
+     */
+    bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> paramHandler) override;
+
+    /**
+     * Set the set of variables required by the task. The variables are stored in the
+     * System::VariablesHandler.
+     * @param variablesHandler reference to a variables handler.
+     * @note The handler must contain a variable named as the parameter
+     * `robot_velocity_variable_name` stored in the parameter handler. The variable represents the
+     * generalized velocity of the robot. Where the generalized robot velocity is a vector
+     * containing the base spatial-velocity (expressed in mixed representation) and the joints
+     * velocity.
+     * @return True in case of success, false otherwise.
+     */
+    bool setVariablesHandler(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 set-point of the trajectory.
+     * @param position position of the CoM
+     * @param velocity velocity of the CoM expressed in mixed representation
+     * @return True in case of success, false otherwise.
+     */
+    bool setSetPoint(Eigen::Ref<const Eigen::Vector3d> position, Eigen::Ref<const Eigen::Vector3d> velocity);
+
+    /**
+     * Get the size of the task. (I.e the number of rows of the vector b)
+     * @return the size of the task.
+     */
+    std::size_t size() const override;
+
+    /**
+     * The CoMTask is an equality task.
+     * @return the size of the task.
+     */
+    Type type() const override;
+};
+
+} // namespace IK
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_IK_SE3_TASK_H