Implement FeasibleContactWrenchTask for TSID component

CHANGELOG.md

@@ -9,6 +9,7 @@ All notable changes to this project are documented in this file.
 - Implement `skew` function in Math component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/352)
 - Implement `QPTSID` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/366)
 - Implement motor pwm, motor encoders, wbd joint torque estimates, pid reading in `YarpSensorBridge`(https://github.com/dic-iit/bipedal-locomotion-framework/pull/359).
+- Implement FeasibleContactWrenchTask for TSID component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/369).
 
 ### Changed
 - Add common Python files to gitignore (https://github.com/dic-iit/bipedal-locomotion-framework/pull/338)

src/Math/include/BipedalLocomotion/Math/ContactWrenchCone.h

@@ -78,7 +78,7 @@ class ContactWrenchCone
      * |        `foot_limits_y`        | `vector<double>` | y coordinate of the foot limits w.r.t the frame attached to the surface |    Yes    |
      * @return true in case of success/false otherwise.
      */
-    bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler);
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler);
 
     /**
      * Get the matrix A.

src/Math/include/BipedalLocomotion/Math/LinearizedFrictionCone.h

@@ -49,7 +49,7 @@ class LinearizedFrictionCone
      * | `static_friction_coefficient`   | `double`  |        Static friction coefficient.     |
      * @return true in case of success/false otherwise.
      */
