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,132 @@
+/**
+ * @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.
+ * @note FeasibleContactWrenchTask::disable will force the wrench associated to the task to be equal
+ * to a null wrench. FeasibleContactWrenchTask::enable will allow the wrench to be different from
+ * zero.
+ */
+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;
+
+    /**
+     * Enable the task. I.e. a contact wrench different from zero is allowed.
+     */
+    void enable();
+
+    /**
+     * Disable the task. I.e. the contact wrench is forced to be equal to zero.
+     */
+    void disable();
+
+    /**
+     * 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