From a8f6dfc05413ea3e72be9fbda7580cecee79c032 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 23 Dec 2020 19:45:38 +0100 Subject: [PATCH 01/19] Implement the skeleton of the OptimalControlElement class --- src/CMakeLists.txt | 1 + src/TSID/CMakeLists.txt | 11 +++ .../TSID/OptimalControlElement.h | 78 +++++++++++++++++++ src/TSID/src/OptimalControlElement.cpp | 38 +++++++++ 4 files changed, 128 insertions(+) create mode 100644 src/TSID/CMakeLists.txt create mode 100644 src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h create mode 100644 src/TSID/src/OptimalControlElement.cpp 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/TSID/CMakeLists.txt b/src/TSID/CMakeLists.txt new file mode 100644 index 0000000000..1cb2628ed2 --- /dev/null +++ b/src/TSID/CMakeLists.txt @@ -0,0 +1,11 @@ +# 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. + +set(H_PREFIX include/BipedalLocomotion/TSID) + +add_bipedal_locomotion_library( + NAME TSID + PUBLIC_HEADERS ${H_PREFIX}/OptimalControlElement.h + SOURCES src/OptimalControlElement.cpp + PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System) diff --git a/src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h b/src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h new file mode 100644 index 0000000000..b42d6cc4cf --- /dev/null +++ b/src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h @@ -0,0 +1,78 @@ +/** + * @file OptimalControlElement.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_OPTIMAL_CONTROL_ELEMENT_H +#define BIPEDAL_LOCOMOTION_TSID_OPTIMAL_CONTROL_ELEMENT_H + +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace TSID +{ + +/** + * OptimalControlElement 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 OptimalControlElement +{ +protected: + Eigen::MatrixXd m_A; /**< Element Matrix */ + Eigen::VectorXd m_b; /**< Element Vector */ + + std::string m_description{"Generic Optimal Control Element"}; /** 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 OptimalControlElement. + */ + const std::string& getDescription() const; +}; + +} // namespace TSID +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_TSID_OPTIMAL_CONTROL_ELEMENT_H diff --git a/src/TSID/src/OptimalControlElement.cpp b/src/TSID/src/OptimalControlElement.cpp new file mode 100644 index 0000000000..7dea66d6e1 --- /dev/null +++ b/src/TSID/src/OptimalControlElement.cpp @@ -0,0 +1,38 @@ +/** + * @file OptimalControlElement.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; + +bool OptimalControlElement::initialize(std::weak_ptr paramHandler, + const VariablesHandler& variablesHandler) +{ + return true; +} + +bool OptimalControlElement::update() +{ + return true; +} + +Eigen::Ref OptimalControlElement::getA() const +{ + return m_A; +} + +Eigen::Ref OptimalControlElement::getB() const +{ + return m_b; +} + +const std::string& OptimalControlElement::getDescription() const +{ + return m_description; +} From cb77ba69cab9cd70cd6e2204050b8917fa8eedd4 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 8 Jan 2021 16:42:59 +0100 Subject: [PATCH 02/19] General refactor of VariablesHandler class --- .../System/VariablesHandler.h | 29 +++++++++++++++---- src/System/src/VariablesHandler.cpp | 26 +++++++++++++++-- 2 files changed, 46 insertions(+), 9 deletions(-) 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 From 170651dd2e3ff5df3cc14368f593b328747b049b Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 8 Jan 2021 16:43:46 +0100 Subject: [PATCH 03/19] Implement toManifRot function in ManifConversions.h --- .../Conversions/ManifConversions.h | 41 ++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) 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 From fed3abce01bd20f9e28c0ea73ef4b6ec40b8e1ba Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 8 Jan 2021 16:49:41 +0100 Subject: [PATCH 04/19] Implement OptimalControlElement::setKinDyn() and OptimalControlElement::subA() methods --- .../TSID/OptimalControlElement.h | 21 +++++++++++++++++++ src/TSID/src/OptimalControlElement.cpp | 11 ++++++++++ 2 files changed, 32 insertions(+) diff --git a/src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h b/src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h index b42d6cc4cf..e8bcb88dfe 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h +++ b/src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h @@ -8,8 +8,12 @@ #ifndef BIPEDAL_LOCOMOTION_TSID_OPTIMAL_CONTROL_ELEMENT_H #define BIPEDAL_LOCOMOTION_TSID_OPTIMAL_CONTROL_ELEMENT_H +#include + #include +#include + #include #include @@ -33,7 +37,24 @@ class OptimalControlElement std::string m_description{"Generic Optimal Control Element"}; /** m_kinDyn; /**< Pointer to a KinDynComputations + object */ + + /** + * Extract the submatrix A associated to a given variable. + */ + Eigen::Ref + subA(const System::VariablesHandler::VariableDescription& description); + 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 diff --git a/src/TSID/src/OptimalControlElement.cpp b/src/TSID/src/OptimalControlElement.cpp index 7dea66d6e1..869f0885e9 100644 --- a/src/TSID/src/OptimalControlElement.cpp +++ b/src/TSID/src/OptimalControlElement.cpp @@ -11,6 +11,17 @@ using namespace BipedalLocomotion::ParametersHandler; using namespace BipedalLocomotion::System; using namespace BipedalLocomotion::TSID; +Eigen::Ref OptimalControlElement::subA(const VariablesHandler::VariableDescription& description) +{ + return m_A.block(0, description.offset, 6, description.size); +} + +bool OptimalControlElement::setKinDyn(std::shared_ptr kinDyn) +{ + m_kinDyn = kinDyn; + return (m_kinDyn != nullptr) && (m_kinDyn->isValid()); +} + bool OptimalControlElement::initialize(std::weak_ptr paramHandler, const VariablesHandler& variablesHandler) { From c6dcfa2fba3d95868eedd86df620c5be07d460d7 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 8 Jan 2021 16:50:50 +0100 Subject: [PATCH 05/19] Implement SE3Task into TSID component --- .../include/BipedalLocomotion/TSID/SE3Task.h | 95 ++++++++++++ src/TSID/src/SE3Task.cpp | 145 ++++++++++++++++++ 2 files changed, 240 insertions(+) create mode 100644 src/TSID/include/BipedalLocomotion/TSID/SE3Task.h create mode 100644 src/TSID/src/SE3Task.cpp diff --git a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h new file mode 100644 index 0000000000..7ad034d526 --- /dev/null +++ b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h @@ -0,0 +1,95 @@ +/** + * @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_OPTIMAL_SE3_TASK_H +#define BIPEDAL_LOCOMOTION_TSID_OPTIMAL_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. + */ +class SE3Task : public OptimalControlElement +{ + 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 */ + Eigen::MatrixXd m_jacobian; /**< Jacobian matrix (here the jacobian is described by the so + called MIXED representation) */ + +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 farme 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_OPTIMAL_SE3_TASK_H diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp new file mode 100644 index 0000000000..deaa8078b4 --- /dev/null +++ b/src/TSID/src/SE3Task.cpp @@ -0,0 +1,145 @@ +/** + * @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; + } + + 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() + 6) + { + 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(6, variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(6); + m_jacobian.resize(6, m_robotAccelerationVariable.size); + + 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, m_jacobian)) + { + std::cerr << "[SE3Task::update] Unable to get the jacobian." << std::endl; + return false; + } + this->subA(m_robotAccelerationVariable) = m_jacobian; + + 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.coeffs().head<3>()}); + ok = ok && m_R3Controller.setFeedForward(mixedAcceleration.coeffs().head<3>()); + + ok = ok && m_SO3Controller.setDesiredState({I_H_F.quat(), mixedVelocity.asSO3()}); + ok = ok && m_SO3Controller.setFeedForward(mixedAcceleration.asSO3()); + + return ok; +} From 9cfb95a16b6eb402cdddecd225773c33f0b031b1 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 8 Jan 2021 16:53:32 +0100 Subject: [PATCH 06/19] Add LieGroupControllers as dependency of TSID component --- cmake/BipedalLocomotionFrameworkFindDependencies.cmake | 7 +++++++ 1 file changed, 7 insertions(+) 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) From 12eac02a91a6f970be576249c3516f51a7deaaaa Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 8 Jan 2021 16:54:36 +0100 Subject: [PATCH 07/19] Implement SE3TaskTest --- src/TSID/tests/CMakeLists.txt | 8 ++ src/TSID/tests/SE3TaskTest.cpp | 144 +++++++++++++++++++++++++++++++++ 2 files changed, 152 insertions(+) create mode 100644 src/TSID/tests/CMakeLists.txt create mode 100644 src/TSID/tests/SE3TaskTest.cpp 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..c8abb05b4b --- /dev/null +++ b/src/TSID/tests/SE3TaskTest.cpp @@ -0,0 +1,144 @@ +/** + * @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); + + 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.asSO3()); + R3Controller.setFeedForward(desiredAcceleration.coeffs().head<3>()); + SO3Controller.setDesiredState({desiredPose.quat(), desiredVelocity.asSO3()}); + R3Controller.setDesiredState({desiredPose.translation(), desiredVelocity.coeffs().head<3>()}); + + 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)); + } + } +} From 99d9a6b6760a7c793a2f754d615b952da1ee8c42 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 8 Jan 2021 16:55:06 +0100 Subject: [PATCH 08/19] Improve the TSID CMakeLists.txt file --- src/TSID/CMakeLists.txt | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/TSID/CMakeLists.txt b/src/TSID/CMakeLists.txt index 1cb2628ed2..d18fa04063 100644 --- a/src/TSID/CMakeLists.txt +++ b/src/TSID/CMakeLists.txt @@ -2,10 +2,16 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -set(H_PREFIX include/BipedalLocomotion/TSID) +if(FRAMEWORK_COMPILE_TSID) -add_bipedal_locomotion_library( - NAME TSID - PUBLIC_HEADERS ${H_PREFIX}/OptimalControlElement.h - SOURCES src/OptimalControlElement.cpp - PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System) + set(H_PREFIX include/BipedalLocomotion/TSID) + + add_bipedal_locomotion_library( + NAME TSID + PUBLIC_HEADERS ${H_PREFIX}/OptimalControlElement.h ${H_PREFIX}/SE3Task.h + SOURCES src/OptimalControlElement.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() From 505681c8754db550fdd0b781d3fc0d2652e3fac9 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 8 Jan 2021 16:56:47 +0100 Subject: [PATCH 09/19] Add LieGroupControllers to the Framework public dependencies --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) 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:: From 4694d90e4cc55cde49def0c134d7f5d86e272ace Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 8 Jan 2021 17:02:25 +0100 Subject: [PATCH 10/19] Install lie-group-controllers in GitHub CI --- .github/workflows/ci.yml | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1debff751d..ade3ad307d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -19,7 +19,9 @@ env: 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 From ba453edc1d5c1c81bb355cb55cd71a8222722a54 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 8 Jan 2021 17:04:32 +0100 Subject: [PATCH 11/19] Update CHANGELOG.md --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) 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/ From 348ee1674120ec5f91705d2f41264bfba322f5d5 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 8 Jan 2021 17:59:09 +0100 Subject: [PATCH 12/19] Use v() and w() methods to get the linear and angular velocity of the manif objects in TSID --- src/TSID/src/SE3Task.cpp | 8 ++++---- src/TSID/tests/SE3TaskTest.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp index deaa8078b4..78f993f9d5 100644 --- a/src/TSID/src/SE3Task.cpp +++ b/src/TSID/src/SE3Task.cpp @@ -135,11 +135,11 @@ bool SE3Task::setReferenceTrajectory(const manif::SE3d& I_H_F, { bool ok = true; - ok = ok && m_R3Controller.setDesiredState({I_H_F.translation(), mixedVelocity.coeffs().head<3>()}); - ok = ok && m_R3Controller.setFeedForward(mixedAcceleration.coeffs().head<3>()); + 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.asSO3()}); - ok = ok && m_SO3Controller.setFeedForward(mixedAcceleration.asSO3()); + 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/tests/SE3TaskTest.cpp b/src/TSID/tests/SE3TaskTest.cpp index c8abb05b4b..b7fc57a874 100644 --- a/src/TSID/tests/SE3TaskTest.cpp +++ b/src/TSID/tests/SE3TaskTest.cpp @@ -117,10 +117,10 @@ TEST_CASE("SE3 Task") SO3Controller.setGains({kp, kd}); R3Controller.setGains({kp, kd}); - SO3Controller.setFeedForward(desiredAcceleration.asSO3()); - R3Controller.setFeedForward(desiredAcceleration.coeffs().head<3>()); - SO3Controller.setDesiredState({desiredPose.quat(), desiredVelocity.asSO3()}); - R3Controller.setDesiredState({desiredPose.translation(), desiredVelocity.coeffs().head<3>()}); + 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()), From 36802ebb0e4529b3c85cb526ee9820517a405640 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 12 Jan 2021 10:55:01 +0100 Subject: [PATCH 13/19] - Rename OptimalControlElement into Task in TSID - Make Task class compliant with robotology/idyntree@64cbaf7e35a0d545167cc9819f4d248d08aa5031 --- src/TSID/CMakeLists.txt | 4 +- .../TSID/{OptimalControlElement.h => Task.h} | 25 ++++++--- src/TSID/src/OptimalControlElement.cpp | 49 ----------------- src/TSID/src/Task.cpp | 55 +++++++++++++++++++ 4 files changed, 73 insertions(+), 60 deletions(-) rename src/TSID/include/BipedalLocomotion/TSID/{OptimalControlElement.h => Task.h} (84%) delete mode 100644 src/TSID/src/OptimalControlElement.cpp create mode 100644 src/TSID/src/Task.cpp diff --git a/src/TSID/CMakeLists.txt b/src/TSID/CMakeLists.txt index d18fa04063..16c0ceb74c 100644 --- a/src/TSID/CMakeLists.txt +++ b/src/TSID/CMakeLists.txt @@ -8,8 +8,8 @@ if(FRAMEWORK_COMPILE_TSID) add_bipedal_locomotion_library( NAME TSID - PUBLIC_HEADERS ${H_PREFIX}/OptimalControlElement.h ${H_PREFIX}/SE3Task.h - SOURCES src/OptimalControlElement.cpp src/SE3Task.cpp + 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) diff --git a/src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h b/src/TSID/include/BipedalLocomotion/TSID/Task.h similarity index 84% rename from src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h rename to src/TSID/include/BipedalLocomotion/TSID/Task.h index e8bcb88dfe..593c797a3b 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h +++ b/src/TSID/include/BipedalLocomotion/TSID/Task.h @@ -1,17 +1,18 @@ /** - * @file OptimalControlElement.h + * @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_OPTIMAL_CONTROL_ELEMENT_H -#define BIPEDAL_LOCOMOTION_TSID_OPTIMAL_CONTROL_ELEMENT_H +#ifndef BIPEDAL_LOCOMOTION_TSID_TASK_H +#define BIPEDAL_LOCOMOTION_TSID_TASK_H #include #include +#include #include #include @@ -23,13 +24,13 @@ namespace TSID { /** - * OptimalControlElement describes a control problem element. The element is described by a matrix + * 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 OptimalControlElement +class Task { protected: Eigen::MatrixXd m_A; /**< Element Matrix */ @@ -44,9 +45,15 @@ class OptimalControlElement /** * Extract the submatrix A associated to a given variable. */ - Eigen::Ref + 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. @@ -62,7 +69,7 @@ class OptimalControlElement * 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 - * OptimalControlElements + * Tasks. * @return True in case of success, false otherwise. */ virtual bool initialize(std::weak_ptr paramHandler, @@ -88,7 +95,7 @@ class OptimalControlElement /** * Get the description of the element. - * @return a string containing the description of the OptimalControlElement. + * @return a string containing the description of the element. */ const std::string& getDescription() const; }; @@ -96,4 +103,4 @@ class OptimalControlElement } // namespace TSID } // namespace BipedalLocomotion -#endif // BIPEDAL_LOCOMOTION_TSID_OPTIMAL_CONTROL_ELEMENT_H +#endif // BIPEDAL_LOCOMOTION_TSID_TASK_H diff --git a/src/TSID/src/OptimalControlElement.cpp b/src/TSID/src/OptimalControlElement.cpp deleted file mode 100644 index 869f0885e9..0000000000 --- a/src/TSID/src/OptimalControlElement.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/** - * @file OptimalControlElement.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; - -Eigen::Ref OptimalControlElement::subA(const VariablesHandler::VariableDescription& description) -{ - return m_A.block(0, description.offset, 6, description.size); -} - -bool OptimalControlElement::setKinDyn(std::shared_ptr kinDyn) -{ - m_kinDyn = kinDyn; - return (m_kinDyn != nullptr) && (m_kinDyn->isValid()); -} - -bool OptimalControlElement::initialize(std::weak_ptr paramHandler, - const VariablesHandler& variablesHandler) -{ - return true; -} - -bool OptimalControlElement::update() -{ - return true; -} - -Eigen::Ref OptimalControlElement::getA() const -{ - return m_A; -} - -Eigen::Ref OptimalControlElement::getB() const -{ - return m_b; -} - -const std::string& OptimalControlElement::getDescription() const -{ - return m_description; -} 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; +} From d3c3f9d7866db020ce1d875a56566e3aaeb28e5b Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 12 Jan 2021 10:58:41 +0100 Subject: [PATCH 14/19] - Make SE3Task compatible with f7cd662288b4c945ea3dbc8be285968027117feb - Avoid to copy the jacobian matrix in SE3Task --- src/TSID/include/BipedalLocomotion/TSID/SE3Task.h | 12 +++++------- src/TSID/src/SE3Task.cpp | 5 ++--- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h index 7ad034d526..55f1408816 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h +++ b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h @@ -5,12 +5,12 @@ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. */ -#ifndef BIPEDAL_LOCOMOTION_TSID_OPTIMAL_SE3_TASK_H -#define BIPEDAL_LOCOMOTION_TSID_OPTIMAL_SE3_TASK_H +#ifndef BIPEDAL_LOCOMOTION_TSID_SE3_TASK_H +#define BIPEDAL_LOCOMOTION_TSID_SE3_TASK_H #include -#include +#include #include @@ -36,7 +36,7 @@ namespace TSID * @note Please refer to https://github.com/dic-iit/lie-group-controllers if you are interested in * the implementation of the PD controllers. */ -class SE3Task : public OptimalControlElement +class SE3Task : public Task { LieGroupControllers::ProportionalDerivativeControllerSO3d m_SO3Controller; /**< PD Controller in SO(3) */ @@ -49,8 +49,6 @@ class SE3Task : public OptimalControlElement (base + joint) */ iDynTree::FrameIndex m_frameIndex; /**< Frame controlled by the OptimalControlElement */ - Eigen::MatrixXd m_jacobian; /**< Jacobian matrix (here the jacobian is described by the so - called MIXED representation) */ public: /** @@ -92,4 +90,4 @@ class SE3Task : public OptimalControlElement } // namespace TSID } // namespace BipedalLocomotion -#endif // BIPEDAL_LOCOMOTION_TSID_OPTIMAL_SE3_TASK_H +#endif // BIPEDAL_LOCOMOTION_TSID_SE3_TASK_H diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp index 78f993f9d5..0808846098 100644 --- a/src/TSID/src/SE3Task.cpp +++ b/src/TSID/src/SE3Task.cpp @@ -93,7 +93,6 @@ bool SE3Task::initialize(std::weak_ptr pa m_A.resize(6, variablesHandler.getNumberOfVariables()); m_A.setZero(); m_b.resize(6); - m_jacobian.resize(6, m_robotAccelerationVariable.size); return true; } @@ -119,12 +118,12 @@ bool SE3Task::update() // Workaround because matrix view is not compatible with Eigen::Ref // https://github.com/robotology/idyntree/issues/797 - if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, m_jacobian)) + if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, + this->subA(m_robotAccelerationVariable))) { std::cerr << "[SE3Task::update] Unable to get the jacobian." << std::endl; return false; } - this->subA(m_robotAccelerationVariable) = m_jacobian; return true; } From e2bb9043b9f442ac2b6d0350f64b1d7616fceb1e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 12 Jan 2021 11:09:09 +0100 Subject: [PATCH 15/19] Bump iDynTree version in GitHub CI --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ade3ad307d..f6da3229a4 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -13,7 +13,7 @@ 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 From 7a776b256581da2ee34de5539b10d1f54875c507 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 12 Jan 2021 11:27:35 +0100 Subject: [PATCH 16/19] Check that MIXED_REPRESENTATION is used in the SE3Task --- src/TSID/src/SE3Task.cpp | 11 +++++++++++ src/TSID/tests/SE3TaskTest.cpp | 3 +++ 2 files changed, 14 insertions(+) diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp index 0808846098..b95310d32a 100644 --- a/src/TSID/src/SE3Task.cpp +++ b/src/TSID/src/SE3Task.cpp @@ -30,6 +30,17 @@ bool SE3Task::initialize(std::weak_ptr pa 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) { diff --git a/src/TSID/tests/SE3TaskTest.cpp b/src/TSID/tests/SE3TaskTest.cpp index b7fc57a874..48c7a2e362 100644 --- a/src/TSID/tests/SE3TaskTest.cpp +++ b/src/TSID/tests/SE3TaskTest.cpp @@ -39,6 +39,9 @@ TEST_CASE("SE3 Task") 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") From f488554a551b373df4c97b689f1fee4fdaa455c0 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 12 Jan 2021 14:03:48 +0100 Subject: [PATCH 17/19] Fix typo in SE3Task documentation Co-authored-by: Prashanth Ramadoss --- src/TSID/include/BipedalLocomotion/TSID/SE3Task.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h index 55f1408816..f4ccade6e5 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h +++ b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h @@ -59,7 +59,7 @@ class SE3Task : public Task * | 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 farme controlled by the SE3Task | 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 | From 72041a72be4e4fb4ff0efb15d0cb0b2b3b646625 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 12 Jan 2021 14:16:33 +0100 Subject: [PATCH 18/19] Improve the documentation of the SE3Task --- src/TSID/include/BipedalLocomotion/TSID/SE3Task.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h index f4ccade6e5..529e750b9f 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h +++ b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h @@ -35,6 +35,11 @@ namespace TSID * 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 { From bfa5eaa1f2d7611911eda7681d6e5960c241078d Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 12 Jan 2021 14:17:44 +0100 Subject: [PATCH 19/19] Avoid to use magic numbers in SE3Task class --- src/TSID/include/BipedalLocomotion/TSID/SE3Task.h | 2 ++ src/TSID/src/SE3Task.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h index 529e750b9f..a40579c6d2 100644 --- a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h +++ b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h @@ -55,6 +55,8 @@ class SE3Task : public Task 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. diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp index b95310d32a..8397a8e820 100644 --- a/src/TSID/src/SE3Task.cpp +++ b/src/TSID/src/SE3Task.cpp @@ -59,7 +59,8 @@ bool SE3Task::initialize(std::weak_ptr pa return false; } - if (m_robotAccelerationVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + 6) + if (m_robotAccelerationVariable.size + != m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize) { std::cerr << errorPrefix << descriptionPrefix << frameName << " - Error while retrieving the robot acceleration variable." << std::endl; @@ -101,9 +102,9 @@ bool SE3Task::initialize(std::weak_ptr pa m_description = std::string(descriptionPrefix) + frameName + "."; // resize the matrices - m_A.resize(6, variablesHandler.getNumberOfVariables()); + m_A.resize(m_spatialVelocitySize, variablesHandler.getNumberOfVariables()); m_A.setZero(); - m_b.resize(6); + m_b.resize(m_spatialVelocitySize); return true; }