From aceaad91dce28324300d2927fba852222f10f489 Mon Sep 17 00:00:00 2001 From: Ehasn Date: Thu, 13 Apr 2023 17:06:13 +0200 Subject: [PATCH 01/17] DistanceTask added m_baseFrameName removed resize world_T_framePosition pow(world_T_framePosition(2),2) added made m_computedDistance public member Fixed use of relative distance task DIstanceTask added made m_computedDistance public member gitignore updated DistanceTask PR review: Naming fixed, m_frameName and m_frameEEName removed, unused headers removed. --- .gitignore | 1 + src/IK/CMakeLists.txt | 4 +- .../BipedalLocomotion/IK/DistanceTask.h | 77 +++++++ src/IK/src/DistanceTask.cpp | 211 ++++++++++++++++++ 4 files changed, 291 insertions(+), 2 deletions(-) create mode 100644 src/IK/include/BipedalLocomotion/IK/DistanceTask.h create mode 100644 src/IK/src/DistanceTask.cpp diff --git a/.gitignore b/.gitignore index d5aaa3ef7f..9725c751a9 100644 --- a/.gitignore +++ b/.gitignore @@ -11,6 +11,7 @@ build* # IDEs .idea* +.vscode* # ====== # Python (https://github.com/github/gitignore/blob/master/Python.gitignore) diff --git a/src/IK/CMakeLists.txt b/src/IK/CMakeLists.txt index f160c6d339..0a922a14ff 100644 --- a/src/IK/CMakeLists.txt +++ b/src/IK/CMakeLists.txt @@ -10,9 +10,9 @@ if(FRAMEWORK_COMPILE_IK) NAME IK PUBLIC_HEADERS ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/QPFixedBaseInverseKinematics.h - ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/IKLinearTask.h + ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h SOURCES src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/CoMTask.cpp src/AngularMomentumTask.cpp src/JointLimitsTask.cpp - src/QPInverseKinematics.cpp src/QPFixedBaseInverseKinematics.cpp src/IKLinearTask.cpp src/IntegrationBasedIK.cpp + src/QPInverseKinematics.cpp src/QPFixedBaseInverseKinematics.cpp src/IKLinearTask.cpp src/IntegrationBasedIK.cpp src/DistanceTask.cpp PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System LieGroupControllers::LieGroupControllers diff --git a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h new file mode 100644 index 0000000000..78ee5defe4 --- /dev/null +++ b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h @@ -0,0 +1,77 @@ +/** + * @file DistanceTask.h + * @authors Ehsan Ranjbari + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef DISTANCE_TASK_H +#define DISTANCE_TASK_H + +#include + +#include + +#include + + +namespace BipedalLocomotion +{ +namespace IK +{ + +class DistanceTask : public IKLinearTask +{ + + System::VariablesHandler::VariableDescription m_robotVelocityVariable; /**< Variable + describing the robot + velocity (base + + joint) */ + + 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. */ + bool m_isValid{false}; /**< True if the task is valid. */ + + std::shared_ptr m_kinDyn; /**< Pointer to a KinDynComputations + object */ + + std::size_t m_DoFs{1}; /**< DoFs associated to the task */ + + Eigen::MatrixXd m_jacobian, m_relativeJacobian; + Eigen::MatrixXd m_world_T_framePosition; + double m_kp; + double m_desiredDistance{0.0}; + + std::string m_baseName; + std::string m_targetFrameName; + iDynTree::FrameIndex m_baseIndex, m_targetFrameIndex; + + +public: + + double m_computedDistance{0.0}; + + bool setKinDyn(std::shared_ptr kinDyn) override; + + bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override; + + bool initialize(std::weak_ptr paramHandler) override; + + bool update() override; + + bool setDesiredDistance(double desiredDistance); + + std::size_t size() const override; + + Type type() const override; + + bool isValid() const override; +}; + +BLF_REGISTER_IK_TASK(DistanceTask); + +} // namespace IK +} // namespace BipedalLocomotion + +#endif // DISTANCE_TASK_H diff --git a/src/IK/src/DistanceTask.cpp b/src/IK/src/DistanceTask.cpp new file mode 100644 index 0000000000..b019b24219 --- /dev/null +++ b/src/IK/src/DistanceTask.cpp @@ -0,0 +1,211 @@ +/** + * @file DistanceTask.cpp + * @authors Ehsan Ranjbari + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +bool DistanceTask::setKinDyn(std::shared_ptr kinDyn) +{ + if ((kinDyn == nullptr) || (!kinDyn->isValid())) + { + log()->error("[DistanceTask::setKinDyn] Invalid kinDyn object."); + return false; + } + + m_kinDyn = kinDyn; + return true; +} + +bool DistanceTask::setVariablesHandler(const System::VariablesHandler& variablesHandler) +{ + if (!m_isInitialized) + { + log()->error("[DistanceTask::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("[DistanceTask::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("[DistanceTask::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_DoFs, variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(m_DoFs); + m_b.setZero(); + m_jacobian.resize(6, 6 + m_kinDyn->getNrOfDegreesOfFreedom()); //? + m_jacobian.setZero(); + m_relativeJacobian.resize(6, m_kinDyn->getNrOfDegreesOfFreedom()); + m_relativeJacobian.setZero(); + + return true; +} + +bool DistanceTask::initialize( + std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[DistanceTask::initialize]"; + + m_description = "DistanceTask"; + + 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; + } + + // set the gains for the controllers + if (!ptr->getParameter("kp", m_kp)) + { + log()->error("{} [{}] to get the proportional linear gain.", errorPrefix, m_description); + return false; + } + + // set the base frame name + if (!ptr->getParameter("base_name", m_baseName)) + { + log()->debug("{} [{}] to get the base name. Using default \"\"", + errorPrefix, + m_description); + } + + // set the finger tip frame Name + if (!ptr->getParameter("target_frame_name", m_targetFrameName)) + { + log()->error("{} [{}] to get the end effector frame name.", errorPrefix, m_description); + return false; + } + + if (m_baseName != "") + { + m_baseIndex = m_kinDyn->getFrameIndex(m_baseName); + + if (m_baseIndex == iDynTree::FRAME_INVALID_INDEX) + return false; + } + + m_targetFrameIndex = m_kinDyn->getFrameIndex(m_targetFrameName); + + // here you need to get the indexes of the frames and check that they exists + + m_world_T_framePosition.resize(3,1); + + m_isInitialized = true; + + return true; +} + +bool DistanceTask::update() +{ + using namespace BipedalLocomotion::Conversions; + using namespace iDynTree; + + m_isValid = false; + + // here to compute the distance, you need to get the transform. + + if (m_baseName == "") + { + m_world_T_framePosition = toEigen(m_kinDyn->getWorldTransform(m_targetFrameName).getPosition()); + + // get the jacobian + if (!m_kinDyn->getFrameFreeFloatingJacobian(m_targetFrameName, m_jacobian)) + { + log()->error("[DistanceTask::update] Unable to get the jacobian."); + return m_isValid; + } + + } + else + { + m_world_T_framePosition = toEigen(m_kinDyn->getRelativeTransform(m_baseName, m_targetFrameName).getPosition()); + + // get the jacobian + if (!m_kinDyn->getRelativeJacobian(m_baseIndex, m_targetFrameIndex, m_relativeJacobian)) + { + log()->error("[DistanceTask::update] Unable to get the relative jacobian."); + return m_isValid; + } + + m_jacobian.topRightCorner(3, m_kinDyn->getNrOfDegreesOfFreedom()) = m_relativeJacobian; + + } + + m_computedDistance = sqrt(pow(m_world_T_framePosition(0),2) + pow(m_world_T_framePosition(1),2) + pow(m_world_T_framePosition(2),2)); + + m_A.resize(1, 6 + m_kinDyn->getNrOfDegreesOfFreedom()); //the jacobian matrix 1x(6+ndofs) + m_A = (m_world_T_framePosition.transpose() * m_jacobian.topRightCorner(3, 6 + m_kinDyn->getNrOfDegreesOfFreedom())) / (std::max(0.001, m_computedDistance)); + m_b << m_kp * (m_desiredDistance - m_computedDistance); + + // A and b are now valid + m_isValid = true; + return m_isValid; +} + +bool DistanceTask::setDesiredDistance(double desiredDistance) +{ + m_desiredDistance = desiredDistance; + + return true; +} + +std::size_t DistanceTask::size() const +{ + return m_DoFs; +} + +DistanceTask::Type DistanceTask::type() const +{ + return Type::equality; +} + +bool DistanceTask::isValid() const +{ + return m_isValid; +} From 13c402145340970ae8f66ea186b621a5dc1faa45 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 31 Jul 2023 12:16:54 +0200 Subject: [PATCH 02/17] Small refactory of the DistanceTask and added documentation. Using fixed dimension vector for frame position in Distance task The input distance is normalized Applied clang-format to DistanceTask --- .../BipedalLocomotion/IK/DistanceTask.h | 117 ++++++++++++++---- src/IK/src/DistanceTask.cpp | 84 ++++++++----- 2 files changed, 149 insertions(+), 52 deletions(-) diff --git a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h index 78ee5defe4..29d8f32b65 100644 --- a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h +++ b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h @@ -14,21 +14,36 @@ #include - namespace BipedalLocomotion { namespace IK { +// clang-format off +/** + * The DistanceTask class controls the distance of a frame with respect the world origin, or another frame. + * 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[ + * \frac{1}{d} p ^ \top J_p \nu = k (d ^ * - d) + * \f] + * where \f$p\f$ is the position of the target frame wrt the base or world frame. + * \f$d\f$ is the current distance. + * \f$J_p\f$ is the linear part of the robot jacobian. + * \f$k\f$ is the controller gain and \f$d ^ *\f$ is the desired distance. + */ +// clang-format on class DistanceTask : public IKLinearTask { - System::VariablesHandler::VariableDescription m_robotVelocityVariable; /**< Variable - describing the robot - velocity (base + - joint) */ - - static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. */ + System::VariablesHandler::VariableDescription m_robotVelocityVariable; /**< Variable describing + the robot velocity + (base + joint) */ + + static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. + */ + static constexpr std::size_t m_DoFs{1}; /**< DoFs associated to the task */ bool m_isInitialized{false}; /**< True if the task has been initialized. */ bool m_isValid{false}; /**< True if the task is valid. */ @@ -36,36 +51,92 @@ class DistanceTask : public IKLinearTask std::shared_ptr m_kinDyn; /**< Pointer to a KinDynComputations object */ - std::size_t m_DoFs{1}; /**< DoFs associated to the task */ - - Eigen::MatrixXd m_jacobian, m_relativeJacobian; - Eigen::MatrixXd m_world_T_framePosition; - double m_kp; - double m_desiredDistance{0.0}; + Eigen::MatrixXd m_jacobian, m_relativeJacobian; /**< Internal buffers to store the jacobian. */ + Eigen::Vector3d m_framePosition; /**< Internal buffer to store the frame position. */ + double m_kp; /**< Controller gain. */ + double m_desiredDistance{0.0}; /**< Desired distance. */ - std::string m_baseName; - std::string m_targetFrameName; - iDynTree::FrameIndex m_baseIndex, m_targetFrameIndex; + std::string m_baseName; /**< Base frame name. */ + std::string m_targetFrameName; /**< Target frame name. */ + iDynTree::FrameIndex m_baseIndex, m_targetFrameIndex; /**< Base and target frame name indexes. + */ + double m_computedDistance{0.0}; /**< Computed distance. */ public: - - double m_computedDistance{0.0}; - + // clang-format off + /** + * 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` | `double` | Gain of the distance controller | Yes | + * | `target_frame_name` | `string` | Name of the frame of which computing the distance | Yes | + * | `base_frame_name` | `string` | Name of the frame with respect to which computing the distance. If empty, the world frame will be used (Default = "") | No | + * @return True in case of success, false otherwise. + */ + // clang-format on + bool + initialize(std::weak_ptr paramHandler) override; + + /** + * 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) 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; - bool initialize(std::weak_ptr paramHandler) override; - + /** + * Update the content of the task. + * @return True in case of success, false otherwise. + */ bool update() override; - bool setDesiredDistance(double desiredDistance); - + /** + * @brief setDesiredDistance + * @param desiredDistance The desired distance expressed in meters + * @note The desired distance is considered without sign. + */ + void setDesiredDistance(double desiredDistance); + + /** + * @brief Get the computed distance between the target frame and either the world origin, or the + * base origin. + * @return The computed distance + */ + double getDistance() const; + + /** + * 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 DistanceTask is an equality task. + * @return the size of the task. + */ Type type() const override; + /** + * Determines the validity of the objects retrieved with getA() and getB() + * @return True if the objects are valid, false otherwise. + */ bool isValid() const override; }; diff --git a/src/IK/src/DistanceTask.cpp b/src/IK/src/DistanceTask.cpp index b019b24219..3e153ccaab 100644 --- a/src/IK/src/DistanceTask.cpp +++ b/src/IK/src/DistanceTask.cpp @@ -48,8 +48,8 @@ bool DistanceTask::setVariablesHandler(const System::VariablesHandler& variables // get the variable if (m_robotVelocityVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize) { - log()->error("[DistanceTask::setVariablesHandler] The size of the robot velocity variable is " - "different from the one expected. Expected size: {}. Given size: {}.", + log()->error("[DistanceTask::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; @@ -60,10 +60,12 @@ bool DistanceTask::setVariablesHandler(const System::VariablesHandler& variables m_A.setZero(); m_b.resize(m_DoFs); m_b.setZero(); - m_jacobian.resize(6, 6 + m_kinDyn->getNrOfDegreesOfFreedom()); //? + m_jacobian.resize(m_spatialVelocitySize, + m_spatialVelocitySize + m_kinDyn->getNrOfDegreesOfFreedom()); m_jacobian.setZero(); - m_relativeJacobian.resize(6, m_kinDyn->getNrOfDegreesOfFreedom()); + m_relativeJacobian.resize(m_spatialVelocitySize, m_kinDyn->getNrOfDegreesOfFreedom()); m_relativeJacobian.setZero(); + m_framePosition.setZero(); return true; } @@ -100,17 +102,27 @@ bool DistanceTask::initialize( return false; } + if (!ptr->getParameter("robot_velocity_variable_name", m_robotVelocityVariable.name)) + { + log()->error("{} [{}] Failed to retrieve the robot velocity variable.", + errorPrefix, + m_description); + return false; + } + // set the gains for the controllers if (!ptr->getParameter("kp", m_kp)) { - log()->error("{} [{}] to get the proportional linear gain.", errorPrefix, m_description); + log()->error("{} [{}] Failed to get the proportional linear gain.", + errorPrefix, + m_description); return false; } // set the base frame name if (!ptr->getParameter("base_name", m_baseName)) { - log()->debug("{} [{}] to get the base name. Using default \"\"", + log()->debug("{} [{}] No base_name specified. Using default \"\"", errorPrefix, m_description); } @@ -118,7 +130,9 @@ bool DistanceTask::initialize( // set the finger tip frame Name if (!ptr->getParameter("target_frame_name", m_targetFrameName)) { - log()->error("{} [{}] to get the end effector frame name.", errorPrefix, m_description); + log()->error("{} [{}] Failed to get the end effector frame name.", + errorPrefix, + m_description); return false; } @@ -127,14 +141,25 @@ bool DistanceTask::initialize( m_baseIndex = m_kinDyn->getFrameIndex(m_baseName); if (m_baseIndex == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{} [{}] The specified base name ({}) does not seem to exist.", + errorPrefix, + m_description, + m_baseName); return false; + } } m_targetFrameIndex = m_kinDyn->getFrameIndex(m_targetFrameName); - - // here you need to get the indexes of the frames and check that they exists - - m_world_T_framePosition.resize(3,1); + + if (m_targetFrameIndex == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{} [{}] The specified target name ({}) does not seem to exist.", + errorPrefix, + m_description, + m_targetFrameName); + return false; + } m_isInitialized = true; @@ -147,26 +172,22 @@ bool DistanceTask::update() using namespace iDynTree; m_isValid = false; - - // here to compute the distance, you need to get the transform. - + if (m_baseName == "") { - m_world_T_framePosition = toEigen(m_kinDyn->getWorldTransform(m_targetFrameName).getPosition()); + m_framePosition = toEigen(m_kinDyn->getWorldTransform(m_targetFrameIndex).getPosition()); - // get the jacobian - if (!m_kinDyn->getFrameFreeFloatingJacobian(m_targetFrameName, m_jacobian)) + if (!m_kinDyn->getFrameFreeFloatingJacobian(m_targetFrameIndex, m_jacobian)) { log()->error("[DistanceTask::update] Unable to get the jacobian."); return m_isValid; } - } - else + } else { - m_world_T_framePosition = toEigen(m_kinDyn->getRelativeTransform(m_baseName, m_targetFrameName).getPosition()); + m_framePosition = toEigen( + m_kinDyn->getRelativeTransform(m_baseIndex, m_targetFrameIndex).getPosition()); - // get the jacobian if (!m_kinDyn->getRelativeJacobian(m_baseIndex, m_targetFrameIndex, m_relativeJacobian)) { log()->error("[DistanceTask::update] Unable to get the relative jacobian."); @@ -174,13 +195,15 @@ bool DistanceTask::update() } m_jacobian.topRightCorner(3, m_kinDyn->getNrOfDegreesOfFreedom()) = m_relativeJacobian; - } - m_computedDistance = sqrt(pow(m_world_T_framePosition(0),2) + pow(m_world_T_framePosition(1),2) + pow(m_world_T_framePosition(2),2)); - - m_A.resize(1, 6 + m_kinDyn->getNrOfDegreesOfFreedom()); //the jacobian matrix 1x(6+ndofs) - m_A = (m_world_T_framePosition.transpose() * m_jacobian.topRightCorner(3, 6 + m_kinDyn->getNrOfDegreesOfFreedom())) / (std::max(0.001, m_computedDistance)); + m_computedDistance = sqrt(pow(m_framePosition(0), 2) + pow(m_framePosition(1), 2) + + pow(m_framePosition(2), 2)); + + m_A = (m_framePosition.transpose() + * m_jacobian.topRightCorner(3, + m_spatialVelocitySize + m_kinDyn->getNrOfDegreesOfFreedom())) + / (std::max(0.001, m_computedDistance)); m_b << m_kp * (m_desiredDistance - m_computedDistance); // A and b are now valid @@ -188,11 +211,14 @@ bool DistanceTask::update() return m_isValid; } -bool DistanceTask::setDesiredDistance(double desiredDistance) +void DistanceTask::setDesiredDistance(double desiredDistance) { - m_desiredDistance = desiredDistance; + m_desiredDistance = std::abs(desiredDistance); +} - return true; +double DistanceTask::getDistance() const +{ + return m_computedDistance; } std::size_t DistanceTask::size() const From 73d03b36c574e4437888f8aaf79c1de27c5562fd Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 31 Jul 2023 18:31:52 +0200 Subject: [PATCH 03/17] Added tests for DistanceTask and bugfix. Added DistanceTask to QPInverseKinematicsTest --- .../BipedalLocomotion/IK/DistanceTask.h | 3 +- src/IK/src/DistanceTask.cpp | 15 +- src/IK/tests/CMakeLists.txt | 5 + src/IK/tests/DistanceTaskTest.cpp | 164 ++++++++++++++++++ src/IK/tests/QPInverseKinematicsTest.cpp | 114 +++++++++--- 5 files changed, 266 insertions(+), 35 deletions(-) create mode 100644 src/IK/tests/DistanceTaskTest.cpp diff --git a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h index 29d8f32b65..9fc1d683fc 100644 --- a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h +++ b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h @@ -111,8 +111,9 @@ class DistanceTask : public IKLinearTask * @brief setDesiredDistance * @param desiredDistance The desired distance expressed in meters * @note The desired distance is considered without sign. + * @return True in case of success, false otherwise. */ - void setDesiredDistance(double desiredDistance); + bool setDesiredDistance(double desiredDistance); /** * @brief Get the computed distance between the target frame and either the world origin, or the diff --git a/src/IK/src/DistanceTask.cpp b/src/IK/src/DistanceTask.cpp index 3e153ccaab..4739313d3e 100644 --- a/src/IK/src/DistanceTask.cpp +++ b/src/IK/src/DistanceTask.cpp @@ -120,7 +120,7 @@ bool DistanceTask::initialize( } // set the base frame name - if (!ptr->getParameter("base_name", m_baseName)) + if (!ptr->getParameter("base_frame_name", m_baseName)) { log()->debug("{} [{}] No base_name specified. Using default \"\"", errorPrefix, @@ -194,26 +194,25 @@ bool DistanceTask::update() return m_isValid; } - m_jacobian.topRightCorner(3, m_kinDyn->getNrOfDegreesOfFreedom()) = m_relativeJacobian; + m_jacobian.rightCols(m_kinDyn->getNrOfDegreesOfFreedom()) = m_relativeJacobian; } m_computedDistance = sqrt(pow(m_framePosition(0), 2) + pow(m_framePosition(1), 2) + pow(m_framePosition(2), 2)); - m_A = (m_framePosition.transpose() - * m_jacobian.topRightCorner(3, - m_spatialVelocitySize + m_kinDyn->getNrOfDegreesOfFreedom())) + toEigen(this->subA(m_robotVelocityVariable)) + = (m_framePosition.transpose() * m_jacobian.topRows<3>()) / (std::max(0.001, m_computedDistance)); - m_b << m_kp * (m_desiredDistance - m_computedDistance); + m_b(0) = m_kp * (m_desiredDistance - m_computedDistance); - // A and b are now valid m_isValid = true; return m_isValid; } -void DistanceTask::setDesiredDistance(double desiredDistance) +bool DistanceTask::setDesiredDistance(double desiredDistance) { m_desiredDistance = std::abs(desiredDistance); + return true; } double DistanceTask::getDistance() const diff --git a/src/IK/tests/CMakeLists.txt b/src/IK/tests/CMakeLists.txt index ff076c220e..66cc045b67 100644 --- a/src/IK/tests/CMakeLists.txt +++ b/src/IK/tests/CMakeLists.txt @@ -37,6 +37,11 @@ add_bipedal_test( SOURCES AngularMomentumTaskTest.cpp LINKS BipedalLocomotion::IK) +add_bipedal_test( + NAME DistanceTaskIK + SOURCES DistanceTaskTest.cpp + LINKS BipedalLocomotion::IK) + if (FRAMEWORK_COMPILE_ContinuousDynamicalSystem) add_bipedal_test( diff --git a/src/IK/tests/DistanceTaskTest.cpp b/src/IK/tests/DistanceTaskTest.cpp new file mode 100644 index 0000000000..662cd762f3 --- /dev/null +++ b/src/IK/tests/DistanceTaskTest.cpp @@ -0,0 +1,164 @@ +/** + * @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 BSD-3-Clause license. + */ + +// Catch2 +#include + +// BipedalLocomotion +#include +#include +#include + +#include +#include + +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +TEST_CASE("Distance 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 < 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 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); + double kp = 2.0; + parameterHandler->setParameter("kp", kp); + + std::string targetName = iDynTree::getRandomLinkOfModel(model); + parameterHandler->setParameter("target_frame_name", targetName); + + bool relative = numberOfJoints % 2 == 0; + std::string baseName; + + if (relative) + { + do + { + baseName = iDynTree::getRandomLinkOfModel(model); + } while (baseName == targetName); + + parameterHandler->setParameter("base_frame_name", baseName); + } else + { + parameterHandler->setParameter("base_frame_name", ""); + } + + DistanceTask task; + REQUIRE(task.setKinDyn(kinDyn)); + REQUIRE(task.initialize(parameterHandler)); + REQUIRE(task.setVariablesHandler(variablesHandler)); + + double desiredDistance = 1.0; + REQUIRE(task.setDesiredDistance(desiredDistance)); + + REQUIRE(task.update()); + REQUIRE(task.isValid()); + + // get A and b + Eigen::Ref A = task.getA(); + Eigen::Ref b = task.getB(); + + Eigen::Vector3d targetPosition; + Eigen::MatrixXd jacobian; + jacobian.setZero(6, 6 + model.getNrOfDOFs()); + + if (relative) + { + targetPosition = iDynTree::toEigen( + kinDyn->getRelativeTransform(baseName, targetName).getPosition()); + Eigen::MatrixXd relativeJacobian; + relativeJacobian.resize(6, model.getNrOfDOFs()); + kinDyn->getRelativeJacobian(model.getFrameIndex(baseName), + model.getFrameIndex(targetName), + relativeJacobian); + jacobian.rightCols(model.getNrOfDOFs()) = relativeJacobian; + + } else + { + targetPosition + = iDynTree::toEigen(kinDyn->getWorldTransform(targetName).getPosition()); + kinDyn->getFrameFreeFloatingJacobian(targetName, jacobian); + } + Eigen::VectorXd distance(1); + distance << targetPosition.norm(); + + Eigen::VectorXd measuredDistance(1); + measuredDistance << task.getDistance(); + + REQUIRE(distance.isApprox(measuredDistance)); + + Eigen::MatrixXd expectedA + = targetPosition.transpose() * jacobian.topRows<3>() / distance(0); + + // 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, + model.getNrOfDOFs() + 6) + .isApprox(expectedA)); + + // check the vector b + Eigen::VectorXd expectedB(task.size()); + expectedB(0) = kp * (desiredDistance - distance(0)); + REQUIRE(b.isApprox(expectedB)); + } + } +} diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp index 563a3aaa5f..dde67db0c1 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -6,6 +6,7 @@ */ // Catch2 +#include #include // std @@ -21,9 +22,10 @@ #include #include +#include #include -#include #include +#include #include #include @@ -50,13 +52,17 @@ struct InverseKinematicsAndTasks std::shared_ptr comTask; std::shared_ptr regularizationTask; std::shared_ptr jointLimitsTask; + std::shared_ptr distanceTask; }; struct DesiredSetPoints { Eigen::Vector3d CoMPosition; + std::string endEffectorFrame; manif::SE3d endEffectorPose; Eigen::VectorXd joints; + std::string targetFrameDistance; + double targetDistance; }; struct System @@ -78,7 +84,8 @@ std::shared_ptr createParameterHandler() std::vector{"SE3_TASK", "COM_TASK", "REGULARIZATION_TASK", - "JOINT_LIMITS_TASK"}); + "JOINT_LIMITS_TASK", + "DISTANCE_TASK"}); /////// SE3 Task auto SE3ParameterHandler = std::make_shared(); @@ -105,7 +112,7 @@ std::shared_ptr createParameterHandler() jointRegularizationHandler->setParameter("priority", 1); parameterHandler->setGroup("REGULARIZATION_TASK", jointRegularizationHandler); - + /////// Joint limits task auto jointLimitsHandler = std::make_shared(); jointLimitsHandler->setParameter("robot_velocity_variable_name", robotVelocity); jointLimitsHandler->setParameter("sampling_time", dT); @@ -114,6 +121,14 @@ std::shared_ptr createParameterHandler() jointLimitsHandler->setParameter("priority", 0); parameterHandler->setGroup("JOINT_LIMITS_TASK", jointLimitsHandler); + /////// Distance task + auto distanceTaskHandler = std::make_shared(); + distanceTaskHandler->setParameter("robot_velocity_variable_name", robotVelocity); + distanceTaskHandler->setParameter("type", "DistanceTask"); + distanceTaskHandler->setParameter("priority", 0); + distanceTaskHandler->setParameter("kp", gain); + parameterHandler->setGroup("DISTANCE_TASK", distanceTaskHandler); + return parameterHandler; } @@ -166,7 +181,9 @@ InverseKinematicsAndTasks createIK(std::shared_ptr handler, REQUIRE(out.ik->addTask(out.comTask, "com_task", highPriority)); REQUIRE(handler->getGroup("REGULARIZATION_TASK").lock()->getParameter("kp", kpRegularization)); - REQUIRE(handler->getGroup("REGULARIZATION_TASK").lock()->getParameter("weight", weightRegularization)); + REQUIRE(handler->getGroup("REGULARIZATION_TASK") + .lock() + ->getParameter("weight", weightRegularization)); out.regularizationTask = std::make_shared(); REQUIRE(out.regularizationTask->setKinDyn(kinDyn)); REQUIRE(out.regularizationTask->initialize(handler->getGroup("REGULARIZATION_TASK"))); @@ -181,7 +198,8 @@ InverseKinematicsAndTasks createIK(std::shared_ptr handler, REQUIRE(out.ik->addTask(out.jointLimitsTask, "joint_limits_task", highPriority)); const Eigen::VectorXd newWeight = 10 * weightRegularization; - auto newWeightProvider = std::make_shared(newWeight); + auto newWeightProvider + = std::make_shared(newWeight); REQUIRE(out.ik->setTaskWeight("regularization_task", newWeightProvider)); auto outWeightProvider = out.ik->getTaskWeightProvider("regularization_task").lock(); @@ -190,6 +208,11 @@ InverseKinematicsAndTasks createIK(std::shared_ptr handler, REQUIRE(out.ik->setTaskWeight("regularization_task", weightRegularization)); + out.distanceTask = std::make_shared(); + REQUIRE(out.distanceTask->setKinDyn(kinDyn)); + REQUIRE(out.distanceTask->initialize(handler->getGroup("DISTANCE_TASK"))); + REQUIRE(out.ik->addTask(out.distanceTask, "distance_task", highPriority)); + REQUIRE(out.ik->finalize(variables)); return out; @@ -226,10 +249,17 @@ DesiredSetPoints getDesiredReference(std::shared_ptrgetCenterOfMassPosition()); - const std::string controlledFrame = kinDyn->model().getFrameName(numberOfJoints); - out.endEffectorPose = toManifPose(kinDyn->getWorldTransform(controlledFrame)); + out.endEffectorFrame = kinDyn->model().getFrameName(numberOfJoints); + out.endEffectorPose = toManifPose(kinDyn->getWorldTransform(out.endEffectorFrame)); out.joints.resize(jointsPos.size()); out.joints = iDynTree::toEigen(jointsPos); + do + { + out.targetFrameDistance = iDynTree::getRandomLinkOfModel(kinDyn->model()); + } while (out.targetFrameDistance == out.endEffectorFrame); + + out.targetDistance + = toManifPose(kinDyn->getWorldTransform(out.targetFrameDistance)).translation().norm(); return out; } @@ -250,7 +280,8 @@ System getSystem(std::shared_ptr kinDyn) Eigen::VectorXd jointPositions(kinDyn->getNrOfDegreesOfFreedom()); Eigen::Vector3d gravity; - REQUIRE(kinDyn->getRobotState(basePose, jointPositions, baseVelocity, jointVelocities, gravity)); + REQUIRE( + kinDyn->getRobotState(basePose, jointPositions, baseVelocity, jointVelocities, gravity)); // perturb the joint position for (int i = 0; i < kinDyn->getNrOfDegreesOfFreedom(); i++) @@ -298,10 +329,13 @@ TEST_CASE("QP-IK") auto system = getSystem(kinDyn); // Set the frame name - const std::string controlledFrame = model.getFrameName(numberOfJoints); parameterHandler->getGroup("SE3_TASK") .lock() - ->setParameter("frame_name", controlledFrame); + ->setParameter("frame_name", desiredSetPoints.endEffectorFrame); + + parameterHandler->getGroup("DISTANCE_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameDistance); // create the IK constexpr double jointLimitDelta = 0.5; @@ -318,8 +352,10 @@ TEST_CASE("QP-IK") Eigen::Vector3d::Zero())); REQUIRE(ikAndTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); + REQUIRE(ikAndTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + // propagate the inverse kinematics for - constexpr std::size_t iterations = 30; + constexpr std::size_t iterations = 35; Eigen::Vector3d gravity; gravity << 0, 0, -9.81; Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); @@ -353,20 +389,26 @@ TEST_CASE("QP-IK") } // check the CoM position - REQUIRE(desiredSetPoints.CoMPosition.isApprox(toEigen(kinDyn->getCenterOfMassPosition()), - tolerance)); + REQUIRE( + desiredSetPoints.CoMPosition.isApprox(toEigen(kinDyn->getCenterOfMassPosition()), + tolerance)); // Check the end-effector pose error - const manif::SE3d endEffectorPose = toManifPose(kinDyn->getWorldTransform(controlledFrame)); + const manif::SE3d endEffectorPose + = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.endEffectorFrame)); // please read it as (log(desiredSetPoints.endEffectorPose^-1 * endEffectorPose))^v const manif::SE3d::Tangent error = endEffectorPose - desiredSetPoints.endEffectorPose; REQUIRE(error.coeffs().isZero(tolerance)); + + REQUIRE(toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameDistance)) + .translation() + .norm() + == Catch::Approx(desiredSetPoints.targetDistance).epsilon(tolerance * 0.1)); } } } - TEST_CASE("QP-IK [With strict limits]") { auto kinDyn = std::make_shared(); @@ -393,8 +435,13 @@ TEST_CASE("QP-IK [With strict limits]") auto system = getSystem(kinDyn); // Set the frame name - const std::string controlledFrame = model.getFrameName(numberOfJoints); - parameterHandler->getGroup("SE3_TASK").lock()->setParameter("frame_name", controlledFrame); + parameterHandler->getGroup("SE3_TASK") + .lock() + ->setParameter("frame_name", desiredSetPoints.endEffectorFrame); + + parameterHandler->getGroup("DISTANCE_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameDistance); // create the IK constexpr double jointLimitDelta = 0.5; @@ -413,8 +460,10 @@ TEST_CASE("QP-IK [With strict limits]") REQUIRE(ikAndTasks.comTask->setSetPoint(desiredSetPoints.CoMPosition, Eigen::Vector3d::Zero())); REQUIRE(ikAndTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); + REQUIRE(ikAndTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + // propagate the inverse kinematics for - constexpr std::size_t iterations = 30; + constexpr std::size_t iterations = 35; Eigen::Vector3d gravity; gravity << 0, 0, -9.81; Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); @@ -459,7 +508,6 @@ TEST_CASE("QP-IK [With strict limits]") } } - TEST_CASE("QP-IK [With builder]") { auto kinDyn = std::make_shared(); @@ -496,8 +544,13 @@ TEST_CASE("QP-IK [With builder]") auto system = getSystem(kinDyn); // Set the frame name - const std::string controlledFrame = model.getFrameName(numberOfJoints); - parameterHandler->getGroup("SE3_TASK").lock()->setParameter("frame_name", controlledFrame); + parameterHandler->getGroup("SE3_TASK") + .lock() + ->setParameter("frame_name", desiredSetPoints.endEffectorFrame); + + parameterHandler->getGroup("DISTANCE_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameDistance); // finalize the parameter handler constexpr double jointLimitDelta = 0.5; @@ -511,7 +564,6 @@ TEST_CASE("QP-IK [With builder]") auto [variablesHandler, weights, ik] = QPInverseKinematics::build(parameterHandler, kinDyn); REQUIRE_FALSE(ik == nullptr); - auto se3Task = std::dynamic_pointer_cast(ik->getTask("SE3_TASK").lock()); REQUIRE_FALSE(se3Task == nullptr); REQUIRE(se3Task->setSetPoint(desiredSetPoints.endEffectorPose, manif::SE3d::Tangent::Zero())); @@ -525,8 +577,13 @@ TEST_CASE("QP-IK [With builder]") REQUIRE_FALSE(regularizationTask == nullptr); REQUIRE(regularizationTask->setSetPoint(desiredSetPoints.joints)); + auto distanceTask + = std::dynamic_pointer_cast(ik->getTask("DISTANCE_TASK").lock()); + REQUIRE_FALSE(distanceTask == nullptr); + REQUIRE(distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + // propagate the inverse kinematics for - constexpr std::size_t iterations = 30; + constexpr std::size_t iterations = 35; Eigen::Vector3d gravity; gravity << 0, 0, -9.81; Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); @@ -551,7 +608,7 @@ TEST_CASE("QP-IK [With builder]") // get the output of the IK baseVelocity = ik->getOutput().baseVelocity.coeffs(); - jointVelocity =ik->getOutput().jointVelocity; + jointVelocity = ik->getOutput().jointVelocity; // propagate the dynamical system system.dynamics->setControlInput({baseVelocity, jointVelocity}); @@ -562,7 +619,6 @@ TEST_CASE("QP-IK [With builder]") = std::dynamic_pointer_cast( ik->getTaskWeightProvider("REGULARIZATION_TASK").lock()); - REQUIRE(weightProvider != nullptr); // check the CoM position @@ -570,9 +626,15 @@ TEST_CASE("QP-IK [With builder]") tolerance)); // Check the end-effector pose error - const manif::SE3d endEffectorPose = toManifPose(kinDyn->getWorldTransform(controlledFrame)); + const manif::SE3d endEffectorPose + = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.endEffectorFrame)); // please read it as (log(desiredSetPoints.endEffectorPose^-1 * endEffectorPose))^v const manif::SE3d::Tangent error = endEffectorPose - desiredSetPoints.endEffectorPose; REQUIRE(error.coeffs().isZero(tolerance)); + + REQUIRE(toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameDistance)) + .translation() + .norm() + == Catch::Approx(desiredSetPoints.targetDistance).epsilon(tolerance * 0.1)); } From cd6d53cf1f17bde01a7a017c4e27be7c9ed6fe7e Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 1 Aug 2023 16:50:30 +0200 Subject: [PATCH 04/17] Added DistanceTask python bindings --- bindings/python/IK/CMakeLists.txt | 4 +- .../bindings/IK/DistanceTask.h | 26 +++++++++++++ bindings/python/IK/src/DistanceTask.cpp | 38 +++++++++++++++++++ bindings/python/IK/src/Module.cpp | 2 + .../IK/tests/test_QP_inverse_kinematics.py | 22 +++++++++++ 5 files changed, 90 insertions(+), 2 deletions(-) create mode 100644 bindings/python/IK/include/BipedalLocomotion/bindings/IK/DistanceTask.h create mode 100644 bindings/python/IK/src/DistanceTask.cpp diff --git a/bindings/python/IK/CMakeLists.txt b/bindings/python/IK/CMakeLists.txt index 9a468ef12e..3058ad627c 100644 --- a/bindings/python/IK/CMakeLists.txt +++ b/bindings/python/IK/CMakeLists.txt @@ -8,8 +8,8 @@ if(TARGET BipedalLocomotion::IK) add_bipedal_locomotion_python_module( NAME IKBindings - SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/JointLimitsTask.cpp src/AngularMomentumTask.cpp src/Module.cpp src/IKLinearTask.cpp - HEADERS ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/IKLinearTask.h + SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/JointLimitsTask.cpp src/AngularMomentumTask.cpp src/Module.cpp src/IKLinearTask.cpp src/DistanceTask.cpp + HEADERS ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h LINK_LIBRARIES BipedalLocomotion::IK MANIF::manif TESTS tests/test_QP_inverse_kinematics.py TESTS_RUNTIME_CONDITIONS FRAMEWORK_USE_icub-models diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/DistanceTask.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/DistanceTask.h new file mode 100644 index 0000000000..6be8d70592 --- /dev/null +++ b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/DistanceTask.h @@ -0,0 +1,26 @@ +/** + * @file DistanceTask.h + * @authors Stefano Dafarra + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_DISTANCE_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_IK_DISTANCE_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace IK +{ + +void CreateDistanceTask(pybind11::module& module); + +} // namespace IK +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_DISTANCE_TASK_H diff --git a/bindings/python/IK/src/DistanceTask.cpp b/bindings/python/IK/src/DistanceTask.cpp new file mode 100644 index 0000000000..dcf145f8d5 --- /dev/null +++ b/bindings/python/IK/src/DistanceTask.cpp @@ -0,0 +1,38 @@ +/** + * @file DistanceTask.cpp + * @authors Stefano Dafarra + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace IK +{ + +void CreateDistanceTask(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::IK; + + py::class_, IKLinearTask>(module, "DistanceTask") + .def(py::init()) + .def("set_desired_distance", &DistanceTask::setDesiredDistance, py::arg("desired_distance")) + .def("get_distance", &DistanceTask::getDistance); +} + +} // namespace IK +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/IK/src/Module.cpp b/bindings/python/IK/src/Module.cpp index 189aaadc5a..104d537d8a 100644 --- a/bindings/python/IK/src/Module.cpp +++ b/bindings/python/IK/src/Module.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -37,6 +38,7 @@ void CreateModule(pybind11::module& module) CreateJointTrackingTask(module); CreateJointLimitsTask(module); CreateAngularMomentumTask(module); + CreateDistanceTask(module); CreateIntegrationBasedIK(module); CreateQPInverseKinematics(module); } diff --git a/bindings/python/IK/tests/test_QP_inverse_kinematics.py b/bindings/python/IK/tests/test_QP_inverse_kinematics.py index 8158d42b7a..87776d01cf 100644 --- a/bindings/python/IK/tests/test_QP_inverse_kinematics.py +++ b/bindings/python/IK/tests/test_QP_inverse_kinematics.py @@ -213,6 +213,28 @@ def test_angular_momentum_task(): assert angular_momentum_task.set_variables_handler(variables_handler=angular_momentum_var_handler) assert angular_momentum_task.set_set_point([0., 0., 0.]) +def test_distance_task(): + # create KinDynComputationsDescriptor + joints_list, kindyn = get_kindyn() + + # Set the parameters + distance_task_param_handler = blf.parameters_handler.StdParametersHandler() + distance_task_param_handler.set_parameter_string(name="robot_velocity_variable_name", value="robotVelocity") + distance_task_param_handler.set_parameter_string(name="target_frame_name", value="chest") + distance_task_param_handler.set_parameter_string(name="base_frame_name", value="root_link") + distance_task_param_handler.set_parameter_float(name="kp",value=5.0) + + # Initialize the task + distance_task = blf.ik.DistanceTask() + assert distance_task.set_kin_dyn(kindyn) + assert distance_task.initialize(param_handler=distance_task_param_handler) + distance_task_var_handler = blf.system.VariablesHandler() + assert distance_task_var_handler.add_variable("robotVelocity", 32) is True # robot velocity size = 26 (joints) + 6 (base) + assert distance_task.set_variables_handler(variables_handler=distance_task_var_handler) + + # Set desired distance + assert distance_task.set_desired_distance(desired_distance=np.random.uniform(-0.5,0.5)) + def test_integration_based_ik_state(): state = blf.ik.IntegrationBasedIKState() From 805ef4fc68110b00940079f87e434cb784584b7a Mon Sep 17 00:00:00 2001 From: Ehasn Date: Sun, 9 Jul 2023 21:41:54 +0200 Subject: [PATCH 05/17] Added GravityTask GravityTask_Preliminary_Implementation GravityTask_Preliminary_ImplementationRev2 Fixed setEstimateGravityDir method matrices size fixed m_robotVelocityVariable.name fixed trailing whitespace fixed m_angularJacobian fixed, setEstimateGravityDir parameter fixed body to world rotation added feedforward gyro added --- src/IK/CMakeLists.txt | 4 +- .../BipedalLocomotion/IK/GravityTask.h | 78 +++++++ src/IK/src/GravityTask.cpp | 221 ++++++++++++++++++ 3 files changed, 301 insertions(+), 2 deletions(-) create mode 100644 src/IK/include/BipedalLocomotion/IK/GravityTask.h create mode 100644 src/IK/src/GravityTask.cpp diff --git a/src/IK/CMakeLists.txt b/src/IK/CMakeLists.txt index 0a922a14ff..cdd27e57fc 100644 --- a/src/IK/CMakeLists.txt +++ b/src/IK/CMakeLists.txt @@ -10,9 +10,9 @@ if(FRAMEWORK_COMPILE_IK) NAME IK PUBLIC_HEADERS ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/QPFixedBaseInverseKinematics.h - ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h + ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h SOURCES src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/CoMTask.cpp src/AngularMomentumTask.cpp src/JointLimitsTask.cpp - src/QPInverseKinematics.cpp src/QPFixedBaseInverseKinematics.cpp src/IKLinearTask.cpp src/IntegrationBasedIK.cpp src/DistanceTask.cpp + src/QPInverseKinematics.cpp src/QPFixedBaseInverseKinematics.cpp src/IKLinearTask.cpp src/IntegrationBasedIK.cpp src/DistanceTask.cpp src/GravityTask.cpp PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System LieGroupControllers::LieGroupControllers diff --git a/src/IK/include/BipedalLocomotion/IK/GravityTask.h b/src/IK/include/BipedalLocomotion/IK/GravityTask.h new file mode 100644 index 0000000000..5c78360018 --- /dev/null +++ b/src/IK/include/BipedalLocomotion/IK/GravityTask.h @@ -0,0 +1,78 @@ +/** + * @file DistanceTask.h + * @authors Ehsan Ranjbari + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef GRAVITY_TASK_H +#define GRAVITY_TASK_H + +#include + +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace IK +{ + +class GravityTask : public IKLinearTask +{ + + System::VariablesHandler::VariableDescription m_robotVelocityVariable;/**< Variable describing the robot velocity (base + joint) */ + + 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. */ + bool m_isValid{false}; /**< True if the task is valid. */ + + std::shared_ptr m_kinDyn; /**< Pointer to a KinDynComputations + object */ + + std::size_t m_DoFs{2}; /**< DoFs associated to the task */ + + double m_kp; + double m_accDenomNorm; + Eigen::MatrixXd m_Angularjacobian, m_relativeJacobian; + Eigen::VectorXd m_currentAcc; + Eigen::VectorXd m_currentAccNorm; + Eigen::VectorXd m_currentGyro; + Eigen::MatrixXd m_Am; + Eigen::MatrixXd m_bm; + + std::string m_baseName; + std::string m_targetFrameName; + iDynTree::FrameIndex m_baseIndex, m_targetFrameIndex; + +public: + + bool setKinDyn(std::shared_ptr kinDyn) override; + + bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override; + + bool initialize(std::weak_ptr paramHandler) override; + + bool update() override; + + bool setEstimateGravityDir(const Eigen::Ref currentGravityDir); + + bool setGyroscope(const Eigen::Ref currentGyro); + + std::size_t size() const override; + + Type type() const override; + + bool isValid() const override; +}; + +BLF_REGISTER_IK_TASK(GravityTask); + +} // namespace IK +} // namespace BipedalLocomotion + +#endif // GRAVITY_TASK_H diff --git a/src/IK/src/GravityTask.cpp b/src/IK/src/GravityTask.cpp new file mode 100644 index 0000000000..85dc691ca9 --- /dev/null +++ b/src/IK/src/GravityTask.cpp @@ -0,0 +1,221 @@ +/** + * @file GravityTask.cpp + * @authors Ehsan Ranjbari + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include + +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +bool GravityTask::setKinDyn(std::shared_ptr kinDyn) +{ + if ((kinDyn == nullptr) || (!kinDyn->isValid())) + { + log()->error("[GravityTask::setKinDyn] Invalid kinDyn object."); + return false; + } + + m_kinDyn = kinDyn; + return true; +} + +bool GravityTask::setVariablesHandler(const System::VariablesHandler& variablesHandler) +{ + if (!m_isInitialized) + { + log()->error("[GravityTask::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("[GravityTask::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("[GravityTask::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_DoFs, variablesHandler.getNumberOfVariables()); + m_A.setZero(); + m_b.resize(m_DoFs); + m_b.setZero(); + m_Angularjacobian.resize(3, variablesHandler.getNumberOfVariables()); + m_Angularjacobian.setZero(); + m_relativeJacobian.resize(6, m_kinDyn->getNrOfDegreesOfFreedom()); + m_relativeJacobian.setZero(); + m_currentAcc.setZero(3); + m_currentGyro.setZero(3); + m_currentAccNorm.setZero(3); + m_Am.resize(2,3); + m_Am << 1, 0, 0, + 0, 1, 0; + m_bm.resize(2,3); + m_bm << 0 ,1, 0, + -1, 0, 0; + + return true; +} + +bool GravityTask::initialize( + std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[GravityTask::initialize]"; + + m_description = "GravityTask"; + + 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 + if (!ptr->getParameter("kp", m_kp)) + { + log()->error("{} [{}] to get the proportional linear gain.", errorPrefix, m_description); + return false; + } + + // set the base frame name + if (!ptr->getParameter("base_name", m_baseName)) + { + log()->debug("{} [{}] to get the base name. Using default \"\"", + errorPrefix, + m_description); + } + + if (m_baseName != "") + { + m_baseIndex = m_kinDyn->getFrameIndex(m_baseName); + + if (m_baseIndex == iDynTree::FRAME_INVALID_INDEX) + return false; + } + + // set the finger tip frame Name + if (!ptr->getParameter("target_frame_name", m_targetFrameName)) + { + log()->error("{} [{}] to get the end effector frame name.", errorPrefix, m_description); + return false; + } + + m_targetFrameIndex = m_kinDyn->getFrameIndex(m_targetFrameName); + + if (m_targetFrameIndex == iDynTree::FRAME_INVALID_INDEX) + return false; + + m_isInitialized = true; + + return true; +} + +bool GravityTask::update() +{ + using namespace iDynTree; + + m_isValid = false; + + // Normalize Accelerometer vector + m_accDenomNorm = sqrt(pow(m_currentAcc(0),2) + pow(m_currentAcc(1),2) + pow(m_currentAcc(2),2)); + + for (size_t i = 0; i < m_currentAcc.size(); i++) + { + m_currentAccNorm(i) = m_currentAcc(i) / m_accDenomNorm; + } + + m_currentAccNorm = toEigen(m_kinDyn->getWorldTransform(m_targetFrameName).getRotation()) * m_currentAccNorm; + + // get the relative jacobian + if (!m_kinDyn->getRelativeJacobian(m_baseIndex, m_targetFrameIndex, m_relativeJacobian)) + { + log()->error("[GravityTask::update] Unable to get the relative jacobian."); + return m_isValid; + } + + // get the angular part of the jacobian + m_Angularjacobian.bottomRightCorner(3, m_kinDyn->getNrOfDegreesOfFreedom()) = m_relativeJacobian.bottomRightCorner(3, m_kinDyn->getNrOfDegreesOfFreedom()); + + m_A = m_Am * m_Angularjacobian; + m_b << ( m_kp * m_bm * m_currentAccNorm) + 1000 * m_currentGyro.block<2,1>(1,0); + + // A and b are now valid + m_isValid = true; + return m_isValid; +} + +bool GravityTask::setEstimateGravityDir(const Eigen::Ref currentGravityDir) +{ + m_currentAcc = currentGravityDir; + + return true; +} + +bool GravityTask::setGyroscope(const Eigen::Ref currentGyro) +{ + m_currentGyro = currentGyro * (M_PI)/180; + + return true; +} + +std::size_t GravityTask::size() const +{ + return m_DoFs; +} + +GravityTask::Type GravityTask::type() const +{ + return Type::equality; +} + +bool GravityTask::isValid() const +{ + return m_isValid; +} From 0dae6b5faea93166d3c93a11a8a8d3d7237763d1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 31 Jul 2023 13:11:02 +0200 Subject: [PATCH 06/17] Small refactory of GravityTask and added documentation. Fixed sign in GravityTask Added gravity task documentation. Applied clang-format to GravityTask --- .../BipedalLocomotion/IK/GravityTask.h | 124 ++++++++++++++---- src/IK/src/GravityTask.cpp | 95 +++++--------- 2 files changed, 136 insertions(+), 83 deletions(-) diff --git a/src/IK/include/BipedalLocomotion/IK/GravityTask.h b/src/IK/include/BipedalLocomotion/IK/GravityTask.h index 5c78360018..4a0f8a07a7 100644 --- a/src/IK/include/BipedalLocomotion/IK/GravityTask.h +++ b/src/IK/include/BipedalLocomotion/IK/GravityTask.h @@ -21,12 +21,39 @@ namespace BipedalLocomotion namespace IK { +// clang-format off +/** + * The GravityTask class aligns the z axis of a frame to a desired gravity direction. + * 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[ + * \begin{bmatrix} + * 1 & 0 & 0 \\ + * 0 & 1 & 0 + * \end{bmatrix} J_\omega \nu = k \begin{bmatrix} + * 0 &-1 & 0 \\ + * 1 & 0 & 0 + * \end{bmatrix} {} ^ W R_f {} ^ f g ^ * + {} ^ W R_f {} ^ f \omega_{ff} + * \f] + * where \f$J_\omega\f$ is the angular part of the frame jacobian. + * \f$k\f$ is the controller gain. + * \f${} ^ W R_f\f$ is the frame rotation. + * \f${} ^ W g ^ *\f$ is the desired z axis direction expressed in the frame coordinates. + * \f${} ^ W \omega_{ff}\f$ is the feed forward velocity expressed in the frame coordinates. + * A more through explanation is provided in https://github.com/ami-iit/bipedal-locomotion-framework/issues/651 + */ +// clang-format on class GravityTask : public IKLinearTask { - System::VariablesHandler::VariableDescription m_robotVelocityVariable;/**< Variable describing the robot velocity (base + joint) */ + System::VariablesHandler::VariableDescription m_robotVelocityVariable; /**< Variable describing + the robot velocity + (base + joint) */ - static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. */ + static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. + */ + static constexpr std::size_t m_DoFs{2}; /**< DoFs associated to the task */ bool m_isInitialized{false}; /**< True if the task has been initialized. */ bool m_isValid{false}; /**< True if the task is valid. */ @@ -34,39 +61,92 @@ class GravityTask : public IKLinearTask std::shared_ptr m_kinDyn; /**< Pointer to a KinDynComputations object */ - std::size_t m_DoFs{2}; /**< DoFs associated to the task */ + double m_kp; /**< Controller gain. */ + Eigen::MatrixXd m_jacobian; /**< Internal buffer to store the jacobian. */ + Eigen::Vector3d m_desiredZDirectionBody; /**< Desired gravity direction in the target frame. */ + Eigen::Vector3d m_feedForwardBody; /**< Feedforward term expressed in body direction. */ + Eigen::MatrixXd m_Am; /**< Matrix filtering jacobian rows. */ + Eigen::MatrixXd m_bm; /**< Matrix filtering acceleration. */ - double m_kp; - double m_accDenomNorm; - Eigen::MatrixXd m_Angularjacobian, m_relativeJacobian; - Eigen::VectorXd m_currentAcc; - Eigen::VectorXd m_currentAccNorm; - Eigen::VectorXd m_currentGyro; - Eigen::MatrixXd m_Am; - Eigen::MatrixXd m_bm; - - std::string m_baseName; - std::string m_targetFrameName; - iDynTree::FrameIndex m_baseIndex, m_targetFrameIndex; + iDynTree::FrameIndex m_targetFrameIndex; /**< Index of the target frame. */ public: - + // clang-format off + /** + * 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` | `double` | Gain of the distance controller | Yes | + * | `target_frame_name` | `string` | Name of the frame to which apply the gravity task | Yes | + * @return True in case of success, false otherwise. + */ + // clang-format on + bool + initialize(std::weak_ptr paramHandler) override; + + /** + * 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) 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; - bool initialize(std::weak_ptr paramHandler) override; - + /** + * Update the content of the task. + * @return True in case of success, false otherwise. + */ bool update() override; - bool setEstimateGravityDir(const Eigen::Ref currentGravityDir); - - bool setGyroscope(const Eigen::Ref currentGyro); - + /** + * @brief Set the desired gravity direction expressed the target frame. + * @param desiredGravityDirection The desired gravity direction. + * + * The input is normalized, unless the norm is too small. + */ + void setDesiredGravityDirectionInTargetFrame( + const Eigen::Ref desiredGravityDirection); + + /** + * @brief Set the feedforward angular velocity expressed in the target frame. + * @param feedforwardVelocity The desired feedforward velocity + * + * Only the first two components are used. + */ + void + setFeedForwardVelocityInTargetFrame(const Eigen::Ref feedforwardVelocity); + + /** + * 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 DistanceTask is an equality task. + * @return the size of the task. + */ Type type() const override; + /** + * Determines the validity of the objects retrieved with getA() and getB() + * @return True if the objects are valid, false otherwise. + */ bool isValid() const override; }; diff --git a/src/IK/src/GravityTask.cpp b/src/IK/src/GravityTask.cpp index 85dc691ca9..b1f38eadb1 100644 --- a/src/IK/src/GravityTask.cpp +++ b/src/IK/src/GravityTask.cpp @@ -47,8 +47,8 @@ bool GravityTask::setVariablesHandler(const System::VariablesHandler& variablesH // get the variable if (m_robotVelocityVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize) { - log()->error("[GravityTask::setVariablesHandler] The size of the robot velocity variable is " - "different from the one expected. Expected size: {}. Given size: {}.", + log()->error("[GravityTask::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; @@ -59,25 +59,19 @@ bool GravityTask::setVariablesHandler(const System::VariablesHandler& variablesH m_A.setZero(); m_b.resize(m_DoFs); m_b.setZero(); - m_Angularjacobian.resize(3, variablesHandler.getNumberOfVariables()); - m_Angularjacobian.setZero(); - m_relativeJacobian.resize(6, m_kinDyn->getNrOfDegreesOfFreedom()); - m_relativeJacobian.setZero(); - m_currentAcc.setZero(3); - m_currentGyro.setZero(3); - m_currentAccNorm.setZero(3); - m_Am.resize(2,3); - m_Am << 1, 0, 0, - 0, 1, 0; - m_bm.resize(2,3); - m_bm << 0 ,1, 0, - -1, 0, 0; + m_jacobian.resize(m_spatialVelocitySize, m_kinDyn->getNrOfDegreesOfFreedom()); + m_jacobian.setZero(); + m_desiredZDirectionBody.setZero(); + m_feedForwardBody.setZero(); + m_Am.resize(2, 3); + m_Am << 1, 0, 0, 0, 1, 0; + m_bm.resize(2, 3); + m_bm << 0, -1, 0, 1, 0, 0; return true; } -bool GravityTask::initialize( - std::weak_ptr paramHandler) +bool GravityTask::initialize(std::weak_ptr paramHandler) { constexpr auto errorPrefix = "[GravityTask::initialize]"; @@ -124,33 +118,23 @@ bool GravityTask::initialize( return false; } - // set the base frame name - if (!ptr->getParameter("base_name", m_baseName)) - { - log()->debug("{} [{}] to get the base name. Using default \"\"", - errorPrefix, - m_description); - } - - if (m_baseName != "") - { - m_baseIndex = m_kinDyn->getFrameIndex(m_baseName); - - if (m_baseIndex == iDynTree::FRAME_INVALID_INDEX) - return false; - } - - // set the finger tip frame Name - if (!ptr->getParameter("target_frame_name", m_targetFrameName)) + std::string targetFrameName; + if (!ptr->getParameter("target_frame_name", targetFrameName)) { log()->error("{} [{}] to get the end effector frame name.", errorPrefix, m_description); return false; } - m_targetFrameIndex = m_kinDyn->getFrameIndex(m_targetFrameName); + m_targetFrameIndex = m_kinDyn->getFrameIndex(targetFrameName); if (m_targetFrameIndex == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{} [{}] The specified target name ({}) does not seem to exist.", + errorPrefix, + m_description, + targetFrameName); return false; + } m_isInitialized = true; @@ -163,46 +147,35 @@ bool GravityTask::update() m_isValid = false; - // Normalize Accelerometer vector - m_accDenomNorm = sqrt(pow(m_currentAcc(0),2) + pow(m_currentAcc(1),2) + pow(m_currentAcc(2),2)); - - for (size_t i = 0; i < m_currentAcc.size(); i++) - { - m_currentAccNorm(i) = m_currentAcc(i) / m_accDenomNorm; - } + auto targetRotation = toEigen(m_kinDyn->getWorldTransform(m_targetFrameIndex).getRotation()); - m_currentAccNorm = toEigen(m_kinDyn->getWorldTransform(m_targetFrameName).getRotation()) * m_currentAccNorm; + Eigen::Vector3d desiredZDirectionAbsolute = targetRotation * m_desiredZDirectionBody; + Eigen::Vector3d feedforwardAbsolute = targetRotation * m_feedForwardBody; - // get the relative jacobian - if (!m_kinDyn->getRelativeJacobian(m_baseIndex, m_targetFrameIndex, m_relativeJacobian)) + if (!m_kinDyn->getFrameFreeFloatingJacobian(m_targetFrameIndex, m_jacobian)) { - log()->error("[GravityTask::update] Unable to get the relative jacobian."); + log()->error("[GravityTask::update] Unable to get the frame jacobian."); return m_isValid; } - // get the angular part of the jacobian - m_Angularjacobian.bottomRightCorner(3, m_kinDyn->getNrOfDegreesOfFreedom()) = m_relativeJacobian.bottomRightCorner(3, m_kinDyn->getNrOfDegreesOfFreedom()); + m_A = m_Am * m_jacobian.bottomRows<3>(); + m_b << m_kp * m_bm * desiredZDirectionAbsolute + feedforwardAbsolute.topRows<2>(); - m_A = m_Am * m_Angularjacobian; - m_b << ( m_kp * m_bm * m_currentAccNorm) + 1000 * m_currentGyro.block<2,1>(1,0); - - // A and b are now valid m_isValid = true; return m_isValid; } -bool GravityTask::setEstimateGravityDir(const Eigen::Ref currentGravityDir) +void GravityTask::setDesiredGravityDirectionInTargetFrame( + const Eigen::Ref desiredGravityDirection) { - m_currentAcc = currentGravityDir; - - return true; + m_desiredZDirectionBody = desiredGravityDirection; + m_desiredZDirectionBody.normalize(); } -bool GravityTask::setGyroscope(const Eigen::Ref currentGyro) +void GravityTask::setFeedForwardVelocityInTargetFrame( + const Eigen::Ref feedforwardVelocity) { - m_currentGyro = currentGyro * (M_PI)/180; - - return true; + m_feedForwardBody = feedforwardVelocity; } std::size_t GravityTask::size() const From 5e1ecc2965a62ceffadaf7288793495276c68b13 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 1 Aug 2023 12:30:23 +0200 Subject: [PATCH 07/17] Added GravityTask tests and bugfix Added GravityTask to QPInverseKinematics Fix failure of QPInveseKinematicsTest by moving Gravity task to low priority Moving also distance task to low priority in QPInverseKinematics Fixed a sign mistake in GravityTask. This might have been the reason why the IK was failing before Added explanation on how the gravity task is obtained directly in the code. --- .../BipedalLocomotion/IK/GravityTask.h | 68 ++++++++- src/IK/src/GravityTask.cpp | 21 ++- src/IK/tests/CMakeLists.txt | 5 + src/IK/tests/GravityTaskTest.cpp | 132 ++++++++++++++++++ src/IK/tests/QPInverseKinematicsTest.cpp | 109 +++++++++++++-- 5 files changed, 313 insertions(+), 22 deletions(-) create mode 100644 src/IK/tests/GravityTaskTest.cpp diff --git a/src/IK/include/BipedalLocomotion/IK/GravityTask.h b/src/IK/include/BipedalLocomotion/IK/GravityTask.h index 4a0f8a07a7..4997476722 100644 --- a/src/IK/include/BipedalLocomotion/IK/GravityTask.h +++ b/src/IK/include/BipedalLocomotion/IK/GravityTask.h @@ -20,7 +20,6 @@ namespace BipedalLocomotion { namespace IK { - // clang-format off /** * The GravityTask class aligns the z axis of a frame to a desired gravity direction. @@ -31,7 +30,7 @@ namespace IK * \begin{bmatrix} * 1 & 0 & 0 \\ * 0 & 1 & 0 - * \end{bmatrix} J_\omega \nu = k \begin{bmatrix} + * \end{bmatrix} J_\omega \nu = -k \begin{bmatrix} * 0 &-1 & 0 \\ * 1 & 0 & 0 * \end{bmatrix} {} ^ W R_f {} ^ f g ^ * + {} ^ W R_f {} ^ f \omega_{ff} @@ -40,8 +39,65 @@ namespace IK * \f$k\f$ is the controller gain. * \f${} ^ W R_f\f$ is the frame rotation. * \f${} ^ W g ^ *\f$ is the desired z axis direction expressed in the frame coordinates. - * \f${} ^ W \omega_{ff}\f$ is the feed forward velocity expressed in the frame coordinates. - * A more through explanation is provided in https://github.com/ami-iit/bipedal-locomotion-framework/issues/651 + * \f${} ^ W \omega_{ff}\f$ is the feed-forward velocity expressed in the frame coordinates. + * The explanation is as follows. + * + * We define ${}^wR_b$ as the orientation of the frame $b$ we want to move, + * and ${}^b\hat{V}$ as the unitary vector fixed in frame $b$ that we want to align to ${}^wz = e_3 = [0 0 1]^\top$. + * + * We define the following Lyapunov function: + * \f[ + * V = 1 - \cos(\theta) \geq 0. + * \f] + * + * Since \f$ a^\top b = ||a|| ~||b|| \cos(\theta) \f$, we have: + * \f[ + * V = 1 - \left({}^wR_b{}^b\hat{V}\right)^\top e_3, + * \f] + * given that both vectors are unitary. + * + * The time derivative of the Lyapunov function is as follows: + * \f[ + * \dot{V} = - \left({}^w\dot{R}_b{}^b\hat{V}\right)^\top e_3, + * \f] + * where \f$ {}^w\dot{R}_b = S({}^w\omega){}^w{R}_b \f$, with \f$ S(\cdot) \f$ being a skew symmetric matrix, + * and \f$ {}^w\omega \in \mathbb{R}^3 \f$ is the right-trivialized angular velocity. Hence, + * \f[ + * \dot{V} = - \left(S({}^w\omega){}^w{R}_b{}^b\hat{V}\right)^\top e_3. + * \f] + * + * Exploiting \f$ S(\cdot)^\top = -S(\cdot) \f$, we have: + * \f[ + * \dot{V} = {}^b\hat{V}^\top{}^w{R}_b^\top S({}^w\omega) e_3, + * \f] + * and \f$ S(a)b = -S(b)a \f$: + * \f[ + * \dot{V} = -{}^b\hat{V}^\top{}^w{R}_b^\top S(e_3){}^w\omega. + * \f] + * + * Then, if we choose: + * \f[ + * {}^w\omega = k \left({}^b\hat{V}^\top{}^w{R}_b^\top S(e_3)\right)^\top + \lambda e_3, + * \f] + * with \f$ k, \lambda \in \mathbb{R}^3 \f$, we have that \f$ \dot{V} \leq 0 \f$. Then, + * \f[ + * {}^w\omega = -k S(e_3) {}^w{R}_b{}^b\hat{V} + \lambda e_3. + * \f] + * + * We can introduce the right-trivialized Jacobian \f$ {}^wJ \f$ such that \f$ {}^wJ\dot{\nu} = {}^w\omega \f$: + * \f[ + * {}^wJ\dot{\nu} = -k S(e_3) {}^w{R}_b{}^b\hat{V} + \lambda e_3. + * \f] + * + * We can filter out the rotation around the world Z-axis, thus obtaining: + * \f[ + * \begin{bmatrix} + * e_1 & e_2 + * \end{bmatrix}^\top {}^wJ\dot{\nu} = -k \begin{bmatrix} + * 0 & -1 & 0\\ + * 1 & 0 & 0 + * \end{bmatrix} {}^w{R}_b{}^b\hat{V}. + * \f] */ // clang-format on class GravityTask : public IKLinearTask @@ -119,7 +175,7 @@ class GravityTask : public IKLinearTask * * The input is normalized, unless the norm is too small. */ - void setDesiredGravityDirectionInTargetFrame( + bool setDesiredGravityDirectionInTargetFrame( const Eigen::Ref desiredGravityDirection); /** @@ -128,7 +184,7 @@ class GravityTask : public IKLinearTask * * Only the first two components are used. */ - void + bool setFeedForwardVelocityInTargetFrame(const Eigen::Ref feedforwardVelocity); /** diff --git a/src/IK/src/GravityTask.cpp b/src/IK/src/GravityTask.cpp index b1f38eadb1..ccfd446c8c 100644 --- a/src/IK/src/GravityTask.cpp +++ b/src/IK/src/GravityTask.cpp @@ -59,14 +59,19 @@ bool GravityTask::setVariablesHandler(const System::VariablesHandler& variablesH m_A.setZero(); m_b.resize(m_DoFs); m_b.setZero(); - m_jacobian.resize(m_spatialVelocitySize, m_kinDyn->getNrOfDegreesOfFreedom()); + m_jacobian.resize(m_spatialVelocitySize, + m_spatialVelocitySize + m_kinDyn->getNrOfDegreesOfFreedom()); m_jacobian.setZero(); m_desiredZDirectionBody.setZero(); m_feedForwardBody.setZero(); m_Am.resize(2, 3); - m_Am << 1, 0, 0, 0, 1, 0; m_bm.resize(2, 3); - m_bm << 0, -1, 0, 1, 0, 0; + // clang-format off + m_Am << 1, 0, 0, + 0, 1, 0; + m_bm << 0, -1, 0, + 1, 0, 0; + // clang-format on return true; } @@ -158,24 +163,26 @@ bool GravityTask::update() return m_isValid; } - m_A = m_Am * m_jacobian.bottomRows<3>(); - m_b << m_kp * m_bm * desiredZDirectionAbsolute + feedforwardAbsolute.topRows<2>(); + toEigen(this->subA(m_robotVelocityVariable)) = m_Am * m_jacobian.bottomRows<3>(); + m_b = -m_kp * m_bm * desiredZDirectionAbsolute + feedforwardAbsolute.topRows<2>(); m_isValid = true; return m_isValid; } -void GravityTask::setDesiredGravityDirectionInTargetFrame( +bool GravityTask::setDesiredGravityDirectionInTargetFrame( const Eigen::Ref desiredGravityDirection) { m_desiredZDirectionBody = desiredGravityDirection; m_desiredZDirectionBody.normalize(); + return true; } -void GravityTask::setFeedForwardVelocityInTargetFrame( +bool GravityTask::setFeedForwardVelocityInTargetFrame( const Eigen::Ref feedforwardVelocity) { m_feedForwardBody = feedforwardVelocity; + return true; } std::size_t GravityTask::size() const diff --git a/src/IK/tests/CMakeLists.txt b/src/IK/tests/CMakeLists.txt index 66cc045b67..8f63b0b735 100644 --- a/src/IK/tests/CMakeLists.txt +++ b/src/IK/tests/CMakeLists.txt @@ -42,6 +42,11 @@ add_bipedal_test( SOURCES DistanceTaskTest.cpp LINKS BipedalLocomotion::IK) +add_bipedal_test( + NAME GravityTaskIK + SOURCES GravityTaskTest.cpp + LINKS BipedalLocomotion::IK) + if (FRAMEWORK_COMPILE_ContinuousDynamicalSystem) add_bipedal_test( diff --git a/src/IK/tests/GravityTaskTest.cpp b/src/IK/tests/GravityTaskTest.cpp new file mode 100644 index 0000000000..34ae776995 --- /dev/null +++ b/src/IK/tests/GravityTaskTest.cpp @@ -0,0 +1,132 @@ +/** + * @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 BSD-3-Clause license. + */ + +// Catch2 +#include + +// BipedalLocomotion +#include +#include +#include + +#include +#include + +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::IK; + +TEST_CASE("Distance 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 < 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 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); + double kp = 2.0; + parameterHandler->setParameter("kp", kp); + + std::string targetName = iDynTree::getRandomLinkOfModel(model); + parameterHandler->setParameter("target_frame_name", targetName); + + GravityTask task; + REQUIRE(task.setKinDyn(kinDyn)); + REQUIRE(task.initialize(parameterHandler)); + REQUIRE(task.setVariablesHandler(variablesHandler)); + + Eigen::Vector3d desiredDirection({1.0, 2.0, 3.0}); + desiredDirection.normalize(); + REQUIRE(task.setDesiredGravityDirectionInTargetFrame(desiredDirection)); + Eigen::Vector3d feedforward({0.1, 0.2, 0.3}); + REQUIRE(task.setFeedForwardVelocityInTargetFrame(feedforward)); + + REQUIRE(task.update()); + REQUIRE(task.isValid()); + + // get A and b + Eigen::Ref A = task.getA(); + Eigen::Ref b = task.getB(); + + Eigen::Matrix3d targetRotation; + Eigen::MatrixXd jacobian; + jacobian.setZero(6, 6 + model.getNrOfDOFs()); + + targetRotation = iDynTree::toEigen(kinDyn->getWorldTransform(targetName).getRotation()); + kinDyn->getFrameFreeFloatingJacobian(targetName, jacobian); + + Eigen::MatrixXd expectedA = jacobian.middleRows<2>(3); + + // 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, + model.getNrOfDOFs() + 6) + .isApprox(expectedA)); + + // check the vector b + Eigen::VectorXd expectedB(task.size()); + Eigen::Vector3d axisAbsolute, feedforwardAbsolute; + axisAbsolute = targetRotation * desiredDirection; + feedforwardAbsolute = targetRotation * feedforward; + expectedB(0) = kp * axisAbsolute(1) + feedforwardAbsolute(0); + expectedB(1) = -kp * axisAbsolute(0) + feedforwardAbsolute(1); + REQUIRE(b.isApprox(expectedB)); + } + } +} diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp index dde67db0c1..814bf5812a 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -53,6 +54,7 @@ struct InverseKinematicsAndTasks std::shared_ptr regularizationTask; std::shared_ptr jointLimitsTask; std::shared_ptr distanceTask; + std::shared_ptr gravityTask; }; struct DesiredSetPoints @@ -63,6 +65,8 @@ struct DesiredSetPoints Eigen::VectorXd joints; std::string targetFrameDistance; double targetDistance; + std::string targetFrameGravity; + Eigen::Vector3d desiredBodyGravity; }; struct System @@ -75,6 +79,7 @@ std::shared_ptr createParameterHandler() { constexpr double gain = 5.0; + constexpr double additionalTasksWeight = 10.0; auto parameterHandler = std::make_shared(); parameterHandler->setParameter("robot_velocity_variable_name", robotVelocity); @@ -85,7 +90,8 @@ std::shared_ptr createParameterHandler() "COM_TASK", "REGULARIZATION_TASK", "JOINT_LIMITS_TASK", - "DISTANCE_TASK"}); + "DISTANCE_TASK", + "GRAVITY_TASK"}); /////// SE3 Task auto SE3ParameterHandler = std::make_shared(); @@ -125,10 +131,23 @@ std::shared_ptr createParameterHandler() auto distanceTaskHandler = std::make_shared(); distanceTaskHandler->setParameter("robot_velocity_variable_name", robotVelocity); distanceTaskHandler->setParameter("type", "DistanceTask"); - distanceTaskHandler->setParameter("priority", 0); + distanceTaskHandler->setParameter("priority", 1); distanceTaskHandler->setParameter("kp", gain); + Eigen::VectorXd distanceWeight(1); + distanceWeight << additionalTasksWeight; + distanceTaskHandler->setParameter("weight", distanceWeight); parameterHandler->setGroup("DISTANCE_TASK", distanceTaskHandler); + /////// Gravity task + auto gravityTaskHandler = std::make_shared(); + gravityTaskHandler->setParameter("robot_velocity_variable_name", robotVelocity); + gravityTaskHandler->setParameter("type", "GravityTask"); + gravityTaskHandler->setParameter("priority", 1); + gravityTaskHandler->setParameter("kp", gain); + const Eigen::Vector2d gravityWeight = additionalTasksWeight * Eigen::Vector2d::Ones(); + gravityTaskHandler->setParameter("weight", gravityWeight); + parameterHandler->setGroup("GRAVITY_TASK", gravityTaskHandler); + return parameterHandler; } @@ -166,6 +185,8 @@ InverseKinematicsAndTasks createIK(std::shared_ptr handler, constexpr std::size_t lowPriority = 1; Eigen::VectorXd kpRegularization; Eigen::VectorXd weightRegularization; + Eigen::VectorXd distanceWeight; + Eigen::VectorXd gravityWeight; out.ik = std::make_shared(); REQUIRE(out.ik->initialize(handler)); @@ -211,7 +232,14 @@ InverseKinematicsAndTasks createIK(std::shared_ptr handler, out.distanceTask = std::make_shared(); REQUIRE(out.distanceTask->setKinDyn(kinDyn)); REQUIRE(out.distanceTask->initialize(handler->getGroup("DISTANCE_TASK"))); - REQUIRE(out.ik->addTask(out.distanceTask, "distance_task", highPriority)); + REQUIRE(handler->getGroup("DISTANCE_TASK").lock()->getParameter("weight", distanceWeight)); + REQUIRE(out.ik->addTask(out.distanceTask, "distance_task", lowPriority, distanceWeight)); + + out.gravityTask = std::make_shared(); + REQUIRE(out.gravityTask->setKinDyn(kinDyn)); + REQUIRE(out.gravityTask->initialize(handler->getGroup("GRAVITY_TASK"))); + REQUIRE(handler->getGroup("GRAVITY_TASK").lock()->getParameter("weight", gravityWeight)); + REQUIRE(out.ik->addTask(out.gravityTask, "gravity_task", lowPriority, gravityWeight)); REQUIRE(out.ik->finalize(variables)); @@ -253,14 +281,31 @@ DesiredSetPoints getDesiredReference(std::shared_ptrgetWorldTransform(out.endEffectorFrame)); out.joints.resize(jointsPos.size()); out.joints = iDynTree::toEigen(jointsPos); + iDynTree::LinkIndex index; do { out.targetFrameDistance = iDynTree::getRandomLinkOfModel(kinDyn->model()); - } while (out.targetFrameDistance == out.endEffectorFrame); + index = kinDyn->model().getFrameIndex(out.targetFrameDistance); + } while (std::abs(index - kinDyn->model().getFrameIndex(out.endEffectorFrame)) < 5); out.targetDistance = toManifPose(kinDyn->getWorldTransform(out.targetFrameDistance)).translation().norm(); + // Find a frame sufficiently far away from those used for the distance and SE3 task + do + { + out.targetFrameGravity = iDynTree::getRandomLinkOfModel(kinDyn->model()); + index = kinDyn->model().getFrameIndex(out.targetFrameGravity); + } while (std::abs(index - kinDyn->model().getFrameIndex(out.endEffectorFrame)) < 5 + || std::abs(index - kinDyn->model().getFrameIndex(out.targetFrameDistance)) < 5); + + out.targetFrameGravity = kinDyn->getFloatingBase(); + + // The desired gravity in body frame is the world z axis in body frame, + // that is the third row of the body rotation matrix + out.desiredBodyGravity + = toManifPose(kinDyn->getWorldTransform(out.targetFrameGravity)).rotation().matrix().row(2); + return out; } @@ -306,7 +351,7 @@ TEST_CASE("QP-IK") auto kinDyn = std::make_shared(); auto parameterHandler = createParameterHandler(); - constexpr double tolerance = 1e-1; + constexpr double tolerance = 5e-1; // set the velocity representation REQUIRE(kinDyn->setFrameVelocityRepresentation( @@ -337,6 +382,10 @@ TEST_CASE("QP-IK") .lock() ->setParameter("target_frame_name", desiredSetPoints.targetFrameDistance); + parameterHandler->getGroup("GRAVITY_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameGravity); + // create the IK constexpr double jointLimitDelta = 0.5; finalizeParameterHandler(parameterHandler, @@ -354,8 +403,11 @@ TEST_CASE("QP-IK") REQUIRE(ikAndTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + REQUIRE(ikAndTasks.gravityTask->setDesiredGravityDirectionInTargetFrame( + desiredSetPoints.desiredBodyGravity)); + // propagate the inverse kinematics for - constexpr std::size_t iterations = 35; + constexpr std::size_t iterations = 30; Eigen::Vector3d gravity; gravity << 0, 0, -9.81; Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); @@ -401,10 +453,22 @@ TEST_CASE("QP-IK") const manif::SE3d::Tangent error = endEffectorPose - desiredSetPoints.endEffectorPose; REQUIRE(error.coeffs().isZero(tolerance)); + // Check the distance error REQUIRE(toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameDistance)) .translation() .norm() == Catch::Approx(desiredSetPoints.targetDistance).epsilon(tolerance * 0.1)); + + // Check the gravity error + Eigen::Vector3d gravityError; + gravityError + = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameGravity)) + .rotation() + .matrix() + .row(2) + .transpose() + - desiredSetPoints.desiredBodyGravity; + REQUIRE(gravityError.isZero(tolerance)); } } } @@ -443,6 +507,10 @@ TEST_CASE("QP-IK [With strict limits]") .lock() ->setParameter("target_frame_name", desiredSetPoints.targetFrameDistance); + parameterHandler->getGroup("GRAVITY_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameGravity); + // create the IK constexpr double jointLimitDelta = 0.5; Eigen::VectorXd limitsDelta @@ -462,8 +530,11 @@ TEST_CASE("QP-IK [With strict limits]") REQUIRE(ikAndTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + REQUIRE(ikAndTasks.gravityTask->setDesiredGravityDirectionInTargetFrame( + desiredSetPoints.desiredBodyGravity)); + // propagate the inverse kinematics for - constexpr std::size_t iterations = 35; + constexpr std::size_t iterations = 30; Eigen::Vector3d gravity; gravity << 0, 0, -9.81; Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); @@ -513,7 +584,7 @@ TEST_CASE("QP-IK [With builder]") auto kinDyn = std::make_shared(); auto parameterHandler = createParameterHandler(); - constexpr double tolerance = 1e-1; + constexpr double tolerance = 5e-1; // set the velocity representation REQUIRE(kinDyn->setFrameVelocityRepresentation( @@ -552,6 +623,10 @@ TEST_CASE("QP-IK [With builder]") .lock() ->setParameter("target_frame_name", desiredSetPoints.targetFrameDistance); + parameterHandler->getGroup("GRAVITY_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameGravity); + // finalize the parameter handler constexpr double jointLimitDelta = 0.5; finalizeParameterHandler(parameterHandler, @@ -582,8 +657,13 @@ TEST_CASE("QP-IK [With builder]") REQUIRE_FALSE(distanceTask == nullptr); REQUIRE(distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + auto gravityTask = std::dynamic_pointer_cast(ik->getTask("GRAVITY_TASK").lock()); + REQUIRE_FALSE(gravityTask == nullptr); + REQUIRE( + gravityTask->setDesiredGravityDirectionInTargetFrame(desiredSetPoints.desiredBodyGravity)); + // propagate the inverse kinematics for - constexpr std::size_t iterations = 35; + constexpr std::size_t iterations = 30; Eigen::Vector3d gravity; gravity << 0, 0, -9.81; Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); @@ -633,8 +713,19 @@ TEST_CASE("QP-IK [With builder]") const manif::SE3d::Tangent error = endEffectorPose - desiredSetPoints.endEffectorPose; REQUIRE(error.coeffs().isZero(tolerance)); + // Check the distance error REQUIRE(toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameDistance)) .translation() .norm() == Catch::Approx(desiredSetPoints.targetDistance).epsilon(tolerance * 0.1)); + + // Check the gravity error + Eigen::Vector3d gravityError; + gravityError = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameGravity)) + .rotation() + .matrix() + .row(2) + .transpose() + - desiredSetPoints.desiredBodyGravity; + REQUIRE(gravityError.isZero(tolerance)); } From 5952b913621e36b3d94544fd2a91b1a6b95070a4 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 1 Aug 2023 17:08:07 +0200 Subject: [PATCH 08/17] Added GravityTask python bindings --- bindings/python/IK/CMakeLists.txt | 4 +- .../bindings/IK/GravityTask.h | 26 ++++++++++++ bindings/python/IK/src/GravityTask.cpp | 42 +++++++++++++++++++ bindings/python/IK/src/Module.cpp | 2 + .../IK/tests/test_QP_inverse_kinematics.py | 23 ++++++++++ 5 files changed, 95 insertions(+), 2 deletions(-) create mode 100644 bindings/python/IK/include/BipedalLocomotion/bindings/IK/GravityTask.h create mode 100644 bindings/python/IK/src/GravityTask.cpp diff --git a/bindings/python/IK/CMakeLists.txt b/bindings/python/IK/CMakeLists.txt index 3058ad627c..85d20e5e1e 100644 --- a/bindings/python/IK/CMakeLists.txt +++ b/bindings/python/IK/CMakeLists.txt @@ -8,8 +8,8 @@ if(TARGET BipedalLocomotion::IK) add_bipedal_locomotion_python_module( NAME IKBindings - SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/JointLimitsTask.cpp src/AngularMomentumTask.cpp src/Module.cpp src/IKLinearTask.cpp src/DistanceTask.cpp - HEADERS ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h + SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/JointLimitsTask.cpp src/AngularMomentumTask.cpp src/Module.cpp src/IKLinearTask.cpp src/DistanceTask.cpp src/GravityTask.cpp + HEADERS ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h LINK_LIBRARIES BipedalLocomotion::IK MANIF::manif TESTS tests/test_QP_inverse_kinematics.py TESTS_RUNTIME_CONDITIONS FRAMEWORK_USE_icub-models diff --git a/bindings/python/IK/include/BipedalLocomotion/bindings/IK/GravityTask.h b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/GravityTask.h new file mode 100644 index 0000000000..7b16d41881 --- /dev/null +++ b/bindings/python/IK/include/BipedalLocomotion/bindings/IK/GravityTask.h @@ -0,0 +1,26 @@ +/** + * @file GravityTask.h + * @authors Stefano Dafarra + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_GRAVITY_TASK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_IK_GRAVITY_TASK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace IK +{ + +void CreateGravityTask(pybind11::module& module); + +} // namespace IK +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_GRAVITY_TASK_H diff --git a/bindings/python/IK/src/GravityTask.cpp b/bindings/python/IK/src/GravityTask.cpp new file mode 100644 index 0000000000..07cc3da6cc --- /dev/null +++ b/bindings/python/IK/src/GravityTask.cpp @@ -0,0 +1,42 @@ +/** + * @file GravityTask.cpp + * @authors Stefano Dafarra + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace IK +{ + +void CreateGravityTask(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::IK; + + py::class_, IKLinearTask>(module, "GravityTask") + .def(py::init()) + .def("set_desired_gravity_direction_in_target_frame", + &GravityTask::setDesiredGravityDirectionInTargetFrame, + py::arg("desired_gravity_direction")) + .def("set_feedforward_velocity_in_target_frame", + &GravityTask::setFeedForwardVelocityInTargetFrame, + py::arg("feedforward_velocity")); +} + +} // namespace IK +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/IK/src/Module.cpp b/bindings/python/IK/src/Module.cpp index 104d537d8a..c2ae7aaf82 100644 --- a/bindings/python/IK/src/Module.cpp +++ b/bindings/python/IK/src/Module.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -39,6 +40,7 @@ void CreateModule(pybind11::module& module) CreateJointLimitsTask(module); CreateAngularMomentumTask(module); CreateDistanceTask(module); + CreateGravityTask(module); CreateIntegrationBasedIK(module); CreateQPInverseKinematics(module); } diff --git a/bindings/python/IK/tests/test_QP_inverse_kinematics.py b/bindings/python/IK/tests/test_QP_inverse_kinematics.py index 87776d01cf..bd523a0598 100644 --- a/bindings/python/IK/tests/test_QP_inverse_kinematics.py +++ b/bindings/python/IK/tests/test_QP_inverse_kinematics.py @@ -235,6 +235,29 @@ def test_distance_task(): # Set desired distance assert distance_task.set_desired_distance(desired_distance=np.random.uniform(-0.5,0.5)) +def test_gravity_task(): + # create KinDynComputationsDescriptor + joints_list, kindyn = get_kindyn() + + # Set the parameters + gravity_task_param_handler = blf.parameters_handler.StdParametersHandler() + gravity_task_param_handler.set_parameter_string(name="robot_velocity_variable_name", value="robotVelocity") + gravity_task_param_handler.set_parameter_string(name="target_frame_name", value="chest") + gravity_task_param_handler.set_parameter_string(name="base_frame_name", value="root_link") + gravity_task_param_handler.set_parameter_float(name="kp",value=5.0) + + # Initialize the task + gravity_task = blf.ik.GravityTask() + assert gravity_task.set_kin_dyn(kindyn) + assert gravity_task.initialize(param_handler=gravity_task_param_handler) + gravity_task_var_handler = blf.system.VariablesHandler() + assert gravity_task_var_handler.add_variable("robotVelocity", 32) is True # robot velocity size = 26 (joints) + 6 (base) + assert gravity_task.set_variables_handler(variables_handler=gravity_task_var_handler) + + # Set desired distance + assert gravity_task.set_desired_gravity_direction_in_target_frame(desired_gravity_direction=[np.random.uniform(-0.5,0.5) for _ in range(3)]) + assert gravity_task.set_feedforward_velocity_in_target_frame(feedforward_velocity=[np.random.uniform(-0.5,0.5) for _ in range(3)]) + def test_integration_based_ik_state(): state = blf.ik.IntegrationBasedIKState() From f0c70a030bad965b0bab3fe4d154578222dacb67 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 11:04:37 +0200 Subject: [PATCH 09/17] Added a test case to test only gravity and distance task separately. It was necessary to refactor a bit the structure of the code, porting in each test case the addition of the tasks we are interested in. Added a small rotation error in the base. This is necessary since the random models generated by iDynTree seem always planar. --- src/IK/tests/QPInverseKinematicsTest.cpp | 335 ++++++++++++++++------- 1 file changed, 230 insertions(+), 105 deletions(-) diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp index 814bf5812a..f90e55915d 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -46,9 +46,8 @@ using namespace std::chrono_literals; constexpr auto robotVelocity = "robotVelocity"; constexpr std::chrono::nanoseconds dT = 10ms; -struct InverseKinematicsAndTasks +struct InverseKinematicsTasks { - std::shared_ptr ik; std::shared_ptr se3Task; std::shared_ptr comTask; std::shared_ptr regularizationTask; @@ -132,7 +131,7 @@ std::shared_ptr createParameterHandler() distanceTaskHandler->setParameter("robot_velocity_variable_name", robotVelocity); distanceTaskHandler->setParameter("type", "DistanceTask"); distanceTaskHandler->setParameter("priority", 1); - distanceTaskHandler->setParameter("kp", gain); + distanceTaskHandler->setParameter("kp", gain * 5); Eigen::VectorXd distanceWeight(1); distanceWeight << additionalTasksWeight; distanceTaskHandler->setParameter("weight", distanceWeight); @@ -143,7 +142,7 @@ std::shared_ptr createParameterHandler() gravityTaskHandler->setParameter("robot_velocity_variable_name", robotVelocity); gravityTaskHandler->setParameter("type", "GravityTask"); gravityTaskHandler->setParameter("priority", 1); - gravityTaskHandler->setParameter("kp", gain); + gravityTaskHandler->setParameter("kp", gain * 10); const Eigen::Vector2d gravityWeight = additionalTasksWeight * Eigen::Vector2d::Ones(); gravityTaskHandler->setParameter("weight", gravityWeight); parameterHandler->setGroup("GRAVITY_TASK", gravityTaskHandler); @@ -174,74 +173,35 @@ void finalizeParameterHandler(std::shared_ptr handler, handler->getGroup("JOINT_LIMITS_TASK").lock()->setParameter("lower_limits", lowerLimits); } -InverseKinematicsAndTasks createIK(std::shared_ptr handler, - std::shared_ptr kinDyn, - const VariablesHandler& variables) +InverseKinematicsTasks createIKTasks(std::shared_ptr handler, + std::shared_ptr kinDyn) { - InverseKinematicsAndTasks out; - - constexpr std::size_t highPriority = 0; - constexpr std::size_t lowPriority = 1; - Eigen::VectorXd kpRegularization; - Eigen::VectorXd weightRegularization; - Eigen::VectorXd distanceWeight; - Eigen::VectorXd gravityWeight; - - out.ik = std::make_shared(); - REQUIRE(out.ik->initialize(handler)); + InverseKinematicsTasks out; 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)); - REQUIRE(handler->getGroup("REGULARIZATION_TASK").lock()->getParameter("kp", kpRegularization)); - REQUIRE(handler->getGroup("REGULARIZATION_TASK") - .lock() - ->getParameter("weight", weightRegularization)); 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)); out.jointLimitsTask = std::make_shared(); REQUIRE(out.jointLimitsTask->setKinDyn(kinDyn)); REQUIRE(out.jointLimitsTask->initialize(handler->getGroup("JOINT_LIMITS_TASK"))); - REQUIRE(out.ik->addTask(out.jointLimitsTask, "joint_limits_task", highPriority)); - - const Eigen::VectorXd newWeight = 10 * weightRegularization; - auto newWeightProvider - = std::make_shared(newWeight); - REQUIRE(out.ik->setTaskWeight("regularization_task", newWeightProvider)); - - auto outWeightProvider = out.ik->getTaskWeightProvider("regularization_task").lock(); - REQUIRE(outWeightProvider); - REQUIRE(outWeightProvider->getOutput().isApprox(newWeight)); - - REQUIRE(out.ik->setTaskWeight("regularization_task", weightRegularization)); out.distanceTask = std::make_shared(); REQUIRE(out.distanceTask->setKinDyn(kinDyn)); REQUIRE(out.distanceTask->initialize(handler->getGroup("DISTANCE_TASK"))); - REQUIRE(handler->getGroup("DISTANCE_TASK").lock()->getParameter("weight", distanceWeight)); - REQUIRE(out.ik->addTask(out.distanceTask, "distance_task", lowPriority, distanceWeight)); out.gravityTask = std::make_shared(); REQUIRE(out.gravityTask->setKinDyn(kinDyn)); REQUIRE(out.gravityTask->initialize(handler->getGroup("GRAVITY_TASK"))); - REQUIRE(handler->getGroup("GRAVITY_TASK").lock()->getParameter("weight", gravityWeight)); - REQUIRE(out.ik->addTask(out.gravityTask, "gravity_task", lowPriority, gravityWeight)); - - REQUIRE(out.ik->finalize(variables)); return out; } @@ -281,25 +241,22 @@ DesiredSetPoints getDesiredReference(std::shared_ptrgetWorldTransform(out.endEffectorFrame)); out.joints.resize(jointsPos.size()); out.joints = iDynTree::toEigen(jointsPos); - iDynTree::LinkIndex index; - do - { - out.targetFrameDistance = iDynTree::getRandomLinkOfModel(kinDyn->model()); - index = kinDyn->model().getFrameIndex(out.targetFrameDistance); - } while (std::abs(index - kinDyn->model().getFrameIndex(out.endEffectorFrame)) < 5); - - out.targetDistance - = toManifPose(kinDyn->getWorldTransform(out.targetFrameDistance)).translation().norm(); + iDynTree::LinkIndex indexDistance, indexGravity; // Find a frame sufficiently far away from those used for the distance and SE3 task do { out.targetFrameGravity = iDynTree::getRandomLinkOfModel(kinDyn->model()); - index = kinDyn->model().getFrameIndex(out.targetFrameGravity); - } while (std::abs(index - kinDyn->model().getFrameIndex(out.endEffectorFrame)) < 5 - || std::abs(index - kinDyn->model().getFrameIndex(out.targetFrameDistance)) < 5); + out.targetFrameDistance = iDynTree::getRandomLinkOfModel(kinDyn->model()); + indexGravity = kinDyn->model().getFrameIndex(out.targetFrameGravity); + indexDistance = kinDyn->model().getFrameIndex(out.targetFrameDistance); + } while (std::abs(indexGravity - kinDyn->model().getFrameIndex(out.endEffectorFrame)) < 5 + || std::abs(indexDistance - kinDyn->model().getFrameIndex(out.endEffectorFrame)) < 5 + || std::abs(indexGravity - indexDistance) < 5 || indexGravity < 3 + || indexDistance < 3); - out.targetFrameGravity = kinDyn->getFloatingBase(); + out.targetDistance + = toManifPose(kinDyn->getWorldTransform(out.targetFrameDistance)).translation().norm(); // The desired gravity in body frame is the world z axis in body frame, // that is the third row of the body rotation matrix @@ -334,15 +291,23 @@ System getSystem(std::shared_ptr kinDyn) jointPositions[i] += distribution(generator); } + Eigen::AngleAxisd baseAngleAxis(basePose.topLeftCorner<3, 3>()); + + double newAngle = baseAngleAxis.angle() + distribution(generator); + + Eigen::AngleAxisd newBaseRotation(newAngle, baseAngleAxis.axis()); + out.dynamics = std::make_shared(); - out.dynamics->setState({basePose.topRightCorner<3, 1>(), - toManifRot(basePose.topLeftCorner<3, 3>()), - jointPositions}); + out.dynamics->setState( + {basePose.topRightCorner<3, 1>(), toManifRot(newBaseRotation.matrix()), jointPositions}); out.integrator = std::make_shared>(); REQUIRE(out.integrator->setIntegrationStep(dT)); out.integrator->setDynamicalSystem(out.dynamics); + REQUIRE( + kinDyn->setRobotState(basePose, jointPositions, baseVelocity, jointVelocities, gravity)); + return out; } @@ -351,7 +316,9 @@ TEST_CASE("QP-IK") auto kinDyn = std::make_shared(); auto parameterHandler = createParameterHandler(); - constexpr double tolerance = 5e-1; + constexpr double tolerance = 2e-1; + constexpr std::size_t highPriority = 0; + constexpr std::size_t lowPriority = 1; // set the velocity representation REQUIRE(kinDyn->setFrameVelocityRepresentation( @@ -393,18 +360,42 @@ TEST_CASE("QP-IK") system, Eigen::VectorXd::Constant(kinDyn->model().getNrOfDOFs(), jointLimitDelta)); - auto ikAndTasks = createIK(parameterHandler, kinDyn, variablesHandler); + auto ikTasks = createIKTasks(parameterHandler, kinDyn); - 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)); + Eigen::VectorXd weightRegularization; - REQUIRE(ikAndTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + auto ik = std::make_shared(); + REQUIRE(ik->initialize(parameterHandler)); + REQUIRE(ik->addTask(ikTasks.se3Task, "se3_task", highPriority)); + REQUIRE(ik->addTask(ikTasks.comTask, "com_task", highPriority)); - REQUIRE(ikAndTasks.gravityTask->setDesiredGravityDirectionInTargetFrame( - desiredSetPoints.desiredBodyGravity)); + REQUIRE(parameterHandler->getGroup("REGULARIZATION_TASK") + .lock() + ->getParameter("weight", weightRegularization)); + REQUIRE(ik->addTask(ikTasks.regularizationTask, + "regularization_task", + lowPriority, + weightRegularization)); + REQUIRE(ik->addTask(ikTasks.jointLimitsTask, "joint_limits_task", highPriority)); + + const Eigen::VectorXd newWeight = 10 * weightRegularization; + auto newWeightProvider + = std::make_shared(newWeight); + REQUIRE(ik->setTaskWeight("regularization_task", newWeightProvider)); + + auto outWeightProvider = ik->getTaskWeightProvider("regularization_task").lock(); + REQUIRE(outWeightProvider); + REQUIRE(outWeightProvider->getOutput().isApprox(newWeight)); + + REQUIRE(ik->setTaskWeight("regularization_task", weightRegularization)); + + REQUIRE(ik->finalize(variablesHandler)); + + REQUIRE(ikTasks.se3Task->setSetPoint(desiredSetPoints.endEffectorPose, + manif::SE3d::Tangent::Zero())); + REQUIRE(ikTasks.comTask->setSetPoint(desiredSetPoints.CoMPosition, + Eigen::Vector3d::Zero())); + REQUIRE(ikTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); // propagate the inverse kinematics for constexpr std::size_t iterations = 30; @@ -429,11 +420,11 @@ TEST_CASE("QP-IK") gravity)); // solve the IK - REQUIRE(ikAndTasks.ik->advance()); + REQUIRE(ik->advance()); // get the output of the IK - baseVelocity = ikAndTasks.ik->getOutput().baseVelocity.coeffs(); - jointVelocity = ikAndTasks.ik->getOutput().jointVelocity; + baseVelocity = ik->getOutput().baseVelocity.coeffs(); + jointVelocity = ik->getOutput().jointVelocity; // propagate the dynamical system system.dynamics->setControlInput({baseVelocity, jointVelocity}); @@ -452,23 +443,6 @@ TEST_CASE("QP-IK") // please read it as (log(desiredSetPoints.endEffectorPose^-1 * endEffectorPose))^v const manif::SE3d::Tangent error = endEffectorPose - desiredSetPoints.endEffectorPose; REQUIRE(error.coeffs().isZero(tolerance)); - - // Check the distance error - REQUIRE(toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameDistance)) - .translation() - .norm() - == Catch::Approx(desiredSetPoints.targetDistance).epsilon(tolerance * 0.1)); - - // Check the gravity error - Eigen::Vector3d gravityError; - gravityError - = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameGravity)) - .rotation() - .matrix() - .row(2) - .transpose() - - desiredSetPoints.desiredBodyGravity; - REQUIRE(gravityError.isZero(tolerance)); } } } @@ -479,6 +453,8 @@ TEST_CASE("QP-IK [With strict limits]") auto parameterHandler = createParameterHandler(); constexpr double tolerance = 1e-2; + constexpr std::size_t highPriority = 0; + constexpr std::size_t lowPriority = 1; // set the velocity representation REQUIRE(kinDyn->setFrameVelocityRepresentation( @@ -521,17 +497,41 @@ TEST_CASE("QP-IK [With strict limits]") limitsDelta(3) = 0; finalizeParameterHandler(parameterHandler, kinDyn, system, limitsDelta); - auto ikAndTasks = createIK(parameterHandler, kinDyn, variablesHandler); + auto ikTasks = createIKTasks(parameterHandler, kinDyn); - 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)); + Eigen::VectorXd weightRegularization; - REQUIRE(ikAndTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + auto ik = std::make_shared(); + REQUIRE(ik->initialize(parameterHandler)); + REQUIRE(ik->addTask(ikTasks.se3Task, "se3_task", highPriority)); + REQUIRE(ik->addTask(ikTasks.comTask, "com_task", highPriority)); - REQUIRE(ikAndTasks.gravityTask->setDesiredGravityDirectionInTargetFrame( - desiredSetPoints.desiredBodyGravity)); + REQUIRE(parameterHandler->getGroup("REGULARIZATION_TASK") + .lock() + ->getParameter("weight", weightRegularization)); + REQUIRE(ik->addTask(ikTasks.regularizationTask, + "regularization_task", + lowPriority, + weightRegularization)); + REQUIRE(ik->addTask(ikTasks.jointLimitsTask, "joint_limits_task", highPriority)); + + const Eigen::VectorXd newWeight = 10 * weightRegularization; + auto newWeightProvider + = std::make_shared(newWeight); + REQUIRE(ik->setTaskWeight("regularization_task", newWeightProvider)); + + auto outWeightProvider = ik->getTaskWeightProvider("regularization_task").lock(); + REQUIRE(outWeightProvider); + REQUIRE(outWeightProvider->getOutput().isApprox(newWeight)); + + REQUIRE(ik->setTaskWeight("regularization_task", weightRegularization)); + + REQUIRE(ik->finalize(variablesHandler)); + + REQUIRE(ikTasks.se3Task->setSetPoint(desiredSetPoints.endEffectorPose, + manif::SE3d::Tangent::Zero())); + REQUIRE(ikTasks.comTask->setSetPoint(desiredSetPoints.CoMPosition, Eigen::Vector3d::Zero())); + REQUIRE(ikTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); // propagate the inverse kinematics for constexpr std::size_t iterations = 30; @@ -562,11 +562,11 @@ TEST_CASE("QP-IK [With strict limits]") gravity)); // solve the IK - REQUIRE(ikAndTasks.ik->advance()); + REQUIRE(ik->advance()); // get the output of the IK - baseVelocity = ikAndTasks.ik->getOutput().baseVelocity.coeffs(); - jointVelocity = ikAndTasks.ik->getOutput().jointVelocity; + baseVelocity = ik->getOutput().baseVelocity.coeffs(); + jointVelocity = ik->getOutput().jointVelocity; // given the joint constraint specified we check that the velocity is satisfied. This should // be zero @@ -584,7 +584,7 @@ TEST_CASE("QP-IK [With builder]") auto kinDyn = std::make_shared(); auto parameterHandler = createParameterHandler(); - constexpr double tolerance = 5e-1; + constexpr double tolerance = 2e-1; // set the velocity representation REQUIRE(kinDyn->setFrameVelocityRepresentation( @@ -596,7 +596,7 @@ TEST_CASE("QP-IK [With builder]") const iDynTree::Model model = iDynTree::getRandomModel(numberOfJoints); REQUIRE(kinDyn->loadRobotModel(model)); - /////// VariableHandler and IK params + // VariableHandler and IK params auto variablesParameterHandler = std::make_shared(); variablesParameterHandler->setParameter("variables_name", std::vector{robotVelocity}); @@ -729,3 +729,128 @@ TEST_CASE("QP-IK [With builder]") - desiredSetPoints.desiredBodyGravity; REQUIRE(gravityError.isZero(tolerance)); } + +TEST_CASE("QP-IK [Distance and Gravity tasks]") +{ + auto kinDyn = std::make_shared(); + auto parameterHandler = createParameterHandler(); + + constexpr double tolerance = 2e-2; + constexpr std::size_t highPriority = 0; + constexpr std::size_t lowPriority = 1; + + // set the velocity representation + REQUIRE(kinDyn->setFrameVelocityRepresentation( + iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)); + + constexpr std::size_t numberOfJoints = 30; + + // 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 + parameterHandler->getGroup("SE3_TASK") + .lock() + ->setParameter("frame_name", desiredSetPoints.endEffectorFrame); + + parameterHandler->getGroup("DISTANCE_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameDistance); + + parameterHandler->getGroup("GRAVITY_TASK") + .lock() + ->setParameter("target_frame_name", desiredSetPoints.targetFrameGravity); + + // create the IK + constexpr double jointLimitDelta = 0.5; + finalizeParameterHandler(parameterHandler, + kinDyn, + system, + Eigen::VectorXd::Constant(kinDyn->model().getNrOfDOFs(), + jointLimitDelta)); + auto ikTasks = createIKTasks(parameterHandler, kinDyn); + + Eigen::VectorXd weightRegularization; + + auto ik = std::make_shared(); + REQUIRE(ik->initialize(parameterHandler)); + + REQUIRE(parameterHandler->getGroup("REGULARIZATION_TASK") + .lock() + ->getParameter("weight", weightRegularization)); + REQUIRE(ik->addTask(ikTasks.regularizationTask, + "regularization_task", + lowPriority, + weightRegularization)); + REQUIRE(ik->addTask(ikTasks.distanceTask, "distance_task", highPriority)); + + REQUIRE(ik->addTask(ikTasks.gravityTask, "gravity_task", highPriority)); + + REQUIRE(ik->finalize(variablesHandler)); + + REQUIRE(ikTasks.regularizationTask->setSetPoint(desiredSetPoints.joints)); + + REQUIRE(ikTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); + + REQUIRE(ikTasks.gravityTask->setDesiredGravityDirectionInTargetFrame( + desiredSetPoints.desiredBodyGravity)); + + // propagate the inverse kinematics for + constexpr std::size_t iterations = 30; + Eigen::Vector3d gravity; + gravity << 0, 0, -9.81; + Eigen::Matrix4d baseTransform = Eigen::Matrix4d::Identity(); + Eigen::Matrix baseVelocity = Eigen::Matrix::Zero(); + Eigen::VectorXd jointVelocity = Eigen::VectorXd::Zero(model.getNrOfDOFs()); + + 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.rotation(); + baseTransform.topRightCorner<3, 1>() = basePosition; + REQUIRE(kinDyn->setRobotState(baseTransform, + jointPosition, + baseVelocity, + jointVelocity, + gravity)); + + // solve the IK + REQUIRE(ik->advance()); + + // get the output of the IK + baseVelocity = ik->getOutput().baseVelocity.coeffs(); + jointVelocity = ik->getOutput().jointVelocity; + + // propagate the dynamical system + system.dynamics->setControlInput({baseVelocity, jointVelocity}); + system.integrator->integrate(0s, dT); + } + + // Check the distance error + REQUIRE(toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameDistance)) + .translation() + .norm() + == Catch::Approx(desiredSetPoints.targetDistance).epsilon(tolerance)); + + // Check the gravity error + Eigen::Vector3d gravityError; + gravityError = toManifPose(kinDyn->getWorldTransform(desiredSetPoints.targetFrameGravity)) + .rotation() + .matrix() + .row(2) + .transpose() + - desiredSetPoints.desiredBodyGravity; + REQUIRE(gravityError.isZero(tolerance)); +} From 03ed81c436a730a1d2c27b7b4936fdcc2dfef4d1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 1 Aug 2023 17:11:48 +0200 Subject: [PATCH 10/17] Updated CHANGELOG. --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 01bbc468e3..9d10d7299c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,7 @@ All notable changes to this project are documented in this file. - Expose the CoM trajectory computed by the `CentroidalMPC` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/712) - Implement python bindings for `CameraBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/695) - Implement `constructRDGBSensorClient()` in `RobotInterface` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/695) +- Added the ``DistanceTask`` and ``GravityTask`` to the IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/717) ### Changed - Use `std::chorno::nanoseconds` in `clock` and `AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/702) From 82de935833dbe075e2b30ed936104f6837509832 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 10 Aug 2023 16:49:53 +0200 Subject: [PATCH 11/17] Specified the unit of measurements in the feedforward of gravity task --- src/IK/include/BipedalLocomotion/IK/GravityTask.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/IK/include/BipedalLocomotion/IK/GravityTask.h b/src/IK/include/BipedalLocomotion/IK/GravityTask.h index 4997476722..ea50f3c81c 100644 --- a/src/IK/include/BipedalLocomotion/IK/GravityTask.h +++ b/src/IK/include/BipedalLocomotion/IK/GravityTask.h @@ -180,7 +180,7 @@ class GravityTask : public IKLinearTask /** * @brief Set the feedforward angular velocity expressed in the target frame. - * @param feedforwardVelocity The desired feedforward velocity + * @param feedforwardVelocity The desired feedforward velocity in rad/s. * * Only the first two components are used. */ From 3521e6883820308d4701fa474cb18aa43709bf5b Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 26 Sep 2023 09:39:25 +0200 Subject: [PATCH 12/17] Renamed base_frame_name to target_frame_name in the distance task --- .../IK/tests/test_QP_inverse_kinematics.py | 2 +- .../BipedalLocomotion/IK/DistanceTask.h | 8 ++++---- src/IK/src/DistanceTask.cpp | 18 ++++++++++-------- src/IK/tests/DistanceTaskTest.cpp | 4 ++-- 4 files changed, 17 insertions(+), 15 deletions(-) diff --git a/bindings/python/IK/tests/test_QP_inverse_kinematics.py b/bindings/python/IK/tests/test_QP_inverse_kinematics.py index bd523a0598..16552954fe 100644 --- a/bindings/python/IK/tests/test_QP_inverse_kinematics.py +++ b/bindings/python/IK/tests/test_QP_inverse_kinematics.py @@ -221,7 +221,7 @@ def test_distance_task(): distance_task_param_handler = blf.parameters_handler.StdParametersHandler() distance_task_param_handler.set_parameter_string(name="robot_velocity_variable_name", value="robotVelocity") distance_task_param_handler.set_parameter_string(name="target_frame_name", value="chest") - distance_task_param_handler.set_parameter_string(name="base_frame_name", value="root_link") + distance_task_param_handler.set_parameter_string(name="reference_frame_name", value="root_link") distance_task_param_handler.set_parameter_float(name="kp",value=5.0) # Initialize the task diff --git a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h index 9fc1d683fc..dcef635955 100644 --- a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h +++ b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h @@ -56,10 +56,10 @@ class DistanceTask : public IKLinearTask double m_kp; /**< Controller gain. */ double m_desiredDistance{0.0}; /**< Desired distance. */ - std::string m_baseName; /**< Base frame name. */ + std::string m_referenceName; /**< Reference frame name. */ std::string m_targetFrameName; /**< Target frame name. */ - iDynTree::FrameIndex m_baseIndex, m_targetFrameIndex; /**< Base and target frame name indexes. - */ + iDynTree::FrameIndex m_referenceFrameIndex, m_targetFrameIndex; /**< Base and target frame name + indexes. */ double m_computedDistance{0.0}; /**< Computed distance. */ @@ -74,7 +74,7 @@ class DistanceTask : public IKLinearTask * | `robot_velocity_variable_name` | `string` | Name of the variable contained in `VariablesHandler` describing the robot velocity | Yes | * | `kp` | `double` | Gain of the distance controller | Yes | * | `target_frame_name` | `string` | Name of the frame of which computing the distance | Yes | - * | `base_frame_name` | `string` | Name of the frame with respect to which computing the distance. If empty, the world frame will be used (Default = "") | No | + * | `reference_frame_name` | `string` | Name of the frame with respect to which computing the distance. If empty, the world frame will be used (Default = "") | No | * @return True in case of success, false otherwise. */ // clang-format on diff --git a/src/IK/src/DistanceTask.cpp b/src/IK/src/DistanceTask.cpp index 4739313d3e..e9bc6ae1e0 100644 --- a/src/IK/src/DistanceTask.cpp +++ b/src/IK/src/DistanceTask.cpp @@ -120,7 +120,7 @@ bool DistanceTask::initialize( } // set the base frame name - if (!ptr->getParameter("base_frame_name", m_baseName)) + if (!ptr->getParameter("reference_frame_name", m_referenceName)) { log()->debug("{} [{}] No base_name specified. Using default \"\"", errorPrefix, @@ -136,16 +136,16 @@ bool DistanceTask::initialize( return false; } - if (m_baseName != "") + if (m_referenceName != "") { - m_baseIndex = m_kinDyn->getFrameIndex(m_baseName); + m_referenceFrameIndex = m_kinDyn->getFrameIndex(m_referenceName); - if (m_baseIndex == iDynTree::FRAME_INVALID_INDEX) + if (m_referenceFrameIndex == iDynTree::FRAME_INVALID_INDEX) { log()->error("{} [{}] The specified base name ({}) does not seem to exist.", errorPrefix, m_description, - m_baseName); + m_referenceName); return false; } } @@ -173,7 +173,7 @@ bool DistanceTask::update() m_isValid = false; - if (m_baseName == "") + if (m_referenceName == "") { m_framePosition = toEigen(m_kinDyn->getWorldTransform(m_targetFrameIndex).getPosition()); @@ -186,9 +186,11 @@ bool DistanceTask::update() } else { m_framePosition = toEigen( - m_kinDyn->getRelativeTransform(m_baseIndex, m_targetFrameIndex).getPosition()); + m_kinDyn->getRelativeTransform(m_referenceFrameIndex, m_targetFrameIndex).getPosition()); - if (!m_kinDyn->getRelativeJacobian(m_baseIndex, m_targetFrameIndex, m_relativeJacobian)) + if (!m_kinDyn->getRelativeJacobian(m_referenceFrameIndex, + m_targetFrameIndex, + m_relativeJacobian)) { log()->error("[DistanceTask::update] Unable to get the relative jacobian."); return m_isValid; diff --git a/src/IK/tests/DistanceTaskTest.cpp b/src/IK/tests/DistanceTaskTest.cpp index 662cd762f3..c44808917a 100644 --- a/src/IK/tests/DistanceTaskTest.cpp +++ b/src/IK/tests/DistanceTaskTest.cpp @@ -89,10 +89,10 @@ TEST_CASE("Distance task") baseName = iDynTree::getRandomLinkOfModel(model); } while (baseName == targetName); - parameterHandler->setParameter("base_frame_name", baseName); + parameterHandler->setParameter("reference_frame_name", baseName); } else { - parameterHandler->setParameter("base_frame_name", ""); + parameterHandler->setParameter("reference_frame_name", ""); } DistanceTask task; From 8daed97a1efe39da533011492de9ab542d7a15ce Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 26 Sep 2023 10:11:06 +0200 Subject: [PATCH 13/17] Added setSetPoint method to distance task --- bindings/python/IK/src/DistanceTask.cpp | 1 + .../python/IK/tests/test_QP_inverse_kinematics.py | 1 + src/IK/include/BipedalLocomotion/IK/DistanceTask.h | 13 ++++++++++++- src/IK/src/DistanceTask.cpp | 5 +++++ src/IK/tests/DistanceTaskTest.cpp | 2 +- 5 files changed, 20 insertions(+), 2 deletions(-) diff --git a/bindings/python/IK/src/DistanceTask.cpp b/bindings/python/IK/src/DistanceTask.cpp index dcf145f8d5..d26f3aaf66 100644 --- a/bindings/python/IK/src/DistanceTask.cpp +++ b/bindings/python/IK/src/DistanceTask.cpp @@ -30,6 +30,7 @@ void CreateDistanceTask(pybind11::module& module) py::class_, IKLinearTask>(module, "DistanceTask") .def(py::init()) .def("set_desired_distance", &DistanceTask::setDesiredDistance, py::arg("desired_distance")) + .def("set_set_point", &DistanceTask::setSetPoint, py::arg("desired_distance")) .def("get_distance", &DistanceTask::getDistance); } diff --git a/bindings/python/IK/tests/test_QP_inverse_kinematics.py b/bindings/python/IK/tests/test_QP_inverse_kinematics.py index 16552954fe..cd996a833e 100644 --- a/bindings/python/IK/tests/test_QP_inverse_kinematics.py +++ b/bindings/python/IK/tests/test_QP_inverse_kinematics.py @@ -234,6 +234,7 @@ def test_distance_task(): # Set desired distance assert distance_task.set_desired_distance(desired_distance=np.random.uniform(-0.5,0.5)) + assert distance_task.set_set_point(desired_distance=np.random.uniform(-0.5,0.5)) def test_gravity_task(): # create KinDynComputationsDescriptor diff --git a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h index dcef635955..8fa46866e2 100644 --- a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h +++ b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h @@ -108,13 +108,24 @@ class DistanceTask : public IKLinearTask bool update() override; /** - * @brief setDesiredDistance + * @brief setDesiredDistance Set the desired distance between the target and the reference frame * @param desiredDistance The desired distance expressed in meters * @note The desired distance is considered without sign. * @return True in case of success, false otherwise. */ bool setDesiredDistance(double desiredDistance); + /** + * @brief setSetPoint Set the desired distance between the target and the reference frame + * @param desiredDistance The desired distance expressed in meters + * + * It is equivalent to setDesiredDistance + * + * @note The desired distance is considered without sign. + * @return True in case of success, false otherwise. + */ + bool setSetPoint(double desiredDistance); + /** * @brief Get the computed distance between the target frame and either the world origin, or the * base origin. diff --git a/src/IK/src/DistanceTask.cpp b/src/IK/src/DistanceTask.cpp index e9bc6ae1e0..182237753b 100644 --- a/src/IK/src/DistanceTask.cpp +++ b/src/IK/src/DistanceTask.cpp @@ -217,6 +217,11 @@ bool DistanceTask::setDesiredDistance(double desiredDistance) return true; } +bool DistanceTask::setSetPoint(double desiredDistance) +{ + return setDesiredDistance(desiredDistance); +} + double DistanceTask::getDistance() const { return m_computedDistance; diff --git a/src/IK/tests/DistanceTaskTest.cpp b/src/IK/tests/DistanceTaskTest.cpp index c44808917a..0d12ae98cd 100644 --- a/src/IK/tests/DistanceTaskTest.cpp +++ b/src/IK/tests/DistanceTaskTest.cpp @@ -101,7 +101,7 @@ TEST_CASE("Distance task") REQUIRE(task.setVariablesHandler(variablesHandler)); double desiredDistance = 1.0; - REQUIRE(task.setDesiredDistance(desiredDistance)); + REQUIRE(task.setSetPoint(desiredDistance)); REQUIRE(task.update()); REQUIRE(task.isValid()); From 36af50f3b52379912bb2fd9ec4de96a37b0bc17b Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 26 Sep 2023 10:11:47 +0200 Subject: [PATCH 14/17] Added setSetPoint method to GravityTask --- bindings/python/IK/src/GravityTask.cpp | 6 +++++- .../IK/tests/test_QP_inverse_kinematics.py | 1 + .../BipedalLocomotion/IK/GravityTask.h | 20 ++++++++++++++++++- src/IK/src/GravityTask.cpp | 7 +++++++ src/IK/tests/GravityTaskTest.cpp | 4 ++-- 5 files changed, 34 insertions(+), 4 deletions(-) diff --git a/bindings/python/IK/src/GravityTask.cpp b/bindings/python/IK/src/GravityTask.cpp index 07cc3da6cc..e0e39cd2cf 100644 --- a/bindings/python/IK/src/GravityTask.cpp +++ b/bindings/python/IK/src/GravityTask.cpp @@ -34,7 +34,11 @@ void CreateGravityTask(pybind11::module& module) py::arg("desired_gravity_direction")) .def("set_feedforward_velocity_in_target_frame", &GravityTask::setFeedForwardVelocityInTargetFrame, - py::arg("feedforward_velocity")); + py::arg("feedforward_velocity")) + .def("set_set_point", + &GravityTask::setSetPoint, + py::arg("desired_gravity_direction"), + py::arg("feedforward_velocity") = Eigen::Vector3d::Zero()); } } // namespace IK diff --git a/bindings/python/IK/tests/test_QP_inverse_kinematics.py b/bindings/python/IK/tests/test_QP_inverse_kinematics.py index cd996a833e..74c267cef0 100644 --- a/bindings/python/IK/tests/test_QP_inverse_kinematics.py +++ b/bindings/python/IK/tests/test_QP_inverse_kinematics.py @@ -258,6 +258,7 @@ def test_gravity_task(): # Set desired distance assert gravity_task.set_desired_gravity_direction_in_target_frame(desired_gravity_direction=[np.random.uniform(-0.5,0.5) for _ in range(3)]) assert gravity_task.set_feedforward_velocity_in_target_frame(feedforward_velocity=[np.random.uniform(-0.5,0.5) for _ in range(3)]) + assert gravity_task.set_set_point(desired_gravity_direction=[np.random.uniform(-0.5,0.5) for _ in range(3)]) def test_integration_based_ik_state(): diff --git a/src/IK/include/BipedalLocomotion/IK/GravityTask.h b/src/IK/include/BipedalLocomotion/IK/GravityTask.h index ea50f3c81c..44604d8166 100644 --- a/src/IK/include/BipedalLocomotion/IK/GravityTask.h +++ b/src/IK/include/BipedalLocomotion/IK/GravityTask.h @@ -172,6 +172,7 @@ class GravityTask : public IKLinearTask /** * @brief Set the desired gravity direction expressed the target frame. * @param desiredGravityDirection The desired gravity direction. + * @return True in case of success, false otherwise. * * The input is normalized, unless the norm is too small. */ @@ -181,12 +182,29 @@ class GravityTask : public IKLinearTask /** * @brief Set the feedforward angular velocity expressed in the target frame. * @param feedforwardVelocity The desired feedforward velocity in rad/s. - * + * @return True in case of success, false otherwise. * Only the first two components are used. */ bool setFeedForwardVelocityInTargetFrame(const Eigen::Ref feedforwardVelocity); + /** + * @brief Set the desired gravity direction expressed the target frame and the feedforward + * velocity. + * @param desiredGravityDirection The desired gravity direction in target frame. + * @param feedforwardVelocity The desired feedforward velocity in rad/s expressed in the target + * frame. + * @return True in case of success, false otherwise. + * + * The desiredGravityDirection is normalized, unless the norm is too small. + * Only the first two components of the feedforwardVelocity are used. + * This is equivalent of using setDesiredGravityDirectionInTargetFrame and + * setFeedForwardVelocityInTargetFrame + */ + bool setSetPoint(const Eigen::Ref desiredGravityDirection, + const Eigen::Ref feedforwardVelocity + = Eigen::Vector3d::Zero()); + /** * Get the size of the task. (I.e the number of rows of the vector b) * @return the size of the task. diff --git a/src/IK/src/GravityTask.cpp b/src/IK/src/GravityTask.cpp index ccfd446c8c..fd4aee826d 100644 --- a/src/IK/src/GravityTask.cpp +++ b/src/IK/src/GravityTask.cpp @@ -185,6 +185,13 @@ bool GravityTask::setFeedForwardVelocityInTargetFrame( return true; } +bool GravityTask::setSetPoint(const Eigen::Ref desiredGravityDirection, + const Eigen::Ref feedforwardVelocity) +{ + return setDesiredGravityDirectionInTargetFrame(desiredGravityDirection) + && setFeedForwardVelocityInTargetFrame(feedforwardVelocity); +} + std::size_t GravityTask::size() const { return m_DoFs; diff --git a/src/IK/tests/GravityTaskTest.cpp b/src/IK/tests/GravityTaskTest.cpp index 34ae776995..38837130e5 100644 --- a/src/IK/tests/GravityTaskTest.cpp +++ b/src/IK/tests/GravityTaskTest.cpp @@ -86,9 +86,9 @@ TEST_CASE("Distance task") Eigen::Vector3d desiredDirection({1.0, 2.0, 3.0}); desiredDirection.normalize(); - REQUIRE(task.setDesiredGravityDirectionInTargetFrame(desiredDirection)); Eigen::Vector3d feedforward({0.1, 0.2, 0.3}); - REQUIRE(task.setFeedForwardVelocityInTargetFrame(feedforward)); + + REQUIRE(task.setSetPoint(desiredDirection, feedforward)); REQUIRE(task.update()); REQUIRE(task.isValid()); From 972ab0496fded0679dda369387ea199cd4e8878a Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 26 Sep 2023 10:13:27 +0200 Subject: [PATCH 15/17] Updated Changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9d10d7299c..9f56367440 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,7 @@ All notable changes to this project are documented in this file. ## [Unreleased] +- Added the ``DistanceTask`` and ``GravityTask`` to the IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/717) ### Added - Add the possibility to control a subset of coordinates in `TSID::CoMTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/724, https://github.com/ami-iit/bipedal-locomotion-framework/pull/727) @@ -21,7 +22,6 @@ All notable changes to this project are documented in this file. - Expose the CoM trajectory computed by the `CentroidalMPC` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/712) - Implement python bindings for `CameraBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/695) - Implement `constructRDGBSensorClient()` in `RobotInterface` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/695) -- Added the ``DistanceTask`` and ``GravityTask`` to the IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/717) ### Changed - Use `std::chorno::nanoseconds` in `clock` and `AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/702) From 88eb49a3e1d47dcfc9fb66d29149ea28e23999a3 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 27 Sep 2023 14:06:57 +0200 Subject: [PATCH 16/17] Added suggestions to improve the distance and gravity task computations Co-authored-by: Giulio Romualdi --- src/IK/src/DistanceTask.cpp | 5 ++--- src/IK/src/GravityTask.cpp | 3 ++- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/IK/src/DistanceTask.cpp b/src/IK/src/DistanceTask.cpp index 182237753b..487a782247 100644 --- a/src/IK/src/DistanceTask.cpp +++ b/src/IK/src/DistanceTask.cpp @@ -199,10 +199,9 @@ bool DistanceTask::update() m_jacobian.rightCols(m_kinDyn->getNrOfDegreesOfFreedom()) = m_relativeJacobian; } - m_computedDistance = sqrt(pow(m_framePosition(0), 2) + pow(m_framePosition(1), 2) - + pow(m_framePosition(2), 2)); + m_computedDistance = m_framePosition.norm(); - toEigen(this->subA(m_robotVelocityVariable)) + toEigen(this->subA(m_robotVelocityVariable)).noalias() = (m_framePosition.transpose() * m_jacobian.topRows<3>()) / (std::max(0.001, m_computedDistance)); m_b(0) = m_kp * (m_desiredDistance - m_computedDistance); diff --git a/src/IK/src/GravityTask.cpp b/src/IK/src/GravityTask.cpp index fd4aee826d..7de70b466f 100644 --- a/src/IK/src/GravityTask.cpp +++ b/src/IK/src/GravityTask.cpp @@ -164,7 +164,8 @@ bool GravityTask::update() } toEigen(this->subA(m_robotVelocityVariable)) = m_Am * m_jacobian.bottomRows<3>(); - m_b = -m_kp * m_bm * desiredZDirectionAbsolute + feedforwardAbsolute.topRows<2>(); + m_b = feedforwardAbsolute.topRows<2>(); + m_b.noalias() -= m_kp * m_bm * desiredZDirectionAbsolute; m_isValid = true; return m_isValid; From c47e5416e7393e9e2d7bb94cf93b688dbcd6f03a Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 27 Sep 2023 14:39:02 +0200 Subject: [PATCH 17/17] Moved a magic number to input parameter in DistanceTask --- src/IK/include/BipedalLocomotion/IK/DistanceTask.h | 3 +++ src/IK/src/DistanceTask.cpp | 11 ++++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h index 8fa46866e2..068e57f101 100644 --- a/src/IK/include/BipedalLocomotion/IK/DistanceTask.h +++ b/src/IK/include/BipedalLocomotion/IK/DistanceTask.h @@ -62,6 +62,8 @@ class DistanceTask : public IKLinearTask indexes. */ double m_computedDistance{0.0}; /**< Computed distance. */ + double m_distanceNumericThreshold{0.001}; /**< Numeric threshold when inverting the computed + distance in the Jacobian computation. */ public: // clang-format off @@ -75,6 +77,7 @@ class DistanceTask : public IKLinearTask * | `kp` | `double` | Gain of the distance controller | Yes | * | `target_frame_name` | `string` | Name of the frame of which computing the distance | Yes | * | `reference_frame_name` | `string` | Name of the frame with respect to which computing the distance. If empty, the world frame will be used (Default = "") | No | + * | `distance_numeric_threshold` | `double` | Lowerbound for the computed distance when inverting it in the task Jacobian (Default = 0.001) | No | * @return True in case of success, false otherwise. */ // clang-format on diff --git a/src/IK/src/DistanceTask.cpp b/src/IK/src/DistanceTask.cpp index 487a782247..67c6eb3fa2 100644 --- a/src/IK/src/DistanceTask.cpp +++ b/src/IK/src/DistanceTask.cpp @@ -136,6 +136,15 @@ bool DistanceTask::initialize( return false; } + // set the gains for the controllers + if (!ptr->getParameter("distance_numeric_threshold", m_distanceNumericThreshold)) + { + log()->debug("{} [{}] No distance_numeric_threshold specified. Using default {}", + errorPrefix, + m_description, + m_distanceNumericThreshold); + } + if (m_referenceName != "") { m_referenceFrameIndex = m_kinDyn->getFrameIndex(m_referenceName); @@ -203,7 +212,7 @@ bool DistanceTask::update() toEigen(this->subA(m_robotVelocityVariable)).noalias() = (m_framePosition.transpose() * m_jacobian.topRows<3>()) - / (std::max(0.001, m_computedDistance)); + / (std::max(m_distanceNumericThreshold, m_computedDistance)); m_b(0) = m_kp * (m_desiredDistance - m_computedDistance); m_isValid = true;