-    bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler);
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler);
 
     /**
      * Get the matrix A.

src/Math/src/ContactWrenchCone.cpp

@@ -17,7 +17,7 @@
 
 using namespace BipedalLocomotion::Math;
 
-bool ContactWrenchCone::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler)
+bool ContactWrenchCone::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler)
 {
     constexpr auto errorPrefix = "[ContactWrenchCone::initialize]";
     constexpr int wrenchSize = Wrench<double>::SizeAtCompileTime;

src/Math/src/LinearizedFrictionCone.cpp

@@ -12,7 +12,7 @@
 
 using namespace BipedalLocomotion::Math;
 
-bool LinearizedFrictionCone::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler)
+bool LinearizedFrictionCone::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler)
 {
     constexpr auto errorPrefix = "[LinearizedFrictionCone::initialize]";
 

src/TSID/CMakeLists.txt

@@ -11,9 +11,11 @@ if(FRAMEWORK_COMPILE_TSID)
     PUBLIC_HEADERS         ${H_PREFIX}/TSIDLinearTask.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/CoMTask.h
                            ${H_PREFIX}/BaseDynamicsTask.h ${H_PREFIX}/JointDynamicsTask.h
                            ${H_PREFIX}/TaskSpaceInverseDynamics.h
+                           ${H_PREFIX}/FeasibleContactWrenchTask.h
                            ${H_PREFIX}/QPFixedBaseTSID.h ${H_PREFIX}/QPTSID.h
     SOURCES                src/SO3Task.cpp src/SE3Task.cpp src/JointTrackingTask.cpp src/CoMTask.cpp
                            src/BaseDynamicsTask.cpp src/JointDynamicsTask.cpp
+                           src/FeasibleContactWrenchTask.cpp
                            src/QPFixedBaseTSID.cpp src/QPTSID.cpp
     PUBLIC_LINK_LIBRARIES  Eigen3::Eigen
                            BipedalLocomotion::Contacts

src/TSID/include/BipedalLocomotion/TSID/FeasibleContactWrenchTask.h

@@ -0,0 +1,125 @@
+/**
+ * @file FeasibleContactWrenchTask.h
+ * @authors Giulio Romualdi
+ * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_TSID_FEASIBLE_CONTACT_WRENCH_TASK_H
+#define BIPEDAL_LOCOMOTION_TSID_FEASIBLE_CONTACT_WRENCH_TASK_H
+
+#include <memory>
+
+#include <manif/manif.h>
+
+#include <BipedalLocomotion/TSID/TSIDLinearTask.h>
+#include <BipedalLocomotion/Math/ContactWrenchCone.h>
+
+
+#include <iDynTree/KinDynComputations.h>
+
+
+namespace BipedalLocomotion
+{
+namespace TSID
+{
+
+/**
+ * FeasibleContactWrenchTask is a concrete implementation of the TSIDLinearTask. Please use this
+ * element if you want to ensure that the contact wrench satisfies the constraints implemented in
+ * BipedalLocomotion::Math::ContactWrenchCone. Differently from the
+ * BipedalLocomotion::Math::ContactWrenchCone class, FeasibleContactWrenchTask requires also the
+ * positivstellensatz of the normal force written in local coordinate.
+ */
+class FeasibleContactWrenchTask : public TSIDLinearTask
+{
+    struct ContactWrench
+    {
+        iDynTree::FrameIndex frameIndex; /**< Frame used to express the contact wrench */
+        System::VariablesHandler::VariableDescription variable; /**< Variable describing the contact
+                                                                   wrench */
+        Eigen::Matrix3d frame_R_inertial;
+    };
+
+    ContactWrench m_contactWrench;
+
+    BipedalLocomotion::Math::ContactWrenchCone m_cone;
+
+    bool m_isInitialized{false}; /**< True if the task has been initialized. */
+    bool m_isValid{false}; /**< True if the task is valid. */
+
+    Eigen::MatrixXd m_AinBodyCoordinate; /**< Matrix A written in body frame */
+
+    std::shared_ptr<iDynTree::KinDynComputations> m_kinDyn; /**< Pointer to a KinDynComputations
+                                                               object */
+public:
+
+    /**
+     * Initialize the task.
+     * @param handler pointer to the parameter handler.
+     * @note The following parameters are required:
+     * |         Parameter Name        |       Type       |                                  Description                                  | Mandatory |
+     * |:-----------------------------:|:----------------:|:-----------------------------------------------------------------------------:|:---------:|
+     * |        `frame_name`           |      `string`    |                  Name of the frame associated to the contact                  |    Yes    |
+     * |       `variable_name`         |      `string`    | Name of the variable contained in `VariablesHandler` describing the contact   |    Yes    |
+     * |       `number_of_slices`      |       `int`      |                     Number of slices used to split 90 deg.                    |    Yes    |
+     * | `static_friction_coefficient` |      `double`    |                          Static friction coefficient.                         |    Yes    |
+     * |        `foot_limits_x`        | `vector<double>` |     x coordinate of the foot limits w.r.t the frame attached to the surface   |    Yes    |
+     * |        `foot_limits_y`        | `vector<double>` |     y coordinate of the foot limits w.r.t the frame attached to the surface   |    Yes    |
+     * @return true in case of success/false otherwise.
+     */
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler);
+
+    /**
+     * Set the kinDynComputations object.
+     * @param kinDyn pointer to a kinDynComputations object.
+     * @return True in case of success, false otherwise.
+     */
+    bool setKinDyn(std::shared_ptr<iDynTree::KinDynComputations> kinDyn);
+
+    /**
+     * Set the set of variables required by the task. The variables are stored in the
+     * System::VariablesHandler.
+     * @param variablesHandler reference to a variables handler.
+     * @note The handler must contain a variable named as the parameter
+     * `variable_name` stored in the parameter handler. The variable represents the contact wrench
+     * acting of the robot.
+     * @return True in case of success, false otherwise.
+     */
+    bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override;
+
+    /**
+     * Update the content of the element.
+     * @return True in case of success, false otherwise.
+     */
+    bool update() override;
+
+    /**
+     * Set if the contact is active or not.
+     * @param isActive true if the contact is active, false otherwise.
+     */
+    void setContactActive(bool isActive);
+
+    /**
+     * Get the size of the task. (I.e the number of rows of the vector b)
+     * @return the size of the task.
+     */
+    std::size_t size() const override;
+
+    /**
+     * The CoMTask is an equality task.
+     * @return the size of the task.
+     */
+    Type type() const override;
+
+    /**
+     * Determines the validity of the objects retrieved with getA() and getB()
+     * @return True if the objects are valid, false otherwise.
+     */
+    bool isValid() const override;
+};
+
+} // namespace TSID
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_TSID_FEASIBLE_CONTACT_WRENCH_TASK_H

