diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1debff751d..f6da3229a4 100644 --- a/.github/workflows/ci.yml +++ b/.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 diff --git a/CHANGELOG.md b/CHANGELOG.md index 13bf2d60f6..ea4440b069 100644 --- a/CHANGELOG.md +++ b/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/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 900cd7f577..3e8f55c88d 100644 --- a/CMakeLists.txt +++ b/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:: diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index 9140b6fa1b..3d357af1dc 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/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) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 660783c50e..9951b213e0 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -13,3 +13,4 @@ add_subdirectory(ContactModels) add_subdirectory(AutoDiff) add_subdirectory(RobotInterface) add_subdirectory(Math) +add_subdirectory(TSID) diff --git a/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h b/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h index ceef81cf8c..d9934a97ba 100644 --- a/src/Conversions/include/BipedalLocomotion/Conversions/ManifConversions.h +++ b/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 + 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 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 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 diff --git a/src/System/include/BipedalLocomotion/System/VariablesHandler.h b/src/System/include/BipedalLocomotion/System/VariablesHandler.h index c9a6850dd1..489b02c007 100644 --- a/src/System/include/BipedalLocomotion/System/VariablesHandler.h +++ b/src/System/include/BipedalLocomotion/System/VariablesHandler.h @@ -8,8 +8,6 @@ #include #include -#include - #ifndef BIPEDAL_LOCOMOTION_SYSTEM_VARIABLES_HANDLER_H #define BIPEDAL_LOCOMOTION_SYSTEM_VARIABLES_HANDLER_H @@ -17,16 +15,27 @@ 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 m_variables; /**< Map containing the name - of a variable and its - index range */ + bool isValid() const; + static VariableDescription InvalidVariable(); + }; + +private: + std::unordered_map 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 diff --git a/src/System/src/VariablesHandler.cpp b/src/System/src/VariablesHandler.cpp index 384877687e..e892309db1 100644 --- a/src/System/src/VariablesHandler.cpp +++ b/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 diff --git a/src/TSID/CMakeLists.txt b/src/TSID/CMakeLists.txt new file mode 100644 index 0000000000..16c0ceb74c --- /dev/null +++ b/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() diff --git a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h new file mode 100644 index 0000000000..a40579c6d2 --- /dev/null +++ b/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 + +#include + +#include + +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 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 diff --git a/src/TSID/include/BipedalLocomotion/TSID/Task.h b/src/TSID/include/BipedalLocomotion/TSID/Task.h new file mode 100644 index 0000000000..593c797a3b --- /dev/null +++ b/src/TSID/include/BipedalLocomotion/TSID/Task.h @@ -0,0 +1,106 @@ +/** + * @file Task.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_TASK_H +#define BIPEDAL_LOCOMOTION_TSID_TASK_H + +#include + +#include + +#include +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace TSID +{ + +/** + * Task describes a control problem element. The element is described by a matrix + * \f$A\f$ and a vector \f$b\f$. This class describes both a linear equality constraint and a linear + * inequality constraint. In case of equality constraint \f$ A \f$ and \f$ b \f$ represents: \f$ Ax + * = b\f$ In case of inequality constraint \f$ A \f$ and \f$ b \f$ represents: \f$ Ax \le b \f$ + * @note Please inherit this class if you want to build your own optimal control problem. + */ +class Task +{ +protected: + Eigen::MatrixXd m_A; /**< Element Matrix */ + Eigen::VectorXd m_b; /**< Element Vector */ + + std::string m_description{"Generic Optimal Control Element"}; /** m_kinDyn; /**< Pointer to a KinDynComputations + object */ + + /** + * Extract the submatrix A associated to a given variable. + */ + iDynTree::MatrixView + subA(const System::VariablesHandler::VariableDescription& description); + + /** + * Extract the submatrix A associated to a given variable. + */ + iDynTree::MatrixView + subA(const System::VariablesHandler::VariableDescription& description) const; + +public: + /** + * Set the kinDynComputations object. + * @param kinDyn pointer to a kinDynComputations object. + * @return True in case of success, false otherwise. + */ + bool setKinDyn(std::shared_ptr kinDyn); + + /** + * Initialize the optimal control element. + * @param paramHandler a pointer to the parameter handler containing all the information + * required by the specific element. Please refer to the documentation of the implemented + * version of the model for further details. + * @param variablesHandler class containing the list of all the optimization parameter used in + * the optimization problem. Please use the same variables handler to initialize all the + * Tasks. + * @return True in case of success, false otherwise. + */ + virtual bool initialize(std::weak_ptr paramHandler, + const System::VariablesHandler& variablesHandler); + + /** + * Update the content of the element. + * @return True in case of success, false otherwise. + */ + virtual bool update(); + + /** + * Get the matrix A. + * @return a const reference to the matrix A. + */ + Eigen::Ref getA() const; + + /** + * Get the vector b. + * @return a const reference to the vector b. + */ + Eigen::Ref getB() const; + + /** + * Get the description of the element. + * @return a string containing the description of the element. + */ + const std::string& getDescription() const; +}; + +} // namespace TSID +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_TSID_TASK_H diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp new file mode 100644 index 0000000000..8397a8e820 --- /dev/null +++ b/src/TSID/src/SE3Task.cpp @@ -0,0 +1,156 @@ +/** + * @file SE3Task.cpp + * @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. + */ + +#include +#include + +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::TSID; + +bool SE3Task::initialize(std::weak_ptr paramHandler, + const System::VariablesHandler& variablesHandler) +{ + constexpr std::string_view errorPrefix = "[SE3Task::initialize] "; + + std::string frameName = "Unknown"; + constexpr std::string_view descriptionPrefix = "SE3Task Optimal Control Element - Frame name: "; + + if(m_kinDyn == nullptr || !m_kinDyn->isValid()) + { + std::cerr << errorPrefix << descriptionPrefix << frameName + << " - KinDynComputations object is not valid." << std::endl; + return false; + } + + if (m_kinDyn->getFrameVelocityRepresentation() + != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION) + { + std::cerr << errorPrefix << descriptionPrefix << frameName + << " - The task supports only quantities expressed in MIXED representation. " + "Please provide a KinDynComputations with Frame velocity representation set " + "to MIXED_REPRESENTATION." + << std::endl; + return false; + } + + auto ptr = paramHandler.lock(); + if(ptr == nullptr) + { + std::cerr << errorPrefix << descriptionPrefix << frameName + << " - The parameter handler is not valid." << std::endl; + return false; + } + + std::string robotAccelerationVariableName; + if (!ptr->getParameter("robot_acceleration_variable_name", robotAccelerationVariableName) + || !variablesHandler.getVariable(robotAccelerationVariableName, + m_robotAccelerationVariable)) + { + std::cerr << errorPrefix << descriptionPrefix << frameName + << " - Error while retrieving the robot acceleration variable." << std::endl; + return false; + } + + if (m_robotAccelerationVariable.size + != m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize) + { + std::cerr << errorPrefix << descriptionPrefix << frameName + << " - Error while retrieving the robot acceleration variable." << std::endl; + return false; + } + + if (!ptr->getParameter("frame_name", frameName) + || (m_frameIndex = m_kinDyn->model().getFrameIndex(frameName)) + == iDynTree::FRAME_INVALID_INDEX) + { + std::cerr << errorPrefix << descriptionPrefix << frameName + << "- Error while retrieving the frame that should be controlled." << std::endl; + return false; + } + + // set the gains for the controllers + double kpLinear, kdLinear; + double kpAngular, kdAngular; + if (!ptr->getParameter("kp_linear", kpLinear) || !ptr->getParameter("kd_linear", kdLinear)) + { + std::cerr << errorPrefix << descriptionPrefix << frameName + << " - Error while the gains for the position controller are retrieved." + << std::endl; + return false; + } + + if (!ptr->getParameter("kp_angular", kpAngular) || !ptr->getParameter("kd_angular", kdAngular)) + { + std::cerr << errorPrefix << descriptionPrefix << frameName + << " - Error while the gains for the rotation controller are retrieved." + << std::endl; + return false; + } + + m_R3Controller.setGains({kpLinear, kdLinear}); + m_SO3Controller.setGains({kpAngular, kdAngular}); + + // set the description + m_description = std::string(descriptionPrefix) + frameName + "."; + + // resize the matrices + m_A.resize(m_spatialVelocitySize, variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(m_spatialVelocitySize); + + return true; +} + +bool SE3Task::update() +{ + m_b = -iDynTree::toEigen(m_kinDyn->getFrameBiasAcc(m_frameIndex)); + + m_SO3Controller.setState( + {BipedalLocomotion::Conversions::toManifRot( + m_kinDyn->getWorldTransform(m_frameIndex).getRotation()), + iDynTree::toEigen(m_kinDyn->getFrameVel(m_frameIndex).getAngularVec3())}); + + m_R3Controller.setState( + {iDynTree::toEigen(m_kinDyn->getWorldTransform(m_frameIndex).getPosition()), + iDynTree::toEigen(m_kinDyn->getFrameVel(m_frameIndex).getLinearVec3())}); + + m_SO3Controller.computeControlLaw(); + m_R3Controller.computeControlLaw(); + + m_b.head<3>() += m_R3Controller.getControl().coeffs(); + m_b.tail<3>() += m_SO3Controller.getControl().coeffs(); + + // Workaround because matrix view is not compatible with Eigen::Ref + // https://github.com/robotology/idyntree/issues/797 + if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, + this->subA(m_robotAccelerationVariable))) + { + std::cerr << "[SE3Task::update] Unable to get the jacobian." << std::endl; + return false; + } + + return true; +} + +bool SE3Task::setReferenceTrajectory(const manif::SE3d& I_H_F, + const manif::SE3d::Tangent& mixedVelocity, + const manif::SE3d::Tangent& mixedAcceleration) +{ + + bool ok = true; + ok = ok && m_R3Controller.setDesiredState({I_H_F.translation(), mixedVelocity.v()}); + ok = ok && m_R3Controller.setFeedForward(mixedAcceleration.v()); + + ok = ok && m_SO3Controller.setDesiredState({I_H_F.quat(), mixedVelocity.w()}); + ok = ok && m_SO3Controller.setFeedForward(mixedAcceleration.w()); + + return ok; +} diff --git a/src/TSID/src/Task.cpp b/src/TSID/src/Task.cpp new file mode 100644 index 0000000000..545d7bc664 --- /dev/null +++ b/src/TSID/src/Task.cpp @@ -0,0 +1,55 @@ +/** + * @file Task.cpp + * @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. + */ + +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::TSID; + +iDynTree::MatrixView Task::subA(const VariablesHandler::VariableDescription& description) +{ + return iDynTree::MatrixView(m_A).block(0, description.offset, 6, description.size); +} + +iDynTree::MatrixView +Task::subA(const VariablesHandler::VariableDescription& description) const +{ + return iDynTree::MatrixView(m_A).block(0, description.offset, 6, description.size); +} + +bool Task::setKinDyn(std::shared_ptr kinDyn) +{ + m_kinDyn = kinDyn; + return (m_kinDyn != nullptr) && (m_kinDyn->isValid()); +} + +bool Task::initialize(std::weak_ptr paramHandler, + const VariablesHandler& variablesHandler) +{ + return true; +} + +bool Task::update() +{ + return true; +} + +Eigen::Ref Task::getA() const +{ + return m_A; +} + +Eigen::Ref Task::getB() const +{ + return m_b; +} + +const std::string& Task::getDescription() const +{ + return m_description; +} diff --git a/src/TSID/tests/CMakeLists.txt b/src/TSID/tests/CMakeLists.txt new file mode 100644 index 0000000000..9123fc0316 --- /dev/null +++ b/src/TSID/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 SE3Task + SOURCES SE3TaskTest.cpp + LINKS BipedalLocomotion::TSID BipedalLocomotion::ManifConversions) diff --git a/src/TSID/tests/SE3TaskTest.cpp b/src/TSID/tests/SE3TaskTest.cpp new file mode 100644 index 0000000000..48c7a2e362 --- /dev/null +++ b/src/TSID/tests/SE3TaskTest.cpp @@ -0,0 +1,147 @@ +/** + * @file SE3TaskTest.cpp + * @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. + */ + +// Catch2 +#include + +// BipedalLocomotion +#include +#include +#include +#include + +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::TSID; + +TEST_CASE("SE3 Task") +{ + constexpr double kp = 1.0; + constexpr double kd = 0.5; + const std::string robotAcceleration = "robotAcceleration"; + + + auto kinDyn = std::make_shared(); + auto parameterHandler = std::make_shared(); + + parameterHandler->setParameter("robot_acceleration_variable_name", + robotAcceleration); + + parameterHandler->setParameter("kp_linear", kp); + parameterHandler->setParameter("kd_linear", kd); + parameterHandler->setParameter("kp_angular", kp); + parameterHandler->setParameter("kd_angular", kd); + + // set the velocity representation + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)); + + for (std::size_t numberOfJoints = 6; numberOfJoints < 1000; numberOfJoints += 15) + { + DYNAMIC_SECTION("Model with " << numberOfJoints << " joints") + { + // create the model + const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); + REQUIRE(kinDyn->loadRobotModel(model)); + + const auto worldBasePos = iDynTree::getRandomTransform(); + const auto baseVel = iDynTree::getRandomTwist(); + iDynTree::VectorDynSize jointsPos(model.getNrOfDOFs()); + iDynTree::VectorDynSize jointsVel(model.getNrOfDOFs()); + iDynTree::Vector3 gravity; + + for (auto& joint : jointsPos) + { + joint = iDynTree::getRandomDouble(); + } + + for (auto& joint : jointsVel) + { + joint = iDynTree::getRandomDouble(); + } + + for (auto& element : gravity) + { + element = iDynTree::getRandomDouble(); + } + + REQUIRE(kinDyn->setRobotState(worldBasePos, jointsPos, baseVel, jointsVel, gravity)); + + // Instantiate the handler + VariablesHandler variablesHandler; + variablesHandler.addVariable("dummy1", 10); + variablesHandler.addVariable(robotAcceleration, model.getNrOfDOFs() + 6); + variablesHandler.addVariable("dummy2", 15); + + const std::string controlledFrame = model.getFrameName(numberOfJoints); + parameterHandler->setParameter("frame_name", controlledFrame); + + SE3Task task; + REQUIRE(task.setKinDyn(kinDyn)); + REQUIRE(task.initialize(parameterHandler, variablesHandler)); + + const auto desiredPose = manif::SE3d::Random(); + const auto desiredVelocity = manif::SE3d::Tangent::Random(); + const auto desiredAcceleration = manif::SE3d::Tangent::Random(); + + REQUIRE(task.setReferenceTrajectory(desiredPose, desiredVelocity, desiredAcceleration)); + + REQUIRE(task.update()); + + // get A and b + Eigen::Ref A = task.getA(); + Eigen::Ref b = task.getB(); + + // check the matrix A + REQUIRE(A.middleCols(variablesHandler.getVariable("dummy1").offset, + variablesHandler.getVariable("dummy1").size) + .isZero()); + + REQUIRE(A.middleCols(variablesHandler.getVariable("dummy2").offset, + variablesHandler.getVariable("dummy2").size) + .isZero()); + + Eigen::MatrixXd jacobian(6, model.getNrOfDOFs() + 6); + REQUIRE(kinDyn->getFrameFreeFloatingJacobian(controlledFrame, jacobian)); + + REQUIRE(A.middleCols(variablesHandler.getVariable(robotAcceleration).offset, + variablesHandler.getVariable(robotAcceleration).size) + .isApprox(jacobian)); + + // check the vector b + LieGroupControllers::ProportionalDerivativeControllerSO3d SO3Controller; + LieGroupControllers::ProportionalDerivativeControllerR3d R3Controller; + SO3Controller.setGains({kp, kd}); + R3Controller.setGains({kp, kd}); + + SO3Controller.setFeedForward(desiredAcceleration.w()); + R3Controller.setFeedForward(desiredAcceleration.v()); + SO3Controller.setDesiredState({desiredPose.quat(), desiredVelocity.w()}); + R3Controller.setDesiredState({desiredPose.translation(), desiredVelocity.v()}); + + SO3Controller.setState({BipedalLocomotion::Conversions::toManifRot( + kinDyn->getWorldTransform(controlledFrame).getRotation()), + iDynTree::toEigen(kinDyn->getFrameVel(controlledFrame).getAngularVec3())}); + + R3Controller.setState( + {iDynTree::toEigen(kinDyn->getWorldTransform(controlledFrame).getPosition()), + iDynTree::toEigen(kinDyn->getFrameVel(controlledFrame).getLinearVec3())}); + + SO3Controller.computeControlLaw(); + R3Controller.computeControlLaw(); + + Eigen::VectorXd expectedB(6); + expectedB = -iDynTree::toEigen(kinDyn->getFrameBiasAcc(controlledFrame)); + expectedB.head<3>() += R3Controller.getControl().coeffs(); + expectedB.tail<3>() += SO3Controller.getControl().coeffs(); + + REQUIRE(b.isApprox(expectedB)); + } + } +}