From 30fd752620364a790fce59cec9e36b41e2aa5dc8 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:29:26 +0100 Subject: [PATCH 01/21] Add the name of the variable in VariableDescription class --- .../BipedalLocomotion/System/VariablesHandler.h | 1 + src/System/src/VariablesHandler.cpp | 10 ++++++---- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/System/include/BipedalLocomotion/System/VariablesHandler.h b/src/System/include/BipedalLocomotion/System/VariablesHandler.h index 489b02c007..b83d1a830b 100644 --- a/src/System/include/BipedalLocomotion/System/VariablesHandler.h +++ b/src/System/include/BipedalLocomotion/System/VariablesHandler.h @@ -27,6 +27,7 @@ class VariablesHandler { std::ptrdiff_t offset; std::ptrdiff_t size; + std::string name; bool isValid() const; static VariableDescription InvalidVariable(); diff --git a/src/System/src/VariablesHandler.cpp b/src/System/src/VariablesHandler.cpp index e892309db1..ab8264ea97 100644 --- a/src/System/src/VariablesHandler.cpp +++ b/src/System/src/VariablesHandler.cpp @@ -32,10 +32,12 @@ bool VariablesHandler::addVariable(const std::string& name, const std::size_t& s return false; } - VariablesHandler::VariableDescription indexRange; - indexRange.size = size; - indexRange.offset = m_numberOfVariables; - m_variables.emplace(name, indexRange); + VariablesHandler::VariableDescription description; + description.size = size; + description.offset = m_numberOfVariables; + description.name = name; + + m_variables.emplace(name, description); m_numberOfVariables += size; return true; From 3cbf75cb975303310dbf0f9c4fab04dc015cb5ea Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:25:29 +0100 Subject: [PATCH 02/21] Implement LinearTask in the IK component --- .../include/BipedalLocomotion/IK/LinearTask.h | 133 ++++++++++++++++++ src/IK/src/LinearTask.cpp | 66 +++++++++ 2 files changed, 199 insertions(+) create mode 100644 src/IK/include/BipedalLocomotion/IK/LinearTask.h create mode 100644 src/IK/src/LinearTask.cpp diff --git a/src/IK/include/BipedalLocomotion/IK/LinearTask.h b/src/IK/include/BipedalLocomotion/IK/LinearTask.h new file mode 100644 index 0000000000..5cf4991f8f --- /dev/null +++ b/src/IK/include/BipedalLocomotion/IK/LinearTask.h @@ -0,0 +1,133 @@ +/** + * @file LinearTask.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_LINEAR_TASK_H +#define BIPEDAL_LOCOMOTION_IK_LINEAR_TASK_H + +#include + +#include + +#include +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace IK +{ + +/** + * LinearTask describes a IK LinearTask element. The LinearTask 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 LinearTask +{ +protected: + Eigen::MatrixXd m_A; /**< Task Matrix */ + Eigen::VectorXd m_b; /**< Task Vector */ + + std::string m_description{"Generic IK Task Element"}; /**< String describing the content of the + task */ + + std::shared_ptr 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: + /** + * Type of the task. Namely an equality or an inequality task. + */ + enum class Type + { + equality, + inequality + }; + + /** + * 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); + + /** + * Set the set of variables required by the task. The variables are stored in the + * System::VariablesHandler + * @param variablesHandler reference to a variables handler. + * @return True in case of success, false otherwise. + */ + virtual bool setVariablesHandler(const System::VariablesHandler& variablesHandler); + + /** + * Initialize the task + * @param paramHandler a pointer to the parameter handler containing all the information + * required by the specific task. Please refer to the documentation of the implemented + * version of the model for further details. + * 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); + + /** + * Update the content of the task. + * @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 task. + * @return a string containing the description of the task. + */ + const std::string& getDescription() const; + + /** + * Get the size of the task. (I.e the number of rows of the vector b) + * @return the size of the task. + */ + virtual std::size_t size() const = 0; + + /** + * Get the type of the task. Namely equality or inequality. + * @return the size of the task. + */ + virtual Type type() const = 0; +}; + +} // namespace IK +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_IK_LINEAR_TASK_H diff --git a/src/IK/src/LinearTask.cpp b/src/IK/src/LinearTask.cpp new file mode 100644 index 0000000000..6e0723a021 --- /dev/null +++ b/src/IK/src/LinearTask.cpp @@ -0,0 +1,66 @@ +/** + * @file LinearTask.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. + */ + +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +iDynTree::MatrixView +LinearTask::subA(const VariablesHandler::VariableDescription& description) +{ + return iDynTree::MatrixView(m_A).block(0, + description.offset, + m_A.rows(), + description.size); +} + +iDynTree::MatrixView +LinearTask::subA(const VariablesHandler::VariableDescription& description) const +{ + return iDynTree::MatrixView(m_A).block(0, + description.offset, + m_A.rows(), + description.size); +} + +bool LinearTask::setKinDyn(std::shared_ptr kinDyn) +{ + m_kinDyn = kinDyn; + return (m_kinDyn != nullptr) && (m_kinDyn->isValid()); +} + +bool LinearTask::setVariablesHandler(const VariablesHandler& variablesHandler) +{ + return true; +} + +bool LinearTask::initialize(std::weak_ptr paramHandler) +{ + return true; +} + +bool LinearTask::update() +{ + return true; +} + +Eigen::Ref LinearTask::getA() const +{ + return m_A; +} + +Eigen::Ref LinearTask::getB() const +{ + return m_b; +} + +const std::string& LinearTask::getDescription() const +{ + return m_description; +} From f976ab6e365756546132de36fabdc1110b37b6da Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:25:51 +0100 Subject: [PATCH 03/21] Implement SO3Task in the IK component --- src/IK/include/BipedalLocomotion/IK/SO3Task.h | 119 ++++++++++++ src/IK/src/SO3Task.cpp | 181 ++++++++++++++++++ 2 files changed, 300 insertions(+) create mode 100644 src/IK/include/BipedalLocomotion/IK/SO3Task.h create mode 100644 src/IK/src/SO3Task.cpp diff --git a/src/IK/include/BipedalLocomotion/IK/SO3Task.h b/src/IK/include/BipedalLocomotion/IK/SO3Task.h new file mode 100644 index 0000000000..e404d5c61e --- /dev/null +++ b/src/IK/include/BipedalLocomotion/IK/SO3Task.h @@ -0,0 +1,119 @@ +/** + * @file SO3Task.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_SO3_TASK_H +#define BIPEDAL_LOCOMOTION_IK_SO3_TASK_H + +#include + +#include + +#include + +#include + + +namespace BipedalLocomotion +{ +namespace IK +{ + +/** + * SO3Task is a concrete implementation of the IK::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$\dot{\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_\omega \nu = \omega ^ * + * \f] + * where \f$J_\omega\f$ is the angular part of the robot Jacobian and \f$\omega ^ *\f$ is the + * desired velocity. The desired velocity is chosen such that the orientation of the frame will + * asymptotically converge to the desired trajectory. \f$\omega ^ *\f$ is computed with 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. + */ +class SO3Task : public LinearTask +{ + LieGroupControllers::ProportionalControllerSO3d m_SO3Controller; /**< P Controller in SO(3) */ + + System::VariablesHandler::VariableDescription m_robotVelocityVariable; /**< Variable + describing the + robot velocity + (base + joint) */ + + iDynTree::FrameIndex m_frameIndex; /**< Frame controlled by the OptimalControlElement */ + + static constexpr std::size_t m_angularVelocitySize{3}; /**< Size of the spatial velocity vector. */ + static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. */ + + bool m_isInitialized{false}; + + Eigen::MatrixXd m_jacobian; + +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 | + * | `frame_name` | `string` | Name of the frame controlled by the SO3Task | Yes | + * | `kp_angular` | `double` | Gain of the orientation controller | Yes | + * 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 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 reference trajectory. + * @param I_R_F Rotation between the link and the inertial frame. + * @param angularVelocity angular velocity written in mixed inertial frame. + * @return True in case of success, false otherwise. + */ + bool setSetPoint(const manif::SO3d& I_H_F, const manif::SO3d::Tangent& angularVelocity); + + /** + * 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 SO3Task is an equality task. + * @return the size of the task. + */ + Type type() const override; +}; + +} // namespace IK +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_IK_SO3_TASK_H diff --git a/src/IK/src/SO3Task.cpp b/src/IK/src/SO3Task.cpp new file mode 100644 index 0000000000..25f33586e1 --- /dev/null +++ b/src/IK/src/SO3Task.cpp @@ -0,0 +1,181 @@ +/** + * @file SO3Task.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. + */ + +#include +#include +#include + +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +bool SO3Task::setVariablesHandler(const System::VariablesHandler& variablesHandler) +{ + if (!m_isInitialized) + { + log()->error("[SO3Task::setVariablesHandler] The task is not initialized. Please call " + "initialize method."); + return false; + } + + // get the variable + if (!variablesHandler.getVariable(m_robotVelocityVariable.name, + m_robotVelocityVariable)) + { + log()->error("[SO3Task::setVariablesHandler] Unable to get the variable named {}.", + m_robotVelocityVariable.name); + return false; + } + + if (m_robotVelocityVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize) + { + log()->error("[SO3Task::setVariablesHandler] The size of the robot velocity variable is " + "different from the one expected. Expected size: {}. Given size: {}.", + m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize, + m_robotVelocityVariable.size); + return false; + } + + // resize the matrices + m_A.resize(m_angularVelocitySize, variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(m_angularVelocitySize); + m_jacobian.resize(m_spatialVelocitySize, + m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize); + + return true; +} + +bool SO3Task::initialize(std::weak_ptr paramHandler) +{ + constexpr std::string_view errorPrefix = "[SO3Task::initialize] "; + + std::string frameName = "Unknown"; + constexpr std::string_view descriptionPrefix = "SO3Task Optimal Control Element - Frame name: "; + + if (m_kinDyn == nullptr || !m_kinDyn->isValid()) + { + log()->error("{}, [{} {}] KinDynComputations object is not valid.", + errorPrefix, + descriptionPrefix, + frameName); + + return false; + } + + if (m_kinDyn->getFrameVelocityRepresentation() + != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION) + { + log()->error("{}, [{} {}] The task supports only quantities expressed in MIXED " + "representation. Please provide a KinDynComputations with Frame velocity " + "representation set to MIXED_REPRESENTATION.", + errorPrefix, + descriptionPrefix, + frameName); + return false; + } + + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{}, [{} {}] The parameter handler is not valid.", + errorPrefix, + descriptionPrefix, + frameName); + return false; + } + + std::string robotVelocityVariableName; + if (!ptr->getParameter("robot_velocity_variable_name", m_robotVelocityVariable.name)) + { + log()->error("{}, [{} {}] Error while retrieving the robot velocity variable.", + errorPrefix, + descriptionPrefix, + frameName); + return false; + } + + if (!ptr->getParameter("frame_name", frameName) + || (m_frameIndex = m_kinDyn->model().getFrameIndex(frameName)) + == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{}, [{} {}] Error while retrieving the frame that should be controlled.", + errorPrefix, + descriptionPrefix, + frameName); + return false; + } + + // set the gains for the controllers + double kpAngular; + + if (!ptr->getParameter("kp_angular", kpAngular)) + { + log()->error("{}, [{} {}] Unable to get the proportional angular gain.", + errorPrefix, + descriptionPrefix, + frameName); + return false; + } + + m_SO3Controller.setGains(kpAngular); + + // set the description + m_description = std::string(descriptionPrefix) + frameName + "."; + + m_isInitialized = true; + return true; +} + +bool SO3Task::update() +{ + using namespace BipedalLocomotion::Conversions; + using namespace iDynTree; + + // set the state + m_SO3Controller.setState(toManifRot(m_kinDyn->getWorldTransform(m_frameIndex).getRotation())); + + // update the controller + m_SO3Controller.computeControlLaw(); + + m_b = m_SO3Controller.getControl().coeffs(); + + if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, + m_jacobian)) + { + log()->error("[SO3Task::update] Unable to get the jacobian."); + return false; + } + + iDynTree::toEigen(this->subA(m_robotVelocityVariable)) = m_jacobian.bottomRows<3>(); + + return true; +} + +bool SO3Task::setSetPoint(const manif::SO3d& I_R_F, const manif::SO3d::Tangent& angularVelocity) +{ + + bool ok = true; + ok = ok && m_SO3Controller.setDesiredState(I_R_F); + ok = ok && m_SO3Controller.setFeedForward(angularVelocity); + + return ok; +} + +std::size_t SO3Task::size() const +{ + return m_angularVelocitySize; +} + +SO3Task::Type SO3Task::type() const +{ + return Type::equality; +} From 3859ec4d8ff1558df6bda19cf0bd47af03ee9d90 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:26:10 +0100 Subject: [PATCH 04/21] Implement SE3Task in the IK component --- src/IK/include/BipedalLocomotion/IK/SE3Task.h | 124 +++++++++++ src/IK/src/SE3Task.cpp | 194 ++++++++++++++++++ 2 files changed, 318 insertions(+) create mode 100644 src/IK/include/BipedalLocomotion/IK/SE3Task.h create mode 100644 src/IK/src/SE3Task.cpp diff --git a/src/IK/include/BipedalLocomotion/IK/SE3Task.h b/src/IK/include/BipedalLocomotion/IK/SE3Task.h new file mode 100644 index 0000000000..f1123a347c --- /dev/null +++ b/src/IK/include/BipedalLocomotion/IK/SE3Task.h @@ -0,0 +1,124 @@ +/** + * @file SE3Task.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_SE3_TASK_H +#define BIPEDAL_LOCOMOTION_IK_SE3_TASK_H + +#include + +#include + +#include + +#include + +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 SE3Task : public LinearTask +{ + LieGroupControllers::ProportionalControllerSO3d m_SO3Controller; /**< P Controller in SO(3) */ + LieGroupControllers::ProportionalControllerR3d m_R3Controller; /**< P Controller in R3 */ + + System::VariablesHandler::VariableDescription m_robotVelocityVariable; /**< Variable + describing the + robot velocity + (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. */ + + 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 | + * | `frame_name` | `string` | Name of the frame controlled by the SE3Task | Yes | + * | `kp_linear` | `double` | Gain of the position controller | Yes | + * | `kp_angular` | `double` | Gain of the orientation 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 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 I_H_F Homogeneous transform between the link and the inertial frame. + * @param mixedVelocity 6D-velocity written in mixed representation.g + * @return True in case of success, false otherwise. + */ + bool setSetPoint(const manif::SE3d& I_H_F, const manif::SE3d::Tangent& mixedVelocity); + + /** + * 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 SE3Task 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 diff --git a/src/IK/src/SE3Task.cpp b/src/IK/src/SE3Task.cpp new file mode 100644 index 0000000000..17f156be25 --- /dev/null +++ b/src/IK/src/SE3Task.cpp @@ -0,0 +1,194 @@ +/** + * @file SE3Task.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. + */ + +#include +#include +#include + +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +bool SE3Task::setVariablesHandler(const System::VariablesHandler& variablesHandler) +{ + if (!m_isInitialized) + { + log()->error("[SE3Task::setVariablesHandler] The task is not initialized. Please call " + "initialize method."); + return false; + } + + // get the variable + if (!variablesHandler.getVariable(m_robotVelocityVariable.name, + m_robotVelocityVariable)) + { + log()->error("[SE3Task::setVariablesHandler] Unable to get the variable named {}.", + m_robotVelocityVariable.name); + return false; + } + + // get the variable + if (m_robotVelocityVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize) + { + log()->error("[SE3Task::setVariablesHandler] The size of the robot velocity variable is " + "different from the one expected. Expected size: {}. Given size: {}.", + m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize, + m_robotVelocityVariable.size); + return false; + } + + // resize the matrices + m_A.resize(m_spatialVelocitySize, variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(m_spatialVelocitySize); + + return true; +} + +bool SE3Task::initialize(std::weak_ptr paramHandler) +{ + 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()) + { + log()->error("{}, [{} {}] KinDynComputations object is not valid.", + errorPrefix, + descriptionPrefix, + frameName); + + return false; + } + + if (m_kinDyn->getFrameVelocityRepresentation() + != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION) + { + log()->error("{}, [{} {}] The task supports only quantities expressed in MIXED " + "representation. Please provide a KinDynComputations with Frame velocity " + "representation set to MIXED_REPRESENTATION.", + errorPrefix, + descriptionPrefix, + frameName); + return false; + } + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{}, [{} {}] The parameter handler is not valid.", + errorPrefix, + descriptionPrefix, + frameName); + return false; + } + + std::string robotVelocityVariableName; + if (!ptr->getParameter("robot_velocity_variable_name", m_robotVelocityVariable.name)) + { + log()->error("{}, [{} {}] Error while retrieving the robot velocity variable.", + errorPrefix, + descriptionPrefix, + frameName); + return false; + } + + if (!ptr->getParameter("frame_name", frameName) + || (m_frameIndex = m_kinDyn->model().getFrameIndex(frameName)) + == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{}, [{} {}] Error while retrieving the frame that should be controlled.", + errorPrefix, + descriptionPrefix, + frameName); + return false; + } + + // set the gains for the controllers + double kpLinear; + double kpAngular; + if (!ptr->getParameter("kp_linear", kpLinear)) + { + log()->error("{}, [{} {}] Unable to get the proportional linear gain.", + errorPrefix, + descriptionPrefix, + frameName); + return false; + } + + if (!ptr->getParameter("kp_angular", kpAngular)) + { + log()->error("{}, [{} {}] Unable to get the proportional angular gain.", + errorPrefix, + descriptionPrefix, + frameName); + return false; + } + + m_R3Controller.setGains(kpLinear); + m_SO3Controller.setGains(kpAngular); + + // set the description + m_description = std::string(descriptionPrefix) + frameName + "."; + + m_isInitialized = true; + + return true; +} + +bool SE3Task::update() +{ + using namespace BipedalLocomotion::Conversions; + using namespace iDynTree; + + // set the state + m_SO3Controller.setState(toManifRot(m_kinDyn->getWorldTransform(m_frameIndex).getRotation())); + m_R3Controller.setState(toEigen(m_kinDyn->getWorldTransform(m_frameIndex).getPosition())); + + // update the controller + m_SO3Controller.computeControlLaw(); + m_R3Controller.computeControlLaw(); + + m_b.head<3>() = m_R3Controller.getControl().coeffs(); + m_b.tail<3>() = m_SO3Controller.getControl().coeffs(); + + if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, + this->subA(m_robotVelocityVariable))) + { + log()->error("[SE3Task::update] Unable to get the jacobian."); + return false; + } + + return true; +} + +bool SE3Task::setSetPoint(const manif::SE3d& I_H_F, const manif::SE3d::Tangent& mixedVelocity) +{ + + bool ok = true; + ok = ok && m_R3Controller.setDesiredState(I_H_F.translation()); + ok = ok && m_R3Controller.setFeedForward(mixedVelocity.v()); + + ok = ok && m_SO3Controller.setDesiredState(I_H_F.quat()); + ok = ok && m_SO3Controller.setFeedForward(mixedVelocity.w()); + + return ok; +} + +std::size_t SE3Task::size() const +{ + return m_spatialVelocitySize; +} + +SE3Task::Type SE3Task::type() const +{ + return Type::equality; +} From 427c926874da0215c8e63d6d908ccb76a56025b9 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:26:37 +0100 Subject: [PATCH 05/21] Implement CoMTask in the IK component --- src/IK/include/BipedalLocomotion/IK/CoMTask.h | 120 ++++++++++++++ src/IK/src/CoMTask.cpp | 151 ++++++++++++++++++ 2 files changed, 271 insertions(+) create mode 100644 src/IK/include/BipedalLocomotion/IK/CoMTask.h create mode 100644 src/IK/src/CoMTask.cpp diff --git a/src/IK/include/BipedalLocomotion/IK/CoMTask.h b/src/IK/include/BipedalLocomotion/IK/CoMTask.h new file mode 100644 index 0000000000..b1a9a0a541 --- /dev/null +++ b/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 + +#include + +#include + +#include + +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 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 position, Eigen::Ref 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 diff --git a/src/IK/src/CoMTask.cpp b/src/IK/src/CoMTask.cpp new file mode 100644 index 0000000000..31c2f0ccb3 --- /dev/null +++ b/src/IK/src/CoMTask.cpp @@ -0,0 +1,151 @@ +/** + * @file CoMTask.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. + */ + +#include +#include +#include + +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +bool CoMTask::setVariablesHandler(const System::VariablesHandler& variablesHandler) +{ + if (!m_isInitialized) + { + log()->error("[CoMTask::setVariablesHandler] The task is not initialized. Please call " + "initialize method."); + return false; + } + + // get the variable + if (!variablesHandler.getVariable(m_robotVelocityVariable.name, m_robotVelocityVariable)) + { + log()->error("[CoMTask::setVariablesHandler] Unable to get the variable named {}.", + m_robotVelocityVariable.name); + return false; + } + + // get the variable + if (m_robotVelocityVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize) + { + log()->error("[CoMTask::setVariablesHandler] The size of the robot velocity variable is " + "different from the one expected. Expected size: {}. Given size: {}.", + m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize, + m_robotVelocityVariable.size); + return false; + } + + // resize the matrices + m_A.resize(m_linearVelocitySize, variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(m_linearVelocitySize); + m_b.setZero(); + + return true; +} + +bool CoMTask::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[CoMTask::initialize]"; + + m_description = "CoMTask Optimal Control Element"; + + if (m_kinDyn == nullptr || !m_kinDyn->isValid()) + { + log()->error("{} [{}] KinDynComputations object is not valid.", errorPrefix, m_description); + + return false; + } + + if (m_kinDyn->getFrameVelocityRepresentation() + != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION) + { + log()->error("{} [{}] task supports only quantities expressed in MIXED " + "representation. Please provide a KinDynComputations with Frame velocity " + "representation set to MIXED_REPRESENTATION.", + errorPrefix, + m_description); + return false; + } + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} [{}] The parameter handler is not valid.", errorPrefix, m_description); + return false; + } + + std::string robotVelocityVariableName; + if (!ptr->getParameter("robot_velocity_variable_name", m_robotVelocityVariable.name)) + { + log()->error("{} [{}] while retrieving the robot velocity variable.", + errorPrefix, + m_description); + return false; + } + + // set the gains for the controllers + double kpLinear; + if (!ptr->getParameter("kp_linear", kpLinear)) + { + log()->error("{} [{}] to get the proportional linear gain.", errorPrefix, m_description); + return false; + } + + m_R3Controller.setGains(kpLinear); + + m_isInitialized = true; + + return true; +} + +bool CoMTask::update() +{ + using namespace BipedalLocomotion::Conversions; + using namespace iDynTree; + + // set the state + m_R3Controller.setState(toEigen(m_kinDyn->getCenterOfMassPosition())); + + // update the controller + m_R3Controller.computeControlLaw(); + + m_b = m_R3Controller.getControl().coeffs(); + + // get the CoM jacobian + if (!m_kinDyn->getCenterOfMassJacobian(this->subA(m_robotVelocityVariable))) + { + log()->error("[CoMTask::update] Unable to get the jacobian."); + return false; + } + + return true; +} + +bool CoMTask::setSetPoint(Eigen::Ref position, + Eigen::Ref velocity) +{ + bool ok = true; + ok = ok && m_R3Controller.setDesiredState(position); + ok = ok && m_R3Controller.setFeedForward(velocity); + + return ok; +} + +std::size_t CoMTask::size() const +{ + return m_linearVelocitySize; +} + +CoMTask::Type CoMTask::type() const +{ + return Type::equality; +} From cb93e151e2e022ef4270444d0b80bdbf6c3be641 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:27:15 +0100 Subject: [PATCH 06/21] Implement JointTrackingTask in the IK component --- .../BipedalLocomotion/IK/JointTrackingTask.h | 112 ++++++++++++ src/IK/src/JointTrackingTask.cpp | 168 ++++++++++++++++++ 2 files changed, 280 insertions(+) create mode 100644 src/IK/include/BipedalLocomotion/IK/JointTrackingTask.h create mode 100644 src/IK/src/JointTrackingTask.cpp diff --git a/src/IK/include/BipedalLocomotion/IK/JointTrackingTask.h b/src/IK/include/BipedalLocomotion/IK/JointTrackingTask.h new file mode 100644 index 0000000000..2f45b51dcb --- /dev/null +++ b/src/IK/include/BipedalLocomotion/IK/JointTrackingTask.h @@ -0,0 +1,112 @@ +/** + * @file JointTrackingTask.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_JOINT_TRACKING_TASK_H +#define BIPEDAL_LOCOMOTION_IK_JOINT_TRACKING_TASK_H + +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace IK +{ + +/** + * JointsTrackingTask is a concrete implementation of the Task. Please use this element if you + * want to control the desired joint position of the robot. + * The task represents the following equation + * \f[ + * \begin{bmatrix} 0_6 & I_n \end{bmatrix} \nu = \dot{s} ^ * + * \f] + * where \f$0_6\f$ and \f$I_n\f$ are the zero and the identity matrix. + * The desired joint velocity is chosen such that the joint will converge to the desired + * trajectory and it is computed with a standard standard PD controller in \f$\mathbb{R}^n\f$. + */ +class JointTrackingTask : public LinearTask +{ + Eigen::VectorXd m_kp; /**< Proportional gain. */ + Eigen::VectorXd m_jointPosition; /**< Joints position in radians */ + Eigen::VectorXd m_desiredJointVelocity; /**< Desired joint velocities in radians per second. */ + Eigen::VectorXd m_desiredJointPosition; /**< Desired joints position in radians. */ + Eigen::VectorXd m_zero; /**< Vector containing zero elements. */ + + std::string m_robotVelocityVariableName; + + bool m_isInitialized{false}; +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 ` | `vector` | Proportional controller gain | 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 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 setpoint. + * @param jointPosition vector containing the desired joint position in radians. + * @note The desired velocity is implicitly set to zero. + * @return True in case of success, false otherwise. + */ + bool setSetPoint(Eigen::Ref jointPosition); + + /** + * Set the desired setpoint. + * @param jointPosition vector containing the desired joint position in radians. + * @param jointVelocity vector containing the desired joint velocity in radians per second. + * @return True in case of success, false otherwise. + */ + bool setSetPoint(Eigen::Ref jointPosition, + Eigen::Ref jointVelocity); + + /** + * 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 JointsTrackingTask is an equality task. + * @return the size of the task. + */ + Type type() const override; +}; + +} // namespace IK +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_IK_JOINT_TRACKING_TASK_H diff --git a/src/IK/src/JointTrackingTask.cpp b/src/IK/src/JointTrackingTask.cpp new file mode 100644 index 0000000000..f62c2c1507 --- /dev/null +++ b/src/IK/src/JointTrackingTask.cpp @@ -0,0 +1,168 @@ +/** + * @file JointTrackingTask.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. + */ + +#include + +#include + +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::IK; + +bool JointTrackingTask::setVariablesHandler(const System::VariablesHandler& variablesHandler) +{ + constexpr auto errorPrefix = "[JointTrackingTask::setVariablesHandler]"; + + System::VariablesHandler::VariableDescription robotVelocityVariable; + + if (!m_isInitialized) + { + log()->error("{} The task is not initialized. Please call initialize method.", errorPrefix); + return false; + } + + + if (!variablesHandler.getVariable(m_robotVelocityVariableName, robotVelocityVariable)) + { + log()->error("{} Error while retrieving the robot velocity variable.", errorPrefix); + return false; + } + + if (robotVelocityVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + 6) + { + log()->error("{} The size of the robot velocity variable does not match with the one " + "stored in kinDynComputations object. Expected: {}. Given: {}", + errorPrefix, + m_kinDyn->getNrOfDegreesOfFreedom() + 6, + robotVelocityVariable.size); + return false; + } + + + // resize the matrices + m_A.resize(m_kinDyn->getNrOfDegreesOfFreedom(), variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(m_kinDyn->getNrOfDegreesOfFreedom()); + + // A is constant + // here we assume that the first robot velocity is stored as [base_velocity; + // joint_velocity] + iDynTree::toEigen(this->subA(robotVelocityVariable)) + .rightCols(m_kinDyn->getNrOfDegreesOfFreedom()) + .setIdentity(); + + return true; +} + +bool JointTrackingTask::initialize( + std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[JointTrackingTask::initialize] "; + + if (m_kinDyn == nullptr || !m_kinDyn->isValid()) + { + log()->error("{} KinDynComputations object is not valid.", errorPrefix); + return false; + } + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + if (!ptr->getParameter("robot_velocity_variable_name", m_robotVelocityVariableName)) + { + log()->error("{} Error while retrieving the robot velocity variable.", errorPrefix); + return false; + } + + // set the gains for the controllers + m_kp.resize(m_kinDyn->getNrOfDegreesOfFreedom()); + if (!ptr->getParameter("kp", m_kp)) + { + log()->error("{} Error while retrieving the proportional gain.", errorPrefix); + return false; + } + + // set the description + m_description = "Joint tracking task"; + + m_zero = Eigen::VectorXd::Zero(m_kinDyn->getNrOfDegreesOfFreedom()); + m_desiredJointPosition = Eigen::VectorXd::Zero(m_kinDyn->getNrOfDegreesOfFreedom()); + m_desiredJointVelocity = Eigen::VectorXd::Zero(m_kinDyn->getNrOfDegreesOfFreedom()); + m_jointPosition = Eigen::VectorXd::Zero(m_kinDyn->getNrOfDegreesOfFreedom()); + + m_isInitialized = true; + + return true; +} + +bool JointTrackingTask::update() +{ + constexpr auto errorPrefix = "[JointTrackingTask::update]"; + + if (!m_kinDyn->getJointPos(m_jointPosition)) + { + log()->error("{} Unable to get the joint position.", errorPrefix); + return false; + } + + m_b = m_desiredJointVelocity + m_kp.asDiagonal() * (m_desiredJointPosition - m_jointPosition); + + return true; +} + +bool JointTrackingTask::setSetPoint(Eigen::Ref jointPosition) +{ + return this->setSetPoint(jointPosition, m_zero); +} + +bool JointTrackingTask::setSetPoint(Eigen::Ref jointPosition, + Eigen::Ref jointVelocity) +{ + constexpr auto errorPrefix = "[JointTrackingTask::setSetPoint]"; + + if (jointPosition.size() != m_kinDyn->getNrOfDegreesOfFreedom() + || jointVelocity.size() != m_kinDyn->getNrOfDegreesOfFreedom()) + { + log()->error("{} Wrong size of the desired reference setpoint. Expected size: {}. Given " + "joint position size: {}, given joint velocity size: {}.", + errorPrefix, + m_kinDyn->getNrOfDegreesOfFreedom(), + jointPosition.size(), + jointVelocity.size()); + return false; + } + + m_desiredJointPosition = jointPosition; + m_desiredJointVelocity = jointVelocity; + + return true; +} + +std::size_t JointTrackingTask::size() const +{ + constexpr auto errorMessage = "[JointTrackingTask::size] Please call setKinDyn method before. " + "A size equal to zero will be returned."; + + assert((m_kinDyn != nullptr) && errorMessage); + + if (m_kinDyn == nullptr) + { + log()->warn(errorMessage); + return 0; + } + return m_kinDyn->getNrOfDegreesOfFreedom(); +} + +JointTrackingTask::Type JointTrackingTask::type() const +{ + return Type::equality; +} From 4660d5461c960c6ddad439f843ddfb8ca9f429c1 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:28:11 +0100 Subject: [PATCH 07/21] Implement IntegrationBasedIK interface in the IK component --- .../BipedalLocomotion/IK/IntegrationBasedIK.h | 118 ++++++++++++++++++ src/IK/src/IntegrationBasedIK.cpp | 15 +++ 2 files changed, 133 insertions(+) create mode 100644 src/IK/include/BipedalLocomotion/IK/IntegrationBasedIK.h create mode 100644 src/IK/src/IntegrationBasedIK.cpp diff --git a/src/IK/include/BipedalLocomotion/IK/IntegrationBasedIK.h b/src/IK/include/BipedalLocomotion/IK/IntegrationBasedIK.h new file mode 100644 index 0000000000..be61ff8323 --- /dev/null +++ b/src/IK/include/BipedalLocomotion/IK/IntegrationBasedIK.h @@ -0,0 +1,118 @@ +/** + * @file IntegrationBasedIK.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_INTEGRATION_BASE_IK_H +#define BIPEDAL_LOCOMOTION_IK_INTEGRATION_BASE_IK_H + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ + +namespace IK +{ + +/** + * State of the InverseKinematics + */ +struct IntegrationBasedIKState +{ + Eigen::VectorXd jointVelocity; /**< Joints velocity in rad per seconds */ + manif::SE3d::Tangent baseVelocity; /**< Mixed spatial velocity of the base */ +}; + +/** + * IntegrationBasedInverseKinematics implements the interface for the integration base inverse + * kinematics. Please inherits this class if you want to implement your custom Integration base + * Inverse Kinematics. The IntegrationBasedInverseKinematics can actually be used as Velocity + * controller or real IK. Indeed it is important to notice that IntegrationBasedIKState is a struct + * containing the joint velocities. When a robot velocity controller is available, one can set these + * joint velocities to the low-level robot controller. In this case, the \f$t ^ d\f$ quantities in + * the following figures can be evaluated by using robot sensor feedback, and the robot is said to + * be velocity controlled. On the other hand, if the robot velocity control is not available, one + * may integrate the outcome of IntegrationBasedIK to obtain the desired joint position to be set to + * a low-level robot position controller. In this case, the \f$t ^d\f$ quantities can be evaluated + * by using the desired integrated quantities instead of sensor feedback, and the block behaves as + * an inverse kinematics module, and the robot is said to be position controlled. + * @subsection vc Velocity Control + * Here you can find an example of the IntegrationBasedInverseKinematics interface used as + * a velocity controller. + *
+ * VelocityControl + * @subsection ik Inverse Kinematics + * If you want to use IntegrationBasedInverseKinematics as IK you need to integrate the output + * velocity. System::FloatingBaseSystemKinematics and System::Integrator classes can be used + * to integrate the output of the IK taking into account the geometrical structure of the + * configuration space (\f$ R^3 \times SO(3) \times R^n\f$) + *
+ * InverseKinematics + */ +class IntegrationBasedIK : public BipedalLocomotion::System::Advanceable +{ + +public: + /** + * Initialize the inverse kinematics algorithm. + * @param handler pointer to the IParametersHandler interface. + * @return true in case of success/false otherwise. + */ + virtual bool initialize(std::weak_ptr handler); + + /** + * Add a linear task in the inverse kinematics + * @param task pointer to a given linear task + * @param priority Priority associated to the task. The lower the number the higher the + * priority. + * @param weight Weight associated to the task. This parameter is optional. The default value is + * an object that does not contain any value. So is an invalid weight. + * @return true if the task has been added to the inverse kinematics. + */ + virtual bool addTask(std::shared_ptr task, + const std::string& taskName, + std::size_t priority, + std::optional> weight = {}) = 0; + + /** + * Get a vector containing the name of the tasks. + * @return an std::vector containing all the names associated to the tasks + */ + virtual std::vector getTaskNames() const = 0; + + /** + * Finalize the IK. + * @param handler parameter handler. + * @note You should call this method after you add ALL the tasks. + * @return true in case of success, false otherwise. + */ + virtual bool finalize(const System::VariablesHandler& handler) = 0; + + /** + * Get a specific task + * @param name name associated to the task. + * @return a weak ptr associated to an existing task in the IK. If the task does not exist a + * nullptr is returned. + */ + virtual std::weak_ptr getTask(const std::string& name) const = 0; + + /** + * Destructor. + */ + virtual ~IntegrationBasedIK() = default; +}; +} // namespace IK +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_IK_INTEGRATION_BASE_INVERSE_KINEMATICS_H diff --git a/src/IK/src/IntegrationBasedIK.cpp b/src/IK/src/IntegrationBasedIK.cpp new file mode 100644 index 0000000000..eeeaf9cba4 --- /dev/null +++ b/src/IK/src/IntegrationBasedIK.cpp @@ -0,0 +1,15 @@ +/** + * @file IntegrationBasedIK.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. + */ + +#include + +using namespace BipedalLocomotion::IK; + +bool IntegrationBasedIK::initialize(std::weak_ptr handler) +{ + return true; +}; From 65ce4b93dd000ae80cc6bfc7c6b1c822654b3390 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:28:40 +0100 Subject: [PATCH 08/21] Implement QPInverseKinematics in the IK component --- .../IK/QPInverseKinematics.h | 136 ++++++ src/IK/src/QPInverseKinematics.cpp | 455 ++++++++++++++++++ 2 files changed, 591 insertions(+) create mode 100644 src/IK/include/BipedalLocomotion/IK/QPInverseKinematics.h create mode 100644 src/IK/src/QPInverseKinematics.cpp diff --git a/src/IK/include/BipedalLocomotion/IK/QPInverseKinematics.h b/src/IK/include/BipedalLocomotion/IK/QPInverseKinematics.h new file mode 100644 index 0000000000..edfa85ec01 --- /dev/null +++ b/src/IK/include/BipedalLocomotion/IK/QPInverseKinematics.h @@ -0,0 +1,136 @@ +/** + * @file QPInverseKinematics.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_QP_INVERSE_KINEMATICS_H +#define BIPEDAL_LOCOMOTION_IK_QP_INVERSE_KINEMATICS_H + +#include +#include +#include + + +#include + +#include +#include +#include +#include + +namespace BipedalLocomotion +{ + +namespace IK +{ + +/** + * QPInverseKinematics is a concrete class and implements an integration base inverse kinematics. + * The inverse kinematics is here implemented as Quadratic Programming (QP) problem. The user should + * set the desired task with the method QPInverseKinematics::addTask. Each task has a given + * priority. Currently we support only priority equal to 0 or 1. If the task priority is set to 0 + * the task will be considered as hard task, thus treated as an equality constraint. If the priority + * is equal to 1 the task will be embedded in the cost function. The class is also able to treat + * inequality constraints. + * A possible usage of the IK can be found in "Romualdi et al. A Benchmarking of DCM Based + * Architectures for Position and Velocity Controlled Walking of Humanoid Robots" + * https://doi.org/10.1109/HUMANOIDS.2018.8625025 + */ +class QPInverseKinematics : public IntegrationBasedIK +{ + /** + * Private implementation + */ + struct Impl; + std::unique_ptr m_pimpl; + +public: + + /** + * Constructor. + */ + QPInverseKinematics(); + + /** + * Destructor. + */ + ~QPInverseKinematics(); + + /** + * Add a linear task in the inverse kinematics + * @param task pointer to a given linear task + * @param priority Priority associated to the task. The lower the number the higher the + * priority. + * @param weight weight associated to the task. This parameter is optional. The default value is + * an object that does not contain any value. So is an invalid weight. + * @note currently we support only task with priority 0 or 1. If the priority is set to 0 the + * task will be considered as a constraint. In this case the weight is not required. + * @warning The QPInverseKinematics cannot handle inequality tasks (please check + * Task::Type) with priority equal to 1. + * @return true if the task has been added to the inverse kinematics. + */ + bool addTask(std::shared_ptr task, + const std::string& taskName, + std::size_t priority, + std::optional> weight = {}) override; + + /** + * Finalize the IK. + * @param handler parameter handler. + * @note You should call this method after you add ALL the tasks. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& handler) override; + + + /** + * Initialize the inverse kinematics algorithm. + * @param handler pointer to the IParametersHandler interface.g + * @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 generalized robot velocity | Yes | + * | `verbosity` | `bool` | Verbosity of the solver. Default value `false` | No | + * Where the generalized robot velocity is a vector containing the base spatialvelocity + (expressed in mixed representation) and the joint velocities. + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr handler) override; + + /** + * Get a vector containing the name of the tasks. + * @return an std::vector containing all the names associated to the tasks + */ + std::vector getTaskNames() const override; + + /** + * Return true if the content of get is valid. + */ + bool isValid() const override; + + /** + * Solve the inverse kinematics. + * @return true in case of success and false otherwise. + */ + bool advance() override; + + /** + * Get the outcome of the optimization problem + * @return the state of the inverse kinematics. + */ + const IntegrationBasedIKState& get() const override; + + /** + * Get a specific task + * @param name name associated to the task. + * @return a weak ptr associated to an existing task in the IK. If the task does not exist a + * nullptr is returned. + */ + std::weak_ptr getTask(const std::string& name) const override; +}; +} // namespace IK +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_IK_QP_INVERSE_KINEMATICS_H diff --git a/src/IK/src/QPInverseKinematics.cpp b/src/IK/src/QPInverseKinematics.cpp new file mode 100644 index 0000000000..b87efba787 --- /dev/null +++ b/src/IK/src/QPInverseKinematics.cpp @@ -0,0 +1,455 @@ +/** + * @file QPInverseKinematics.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. + */ + +#include + +#include + +#include +#include + +using namespace BipedalLocomotion::IK; + +struct QPInverseKinematics::Impl +{ + struct TaskWithPriority + { + std::shared_ptr task; + std::size_t priority; + Eigen::VectorXd weight; + Eigen::MatrixXd tmp; /**< This is a temporary matrix usefull to reduce dynamics allocation + in advance method */ + }; + + IntegrationBasedIKState solution; + + System::VariablesHandler::VariableDescription robotVelocityVariable; + + OsqpEigen::Solver solver; /**< Optimization solver. */ + bool isVerbose{false}; + + std::unordered_map tasks; + + std::vector> constraints; + std::vector> costs; + std::size_t numberOfConstraints; + + Eigen::MatrixXd hessian; + Eigen::VectorXd gradient; + Eigen::MatrixXd constraintMatrix; + Eigen::VectorXd lowerBound; + Eigen::VectorXd upperBound; + + bool isFirstIteration{true}; + bool isValid{false}; + bool isInitialized{false}; + bool isFinalized{false}; + + bool initializeSolver() + { + constexpr auto logPrefix = "[QPInversekinematics::Impl::initializeSolver]"; + // Hessian matrix + Eigen::SparseMatrix hessianSparse = this->hessian.sparseView(); + if (!this->solver.data()->setHessianMatrix(hessianSparse)) + { + log()->error("{} Unable to set the hessian matrix.", logPrefix); + return false; + } + + // gradient + if (!this->solver.data()->setGradient(gradient)) + { + log()->error("{} Unable to set the gradient vector.", logPrefix); + return false; + } + + Eigen::SparseMatrix constraintsMatrixSparse = this->constraintMatrix.sparseView(); + if (!this->solver.data()->setLinearConstraintsMatrix(constraintsMatrixSparse)) + { + log()->error("{} Unable to set the constraint matrix.", logPrefix); + return false; + } + + if (!this->solver.data()->setBounds(this->lowerBound, this->upperBound)) + { + log()->error("{} Unable to set the bounds.", logPrefix); + return false; + } + + if (!this->solver.initSolver()) + { + log()->error("{} Unable to initialize the solver.", logPrefix); + return false; + } + + return true; + } + + bool updateSolver() + { + constexpr auto logPrefix = "[QPInversekinematics::Impl::updateSolver]"; + // Hessian matrix + Eigen::SparseMatrix hessianSparse = this->hessian.sparseView(); + if (!this->solver.updateHessianMatrix(hessianSparse)) + { + log()->error("{} Unable to set the hessian matrix.", logPrefix); + return false; + } + + // gradient + if (!this->solver.updateGradient(gradient)) + { + log()->error("{} Unable to set the gradient vector.", logPrefix); + return false; + } + + Eigen::SparseMatrix constraintsMatrixSparse = this->constraintMatrix.sparseView(); + if (!this->solver.updateLinearConstraintsMatrix(constraintsMatrixSparse)) + { + log()->error("{} Unable to set the constraint matrix.", logPrefix); + return false; + } + + if (!this->solver.updateBounds(this->lowerBound, this->upperBound)) + { + log()->error("{} Unable to set the bounds.", logPrefix); + return false; + } + + return true; + } +}; + +QPInverseKinematics::QPInverseKinematics() +{ + m_pimpl = std::make_unique(); +} + +QPInverseKinematics::~QPInverseKinematics() = default; + +bool QPInverseKinematics::addTask(std::shared_ptr task, + const std::string& taskName, + std::size_t priority, + std::optional> weight) +{ + constexpr auto logPrefix = "[QPInverseKinematics::addTask]"; + + // check if the task already exist + const bool taskExist = (m_pimpl->tasks.find(taskName) != m_pimpl->tasks.end()); + if (taskExist) + { + log()->error("{} The task named {} already exist.", logPrefix, taskName); + return false; + } + + if (priority != 0 && priority != 1) + { + log()->error("{} - [Task name: '{}'] For the time being we support only priority equal to " + "0 or 1.", + logPrefix, + taskName); + return false; + } + + if (priority == 1 && !weight) + { + log()->error("{} - [Task name: '{}'] In case of priority equal to 1 the weight is " + "mandatory.", + logPrefix, + taskName); + return false; + } + + if (priority == 1 && task->type() == LinearTask::Type::inequality) + { + log()->error("{} - [Task name: '{}'] This implementation of the inverse kinematics cannot " + "handle inequality tasks with priority equal to 1.", + logPrefix, + taskName); + return false; + } + + // Store the task inside the InverseKinematics + m_pimpl->tasks[taskName].task = task; + m_pimpl->tasks[taskName].priority = priority; + + if (priority == 1) + { + if (weight.value().size() != task->size()) + { + log()->error("{} - [Task name: '{}'] The size of the weight is not coherent with the " + "size of the task. Expected: {}. Given: {}.", + logPrefix, + taskName, + m_pimpl->tasks[taskName].task->size(), + weight.value().size()); + + // erase the task since it is not valid + m_pimpl->tasks.erase(taskName); + + return false; + } + + // add the weight + m_pimpl->tasks[taskName].weight = weight.value(); + + // add the task to the list of the element that are used to build the cost function + m_pimpl->costs.push_back(m_pimpl->tasks[taskName]); + } else + { + m_pimpl->constraints.push_back(m_pimpl->tasks[taskName]); + } + + // if you add a new task you should call finalize again + m_pimpl->isFinalized = false; + + return true; +} + +bool QPInverseKinematics::finalize(const System::VariablesHandler& handler) +{ + constexpr auto logPrefix = "[QPInverseKinematics::finalize]"; + + // clear the solver + m_pimpl->solver.clearSolver(); + + // set the some internal parameter of osqp-eigen + m_pimpl->solver.settings()->setVerbosity(m_pimpl->isVerbose); + + // set the variable handler for all the tasks + m_pimpl->numberOfConstraints = 0; + for (auto& [name, task] : m_pimpl->tasks) + { + if (!task.task->setVariablesHandler(handler)) + { + log()->error("{} Error while setting the variable handler in the task named {}, having " + "the following description {}.", + logPrefix, + name, + task.task->getDescription()); + return false; + } + + // if priority is equal to 0 the task is considered as hard constraint. + if (task.priority == 0) + { + m_pimpl->numberOfConstraints += task.task->size(); + } + } + + // resize the temporary matrix usefull to reduce dynamics allocation when advance() is called + for (auto& cost : m_pimpl->costs) + { + cost.get().tmp.resize(handler.getNumberOfVariables(), cost.get().weight.size()); + } + + m_pimpl->solver.data()->setNumberOfVariables(handler.getNumberOfVariables()); + m_pimpl->solver.data()->setNumberOfConstraints(m_pimpl->numberOfConstraints); + + // resize matrices + m_pimpl->hessian.resize(handler.getNumberOfVariables(), handler.getNumberOfVariables()); + m_pimpl->gradient.resize(handler.getNumberOfVariables()); + + m_pimpl->constraintMatrix.resize(m_pimpl->numberOfConstraints, handler.getNumberOfVariables()); + m_pimpl->upperBound.resize(m_pimpl->numberOfConstraints); + m_pimpl->lowerBound.resize(m_pimpl->numberOfConstraints); + + if (!handler.getVariable(m_pimpl->robotVelocityVariable.name, m_pimpl->robotVelocityVariable)) + { + log()->error("{} Error while retrieving the robot velocity variable.", logPrefix); + return false; + } + + m_pimpl->isFinalized = true; + + return true; +} + +std::vector QPInverseKinematics::getTaskNames() const +{ + std::vector tasksName; + + for (const auto& [name, task] : m_pimpl->tasks) + { + tasksName.push_back(name); + } + + return tasksName; +} + +bool QPInverseKinematics::advance() +{ + constexpr auto logPrefix = "[QPInverseKinematics::advance]"; + + // when advance is called the previous solution is no more valid + m_pimpl->isValid = false; + + if (!m_pimpl->isInitialized) + { + log()->error("{} Please call initialize() before advance().", logPrefix); + return false; + } + + if (!m_pimpl->isFinalized) + { + log()->error("{} Please call finalize() before advance().", logPrefix); + return false; + } + + + // update of all the tasks + for (auto& [name, task] : m_pimpl->tasks) + { + task.task->update(); + } + + // Compute the gradient and the hessian + m_pimpl->hessian.setZero(); + m_pimpl->gradient.setZero(); + for (auto& cost : m_pimpl->costs) + { + Eigen::Ref A = cost.get().task->getA(); + Eigen::Ref b = cost.get().task->getB(); + + // Here we avoid to have dynamic allocation + cost.get().tmp.noalias() = A.transpose() * cost.get().weight.asDiagonal(); + m_pimpl->hessian.noalias() += cost.get().tmp * A; + m_pimpl->gradient.noalias() -= cost.get().tmp * b; + } + + // compute the constraints + std::size_t index = 0; + for (const auto& constraint : m_pimpl->constraints) + { + // check if the number of constraints changed + if (m_pimpl->numberOfConstraints < index + constraint.get().task->size()) + { + log()->error("{} The number of constraints changed. Please call finalize() again. " + "Expected number of constraints: {}. Adding the constrained named {}, the " + "number of constraints becomes {}.", + logPrefix, + m_pimpl->numberOfConstraints, + constraint.get().task->getDescription(), + index + constraint.get().task->size()); + return false; + } + + Eigen::Ref A = constraint.get().task->getA(); + Eigen::Ref b = constraint.get().task->getB(); + + m_pimpl->constraintMatrix.middleRows(index, constraint.get().task->size()) = A; + m_pimpl->upperBound.segment(index, constraint.get().task->size()) = b; + + if (constraint.get().task->type() == LinearTask::Type::inequality) + { + m_pimpl->lowerBound.segment(index, constraint.get().task->size()) + .setConstant(-OsqpEigen::INFTY); + } else + { + m_pimpl->lowerBound.segment(index, constraint.get().task->size()) = b; + } + + index += constraint.get().task->size(); + } + + // update the solver + if (!m_pimpl->isFirstIteration) + { + if (!m_pimpl->updateSolver()) + { + log()->error("{} Unable to update the QP solver.", logPrefix); + return false; + } + } else + { + if (!m_pimpl->initializeSolver()) + { + log()->error("{} Unable to run the QP solver the first time.", logPrefix); + return false; + } + m_pimpl->isFirstIteration = false; + } + + // solve the QP + if (!m_pimpl->solver.solve()) + { + log()->error("{} Unable to to solve the problem.", logPrefix); + return false; + } + + // retrieve the solution + constexpr std::size_t spatialVelocitySize = 6; + const std::size_t joints = m_pimpl->robotVelocityVariable.size - spatialVelocitySize; + + m_pimpl->solution.jointVelocity + = m_pimpl->solver.getSolution().segment(m_pimpl->robotVelocityVariable.offset + + spatialVelocitySize, + joints); + + m_pimpl->solution.baseVelocity.coeffs() + = m_pimpl->solver.getSolution().segment( + m_pimpl->robotVelocityVariable.offset); + + m_pimpl->isValid = true; + + return true; +} + +bool QPInverseKinematics::isValid() const +{ + return m_pimpl->isValid; +} + +const IntegrationBasedIKState& QPInverseKinematics::get() const +{ + return m_pimpl->solution; +} + +std::weak_ptr QPInverseKinematics::getTask(const std::string& name) const +{ + constexpr auto logPrefix = "[QPInverseKinematics::getTask]"; + auto task = m_pimpl->tasks.find(name); + + if (task == m_pimpl->tasks.end()) + { + log()->warn("{} The task named {} does not exist. A nullptr will be returned.", + logPrefix, + name); + return std::shared_ptr(nullptr); + } + + return task->second.task; +} + +bool QPInverseKinematics::initialize(std::weak_ptr handler) +{ + constexpr auto logPrefix = "[QPInverseKinematics::initialize]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", logPrefix); + return false; + } + + if (!ptr->getParameter("robot_velocity_variable_name", m_pimpl->robotVelocityVariable.name)) + { + log()->error("{} Error while retrieving the robot velocity variable.", logPrefix); + return false; + } + + if (!ptr->getParameter("verbosity", m_pimpl->isVerbose)) + { + log()->info("{} 'verbosity' not found. The following parameter will be used '{}'.", + logPrefix, + m_pimpl->isVerbose); + } + + m_pimpl->isInitialized = true; + + return true; +} From 079fe51ddded1dd644449bc02f26f25d1cb6118f Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:29:59 +0100 Subject: [PATCH 09/21] Implement SO3TaskTest in the IK component --- src/IK/tests/SO3TaskTest.cpp | 131 +++++++++++++++++++++++++++++++++++ 1 file changed, 131 insertions(+) create mode 100644 src/IK/tests/SO3TaskTest.cpp diff --git a/src/IK/tests/SO3TaskTest.cpp b/src/IK/tests/SO3TaskTest.cpp new file mode 100644 index 0000000000..7286dfe0d3 --- /dev/null +++ b/src/IK/tests/SO3TaskTest.cpp @@ -0,0 +1,131 @@ +/** + * @file SO3TaskTest.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::IK; + +TEST_CASE("SO3 Task") +{ + constexpr double kp = 1.0; + constexpr auto robotVelocity = "robotVelocity"; + + + auto kinDyn = std::make_shared(); + auto parameterHandler = std::make_shared(); + + parameterHandler->setParameter("robot_velocity_variable_name", + robotVelocity); + + parameterHandler->setParameter("kp_angular", kp); + + // set the velocity representation + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)); + + for (std::size_t numberOfJoints = 6; numberOfJoints < 200; 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(robotVelocity, model.getNrOfDOFs() + 6); + variablesHandler.addVariable("dummy2", 15); + + const std::string controlledFrame = model.getFrameName(numberOfJoints); + parameterHandler->setParameter("frame_name", controlledFrame); + + SO3Task task; + REQUIRE(task.setKinDyn(kinDyn)); + REQUIRE(task.initialize(parameterHandler)); + REQUIRE(task.setVariablesHandler(variablesHandler)); + + const auto desiredRotation = manif::SO3d::Random(); + const auto desiredVelocity = manif::SO3d::Tangent::Random(); + + REQUIRE(task.setSetPoint(desiredRotation, desiredVelocity)); + + 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(robotVelocity).offset, + variablesHandler.getVariable(robotVelocity).size) + .isApprox(jacobian.bottomRows<3>())); + + // check the vector b + LieGroupControllers::ProportionalControllerSO3d SO3Controller; + SO3Controller.setGains(kp); + + SO3Controller.setFeedForward(desiredVelocity); + SO3Controller.setDesiredState(desiredRotation); + + SO3Controller.setState(BipedalLocomotion::Conversions::toManifRot( + kinDyn->getWorldTransform(controlledFrame).getRotation())); + + SO3Controller.computeControlLaw(); + + Eigen::Vector3d expectedB = SO3Controller.getControl().coeffs(); + + REQUIRE(b.isApprox(expectedB)); + } + } +} From 9f5a1595d86eec30f66e74a3065c9ea42eed4669 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:30:06 +0100 Subject: [PATCH 10/21] Implement SE3TaskTest in the IK component --- src/IK/tests/SE3TaskTest.cpp | 141 +++++++++++++++++++++++++++++++++++ 1 file changed, 141 insertions(+) create mode 100644 src/IK/tests/SE3TaskTest.cpp diff --git a/src/IK/tests/SE3TaskTest.cpp b/src/IK/tests/SE3TaskTest.cpp new file mode 100644 index 0000000000..f8129ac9ba --- /dev/null +++ b/src/IK/tests/SE3TaskTest.cpp @@ -0,0 +1,141 @@ +/** + * @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::IK; + +TEST_CASE("SE3 Task") +{ + constexpr double kp = 1.0; + const std::string robotVelocity = "robotVelocity"; + + + auto kinDyn = std::make_shared(); + auto parameterHandler = std::make_shared(); + + parameterHandler->setParameter("robot_velocity_variable_name", + robotVelocity); + + parameterHandler->setParameter("kp_linear", kp); + parameterHandler->setParameter("kp_angular", kp); + + // set the velocity representation + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)); + + for (std::size_t numberOfJoints = 6; numberOfJoints < 200; 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(robotVelocity, 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)); + REQUIRE(task.setVariablesHandler(variablesHandler)); + + const auto desiredPose = manif::SE3d::Random(); + const auto desiredVelocity = manif::SE3d::Tangent::Random(); + + REQUIRE(task.setSetPoint(desiredPose, desiredVelocity)); + + 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(robotVelocity).offset, + variablesHandler.getVariable(robotVelocity).size) + .isApprox(jacobian)); + + // check the vector b + LieGroupControllers::ProportionalControllerSO3d SO3Controller; + LieGroupControllers::ProportionalControllerR3d R3Controller; + SO3Controller.setGains(kp); + R3Controller.setGains(kp); + + SO3Controller.setFeedForward(desiredVelocity.w()); + R3Controller.setFeedForward(desiredVelocity.v()); + SO3Controller.setDesiredState(desiredPose.quat()); + R3Controller.setDesiredState(desiredPose.translation()); + + SO3Controller.setState(BipedalLocomotion::Conversions::toManifRot( + kinDyn->getWorldTransform(controlledFrame).getRotation())); + + R3Controller.setState( + iDynTree::toEigen(kinDyn->getWorldTransform(controlledFrame).getPosition())); + + SO3Controller.computeControlLaw(); + R3Controller.computeControlLaw(); + + Eigen::VectorXd expectedB(6); + expectedB.head<3>() = R3Controller.getControl().coeffs(); + expectedB.tail<3>() = SO3Controller.getControl().coeffs(); + + REQUIRE(b.isApprox(expectedB)); + } + } +} From 2ff2684b17253551a3617ccedfaf11de24ee5914 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:30:26 +0100 Subject: [PATCH 11/21] Implement CoMTaskTest in the IK component --- src/IK/tests/CoMTaskTest.cpp | 129 +++++++++++++++++++++++++++++++++++ 1 file changed, 129 insertions(+) create mode 100644 src/IK/tests/CoMTaskTest.cpp diff --git a/src/IK/tests/CoMTaskTest.cpp b/src/IK/tests/CoMTaskTest.cpp new file mode 100644 index 0000000000..2b98b57003 --- /dev/null +++ b/src/IK/tests/CoMTaskTest.cpp @@ -0,0 +1,129 @@ +/** + * @file CoMTaskTest.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 + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +TEST_CASE("CoM Task") +{ + constexpr double kp = 1.0; + constexpr auto robotVelocity = "robotVelocity"; + + + auto kinDyn = std::make_shared(); + auto parameterHandler = std::make_shared(); + + parameterHandler->setParameter("robot_velocity_variable_name", + robotVelocity); + + parameterHandler->setParameter("kp_linear", kp); + + // set the velocity representation + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)); + + for (std::size_t numberOfJoints = 6; numberOfJoints < 200; 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(robotVelocity, model.getNrOfDOFs() + 6); + variablesHandler.addVariable("dummy2", 15); + + const std::string controlledFrame = model.getFrameName(numberOfJoints); + parameterHandler->setParameter("frame_name", controlledFrame); + + CoMTask task; + REQUIRE(task.setKinDyn(kinDyn)); + REQUIRE(task.initialize(parameterHandler)); + REQUIRE(task.setVariablesHandler(variablesHandler)); + + const auto desiredPosition = manif::R3d::Random(); + const auto desiredVelocity = manif::R3d::Tangent::Random(); + + REQUIRE(task.setSetPoint(desiredPosition.coeffs(), desiredVelocity.coeffs())); + + 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(3, model.getNrOfDOFs() + 6); + REQUIRE(kinDyn->getCenterOfMassJacobian(jacobian)); + + REQUIRE(A.middleCols(variablesHandler.getVariable(robotVelocity).offset, + variablesHandler.getVariable(robotVelocity).size) + .isApprox(jacobian)); + + // check the vector b + LieGroupControllers::ProportionalControllerR3d R3Controller; + R3Controller.setGains(kp); + + R3Controller.setFeedForward(desiredVelocity); + R3Controller.setDesiredState(desiredPosition); + + R3Controller.setState(toEigen(kinDyn->getCenterOfMassPosition())); + + R3Controller.computeControlLaw(); + + Eigen::Vector3d expectedB = R3Controller.getControl().coeffs(); + + REQUIRE(b.isApprox(expectedB)); + } + } +} From dae08921e4782b77c38fc70534d7c6b05dd27e0e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:30:37 +0100 Subject: [PATCH 12/21] Implement JointTrackingTaskTest in the IK component --- src/IK/tests/JointTrackingTaskTest.cpp | 116 +++++++++++++++++++++++++ 1 file changed, 116 insertions(+) create mode 100644 src/IK/tests/JointTrackingTaskTest.cpp diff --git a/src/IK/tests/JointTrackingTaskTest.cpp b/src/IK/tests/JointTrackingTaskTest.cpp new file mode 100644 index 0000000000..98f0acf519 --- /dev/null +++ b/src/IK/tests/JointTrackingTaskTest.cpp @@ -0,0 +1,116 @@ +/** + * @file JointTrackingTaskTest.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::IK; + +TEST_CASE("Joint Regularization task") +{ + const std::string robotVelocity = "robotVelocity"; + + Eigen::VectorXd kp; + + auto kinDyn = std::make_shared(); + auto parameterHandler = std::make_shared(); + + parameterHandler->setParameter("robot_velocity_variable_name", + robotVelocity); + + // set the velocity representation + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)); + + for (std::size_t numberOfJoints = 6; numberOfJoints < 200; 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(robotVelocity, model.getNrOfDOFs() + 6); + variablesHandler.addVariable("dummy2", 15); + + + kp.resize(model.getNrOfDOFs()); + kp.setOnes(); + parameterHandler->setParameter("kp", kp); + + + JointTrackingTask task; + REQUIRE(task.setKinDyn(kinDyn)); + REQUIRE(task.initialize(parameterHandler)); + REQUIRE(task.setVariablesHandler(variablesHandler)); + + REQUIRE(task.setSetPoint(Eigen::VectorXd::Zero(model.getNrOfDOFs()))); + + 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()); + + REQUIRE(A.middleCols(variablesHandler.getVariable(robotVelocity).offset + 6, + model.getNrOfDOFs()) + .isIdentity()); + + // check the vector b + Eigen::VectorXd expectedB(model.getNrOfDOFs()); + expectedB = -(kp.asDiagonal() * iDynTree::toEigen(jointsPos)); + REQUIRE(b.isApprox(expectedB)); + } + } +} From be80e26e2822e1d9b679f4e2b1ed03ace28451c2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:31:02 +0100 Subject: [PATCH 13/21] Implement QPInverseKinematicsTest in the IK component --- src/IK/tests/QPInverseKinematicsTest.cpp | 299 +++++++++++++++++++++++ 1 file changed, 299 insertions(+) create mode 100644 src/IK/tests/QPInverseKinematicsTest.cpp diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp new file mode 100644 index 0000000000..03a279a700 --- /dev/null +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -0,0 +1,299 @@ +/** + * @file QPInverseKinematicsTest.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 + +// std +#include + +// BipedalLocomotion +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; +using namespace BipedalLocomotion::Conversions; + +constexpr auto robotVelocity = "robotVelocity"; +constexpr double dT = 0.01; + +struct InverseKinematicsAndTasks +{ + std::shared_ptr ik; + std::shared_ptr se3Task; + std::shared_ptr comTask; + std::shared_ptr regularizationTask; +}; + +struct DesiredSetPoints +{ + Eigen::Vector3d CoMPosition; + manif::SE3d endEffectorPose; + Eigen::VectorXd joints; +}; + +struct System +{ + std::shared_ptr> integrator; + std::shared_ptr dynamics; +}; + +std::shared_ptr createParameterHandler() +{ + constexpr double gain = 5.0; + + auto parameterHandler = std::make_shared(); + parameterHandler->setParameter("robot_velocity_variable_name", robotVelocity); + + parameterHandler->setParameter("verbosity", false); + + /////// SE3 Task + auto SE3ParameterHandler = std::make_shared(); + SE3ParameterHandler->setParameter("robot_velocity_variable_name", robotVelocity); + + SE3ParameterHandler->setParameter("kp_linear", gain); + SE3ParameterHandler->setParameter("kp_angular", gain); + parameterHandler->setGroup("SE3_TASK", SE3ParameterHandler); + + /////// CoM task + auto CoMParameterHandler = std::make_shared(); + CoMParameterHandler->setParameter("robot_velocity_variable_name", robotVelocity); + CoMParameterHandler->setParameter("kp_linear", gain); + parameterHandler->setGroup("COM_TASK", CoMParameterHandler); + + /////// Joint regularization task + auto jointRegularizationHandler = std::make_shared(); + jointRegularizationHandler->setParameter("robot_velocity_variable_name", robotVelocity); + parameterHandler->setGroup("REGULARIZATION_TASK", jointRegularizationHandler); + + return parameterHandler; +} + +InverseKinematicsAndTasks createIK(std::shared_ptr handler, + std::shared_ptr kinDyn, + const VariablesHandler& variables) +{ + // prepare the parameters related to the size of the system + const Eigen::VectorXd kpRegularization = Eigen::VectorXd::Ones(kinDyn->model().getNrOfDOFs()); + const Eigen::VectorXd weightRegularization = kpRegularization; + handler->getGroup("REGULARIZATION_TASK").lock()->setParameter("kp", kpRegularization); + + InverseKinematicsAndTasks out; + + constexpr std::size_t highPriority = 0; + constexpr std::size_t lowPriority = 1; + + out.ik = std::make_shared(); + REQUIRE(out.ik->initialize(handler)); + + out.se3Task = std::make_shared(); + REQUIRE(out.se3Task->setKinDyn(kinDyn)); + REQUIRE(out.se3Task->initialize(handler->getGroup("SE3_TASK"))); + REQUIRE(out.ik->addTask(out.se3Task, "se3_task", highPriority)); + + out.comTask = std::make_shared(); + REQUIRE(out.comTask->setKinDyn(kinDyn)); + REQUIRE(out.comTask->initialize(handler->getGroup("COM_TASK"))); + REQUIRE(out.ik->addTask(out.comTask, "com_task", highPriority)); + + out.regularizationTask = std::make_shared(); + + + REQUIRE(out.regularizationTask->setKinDyn(kinDyn)); + REQUIRE(out.regularizationTask->initialize(handler->getGroup("REGULARIZATION_TASK"))); + REQUIRE(out.ik->addTask(out.regularizationTask, + "regularization_task", + lowPriority, + weightRegularization)); + + REQUIRE(out.ik->finalize(variables)); + + return out; +} + +DesiredSetPoints getDesiredReference(std::shared_ptr kinDyn, + std::size_t numberOfJoints) +{ + DesiredSetPoints out; + + const auto worldBasePos = iDynTree::getRandomTransform(); + const auto baseVel = iDynTree::Twist::Zero(); + + iDynTree::VectorDynSize jointsPos(kinDyn->model().getNrOfDOFs()); + iDynTree::VectorDynSize jointsVel(kinDyn->model().getNrOfDOFs()); + iDynTree::Vector3 gravity; + + for (auto& joint : jointsPos) + { + joint = iDynTree::getRandomDouble(); + } + + for (auto& joint : jointsVel) + { + joint = 0; + } + + for (auto& element : gravity) + { + element = iDynTree::getRandomDouble(); + } + + REQUIRE(kinDyn->setRobotState(worldBasePos, jointsPos, baseVel, jointsVel, gravity)); + + // getCoMPosition and velocity + out.CoMPosition = iDynTree::toEigen(kinDyn->getCenterOfMassPosition()); + const std::string controlledFrame = kinDyn->model().getFrameName(numberOfJoints); + out.endEffectorPose = toManifPose(kinDyn->getWorldTransform(controlledFrame)); + out.joints.resize(jointsPos.size()); + out.joints = iDynTree::toEigen(jointsPos); + + return out; +} + +System getSystem(std::shared_ptr kinDyn) +{ + System out; + + // random noise in the joints position + std::default_random_engine generator; + generator.seed(42); + std::normal_distribution distribution(0.0, 0.1); + + // create the System + Eigen::Matrix baseVelocity; + Eigen::VectorXd jointVelocities(kinDyn->getNrOfDegreesOfFreedom()); + Eigen::Matrix4d basePose; + Eigen::VectorXd jointPositions(kinDyn->getNrOfDegreesOfFreedom()); + Eigen::Vector3d gravity; + + REQUIRE(kinDyn->getRobotState(basePose, jointPositions, baseVelocity, jointVelocities, gravity)); + + // perturb the joint position + for (int i = 0; i < kinDyn->getNrOfDegreesOfFreedom(); i++) + { + jointPositions[i] += distribution(generator); + } + + out.dynamics = std::make_shared(); + out.dynamics->setState({basePose.topRightCorner<3, 1>(), + basePose.topLeftCorner<3, 3>(), + jointPositions}); + + out.integrator = std::make_shared>(dT); + out.integrator->setDynamicalSystem(out.dynamics); + + return out; +} + +TEST_CASE("QP-IK") +{ + auto kinDyn = std::make_shared(); + auto parameterHandler = createParameterHandler(); + + constexpr double tolerance = 5e-2; + + // set the velocity representation + REQUIRE(kinDyn->setFrameVelocityRepresentation( + iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)); + + for (std::size_t numberOfJoints = 20; numberOfJoints < 40; numberOfJoints += 15) + { + DYNAMIC_SECTION("Model with " << numberOfJoints << " joints") + { + // create the model + const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); + REQUIRE(kinDyn->loadRobotModel(model)); + + const auto desiredSetPoints = getDesiredReference(kinDyn, numberOfJoints); + + // Instantiate the handler + VariablesHandler variablesHandler; + variablesHandler.addVariable(robotVelocity, model.getNrOfDOFs() + 6); + + auto system = getSystem(kinDyn); + + // Set the frame name + const std::string controlledFrame = model.getFrameName(numberOfJoints); + parameterHandler->getGroup("SE3_TASK") + .lock() + ->setParameter("frame_name", controlledFrame); + + // create the IK + auto ikAndTasks = createIK(parameterHandler, kinDyn, variablesHandler); + + REQUIRE(ikAndTasks.se3Task->setSetPoint(desiredSetPoints.endEffectorPose, + manif::SE3d::Tangent::Zero())); + REQUIRE(ikAndTasks.comTask->setSetPoint(desiredSetPoints.CoMPosition, + Eigen::Vector3d::Zero())); + REQUIRE(ikAndTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); + + // propagate the inverse kinematics for + constexpr std::size_t iterations = 80; + Eigen::Vector3d gravity; + gravity << 0, 0, -9.81; + Eigen::Matrix4d baseTransform; + baseTransform.setZero(); + Eigen::Matrix baseVelocity; + baseVelocity.setZero(); + Eigen::VectorXd jointVelocity(model.getNrOfDOFs()); + jointVelocity.setZero(); + + for (std::size_t iteration = 0; iteration < iterations; iteration++) + { + // get the solution of the integrator + const auto& [basePosition, baseRotation, jointPosition] + = system.integrator->getSolution(); + + // update the KinDynComputations object + baseTransform.topLeftCorner<3, 3>() = baseRotation; + baseTransform.topRightCorner<3, 1>() = basePosition; + REQUIRE(kinDyn->setRobotState(baseTransform, + jointPosition, + baseVelocity, + jointVelocity, + gravity)); + + // solve the IK + REQUIRE(ikAndTasks.ik->advance()); + + // get the output of the IK + baseVelocity = ikAndTasks.ik->get().baseVelocity.coeffs(); + jointVelocity = ikAndTasks.ik->get().jointVelocity; + + // propagate the dynamical system + system.dynamics->setControlInput({baseVelocity, jointVelocity}); + system.integrator->integrate(0, dT); + } + + // check the CoM position + REQUIRE(desiredSetPoints.CoMPosition.isApprox(toEigen(kinDyn->getCenterOfMassPosition()), + tolerance)); + + // Check the end-effector pose error + const manif::SE3d endEffectorPose = toManifPose(kinDyn->getWorldTransform(controlledFrame)); + + // please read it as (log(desiredSetPoints.endEffectorPose^-1 * endEffectorPose))^v + const manif::SE3d::Tangent error = endEffectorPose - desiredSetPoints.endEffectorPose; + REQUIRE(error.coeffs().isZero(tolerance)); + } + } +} From a1c9dbdfd561435e46b17d237f336744804a3717 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:31:23 +0100 Subject: [PATCH 14/21] Enable the compilation of the tests in the IK component --- src/IK/tests/CMakeLists.txt | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 src/IK/tests/CMakeLists.txt diff --git a/src/IK/tests/CMakeLists.txt b/src/IK/tests/CMakeLists.txt new file mode 100644 index 0000000000..02ed199acf --- /dev/null +++ b/src/IK/tests/CMakeLists.txt @@ -0,0 +1,28 @@ +# Copyright (C) 2021 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 SE3TaskIK + SOURCES SE3TaskTest.cpp + LINKS BipedalLocomotion::IK BipedalLocomotion::ManifConversions) + +add_bipedal_test( + NAME SO3TaskIK + SOURCES SO3TaskTest.cpp + LINKS BipedalLocomotion::IK BipedalLocomotion::ManifConversions) + +add_bipedal_test( + NAME JointsTrackingTaskIK + SOURCES JointTrackingTaskTest.cpp + LINKS BipedalLocomotion::IK) + +add_bipedal_test( + NAME CoMTaskIK + SOURCES CoMTaskTest.cpp + LINKS BipedalLocomotion::IK) + +add_bipedal_test( + NAME QPInverseKinematics + SOURCES QPInverseKinematicsTest.cpp + LINKS BipedalLocomotion::IK BipedalLocomotion::ManifConversions) From 476ee289acca935f47167d803ec3599c24c0bb13 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:32:18 +0100 Subject: [PATCH 15/21] Implement CMakeLists.txt file in IK component --- src/IK/CMakeLists.txt | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 src/IK/CMakeLists.txt diff --git a/src/IK/CMakeLists.txt b/src/IK/CMakeLists.txt new file mode 100644 index 0000000000..c6bb251c37 --- /dev/null +++ b/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() From 1938e7be030f3a61f85a731defe2581d63fc8abe Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:32:34 +0100 Subject: [PATCH 16/21] Enable the compilation of the IK component --- src/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 92638b9b64..bc5d2b5b4d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -17,3 +17,4 @@ add_subdirectory(RobotInterface) add_subdirectory(Math) add_subdirectory(TSID) add_subdirectory(Perception) +add_subdirectory(IK) From 7d514add7e8e8fe7c99eb3a34f944ee3e9347c2c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:33:14 +0100 Subject: [PATCH 17/21] Add OsqpEigen as optional dependency of the framework --- cmake/BipedalLocomotionFrameworkDependencies.cmake | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake index 77eec27f3e..9f898cb68e 100644 --- a/cmake/BipedalLocomotionFrameworkDependencies.cmake +++ b/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) From e3c22844d227b3f036da6dbe25a5f64fd1fbe498 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:33:59 +0100 Subject: [PATCH 18/21] [GitHub CI] Install osqp and osqp-eigen --- .github/workflows/ci.yml | 64 +++++++++++++++++++++++++++++++++++----- 1 file changed, 57 insertions(+), 7 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 62a693d13c..a3c7ba02e1 100644 --- a/.github/workflows/ci.yml +++ b/.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 From e72823e2b71820e455f8ea57995049e01b6680e7 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:34:39 +0100 Subject: [PATCH 19/21] [GitHub CI - conda] Disable the compilation of the IK component --- .github/workflows/conda-forge-ci.yml | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/.github/workflows/conda-forge-ci.yml b/.github/workflows/conda-forge-ci.yml index 228ee19e71..83fd73041e 100644 --- a/.github/workflows/conda-forge-ci.yml +++ b/.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: | From be2640ed18e27103ea673d283b05de70bd21e621 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:34:49 +0100 Subject: [PATCH 20/21] Update the CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dd95f159f2..b5bb876222 100644 --- a/CHANGELOG.md +++ b/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) From 27e4f625cb735f4d7bfd6527f0f3c1dafba27f0c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 11 Mar 2021 18:35:09 +0100 Subject: [PATCH 21/21] Update the README.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index aecb3eb85d..2edb40d632 100644 --- a/README.md +++ b/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/) |