src/TSID/src/FeasibleContactWrenchTask.cpp

@@ -0,0 +1,216 @@
+/**
+ * @file FeasibleContactWrenchTask.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include <BipedalLocomotion/Math/Wrench.h>
+#include <BipedalLocomotion/TSID/FeasibleContactWrenchTask.h>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+
+#include <iDynTree/Core/EigenHelpers.h>
+#include <iDynTree/Model/Model.h>
+
+using namespace BipedalLocomotion::ParametersHandler;
+using namespace BipedalLocomotion::TSID;
+
+bool FeasibleContactWrenchTask::setKinDyn(std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
+{
+    if ((kinDyn == nullptr) || (!kinDyn->isValid()))
+    {
+        log()->error("[FeasibleContactWrenchTask::setKinDyn] Invalid kinDyn object.");
+        return false;
+    }
+
+    m_kinDyn = kinDyn;
+    return true;
+}
+
+bool FeasibleContactWrenchTask::setVariablesHandler(const System::VariablesHandler& variablesHandler)
+{
+    constexpr auto errorPrefix = "[FeasibleContactWrenchTask::setVariablesHandler]";
+
+    if (!m_isInitialized)
+    {
+        log()->error("{} The task is not initialized. Please call initialize method.", errorPrefix);
+        return false;
+    }
+
+    if (!variablesHandler.getVariable(m_contactWrench.variable.name, m_contactWrench.variable))
+    {
+        log()->error("{} Error while retrieving the contact variable named {}.",
+                     errorPrefix,
+                     m_contactWrench.variable.name);
+        return false;
+    }
+
+    if (m_contactWrench.variable.size != BipedalLocomotion::Math::Wrenchd::SizeAtCompileTime)
+    {
+        log()->error("{} The variable size associated to the contact named {} is different "
+                     "from {}.",
+                     errorPrefix,
+                     m_contactWrench.variable.size,
+                     BipedalLocomotion::Math::Wrenchd::SizeAtCompileTime);
+        return false;
+    }
+
+    // the additional constrains refer to the normal force
+    constexpr std::size_t normalForceFeasibilityConstraints = 2;
+    const auto rowsOfConeMatrix = m_cone.getA().rows();
+
+    // resize the matrices
+    m_A.resize(normalForceFeasibilityConstraints + rowsOfConeMatrix,
+               variablesHandler.getNumberOfVariables());
+    m_A.setZero();
+    m_b.resize(normalForceFeasibilityConstraints + rowsOfConeMatrix);
+
+    // the vector b will contains the cone vector plus the constraint related to the contact normal
+    // force
+    // 0 <= fz <= max_normal_force
+    // [0 0 -1 0 0 0 ] * [fx fy fz taux tauy tauz]' <= 0
+    // [0 0 1 0 0 0 ] * [fx fy fz taux tauy tauz]' <= max_normal_force
+    // if the task is enabled max_normal_force is equal to the max double. If the task is disabled
+    // max_normal_force is equal to 0
+    m_b.head(rowsOfConeMatrix) = m_cone.getB();
+    m_b.tail<2>()(0) = 0;
+    m_b.tail<2>()(1) = std::numeric_limits<double>::max();
+
+    // the matrix A in body coordinate is fixed
+    m_AinBodyCoordinate.resize(normalForceFeasibilityConstraints + rowsOfConeMatrix,
+                               BipedalLocomotion::Math::Wrenchd::SizeAtCompileTime);
+    m_AinBodyCoordinate.topRows(rowsOfConeMatrix) = m_cone.getA();
+    m_AinBodyCoordinate.bottomRows<2>() << 0, 0, -1, 0, 0, 0,
+                                           0, 0,  1, 0, 0, 0;
+
+    return true;
+}
+
+bool FeasibleContactWrenchTask::initialize(std::weak_ptr<const IParametersHandler> paramHandler)
+{
+    constexpr auto errorPrefix = "[FeasibleContactWrenchTask::initialize]";
+
+    if (m_kinDyn == nullptr || !m_kinDyn->isValid())
+    {
+        log()->error("{} KinDynComputations object is not valid.", errorPrefix);
+        return false;
+    }
+
+    if (m_kinDyn->getFrameVelocityRepresentation()
+        != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)
+    {
+        log()->error("{} The task supports only quantities expressed in MIXED representation. "
+                     "Please provide a KinDynComputations with Frame velocity representation set "
+                     "to MIXED_REPRESENTATION.",
+                     errorPrefix);
+        return false;
+    }
+
+    auto ptr = paramHandler.lock();
+    if (ptr == nullptr)
+    {
+        log()->error("{} The parameter handler is not valid.", errorPrefix);
+        return false;
+    }
+
+    std::string frameName;
+    if (!ptr->getParameter("frame_name", frameName)
+        || (m_contactWrench.frameIndex = m_kinDyn->model().getFrameIndex(frameName))
+               == iDynTree::FRAME_INVALID_INDEX)
+    {
+        log()->error("{} Error while retrieving the frame associated to the contact.", errorPrefix);
+        return false;
+    }
+
+    if (!ptr->getParameter("variable_name", m_contactWrench.variable.name))
+    {
+        log()->error("{} Error while retrieving the variable name associated to the contact.",
+                     errorPrefix);
+        return false;
+    }
+
+    // initialize the cone
+    if (!m_cone.initialize(paramHandler))
+    {
+        log()->error("{} Unable to initialize the cone.", errorPrefix);
+        return false;
+    }
+
+    // set the description
+    m_description = "Feasible contact wrench - Frame: " + frameName + ".";
+
+    m_isInitialized = true;
+
+    return true;
+}
+
+bool FeasibleContactWrenchTask::update()
+{
+    constexpr auto errorPrefix = "[FeasibleContactWrenchTask::update]";
+
+    m_isValid = false;
+
+    if (m_kinDyn == nullptr)
+    {
+        log()->error("{} KinDynComputations object is not valid. Please call setKinDyn().",
+                     errorPrefix);
+        return false;
+    }
+
+    if (!m_contactWrench.variable.isValid())
+    {
+        log()->error("{} The contact wrench variable is not valid. Please call "
+                     "setVariablesHandler().",
+                     errorPrefix);
+        return false;
+    }
+
+    if (!m_isInitialized)
+    {
+        log()->error("{} The task is not initialized. Please call initialize().", errorPrefix);
+        return false;
+    }
+
+    using namespace iDynTree;
+
+    m_contactWrench.frame_R_inertial
+        = toEigen(m_kinDyn->getWorldTransform(m_contactWrench.frameIndex).getRotation().inverse());
+
+    toEigen(this->subA(m_contactWrench.variable)).leftCols<3>().noalias()
+        = m_AinBodyCoordinate.leftCols<3>() * m_contactWrench.frame_R_inertial;
+    toEigen(this->subA(m_contactWrench.variable)).rightCols<3>().noalias()
+        = m_AinBodyCoordinate.rightCols<3>() * m_contactWrench.frame_R_inertial;
+
+    m_isValid = true;
+
+    // no need to update b
+    return m_isValid;
+}
+
+void FeasibleContactWrenchTask::setContactActive(bool isActive)
+{
+    if (isActive)
+    {
+        // the last element of the vector b can be used to disable / enable the task
+        m_b.tail<1>()(0) = std::numeric_limits<double>::max();
+    } else
+    {
+        // the last element of the vector b can be used to disable / enable the task
+        m_b.tail<1>()(0) = 0;
+    }
+}
+
+std::size_t FeasibleContactWrenchTask::size() const
+{
+    return m_A.rows();
+}
+
+FeasibleContactWrenchTask::Type FeasibleContactWrenchTask::type() const
+{
+    return Type::inequality;
+}
+
+bool FeasibleContactWrenchTask::isValid() const
+{
+    return m_isValid;
+}