From 0a98afd145fc97ac2488b218b74b289ae6f1a5e3 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Sun, 12 Jan 2025 17:39:54 +0100
Subject: [PATCH 01/20] added 'VariableLinearTask.h and VariableLinearTask.cpp'

---
 .../TSID/VariableLinearTask.h                 |  88 ++++++++++++++
 src/TSID/src/VariableLinearTask.cpp           | 114 ++++++++++++++++++
 2 files changed, 202 insertions(+)
 create mode 100644 src/TSID/include/BipedalLocomotion/TSID/VariableLinearTask.h
 create mode 100644 src/TSID/src/VariableLinearTask.cpp

diff --git a/src/TSID/include/BipedalLocomotion/TSID/VariableLinearTask.h b/src/TSID/include/BipedalLocomotion/TSID/VariableLinearTask.h
new file mode 100644
index 0000000000..5966945009
--- /dev/null
+++ b/src/TSID/include/BipedalLocomotion/TSID/VariableLinearTask.h
@@ -0,0 +1,88 @@
+/**
+ * @file VariableLinearTask.h
+ * @authors Roberto Mauceri
+ * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_TSID_VARIABLE_LINEAR_TASK_H
+#define BIPEDAL_LOCOMOTION_TSID_VARIABLE_LINEAR_TASK_H
+
+#include <BipedalLocomotion/TSID/TSIDLinearTask.h>
+
+namespace BipedalLocomotion
+{
+namespace TSID
+{
+/**
+ * VariableLinearTask is a concrete implementation of the Task. Please use this element if
+ * you want to create a inequality linear in the variable. The task represents the following equation
+ * \f[
+ * Ax < b
+ * \f]
+ * where \f$x\f$ are the elements of the variable you want to constraint.
+ */    
+
+class VariableLinearTask : public TSIDLinearTask
+{
+    std::string m_variableName; /**< Name of the variable considered that will be regularized */
+    std::vector<std::string> m_controlledElements; /**< Name of the variable elements considered in
+                                                     the task */
+    bool m_isInitialized{false}; /**< True if the task has been initialized. */
+    bool m_isValid{false}; /**< True if the task is valid. */
+    std::size_t m_variableSize{0}; /**< Size of the regularized variable. */
+
+public:
+    /**
+     * Initialize the planner.
+     * @param paramHandler pointer to the parameters handler.
+     * @param variablesHandler reference to a variables handler.
+     * @note the following parameters are required by the class
+     * |  Parameter Name  |   Type   |                                   Description                                          | Mandatory |
+     * |:----------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:|
+     * | `variable_name`  | `string` | Name of the variable that you want to regularize. |    Yes    |
+     * | `variable_size`  |   `int`  | Number of the elements that will be regularized. |    Yes     |
+     * | `elements_name`  | `vector` | Name of the elements to consider. If not specified all the elements are constrained |    No     |
+     * @return True in case of success, false otherwise.
+     */
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+
+    /**
+     * Set the set of variables required by the TSIDLinearTask. The variables are stored in the
+     * System::VariablesHandler.
+     * @param variablesHandler reference to a variables handler.
+     * @note The variablesHandler must contain a variable named as the parameter `variable_name`.
+     * @return True in case of success, false otherwise.
+     */
+    bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override;
+
+    /**
+     * Set the upper bound.
+     * @param upperBound vector containing the upper bounds for the variable elements.
+     * @return True in case of success, false otherwise.
+     */
+    bool setUpperBound(Eigen::Ref<const Eigen::VectorXd> setPoint);
+
+    /**
+     * 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 VariableLinearTask is an inequality task.
+     * @return the type 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_VARIABLE_LINEAR_TASK_H
\ No newline at end of file
diff --git a/src/TSID/src/VariableLinearTask.cpp b/src/TSID/src/VariableLinearTask.cpp
new file mode 100644
index 0000000000..3bb3f633e5
--- /dev/null
+++ b/src/TSID/src/VariableLinearTask.cpp
@@ -0,0 +1,114 @@
+/**
+ * @file VariableLinearTask.cpp
+ * @authors Roberto Mauceri
+ * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <BipedalLocomotion/TSID/VariableLinearTask.h>
+#include <BipedalLocomotion/System/VariablesHandler.h>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+
+#include <iDynTree/EigenHelpers.h>
+
+using namespace BipedalLocomotion::ParametersHandler;
+using namespace BipedalLocomotion::TSID;
+using namespace BipedalLocomotion::System;
+using namespace BipedalLocomotion;
+
+bool VariableLinearTask::setVariablesHandler(const VariablesHandler& variablesHandler)
+{
+    constexpr auto errorPrefix = "[VariableLinearTask::setVariablesHandler]";
+
+    System::VariablesHandler::VariableDescription variable;
+
+    if (!m_isInitialized)
+    {
+        log()->error("{} The task is not initialized. Please call initialize method.", errorPrefix);
+        return false;
+    }
+
+    if (!variablesHandler.getVariable(m_variableName, variable))
+    {
+        log()->error("{} Error while retrieving the variable named {}.",
+                     errorPrefix,
+                     m_variableName);
+        return false;
+    }
+
+    if (m_variableSize > variable.size)
+    {
+        log()->error("{} The expect size is greater than the one stored in the variable named {}. "
+                     "This task is used to regularize a subset of elements of a variable.",
+                     errorPrefix,
+                     m_variableName);
+        return false;
+    }
+
+    // resize the matrices
+    m_S.resize(2 * m_variableSize, variablesHandler.getNumberOfVariables());
+    m_S.setZero();
+    m_b.resize(2 * m_variableSize);
+
+    // S (selection matrix) is constant
+    if (m_controlledElements.size() != 0)
+    {
+        for (int i = 0; i < m_controlledElements.size(); i++)
+        {
+            const auto& element = m_controlledElements[i];
+            auto index = variable.getElementIndex(m_controlledElements[i]);
+            if (index < 0)
+            {
+                log()->error("{} Unable to find the element named {} in the variable {}",
+                             errorPrefix,
+                             element,
+                             m_variableName);
+                return false;
+            }
+
+            m_S(i, index) = 1;
+        }
+    } else
+    {
+        if (m_variableSize != variable.size)
+        {
+            log()->error("{} The size of the variable named {} is different from expected. "
+                         "Expected: {}, Passed: {}.",
+                         errorPrefix,
+                         m_variableName,
+                         m_variableSize,
+                         variable.size);
+            return false;
+        }
+        // if the size of the m_controlledElements vector is zero, this means that the entire
+        // variable is regularized
+        iDynTree::toEigen(this->subA(variable)).setIdentity();
+    }
+
+    return true;
+}
+
+bool VariableLinearTask::initialize(std::weak_ptr<const IParametersHandler> paramHandler)
+{
+
+}
+
+bool VariableLinearTask::setUpperBound(Eigen::Ref<const Eigen::VectorXd> setPoint)
+{
+
+}
+
+std::size_t VariableLinearTask::size() const
+{
+    return m_b.size();
+}
+
+VariableLinearTask::Type VariableLinearTask::type() const
+{
+    return Type::inequality; // By default, the "<" operator is considered
+}
+
+bool VariableLinearTask::isValid() const
+{
+    return m_isValid;
+}

From 512d78edbdff6394e32f477cf9d532a1263f07d0 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Sun, 12 Jan 2025 18:57:31 +0100
Subject: [PATCH 02/20] changed the task name from VariableLinearTask to
 SPUTorqueLimitTask

---
 ...iableLinearTask.h => SPUTorqueLimitTask.h} |  8 ++--
 ...eLinearTask.cpp => SPUTorqueLimitTask.cpp} | 40 +++++++++++++------
 2 files changed, 31 insertions(+), 17 deletions(-)
 rename src/TSID/include/BipedalLocomotion/TSID/{VariableLinearTask.h => SPUTorqueLimitTask.h} (94%)
 rename src/TSID/src/{VariableLinearTask.cpp => SPUTorqueLimitTask.cpp} (76%)

diff --git a/src/TSID/include/BipedalLocomotion/TSID/VariableLinearTask.h b/src/TSID/include/BipedalLocomotion/TSID/SPUTorqueLimitTask.h
similarity index 94%
rename from src/TSID/include/BipedalLocomotion/TSID/VariableLinearTask.h
rename to src/TSID/include/BipedalLocomotion/TSID/SPUTorqueLimitTask.h
index 5966945009..895d440965 100644
--- a/src/TSID/include/BipedalLocomotion/TSID/VariableLinearTask.h
+++ b/src/TSID/include/BipedalLocomotion/TSID/SPUTorqueLimitTask.h
@@ -1,5 +1,5 @@
 /**
- * @file VariableLinearTask.h
+ * @file SPUTorqueLimitTask.h
  * @authors Roberto Mauceri
  * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
@@ -15,7 +15,7 @@ namespace BipedalLocomotion
 namespace TSID
 {
 /**
- * VariableLinearTask is a concrete implementation of the Task. Please use this element if
+ * SPUTorqueLimitTask is a concrete implementation of the Task. Please use this element if
  * you want to create a inequality linear in the variable. The task represents the following equation
  * \f[
  * Ax < b
@@ -23,7 +23,7 @@ namespace TSID
  * where \f$x\f$ are the elements of the variable you want to constraint.
  */    
 
-class VariableLinearTask : public TSIDLinearTask
+class SPUTorqueLimitTask : public TSIDLinearTask
 {
     std::string m_variableName; /**< Name of the variable considered that will be regularized */
     std::vector<std::string> m_controlledElements; /**< Name of the variable elements considered in
@@ -70,7 +70,7 @@ class VariableLinearTask : public TSIDLinearTask
     std::size_t size() const override;
 
     /**
-     * The VariableLinearTask is an inequality task.
+     * The SPUTorqueLimitTask is an inequality task.
      * @return the type of the task.
      */
     Type type() const override;
diff --git a/src/TSID/src/VariableLinearTask.cpp b/src/TSID/src/SPUTorqueLimitTask.cpp
similarity index 76%
rename from src/TSID/src/VariableLinearTask.cpp
rename to src/TSID/src/SPUTorqueLimitTask.cpp
index 3bb3f633e5..7a936367b6 100644
--- a/src/TSID/src/VariableLinearTask.cpp
+++ b/src/TSID/src/SPUTorqueLimitTask.cpp
@@ -1,11 +1,11 @@
 /**
- * @file VariableLinearTask.cpp
+ * @file SPUTorqueLimitTask.cpp
  * @authors Roberto Mauceri
  * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
  */
 
-#include <BipedalLocomotion/TSID/VariableLinearTask.h>
+#include <BipedalLocomotion/TSID/SPUTorqueLimitTask.h>
 #include <BipedalLocomotion/System/VariablesHandler.h>
 #include <BipedalLocomotion/TextLogging/Logger.h>
 
@@ -16,9 +16,9 @@ using namespace BipedalLocomotion::TSID;
 using namespace BipedalLocomotion::System;
 using namespace BipedalLocomotion;
 
-bool VariableLinearTask::setVariablesHandler(const VariablesHandler& variablesHandler)
+bool SPUTorqueLimitTask::setVariablesHandler(const VariablesHandler& variablesHandler)
 {
-    constexpr auto errorPrefix = "[VariableLinearTask::setVariablesHandler]";
+    constexpr auto errorPrefix = "[SPUTorqueLimitTask::setVariablesHandler]";
 
     System::VariablesHandler::VariableDescription variable;
 
@@ -46,11 +46,11 @@ bool VariableLinearTask::setVariablesHandler(const VariablesHandler& variablesHa
     }
 
     // resize the matrices
-    m_S.resize(2 * m_variableSize, variablesHandler.getNumberOfVariables());
-    m_S.setZero();
+    m_A.resize(2 * m_variableSize, variablesHandler.getNumberOfVariables());
+    m_A.setZero();
     m_b.resize(2 * m_variableSize);
 
-    // S (selection matrix) is constant
+    // A (selection matrix) is constant
     if (m_controlledElements.size() != 0)
     {
         for (int i = 0; i < m_controlledElements.size(); i++)
@@ -66,7 +66,7 @@ bool VariableLinearTask::setVariablesHandler(const VariablesHandler& variablesHa
                 return false;
             }
 
-            m_S(i, index) = 1;
+            m_A(i, index) = 1;
         }
     } else
     {
@@ -88,27 +88,41 @@ bool VariableLinearTask::setVariablesHandler(const VariablesHandler& variablesHa
     return true;
 }
 
-bool VariableLinearTask::initialize(std::weak_ptr<const IParametersHandler> paramHandler)
+bool SPUTorqueLimitTask::initialize(std::weak_ptr<const IParametersHandler> paramHandler)
 {
 
 }
 
-bool VariableLinearTask::setUpperBound(Eigen::Ref<const Eigen::VectorXd> setPoint)
+bool SPUTorqueLimitTask::setUpperBound(Eigen::Ref<const Eigen::VectorXd> setPoint)
 {
 
 }
 
-std::size_t VariableLinearTask::size() const
+bool SPUTorqueLimitTask::update()
+{
+    constexpr auto errorPrefix = "[SPUTorqueLimitTask::update]";
+
+    m_isValid = false;
+    
+    m_A = 0;
+    m_b = 0
+
+    m_isValid = true;
+
+    return m_isValid;
+}
+
+std::size_t SPUTorqueLimitTask::size() const
 {
     return m_b.size();
 }
 
-VariableLinearTask::Type VariableLinearTask::type() const
+SPUTorqueLimitTask::Type SPUTorqueLimitTask::type() const
 {
     return Type::inequality; // By default, the "<" operator is considered
 }
 
-bool VariableLinearTask::isValid() const
+bool SPUTorqueLimitTask::isValid() const
 {
     return m_isValid;
 }

From 08d4a589917968a218a796d9c0379d28ad8f8945 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Sun, 12 Jan 2025 20:57:32 +0100
Subject: [PATCH 03/20] renamed the task from 'SPUTorqueLimitTask' to
 'TorqueFeasibleRegionTask'

---
 ...LimitTask.h => TorqueFeasibleRegionTask.h} |  23 ++-
 src/TSID/src/SPUTorqueLimitTask.cpp           | 128 --------------
 src/TSID/src/TorqueFeasibleRegionTask.cpp     | 166 ++++++++++++++++++
 3 files changed, 182 insertions(+), 135 deletions(-)
 rename src/TSID/include/BipedalLocomotion/TSID/{SPUTorqueLimitTask.h => TorqueFeasibleRegionTask.h} (82%)
 delete mode 100644 src/TSID/src/SPUTorqueLimitTask.cpp
 create mode 100644 src/TSID/src/TorqueFeasibleRegionTask.cpp

diff --git a/src/TSID/include/BipedalLocomotion/TSID/SPUTorqueLimitTask.h b/src/TSID/include/BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h
similarity index 82%
rename from src/TSID/include/BipedalLocomotion/TSID/SPUTorqueLimitTask.h
rename to src/TSID/include/BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h
index 895d440965..261b9b57da 100644
--- a/src/TSID/include/BipedalLocomotion/TSID/SPUTorqueLimitTask.h
+++ b/src/TSID/include/BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h
@@ -1,5 +1,5 @@
 /**
- * @file SPUTorqueLimitTask.h
+ * @file TorqueFeasibleRegionTask.h
  * @authors Roberto Mauceri
  * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
@@ -15,7 +15,7 @@ namespace BipedalLocomotion
 namespace TSID
 {
 /**
- * SPUTorqueLimitTask is a concrete implementation of the Task. Please use this element if
+ * TorqueFeasibleRegionTask is a concrete implementation of the Task. Please use this element if
  * you want to create a inequality linear in the variable. The task represents the following equation
  * \f[
  * Ax < b
@@ -23,7 +23,7 @@ namespace TSID
  * where \f$x\f$ are the elements of the variable you want to constraint.
  */    
 
-class SPUTorqueLimitTask : public TSIDLinearTask
+class TorqueFeasibleRegionTask : public TSIDLinearTask
 {
     std::string m_variableName; /**< Name of the variable considered that will be regularized */
     std::vector<std::string> m_controlledElements; /**< Name of the variable elements considered in
@@ -32,6 +32,9 @@ class SPUTorqueLimitTask : public TSIDLinearTask
     bool m_isValid{false}; /**< True if the task is valid. */
     std::size_t m_variableSize{0}; /**< Size of the regularized variable. */
 
+protected:
+    Eigen::MatrixXd m_S; /** Selection Matrix */
+
 public:
     /**
      * Initialize the planner.
@@ -57,11 +60,17 @@ class SPUTorqueLimitTask : public TSIDLinearTask
     bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override;
 
     /**
-     * Set the upper bound.
-     * @param upperBound vector containing the upper bounds for the variable elements.
+     * Set the matrices that define the feasible region for the SPU torques
+     * @param Q change of coordinates matrix (2d)
+     * @param l lower_bounds (2d)
+     * @param u upper_bounds (2d)
      * @return True in case of success, false otherwise.
      */
-    bool setUpperBound(Eigen::Ref<const Eigen::VectorXd> setPoint);
+    bool setFeasibleRegion(
+        Eigen::Ref<const Eigen::MatrixXd> Q, 
+        Eigen::Ref<const Eigen::VectorXd> l, 
+        Eigen::Ref<const Eigen::VectorXd> u
+    );
 
     /**
      * Get the size of the task. (I.e the number of rows of the vector b)
@@ -70,7 +79,7 @@ class SPUTorqueLimitTask : public TSIDLinearTask
     std::size_t size() const override;
 
     /**
-     * The SPUTorqueLimitTask is an inequality task.
+     * The TorqueFeasibleRegionTask is an inequality task.
      * @return the type of the task.
      */
     Type type() const override;
diff --git a/src/TSID/src/SPUTorqueLimitTask.cpp b/src/TSID/src/SPUTorqueLimitTask.cpp
deleted file mode 100644
index 7a936367b6..0000000000
--- a/src/TSID/src/SPUTorqueLimitTask.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-/**
- * @file SPUTorqueLimitTask.cpp
- * @authors Roberto Mauceri
- * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
- * distributed under the terms of the BSD-3-Clause license.
- */
-
-#include <BipedalLocomotion/TSID/SPUTorqueLimitTask.h>
-#include <BipedalLocomotion/System/VariablesHandler.h>
-#include <BipedalLocomotion/TextLogging/Logger.h>
-
-#include <iDynTree/EigenHelpers.h>
-
-using namespace BipedalLocomotion::ParametersHandler;
-using namespace BipedalLocomotion::TSID;
-using namespace BipedalLocomotion::System;
-using namespace BipedalLocomotion;
-
-bool SPUTorqueLimitTask::setVariablesHandler(const VariablesHandler& variablesHandler)
-{
-    constexpr auto errorPrefix = "[SPUTorqueLimitTask::setVariablesHandler]";
-
-    System::VariablesHandler::VariableDescription variable;
-
-    if (!m_isInitialized)
-    {
-        log()->error("{} The task is not initialized. Please call initialize method.", errorPrefix);
-        return false;
-    }
-
-    if (!variablesHandler.getVariable(m_variableName, variable))
-    {
-        log()->error("{} Error while retrieving the variable named {}.",
-                     errorPrefix,
-                     m_variableName);
-        return false;
-    }
-
-    if (m_variableSize > variable.size)
-    {
-        log()->error("{} The expect size is greater than the one stored in the variable named {}. "
-                     "This task is used to regularize a subset of elements of a variable.",
-                     errorPrefix,
-                     m_variableName);
-        return false;
-    }
-
-    // resize the matrices
-    m_A.resize(2 * m_variableSize, variablesHandler.getNumberOfVariables());
-    m_A.setZero();
-    m_b.resize(2 * m_variableSize);
-
-    // A (selection matrix) is constant
-    if (m_controlledElements.size() != 0)
-    {
-        for (int i = 0; i < m_controlledElements.size(); i++)
-        {
-            const auto& element = m_controlledElements[i];
-            auto index = variable.getElementIndex(m_controlledElements[i]);
-            if (index < 0)
-            {
-                log()->error("{} Unable to find the element named {} in the variable {}",
-                             errorPrefix,
-                             element,
-                             m_variableName);
-                return false;
-            }
-
-            m_A(i, index) = 1;
-        }
-    } else
-    {
-        if (m_variableSize != variable.size)
-        {
-            log()->error("{} The size of the variable named {} is different from expected. "
-                         "Expected: {}, Passed: {}.",
-                         errorPrefix,
-                         m_variableName,
-                         m_variableSize,
-                         variable.size);
-            return false;
-        }
-        // if the size of the m_controlledElements vector is zero, this means that the entire
-        // variable is regularized
-        iDynTree::toEigen(this->subA(variable)).setIdentity();
-    }
-
-    return true;
-}
-
-bool SPUTorqueLimitTask::initialize(std::weak_ptr<const IParametersHandler> paramHandler)
-{
-
-}
-
-bool SPUTorqueLimitTask::setUpperBound(Eigen::Ref<const Eigen::VectorXd> setPoint)
-{
-
-}
-
-bool SPUTorqueLimitTask::update()
-{
-    constexpr auto errorPrefix = "[SPUTorqueLimitTask::update]";
-
-    m_isValid = false;
-    
-    m_A = 0;
-    m_b = 0
-
-    m_isValid = true;
-
-    return m_isValid;
-}
-
-std::size_t SPUTorqueLimitTask::size() const
-{
-    return m_b.size();
-}
-
-SPUTorqueLimitTask::Type SPUTorqueLimitTask::type() const
-{
-    return Type::inequality; // By default, the "<" operator is considered
-}
-
-bool SPUTorqueLimitTask::isValid() const
-{
-    return m_isValid;
-}
diff --git a/src/TSID/src/TorqueFeasibleRegionTask.cpp b/src/TSID/src/TorqueFeasibleRegionTask.cpp
new file mode 100644
index 0000000000..80a82801e3
--- /dev/null
+++ b/src/TSID/src/TorqueFeasibleRegionTask.cpp
@@ -0,0 +1,166 @@
+/**
+ * @file TorqueFeasibleRegionTask.cpp
+ * @authors Roberto Mauceri
+ * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h>
+#include <BipedalLocomotion/System/VariablesHandler.h>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+
+#include <iDynTree/EigenHelpers.h>
+
+using namespace BipedalLocomotion::ParametersHandler;
+using namespace BipedalLocomotion::TSID;
+using namespace BipedalLocomotion::System;
+using namespace BipedalLocomotion;
+
+bool TorqueFeasibleRegionTask::setVariablesHandler(const VariablesHandler& variablesHandler)
+{
+    constexpr auto errorPrefix = "[TorqueFeasibleRegionTask::setVariablesHandler]";
+
+    System::VariablesHandler::VariableDescription variable;
+
+    if (!m_isInitialized)
+    {
+        log()->error("{} The task is not initialized. Please call initialize() method.", errorPrefix);
+        return false;
+    }
+
+    if (!variablesHandler.getVariable(m_variableName, variable))
+    {
+        log()->error("{} Error while retrieving the variable named {}.",
+                     errorPrefix,
+                     m_variableName);
+        return false;
+    }
+
+    if (m_variableSize > variable.size)
+    {
+        log()->error("{} The expected size is greater than the one stored in the variable named {}.",
+                     errorPrefix,
+                     m_variableName);
+        return false;
+    }
+
+    // resize the matrices
+    m_A.resize(2 * m_variableSize, variablesHandler.getNumberOfVariables());
+    m_b.resize(2 * m_variableSize);
+
+    // m_S (selection matrix)
+    m_S.resize(m_variableSize, variablesHandler.getNumberOfVariables());
+    m_S.setZero();
+    // S is constant
+    if (m_controlledElements.size() == 2)
+    {
+        for (int i = 0; i < m_controlledElements.size(); i++)
+        {
+            const auto& element = m_controlledElements[i];
+            auto index = variable.getElementIndex(m_controlledElements[i]);
+            if (index < 0)
+            {
+                log()->error("{} Unable to find the element named {} in the variable {}",
+                             errorPrefix,
+                             element,
+                             m_variableName);
+                return false;
+            }
+
+            m_S(i, index) = 1;
+        }
+    } else
+    {
+        log()->error("{} The size of the variable named {} is different from expected. "
+                        "Expected: 2, Passed: {}.",
+                        errorPrefix,
+                        m_variableName,
+                        variable.size);
+        return false;
+    }
+
+    return true;
+}
+
+bool TorqueFeasibleRegionTask::initialize(std::weak_ptr<const IParametersHandler> paramHandler)
+{
+   constexpr auto errorPrefix = "[TorqueFeasibleRegionTask::initialize]";
+
+    auto ptr = paramHandler.lock();
+    if (ptr == nullptr)
+    {
+        log()->error("{} The parameter handler is not valid.", errorPrefix);
+        return false;
+    }
+
+    if (!ptr->getParameter("variable_name", m_variableName))
+    {
+        log()->error("{} Error while retrieving the variable name.", errorPrefix);
+        return false;
+    }
+
+    int variableSize{-1};
+    if (!ptr->getParameter("variable_size", variableSize) || variableSize < 0)
+    {
+        log()->error("{} Error while retrieving the size of the variable.", errorPrefix);
+        return false;
+    }
+    m_variableSize = variableSize;
+
+    // set the description
+    m_description = "SPU Torque Limit Task [variable: " + m_variableName + ", elements: 2]";
+
+    m_isInitialized = true;
+    return true;
+
+}
+
+bool TorqueFeasibleRegionTask::setTorqueFeasibleRegion(
+    Eigen::Ref<const Eigen::MatrixXd> Q, 
+    Eigen::Ref<const Eigen::VectorXd> l, 
+    Eigen::Ref<const Eigen::VectorXd> u
+)
+{
+    constexpr auto errorPrefix = "[TorqueFeasibleRegionTask::setTorqueFeasibleRegion]";
+
+    m_isValid = false;
+    if (Q.rows() != m_variableSize || Q.rows() != m_variableSize)
+    {
+        log()->error("{} The matrix Q must have an order equal to {}", errorPrefix, m_variableSize);
+        return false;
+    }
+
+        if (Q.rows() != m_variableSize || Q.rows() != m_variableSize)
+    {
+        log()->error("{} The vector l must have a dimension equal to {}", errorPrefix, m_variableSize);
+        return false;
+    }
+
+        if (Q.rows() != m_variableSize || Q.rows() != m_variableSize)
+    {
+        log()->error("{} The vector u must have a dimension equal to {}", errorPrefix, m_variableSize);
+        return false;
+    }
+
+    m_A << Q, -Q;
+    m_A = m_A * m_S;
+
+    m_b << u, -l;
+    m_isValid = true;
+    return true
+}
+
+std::size_t TorqueFeasibleRegionTask::size() const
+{
+    return m_b.size();
+}
+
+TorqueFeasibleRegionTask::Type TorqueFeasibleRegionTask::type() const
+{
+    return Type::inequality;    // by default, the "<" operator is considered
+}
+
+bool TorqueFeasibleRegionTask::isValid() const
+{
+    return m_isValid;
+}

From c9e3407e92d09d2836c6e8666075f082e7167877 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Sun, 12 Jan 2025 22:14:43 +0100
Subject: [PATCH 04/20] minor changes

---
 .../TSID/TorqueFeasibleRegionTask.h           | 10 +++-----
 src/TSID/src/TorqueFeasibleRegionTask.cpp     | 25 +++----------------
 2 files changed, 7 insertions(+), 28 deletions(-)

diff --git a/src/TSID/include/BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h b/src/TSID/include/BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h
index 261b9b57da..0ff6fcbf65 100644
--- a/src/TSID/include/BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h
+++ b/src/TSID/include/BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h
@@ -32,8 +32,7 @@ class TorqueFeasibleRegionTask : public TSIDLinearTask
     bool m_isValid{false}; /**< True if the task is valid. */
     std::size_t m_variableSize{0}; /**< Size of the regularized variable. */
 
-protected:
-    Eigen::MatrixXd m_S; /** Selection Matrix */
+    Eigen::MatrixXd m_S; /**< Selection Matrix */
 
 public:
     /**
@@ -67,10 +66,9 @@ class TorqueFeasibleRegionTask : public TSIDLinearTask
      * @return True in case of success, false otherwise.
      */
     bool setFeasibleRegion(
-        Eigen::Ref<const Eigen::MatrixXd> Q, 
-        Eigen::Ref<const Eigen::VectorXd> l, 
-        Eigen::Ref<const Eigen::VectorXd> u
-    );
+        Eigen::Ref<const Eigen::Matrix2d> Q, 
+        Eigen::Ref<const Eigen::Vector2d> l, 
+        Eigen::Ref<const Eigen::Vector2d> u);
 
     /**
      * Get the size of the task. (I.e the number of rows of the vector b)
diff --git a/src/TSID/src/TorqueFeasibleRegionTask.cpp b/src/TSID/src/TorqueFeasibleRegionTask.cpp
index 80a82801e3..ebec90c110 100644
--- a/src/TSID/src/TorqueFeasibleRegionTask.cpp
+++ b/src/TSID/src/TorqueFeasibleRegionTask.cpp
@@ -116,32 +116,13 @@ bool TorqueFeasibleRegionTask::initialize(std::weak_ptr<const IParametersHandler
 }
 
 bool TorqueFeasibleRegionTask::setTorqueFeasibleRegion(
-    Eigen::Ref<const Eigen::MatrixXd> Q, 
-    Eigen::Ref<const Eigen::VectorXd> l, 
-    Eigen::Ref<const Eigen::VectorXd> u
-)
+    Eigen::Ref<const Eigen::Matrix2d> Q, 
+    Eigen::Ref<const Eigen::Vector2d> l, 
+    Eigen::Ref<const Eigen::Vector2d> u)
 {
     constexpr auto errorPrefix = "[TorqueFeasibleRegionTask::setTorqueFeasibleRegion]";
 
     m_isValid = false;
-    if (Q.rows() != m_variableSize || Q.rows() != m_variableSize)
-    {
-        log()->error("{} The matrix Q must have an order equal to {}", errorPrefix, m_variableSize);
-        return false;
-    }
-
-        if (Q.rows() != m_variableSize || Q.rows() != m_variableSize)
-    {
-        log()->error("{} The vector l must have a dimension equal to {}", errorPrefix, m_variableSize);
-        return false;
-    }
-
-        if (Q.rows() != m_variableSize || Q.rows() != m_variableSize)
-    {
-        log()->error("{} The vector u must have a dimension equal to {}", errorPrefix, m_variableSize);
-        return false;
-    }
-
     m_A << Q, -Q;
     m_A = m_A * m_S;
 

From 1947d559fa30004bd6f0bef0d1d26fcf43f11fcc Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Mon, 13 Jan 2025 17:58:21 +0100
Subject: [PATCH 05/20] renamed the task from 'TorqueFeasibleRegionTask' to
 'VariableFeasibleRegionTask'

---
 src/TSID/CMakeLists.txt                       |  2 +
 ...ionTask.h => VariableFeasibleRegionTask.h} | 31 +++---
 ...ask.cpp => VariableFeasibleRegionTask.cpp} | 95 +++++++++++++------
 3 files changed, 82 insertions(+), 46 deletions(-)
 rename src/TSID/include/BipedalLocomotion/TSID/{TorqueFeasibleRegionTask.h => VariableFeasibleRegionTask.h} (78%)
 rename src/TSID/src/{TorqueFeasibleRegionTask.cpp => VariableFeasibleRegionTask.cpp} (50%)

diff --git a/src/TSID/CMakeLists.txt b/src/TSID/CMakeLists.txt
index aa0e0e6180..ab5936dcd0 100644
--- a/src/TSID/CMakeLists.txt
+++ b/src/TSID/CMakeLists.txt
@@ -12,11 +12,13 @@ if(FRAMEWORK_COMPILE_TSID)
                            ${H_PREFIX}/BaseDynamicsTask.h ${H_PREFIX}/JointDynamicsTask.h
                            ${H_PREFIX}/TaskSpaceInverseDynamics.h
                            ${H_PREFIX}/FeasibleContactWrenchTask.h
+                           ${H_PREFIX}/VariableFeasibleRegionTask.h
                            ${H_PREFIX}/VariableRegularizationTask.h
                            ${H_PREFIX}/QPFixedBaseTSID.h ${H_PREFIX}/QPTSID.h
     SOURCES                src/TSIDLinearTask.cpp src/SO3Task.cpp src/SE3Task.cpp src/JointTrackingTask.cpp src/CoMTask.cpp src/AngularMomentumTask.cpp src/R3Task.cpp
                            src/BaseDynamicsTask.cpp src/JointDynamicsTask.cpp
                            src/FeasibleContactWrenchTask.cpp
+                           src/VariableFeasibleRegionTask.cpp
                            src/VariableRegularizationTask.cpp
                            src/QPFixedBaseTSID.cpp src/QPTSID.cpp src/TaskSpaceInverseDynamics.cpp
     PUBLIC_LINK_LIBRARIES  Eigen3::Eigen
diff --git a/src/TSID/include/BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h b/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
similarity index 78%
rename from src/TSID/include/BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h
rename to src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
index 0ff6fcbf65..ecef76e928 100644
--- a/src/TSID/include/BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h
+++ b/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
@@ -1,5 +1,5 @@
 /**
- * @file TorqueFeasibleRegionTask.h
+ * @file VariableFeasibleRegionTask.h
  * @authors Roberto Mauceri
  * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
@@ -15,7 +15,7 @@ namespace BipedalLocomotion
 namespace TSID
 {
 /**
- * TorqueFeasibleRegionTask is a concrete implementation of the Task. Please use this element if
+ * VariableFeasibleRegionTask is a concrete implementation of the Task. Please use this element if
  * you want to create a inequality linear in the variable. The task represents the following equation
  * \f[
  * Ax < b
@@ -23,16 +23,17 @@ namespace TSID
  * where \f$x\f$ are the elements of the variable you want to constraint.
  */    
 
-class TorqueFeasibleRegionTask : public TSIDLinearTask
+class VariableFeasibleRegionTask : public TSIDLinearTask
 {
-    std::string m_variableName; /**< Name of the variable considered that will be regularized */
+    std::string m_variableName; /**< Name of the variable considered in the task. */
     std::vector<std::string> m_controlledElements; /**< Name of the variable elements considered in
-                                                     the task */
+                                                     the task. */
     bool m_isInitialized{false}; /**< True if the task has been initialized. */
     bool m_isValid{false}; /**< True if the task is valid. */
-    std::size_t m_variableSize{0}; /**< Size of the regularized variable. */
+    std::size_t m_variableSize{0}; /**< Size of the variable considered in the task. */
 
-    Eigen::MatrixXd m_S; /**< Selection Matrix */
+    Eigen::MatrixXd m_S; /**< Selection Matrix. */
+    Eigen::MatrixXd m_C; /**< Change of Coordinate Matrix. */
 
 public:
     /**
@@ -59,16 +60,16 @@ class TorqueFeasibleRegionTask : public TSIDLinearTask
     bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override;
 
     /**
-     * Set the matrices that define the feasible region for the SPU torques
-     * @param Q change of coordinates matrix (2d)
-     * @param l lower_bounds (2d)
-     * @param u upper_bounds (2d)
+     * Set the region of feasibility for the desired elements of the variable.
+     * @param C change of coordinates matrix
+     * @param l lower_bounds
+     * @param u upper_bounds
      * @return True in case of success, false otherwise.
      */
     bool setFeasibleRegion(
-        Eigen::Ref<const Eigen::Matrix2d> Q, 
-        Eigen::Ref<const Eigen::Vector2d> l, 
-        Eigen::Ref<const Eigen::Vector2d> u);
+        Eigen::Ref<const Eigen::MatrixXd> C, 
+        Eigen::Ref<const Eigen::VectorXd> l, 
+        Eigen::Ref<const Eigen::VectorXd> u);
 
     /**
      * Get the size of the task. (I.e the number of rows of the vector b)
@@ -77,7 +78,7 @@ class TorqueFeasibleRegionTask : public TSIDLinearTask
     std::size_t size() const override;
 
     /**
-     * The TorqueFeasibleRegionTask is an inequality task.
+     * The VariableFeasibleRegionTask is an inequality task.
      * @return the type of the task.
      */
     Type type() const override;
diff --git a/src/TSID/src/TorqueFeasibleRegionTask.cpp b/src/TSID/src/VariableFeasibleRegionTask.cpp
similarity index 50%
rename from src/TSID/src/TorqueFeasibleRegionTask.cpp
rename to src/TSID/src/VariableFeasibleRegionTask.cpp
index ebec90c110..e851936576 100644
--- a/src/TSID/src/TorqueFeasibleRegionTask.cpp
+++ b/src/TSID/src/VariableFeasibleRegionTask.cpp
@@ -1,11 +1,11 @@
 /**
- * @file TorqueFeasibleRegionTask.cpp
+ * @file VariableFeasibleRegionTask.cpp
  * @authors Roberto Mauceri
  * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
  */
 
-#include <BipedalLocomotion/TSID/TorqueFeasibleRegionTask.h>
+#include <BipedalLocomotion/TSID/VariableFeasibleRegionTask.h>
 #include <BipedalLocomotion/System/VariablesHandler.h>
 #include <BipedalLocomotion/TextLogging/Logger.h>
 
@@ -16,9 +16,9 @@ using namespace BipedalLocomotion::TSID;
 using namespace BipedalLocomotion::System;
 using namespace BipedalLocomotion;
 
-bool TorqueFeasibleRegionTask::setVariablesHandler(const VariablesHandler& variablesHandler)
+bool VariableFeasibleRegionTask::setVariablesHandler(const VariablesHandler& variablesHandler)
 {
-    constexpr auto errorPrefix = "[TorqueFeasibleRegionTask::setVariablesHandler]";
+    constexpr auto errorPrefix = "[VariableFeasibleRegionTask::setVariablesHandler]";
 
     System::VariablesHandler::VariableDescription variable;
 
@@ -48,11 +48,14 @@ bool TorqueFeasibleRegionTask::setVariablesHandler(const VariablesHandler& varia
     m_A.resize(2 * m_variableSize, variablesHandler.getNumberOfVariables());
     m_b.resize(2 * m_variableSize);
 
+    // m_C (change of coordinate matrix)
+    m_C.resize(2 * m_variableSize, m_variableSize);
+
     // m_S (selection matrix)
     m_S.resize(m_variableSize, variablesHandler.getNumberOfVariables());
     m_S.setZero();
-    // S is constant
-    if (m_controlledElements.size() == 2)
+    // m_S is constant
+    if (m_controlledElements.size() != 0)
     {
         for (int i = 0; i < m_controlledElements.size(); i++)
         {
@@ -66,25 +69,31 @@ bool TorqueFeasibleRegionTask::setVariablesHandler(const VariablesHandler& varia
                              m_variableName);
                 return false;
             }
-
             m_S(i, index) = 1;
         }
     } else
     {
-        log()->error("{} The size of the variable named {} is different from expected. "
-                        "Expected: 2, Passed: {}.",
-                        errorPrefix,
-                        m_variableName,
-                        variable.size);
-        return false;
+        if (m_variableSize != variable.size)
+        {
+            log()->error("{} The size of the variable named {} is different from expected. "
+                         "Expected: {}, Passed: {}.",
+                         errorPrefix,
+                         m_variableName,
+                         m_variableSize,
+                         variable.size);
+            return false;
+        }
+        // if the size of the m_controlledElements vector is zero, this means that the entire
+        // variable is regularized
+        iDynTree::toEigen(this->subA(variable)).setIdentity(); // devo sostituirlo col comando sotto?
+        m_S = Eigen::MatrixXd::Identity(variable.size, variable.size);
     }
-
     return true;
 }
 
-bool TorqueFeasibleRegionTask::initialize(std::weak_ptr<const IParametersHandler> paramHandler)
+bool VariableFeasibleRegionTask::initialize(std::weak_ptr<const IParametersHandler> paramHandler)
 {
-   constexpr auto errorPrefix = "[TorqueFeasibleRegionTask::initialize]";
+   constexpr auto errorPrefix = "[VariableFeasibleRegionTask::initialize]";
 
     auto ptr = paramHandler.lock();
     if (ptr == nullptr)
@@ -105,43 +114,67 @@ bool TorqueFeasibleRegionTask::initialize(std::weak_ptr<const IParametersHandler
         log()->error("{} Error while retrieving the size of the variable.", errorPrefix);
         return false;
     }
+    // set the variable size
     m_variableSize = variableSize;
 
     // set the description
-    m_description = "SPU Torque Limit Task [variable: " + m_variableName + ", elements: 2]";
+    m_description = "Variable Feasible Region Task [variable: " + m_variableName + ", elements: ";
+
+    if (!ptr->getParameter("elements_name", m_controlledElements))
+    {
+        m_description += "All";
+        log()->debug("{} The elements_name is not set. All the {} elements will be considered.",
+                     errorPrefix,
+                     m_variableSize);
+    } else
+    {
+        if (m_variableSize != m_controlledElements.size())
+        {
+            log()->error("{} The size of the elements_name vector is different from the one "
+                         "expected. Expected: {}, Retrieved: {}",
+                         errorPrefix,
+                         m_variableSize,
+                         m_controlledElements.size());
+            return false;
+        }
+        for (const auto& element : m_controlledElements)
+        {
+            m_description += " " + element;
+        }
+    }
+    m_description += "]";
 
     m_isInitialized = true;
     return true;
 
 }
 
-bool TorqueFeasibleRegionTask::setTorqueFeasibleRegion(
-    Eigen::Ref<const Eigen::Matrix2d> Q, 
-    Eigen::Ref<const Eigen::Vector2d> l, 
-    Eigen::Ref<const Eigen::Vector2d> u)
+bool VariableFeasibleRegionTask::setFeasibleRegion(
+    Eigen::Ref<const Eigen::MatrixXd> C, 
+    Eigen::Ref<const Eigen::VectorXd> l, 
+    Eigen::Ref<const Eigen::VectorXd> u)
 {
-    constexpr auto errorPrefix = "[TorqueFeasibleRegionTask::setTorqueFeasibleRegion]";
+    constexpr auto errorPrefix = "[VariableFeasibleRegionTask::setFeasibleRegion]";
+    // check if the size of the matrices is correct
 
-    m_isValid = false;
-    m_A << Q, -Q;
-    m_A = m_A * m_S;
+    m_C << C, -C;
+    m_A = m_C * m_S;
 
     m_b << u, -l;
-    m_isValid = true;
-    return true
+    return true;
 }
 
-std::size_t TorqueFeasibleRegionTask::size() const
+std::size_t VariableFeasibleRegionTask::size() const
 {
     return m_b.size();
 }
 
-TorqueFeasibleRegionTask::Type TorqueFeasibleRegionTask::type() const
+VariableFeasibleRegionTask::Type VariableFeasibleRegionTask::type() const
 {
-    return Type::inequality;    // by default, the "<" operator is considered
+    return Type::inequality;
 }
 
-bool TorqueFeasibleRegionTask::isValid() const
+bool VariableFeasibleRegionTask::isValid() const
 {
     return m_isValid;
 }

From 5c2b3f606b13580d54c587542f6809e332e6c204 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Thu, 16 Jan 2025 09:36:10 +0100
Subject: [PATCH 06/20] made 'VariableFeasibleRegionTask' independent from the
 size of the controlled elements

---
 src/TSID/src/VariableFeasibleRegionTask.cpp | 34 ++++++++++++++++++---
 1 file changed, 30 insertions(+), 4 deletions(-)

diff --git a/src/TSID/src/VariableFeasibleRegionTask.cpp b/src/TSID/src/VariableFeasibleRegionTask.cpp
index e851936576..b68c614ab8 100644
--- a/src/TSID/src/VariableFeasibleRegionTask.cpp
+++ b/src/TSID/src/VariableFeasibleRegionTask.cpp
@@ -71,7 +71,8 @@ bool VariableFeasibleRegionTask::setVariablesHandler(const VariablesHandler& var
             }
             m_S(i, index) = 1;
         }
-    } else
+    } 
+    else
     {
         if (m_variableSize != variable.size)
         {
@@ -85,8 +86,8 @@ bool VariableFeasibleRegionTask::setVariablesHandler(const VariablesHandler& var
         }
         // if the size of the m_controlledElements vector is zero, this means that the entire
         // variable is regularized
-        iDynTree::toEigen(this->subA(variable)).setIdentity(); // devo sostituirlo col comando sotto?
-        m_S = Eigen::MatrixXd::Identity(variable.size, variable.size);
+        // iDynTree::toEigen(this->subA(variable)).setIdentity(); // devo sostituirlo col comando sotto?
+        m_S = Eigen::MatrixXd::Identity(m_variableSize, m_variableSize);
     }
     return true;
 }
@@ -146,7 +147,6 @@ bool VariableFeasibleRegionTask::initialize(std::weak_ptr<const IParametersHandl
 
     m_isInitialized = true;
     return true;
-
 }
 
 bool VariableFeasibleRegionTask::setFeasibleRegion(
@@ -155,8 +155,34 @@ bool VariableFeasibleRegionTask::setFeasibleRegion(
     Eigen::Ref<const Eigen::VectorXd> u)
 {
     constexpr auto errorPrefix = "[VariableFeasibleRegionTask::setFeasibleRegion]";
+
     // check if the size of the matrices is correct
+    if (C.cols() != m_variableSize)
+    {
+        log()->error("{} The number of columns of the matrix C is not correct. Expected: {}, Passed: {}.",
+                     errorPrefix,
+                     m_variableSize,
+                     C.cols());
+        return false;
+    }
+    if (l.size() != C.rows())
+    {
+        log()->error("{} The size of the vector l is not correct. Expected: {}, Passed: {}.",
+                     errorPrefix,
+                     C.rows(),
+                     l.size());
+        return false;
+    }
+    if (u.size() != C.rows())
+    {
+        log()->error("{} The size of the vector u is not correct. Expected: {}, Passed: {}.",
+                     errorPrefix,
+                     C.rows(),
+                     u.size());
+        return false;
+    }
 
+    // set the matrices
     m_C << C, -C;
     m_A = m_C * m_S;
 

From de18e9d48cffd7309dfaf2268c30caf1761bfc33 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Thu, 16 Jan 2025 09:45:33 +0100
Subject: [PATCH 07/20] added bipedal test to the file 'tests/CMakeLists.txt'

---
 src/TSID/tests/CMakeLists.txt | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/TSID/tests/CMakeLists.txt b/src/TSID/tests/CMakeLists.txt
index 954e20de79..8838fc4807 100644
--- a/src/TSID/tests/CMakeLists.txt
+++ b/src/TSID/tests/CMakeLists.txt
@@ -47,6 +47,10 @@ add_bipedal_test(
   SOURCES AngularMomentumTaskTest.cpp
   LINKS BipedalLocomotion::TSID)
 
+add_bipedal_test(
+  NAME VariableFeasibleRegionTask
+  SOURCES VariableFeasibleRegionTaskTest.cpp
+  LINKS BipedalLocomotion::TSID)
 
 add_bipedal_test(
   NAME QPFixedBaseTSID

From 5491de86a11382641c191ff4aaf1a78385f048f8 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Thu, 16 Jan 2025 09:49:39 +0100
Subject: [PATCH 08/20] created the file 'VariableFeasibleRegionTaskTest.cpp'

---
 .../tests/VariableFeasibleRegionTaskTest.cpp  | 20 +++++++++++++++++++
 1 file changed, 20 insertions(+)
 create mode 100644 src/TSID/tests/VariableFeasibleRegionTaskTest.cpp

diff --git a/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp b/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
new file mode 100644
index 0000000000..8129480095
--- /dev/null
+++ b/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
@@ -0,0 +1,20 @@
+/**
+ * @file VariableFeasibleRegionTaskTest.cpp
+ * @authors Roberto Mauceri
+ * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+// Catch2
+#include <catch2/catch_test_macros.hpp>
+
+// BipedalLocomotion
+#include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
+#include <BipedalLocomotion/System/VariablesHandler.h>
+#include <BipedalLocomotion/TSID/VariableRegularizationTask.h>
+
+#include <Eigen/Dense>
+
+using namespace BipedalLocomotion::ParametersHandler;
+using namespace BipedalLocomotion::System;
+using namespace BipedalLocomotion::TSID;
\ No newline at end of file

From cf73434bcd1f62da307d67ba01747e4bb5f76999 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Thu, 16 Jan 2025 17:52:45 +0100
Subject: [PATCH 09/20] added tests for 'VariableFeasibleRegionTask' (all
 passed).

---
 .../TSID/VariableFeasibleRegionTask.h         |   3 +-
 src/TSID/src/VariableFeasibleRegionTask.cpp   |  30 ++--
 src/TSID/tests/CMakeLists.txt                 |   8 +-
 .../tests/VariableFeasibleRegionTaskTest.cpp  | 136 +++++++++++++++++-
 4 files changed, 158 insertions(+), 19 deletions(-)

diff --git a/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h b/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
index ecef76e928..04c6e6ffba 100644
--- a/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
+++ b/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
@@ -30,10 +30,11 @@ class VariableFeasibleRegionTask : public TSIDLinearTask
                                                      the task. */
     bool m_isInitialized{false}; /**< True if the task has been initialized. */
     bool m_isValid{false}; /**< True if the task is valid. */
+    std::size_t m_NumberOfVariables{0}; /**< Number of variables. */
     std::size_t m_variableSize{0}; /**< Size of the variable considered in the task. */
 
     Eigen::MatrixXd m_S; /**< Selection Matrix. */
-    Eigen::MatrixXd m_C; /**< Change of Coordinate Matrix. */
+    Eigen::MatrixXd m_T; /**< Transformation Matrix. */
 
 public:
     /**
diff --git a/src/TSID/src/VariableFeasibleRegionTask.cpp b/src/TSID/src/VariableFeasibleRegionTask.cpp
index b68c614ab8..1d7c5882ac 100644
--- a/src/TSID/src/VariableFeasibleRegionTask.cpp
+++ b/src/TSID/src/VariableFeasibleRegionTask.cpp
@@ -43,16 +43,10 @@ bool VariableFeasibleRegionTask::setVariablesHandler(const VariablesHandler& var
                      m_variableName);
         return false;
     }
-
-    // resize the matrices
-    m_A.resize(2 * m_variableSize, variablesHandler.getNumberOfVariables());
-    m_b.resize(2 * m_variableSize);
-
-    // m_C (change of coordinate matrix)
-    m_C.resize(2 * m_variableSize, m_variableSize);
+    m_NumberOfVariables = variablesHandler.getNumberOfVariables();
 
     // m_S (selection matrix)
-    m_S.resize(m_variableSize, variablesHandler.getNumberOfVariables());
+    m_S.resize(m_variableSize, m_NumberOfVariables);
     m_S.setZero();
     // m_S is constant
     if (m_controlledElements.size() != 0)
@@ -165,7 +159,7 @@ bool VariableFeasibleRegionTask::setFeasibleRegion(
                      C.cols());
         return false;
     }
-    if (l.size() != C.rows())
+    if (C.rows() != l.size())
     {
         log()->error("{} The size of the vector l is not correct. Expected: {}, Passed: {}.",
                      errorPrefix,
@@ -173,7 +167,7 @@ bool VariableFeasibleRegionTask::setFeasibleRegion(
                      l.size());
         return false;
     }
-    if (u.size() != C.rows())
+    if (C.rows() != u.size())
     {
         log()->error("{} The size of the vector u is not correct. Expected: {}, Passed: {}.",
                      errorPrefix,
@@ -181,10 +175,22 @@ bool VariableFeasibleRegionTask::setFeasibleRegion(
                      u.size());
         return false;
     }
+    if (!(u.array() >= l.array()).all())
+    {
+        log()->error("{} The elements of the vector u must be greater than or equal to"
+        " the elements of the vector l.",
+                     errorPrefix);
+        return false;
+    }
+
+    // resize the matrices
+    m_A.resize(2 * C.rows(), m_NumberOfVariables);
+    m_b.resize(2 * C.rows());
+    m_T.resize(2 * C.rows(), C.cols());
 
     // set the matrices
-    m_C << C, -C;
-    m_A = m_C * m_S;
+    m_T << C, -C;
+    m_A = m_T * m_S;
 
     m_b << u, -l;
     return true;
diff --git a/src/TSID/tests/CMakeLists.txt b/src/TSID/tests/CMakeLists.txt
index 8838fc4807..da615a7ad9 100644
--- a/src/TSID/tests/CMakeLists.txt
+++ b/src/TSID/tests/CMakeLists.txt
@@ -43,13 +43,13 @@ add_bipedal_test(
   LINKS BipedalLocomotion::TSID)
 
 add_bipedal_test(
-  NAME AngularMomentumTaskTSID
-  SOURCES AngularMomentumTaskTest.cpp
+  NAME VariableFeasibleRegionTaskTSID
+  SOURCES VariableFeasibleRegionTaskTest.cpp
   LINKS BipedalLocomotion::TSID)
 
 add_bipedal_test(
-  NAME VariableFeasibleRegionTask
-  SOURCES VariableFeasibleRegionTaskTest.cpp
+  NAME AngularMomentumTaskTSID
+  SOURCES AngularMomentumTaskTest.cpp
   LINKS BipedalLocomotion::TSID)
 
 add_bipedal_test(
diff --git a/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp b/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
index 8129480095..43a7d278fe 100644
--- a/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
+++ b/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
@@ -11,10 +11,142 @@
 // BipedalLocomotion
 #include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
 #include <BipedalLocomotion/System/VariablesHandler.h>
-#include <BipedalLocomotion/TSID/VariableRegularizationTask.h>
+#include <BipedalLocomotion/TSID/VariableFeasibleRegionTask.h>
 
 #include <Eigen/Dense>
 
 using namespace BipedalLocomotion::ParametersHandler;
 using namespace BipedalLocomotion::System;
-using namespace BipedalLocomotion::TSID;
\ No newline at end of file
+using namespace BipedalLocomotion::TSID;
+
+TEST_CASE("Variable Feasible Region Task")
+{
+    const std::string variable1Name = "variable_1";
+    const std::string variable2Name = "variable_2";
+    constexpr int variable2Size = 20;
+    const auto variable1ElementNames = std::vector<std::string>{"foo", "bar", "baz"};
+    const auto variable1ControlledElementNames = std::vector<std::string>{"foo", "baz"};
+
+    auto parameterHandler1 = std::make_shared<StdImplementation>();
+    parameterHandler1->setParameter("variable_name", variable1Name);
+    parameterHandler1->setParameter("variable_size", (int)variable1ControlledElementNames.size());
+    parameterHandler1->setParameter("elements_name", variable1ControlledElementNames);
+
+    auto parameterHandler2 = std::make_shared<StdImplementation>();
+    parameterHandler2->setParameter("variable_name", variable2Name);
+    parameterHandler2->setParameter("variable_size", variable2Size);
+
+    VariablesHandler variablesHandler;
+
+    REQUIRE(variablesHandler.addVariable("dummy1", 10));
+    REQUIRE(variablesHandler.addVariable(variable1Name, variable1ElementNames));
+    REQUIRE(variablesHandler.addVariable("dummy2", 15));
+    REQUIRE(variablesHandler.addVariable(variable2Name, variable2Size));
+
+    SECTION("Element subgroup")
+    {
+        VariableFeasibleRegionTask task;
+        REQUIRE_FALSE(task.isValid());
+        REQUIRE(task.initialize(parameterHandler1));
+        REQUIRE(task.setVariablesHandler(variablesHandler));
+        REQUIRE_FALSE(task.isValid());
+
+        const int variableTotalSize = variablesHandler.getNumberOfVariables();
+        const int anyDimension = 12;
+
+        // the matrix C can have any number of rows
+        Eigen::MatrixXd C(anyDimension, variable1ControlledElementNames.size());
+        C.setRandom();
+
+        Eigen::VectorXd l;
+        Eigen::VectorXd u;
+
+        // the vectors l and u must have the same size
+        l = Eigen::VectorXd::Ones(anyDimension);
+        u = Eigen::VectorXd::Ones(anyDimension + 1);
+        REQUIRE_FALSE(task.setFeasibleRegion(C, l, u));
+
+        // the elements of the vector l must be less than the elements of the vector u
+        l = Eigen::VectorXd::Ones(anyDimension) * 1.5;
+        u = Eigen::VectorXd::Ones(anyDimension);
+        REQUIRE_FALSE(task.setFeasibleRegion(C, l, u));
+
+        // valid input
+        l = Eigen::VectorXd::Ones(anyDimension);
+        u = Eigen::VectorXd::Ones(anyDimension) * 2.5;
+        REQUIRE(task.setFeasibleRegion(C, l, u));
+
+        Eigen::Ref<const Eigen::MatrixXd> A = task.getA();
+        Eigen::Ref<const Eigen::MatrixXd> B = task.getB();
+
+        // check the matrix A
+        REQUIRE(A.middleCols(variablesHandler.getVariable("dummy1").offset,
+                             variablesHandler.getVariable("dummy1").size)
+                    .isZero());
+
+        REQUIRE(A.middleCols(variablesHandler.getVariable("dummy2").offset,
+                             variablesHandler.getVariable("dummy2").size)
+                    .isZero());
+
+        REQUIRE(A.middleCols(variablesHandler.getVariable(variable2Name).offset,
+                              variablesHandler.getVariable(variable2Name).size)
+                     .isZero());
+
+        // check the matrix A
+        Eigen::MatrixXd expectedA(2*anyDimension, variableTotalSize);
+        Eigen::MatrixXd expectedT(2*anyDimension, variable1ControlledElementNames.size());
+        Eigen::MatrixXd expectedS(variable1ControlledElementNames.size(), variableTotalSize);
+        std::size_t index = variablesHandler.getVariable(variable1Name).offset;
+
+        expectedS.setZero();
+        expectedS(0, index + 0) = 1.0;
+        expectedS(1, index + 2) = 1.0;
+        
+        expectedT << C, -C;
+        expectedA = expectedT * expectedS;
+        REQUIRE(A.isApprox(expectedA));
+
+        // check the vector B
+        Eigen::VectorXd expectedB(2*anyDimension);
+        expectedB << u, -l;
+        REQUIRE(B.isApprox(expectedB));
+    }
+
+    SECTION("Entire variable")
+    {
+        VariableFeasibleRegionTask task2;
+        REQUIRE_FALSE(task2.isValid());
+        REQUIRE(task2.initialize(parameterHandler2));
+        REQUIRE(task2.setVariablesHandler(variablesHandler));
+        REQUIRE_FALSE(task2.isValid());
+
+        const int variable2ElementSize = variablesHandler.getVariable(variable2Name).size;
+        const int variableTotalSize = variablesHandler.getNumberOfVariables();
+        const int anyDimension = 1234;
+
+        // the matrix C can have any number of rows
+        Eigen::MatrixXd C(anyDimension, variable2ElementSize);
+        C.setRandom();
+
+        Eigen::VectorXd l;
+        Eigen::VectorXd u;
+
+        // valid input
+        l = Eigen::VectorXd::Ones(anyDimension);
+        u = Eigen::VectorXd::Ones(anyDimension) * 2.5;
+        REQUIRE(task2.setFeasibleRegion(C, l, u));
+
+        Eigen::Ref<const Eigen::MatrixXd> A = task2.getA();
+        Eigen::Ref<const Eigen::VectorXd> B = task2.getB();
+
+        // A == expectedT imply S == identity
+        Eigen::MatrixXd expectedT(2*anyDimension, variable2ElementSize);
+        expectedT << C, -C;
+        REQUIRE(A.isApprox(expectedT));
+
+        // check the vector B
+        Eigen::VectorXd expectedB(2*anyDimension);
+        expectedB << u, -l;
+        REQUIRE(B.isApprox(expectedB));
+    }
+}

From 5094acc991fd713f066ff2da6ea7519abdc6b039 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Fri, 17 Jan 2025 15:13:47 +0100
Subject: [PATCH 10/20] added m_isValid = 'true' in 'setFeasibleRegion()' +
 minor changes

---
 .../BipedalLocomotion/TSID/VariableFeasibleRegionTask.h        | 2 +-
 src/TSID/src/VariableFeasibleRegionTask.cpp                    | 3 ++-
 src/TSID/tests/VariableFeasibleRegionTaskTest.cpp              | 2 +-
 3 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h b/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
index 04c6e6ffba..21fb4e08cf 100644
--- a/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
+++ b/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
@@ -94,4 +94,4 @@ class VariableFeasibleRegionTask : public TSIDLinearTask
 } // namespace TSID
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_TSID_VARIABLE_LINEAR_TASK_H
\ No newline at end of file
+#endif // BIPEDAL_LOCOMOTION_TSID_VARIABLE_LINEAR_TASK_H
diff --git a/src/TSID/src/VariableFeasibleRegionTask.cpp b/src/TSID/src/VariableFeasibleRegionTask.cpp
index 1d7c5882ac..6ef187eb1e 100644
--- a/src/TSID/src/VariableFeasibleRegionTask.cpp
+++ b/src/TSID/src/VariableFeasibleRegionTask.cpp
@@ -148,8 +148,8 @@ bool VariableFeasibleRegionTask::setFeasibleRegion(
     Eigen::Ref<const Eigen::VectorXd> l, 
     Eigen::Ref<const Eigen::VectorXd> u)
 {
+    m_isValid = false;
     constexpr auto errorPrefix = "[VariableFeasibleRegionTask::setFeasibleRegion]";
-
     // check if the size of the matrices is correct
     if (C.cols() != m_variableSize)
     {
@@ -182,6 +182,7 @@ bool VariableFeasibleRegionTask::setFeasibleRegion(
                      errorPrefix);
         return false;
     }
+    m_isValid = true;
 
     // resize the matrices
     m_A.resize(2 * C.rows(), m_NumberOfVariables);
diff --git a/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp b/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
index 43a7d278fe..9218ea8481 100644
--- a/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
+++ b/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
@@ -139,7 +139,7 @@ TEST_CASE("Variable Feasible Region Task")
         Eigen::Ref<const Eigen::MatrixXd> A = task2.getA();
         Eigen::Ref<const Eigen::VectorXd> B = task2.getB();
 
-        // A == expectedT imply S == identity
+        // A == expectedT implies S == identity
         Eigen::MatrixXd expectedT(2*anyDimension, variable2ElementSize);
         expectedT << C, -C;
         REQUIRE(A.isApprox(expectedT));

From 41f704e92e5b002cb8cdb948549d4b8d9b35858c Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Fri, 17 Jan 2025 16:28:06 +0100
Subject: [PATCH 11/20] created binding files (.h and .cpp)

---
 .../TSID/VariableFeasibleRegionTask.h         | 26 +++++++++++++
 .../TSID/src/VariableFeasibleRegionTask.cpp   | 37 +++++++++++++++++++
 src/TSID/src/VariableFeasibleRegionTask.cpp   |  3 +-
 3 files changed, 64 insertions(+), 2 deletions(-)
 create mode 100644 bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/VariableFeasibleRegionTask.h
 create mode 100644 bindings/python/TSID/src/VariableFeasibleRegionTask.cpp

diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/VariableFeasibleRegionTask.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/VariableFeasibleRegionTask.h
new file mode 100644
index 0000000000..c1094d0f20
--- /dev/null
+++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/VariableFeasibleRegionTask.h
@@ -0,0 +1,26 @@
+/**
+ * @file VariableFeaibleRegionTask.h
+ * @authors Roberto Mauceri
+ * @copyright 2025 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_TSID_VARIABLE_FEASIBLE_REGION_TASK_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_TSID_VARIABLE_FEASIBLE_REGION_TASK_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace TSID
+{
+
+void CreateVariableFeasibleRegionTask(pybind11::module& module);
+
+} // namespace TSID
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_TSID_VARIABLE_FEASIBLE_REGION_TASK_H
diff --git a/bindings/python/TSID/src/VariableFeasibleRegionTask.cpp b/bindings/python/TSID/src/VariableFeasibleRegionTask.cpp
new file mode 100644
index 0000000000..9583892547
--- /dev/null
+++ b/bindings/python/TSID/src/VariableFeasibleRegionTask.cpp
@@ -0,0 +1,37 @@
+/**
+ * @file VariableFeasibleRegionTask.cpp
+ * @authors Roberto Mauceri
+ * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/TSID/TSIDLinearTask.h>
+#include <BipedalLocomotion/TSID/VariableFeasibleRegionTask.h>
+#include <BipedalLocomotion/bindings/TSID/VariableFeasibleRegionTask.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace TSID
+{
+
+void CreateVariableRegularizationTask(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    using namespace ::BipedalLocomotion::TSID;
+
+    py::class_<VariableFeasibleRegionTask,
+               std::shared_ptr<VariableFeasibleRegionTask>,
+               TSIDLinearTask>(module, "VariableFeasibleRegionTask")
+        .def(py::init())
+        .def("set_feasible_region", &VariableFeasibleRegionTask::setFeasibleRegion, py::arg("C"), py::arg("l"), py::arg("u"));
+}
+
+} // namespace TSID
+} // namespace bindings
+} // namespace BipedalLocomotion
diff --git a/src/TSID/src/VariableFeasibleRegionTask.cpp b/src/TSID/src/VariableFeasibleRegionTask.cpp
index 6ef187eb1e..b2407c66c9 100644
--- a/src/TSID/src/VariableFeasibleRegionTask.cpp
+++ b/src/TSID/src/VariableFeasibleRegionTask.cpp
@@ -185,14 +185,13 @@ bool VariableFeasibleRegionTask::setFeasibleRegion(
     m_isValid = true;
 
     // resize the matrices
+    m_T.resize(2 * C.rows(), C.cols());
     m_A.resize(2 * C.rows(), m_NumberOfVariables);
     m_b.resize(2 * C.rows());
-    m_T.resize(2 * C.rows(), C.cols());
 
     // set the matrices
     m_T << C, -C;
     m_A = m_T * m_S;
-
     m_b << u, -l;
     return true;
 }

From 69ac8a8fda087930b7ad2536b10c628bfa54c82a Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Fri, 17 Jan 2025 16:58:39 +0100
Subject: [PATCH 12/20] modified 'Module.cpp' and 'CHANGELOG.md'. Formatted
 with clang.

---
 CHANGELOG.md                                  |  1 +
 .../TSID/VariableFeasibleRegionTask.h         |  2 +-
 bindings/python/TSID/src/Module.cpp           |  1 +
 .../TSID/VariableFeasibleRegionTask.h         | 37 +++++++++++--------
 src/TSID/src/VariableFeasibleRegionTask.cpp   | 28 +++++++-------
 5 files changed, 39 insertions(+), 30 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index a7462cab84..001db19cc8 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -3,6 +3,7 @@ All notable changes to this project are documented in this file.
 
 ## [unreleased]
 ### Added
+- Add `VariableFeasibleRegionTask` in the TSID controller (https://github.com/ami-iit/bipedal-locomotion-framework/pull/922)
 ### Changed
 - Some improvements on the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910)
 - Removed ROS1 device publisher and the corresponding example (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910)
diff --git a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/VariableFeasibleRegionTask.h b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/VariableFeasibleRegionTask.h
index c1094d0f20..dd63004bb5 100644
--- a/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/VariableFeasibleRegionTask.h
+++ b/bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/VariableFeasibleRegionTask.h
@@ -1,5 +1,5 @@
 /**
- * @file VariableFeaibleRegionTask.h
+ * @file VariableFeasibleRegionTask.h
  * @authors Roberto Mauceri
  * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
diff --git a/bindings/python/TSID/src/Module.cpp b/bindings/python/TSID/src/Module.cpp
index 8dd7a19df5..881585bf40 100644
--- a/bindings/python/TSID/src/Module.cpp
+++ b/bindings/python/TSID/src/Module.cpp
@@ -43,6 +43,7 @@ void CreateModule(pybind11::module& module)
     CreateFeasibleContactWrenchTask(module);
     CreateTaskSpaceInverseDynamics(module);
     CreateVariableRegularizationTask(module);
+    CreateVariableFeasibleRegionTask(module);
     CreateAngularMomentumTask(module);
     CreateQPTSID(module);
     CreateQPFixedBaseTSID(module);
diff --git a/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h b/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
index 21fb4e08cf..e25b006dd7 100644
--- a/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
+++ b/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
@@ -16,18 +16,21 @@ namespace TSID
 {
 /**
  * VariableFeasibleRegionTask is a concrete implementation of the Task. Please use this element if
- * you want to create a inequality linear in the variable. The task represents the following equation
+ * you want to create a inequality linear constraint on a subset of the optimization variable. The
+ * task represents the following equation:
  * \f[
- * Ax < b
+ * l < Cx < u
  * \f]
- * where \f$x\f$ are the elements of the variable you want to constraint.
- */    
-
+ * where \f$C\f$ is a generic transformation matrix (m x n), \f$l\f$ is a vector of lower bounds (m
+ * x 1), \f$u\f$ is a vector of upper bounds (m x 1), and \f$x\f$ are the elements of the variable
+ * you want to consider (n x 1). \f$m\f$ is the number of constraints and \f$n\f$ is the number of
+ * variables.
+ */
 class VariableFeasibleRegionTask : public TSIDLinearTask
 {
     std::string m_variableName; /**< Name of the variable considered in the task. */
     std::vector<std::string> m_controlledElements; /**< Name of the variable elements considered in
-                                                     the task. */
+                                                      the task. */
     bool m_isInitialized{false}; /**< True if the task has been initialized. */
     bool m_isValid{false}; /**< True if the task is valid. */
     std::size_t m_NumberOfVariables{0}; /**< Number of variables. */
@@ -37,6 +40,7 @@ class VariableFeasibleRegionTask : public TSIDLinearTask
     Eigen::MatrixXd m_T; /**< Transformation Matrix. */
 
 public:
+    // clang-format off
     /**
      * Initialize the planner.
      * @param paramHandler pointer to the parameters handler.
@@ -44,12 +48,13 @@ class VariableFeasibleRegionTask : public TSIDLinearTask
      * @note the following parameters are required by the class
      * |  Parameter Name  |   Type   |                                   Description                                          | Mandatory |
      * |:----------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:|
-     * | `variable_name`  | `string` | Name of the variable that you want to regularize. |    Yes    |
-     * | `variable_size`  |   `int`  | Number of the elements that will be regularized. |    Yes     |
-     * | `elements_name`  | `vector` | Name of the elements to consider. If not specified all the elements are constrained |    No     |
+     * | `variable_name`  | `string` | Name of the variable that you want to regularize.                                      |    Yes    |
+     * | `variable_size`  |   `int`  | Number of the elements that will be regularized.                                       |    Yes    |
+     * | `elements_name`  | `vector` | Name of the elements to consider. If not specified all the elements are constrained    |    No     |
      * @return True in case of success, false otherwise.
      */
     bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+    // clang-format on
 
     /**
      * Set the set of variables required by the TSIDLinearTask. The variables are stored in the
@@ -62,15 +67,15 @@ class VariableFeasibleRegionTask : public TSIDLinearTask
 
     /**
      * Set the region of feasibility for the desired elements of the variable.
-     * @param C change of coordinates matrix
-     * @param l lower_bounds
-     * @param u upper_bounds
+     * @param C generic linear transformation matrix (m x n) where m is the number of constraints
+     * and n is the number of variables.
+     * @param l lower_bounds (m x 1)
+     * @param u upper_bounds (m x 1)
      * @return True in case of success, false otherwise.
      */
-    bool setFeasibleRegion(
-        Eigen::Ref<const Eigen::MatrixXd> C, 
-        Eigen::Ref<const Eigen::VectorXd> l, 
-        Eigen::Ref<const Eigen::VectorXd> u);
+    bool setFeasibleRegion(Eigen::Ref<const Eigen::MatrixXd> C,
+                           Eigen::Ref<const Eigen::VectorXd> l,
+                           Eigen::Ref<const Eigen::VectorXd> u);
 
     /**
      * Get the size of the task. (I.e the number of rows of the vector b)
diff --git a/src/TSID/src/VariableFeasibleRegionTask.cpp b/src/TSID/src/VariableFeasibleRegionTask.cpp
index b2407c66c9..eb8b9a7201 100644
--- a/src/TSID/src/VariableFeasibleRegionTask.cpp
+++ b/src/TSID/src/VariableFeasibleRegionTask.cpp
@@ -5,8 +5,8 @@
  * distributed under the terms of the BSD-3-Clause license.
  */
 
-#include <BipedalLocomotion/TSID/VariableFeasibleRegionTask.h>
 #include <BipedalLocomotion/System/VariablesHandler.h>
+#include <BipedalLocomotion/TSID/VariableFeasibleRegionTask.h>
 #include <BipedalLocomotion/TextLogging/Logger.h>
 
 #include <iDynTree/EigenHelpers.h>
@@ -24,7 +24,8 @@ bool VariableFeasibleRegionTask::setVariablesHandler(const VariablesHandler& var
 
     if (!m_isInitialized)
     {
-        log()->error("{} The task is not initialized. Please call initialize() method.", errorPrefix);
+        log()->error("{} The task is not initialized. Please call initialize() method.",
+                     errorPrefix);
         return false;
     }
 
@@ -38,7 +39,8 @@ bool VariableFeasibleRegionTask::setVariablesHandler(const VariablesHandler& var
 
     if (m_variableSize > variable.size)
     {
-        log()->error("{} The expected size is greater than the one stored in the variable named {}.",
+        log()->error("{} The expected size is greater than the one stored in the variable named "
+                     "{}.",
                      errorPrefix,
                      m_variableName);
         return false;
@@ -65,8 +67,7 @@ bool VariableFeasibleRegionTask::setVariablesHandler(const VariablesHandler& var
             }
             m_S(i, index) = 1;
         }
-    } 
-    else
+    } else
     {
         if (m_variableSize != variable.size)
         {
@@ -80,7 +81,8 @@ bool VariableFeasibleRegionTask::setVariablesHandler(const VariablesHandler& var
         }
         // if the size of the m_controlledElements vector is zero, this means that the entire
         // variable is regularized
-        // iDynTree::toEigen(this->subA(variable)).setIdentity(); // devo sostituirlo col comando sotto?
+        // iDynTree::toEigen(this->subA(variable)).setIdentity(); // devo sostituirlo col comando
+        // sotto?
         m_S = Eigen::MatrixXd::Identity(m_variableSize, m_variableSize);
     }
     return true;
@@ -88,7 +90,7 @@ bool VariableFeasibleRegionTask::setVariablesHandler(const VariablesHandler& var
 
 bool VariableFeasibleRegionTask::initialize(std::weak_ptr<const IParametersHandler> paramHandler)
 {
-   constexpr auto errorPrefix = "[VariableFeasibleRegionTask::initialize]";
+    constexpr auto errorPrefix = "[VariableFeasibleRegionTask::initialize]";
 
     auto ptr = paramHandler.lock();
     if (ptr == nullptr)
@@ -143,17 +145,17 @@ bool VariableFeasibleRegionTask::initialize(std::weak_ptr<const IParametersHandl
     return true;
 }
 
-bool VariableFeasibleRegionTask::setFeasibleRegion(
-    Eigen::Ref<const Eigen::MatrixXd> C, 
-    Eigen::Ref<const Eigen::VectorXd> l, 
-    Eigen::Ref<const Eigen::VectorXd> u)
+bool VariableFeasibleRegionTask::setFeasibleRegion(Eigen::Ref<const Eigen::MatrixXd> C,
+                                                   Eigen::Ref<const Eigen::VectorXd> l,
+                                                   Eigen::Ref<const Eigen::VectorXd> u)
 {
     m_isValid = false;
     constexpr auto errorPrefix = "[VariableFeasibleRegionTask::setFeasibleRegion]";
     // check if the size of the matrices is correct
     if (C.cols() != m_variableSize)
     {
-        log()->error("{} The number of columns of the matrix C is not correct. Expected: {}, Passed: {}.",
+        log()->error("{} The number of columns of the matrix C is not correct. Expected: {}, "
+                     "Passed: {}.",
                      errorPrefix,
                      m_variableSize,
                      C.cols());
@@ -178,7 +180,7 @@ bool VariableFeasibleRegionTask::setFeasibleRegion(
     if (!(u.array() >= l.array()).all())
     {
         log()->error("{} The elements of the vector u must be greater than or equal to"
-        " the elements of the vector l.",
+                     " the elements of the vector l.",
                      errorPrefix);
         return false;
     }

From 2e59d9c5ab008e8af1c4bf72755398b0105c4eba Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Fri, 17 Jan 2025 17:04:01 +0100
Subject: [PATCH 13/20] formatted the file 'VariableFeasibleRegionTaskTest.cpp'

---
 .../tests/VariableFeasibleRegionTaskTest.cpp     | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp b/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
index 9218ea8481..12a66be420 100644
--- a/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
+++ b/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
@@ -89,25 +89,25 @@ TEST_CASE("Variable Feasible Region Task")
                     .isZero());
 
         REQUIRE(A.middleCols(variablesHandler.getVariable(variable2Name).offset,
-                              variablesHandler.getVariable(variable2Name).size)
-                     .isZero());
+                             variablesHandler.getVariable(variable2Name).size)
+                    .isZero());
 
         // check the matrix A
-        Eigen::MatrixXd expectedA(2*anyDimension, variableTotalSize);
-        Eigen::MatrixXd expectedT(2*anyDimension, variable1ControlledElementNames.size());
+        Eigen::MatrixXd expectedA(2 * anyDimension, variableTotalSize);
+        Eigen::MatrixXd expectedT(2 * anyDimension, variable1ControlledElementNames.size());
         Eigen::MatrixXd expectedS(variable1ControlledElementNames.size(), variableTotalSize);
         std::size_t index = variablesHandler.getVariable(variable1Name).offset;
 
         expectedS.setZero();
         expectedS(0, index + 0) = 1.0;
         expectedS(1, index + 2) = 1.0;
-        
+
         expectedT << C, -C;
         expectedA = expectedT * expectedS;
         REQUIRE(A.isApprox(expectedA));
 
         // check the vector B
-        Eigen::VectorXd expectedB(2*anyDimension);
+        Eigen::VectorXd expectedB(2 * anyDimension);
         expectedB << u, -l;
         REQUIRE(B.isApprox(expectedB));
     }
@@ -140,12 +140,12 @@ TEST_CASE("Variable Feasible Region Task")
         Eigen::Ref<const Eigen::VectorXd> B = task2.getB();
 
         // A == expectedT implies S == identity
-        Eigen::MatrixXd expectedT(2*anyDimension, variable2ElementSize);
+        Eigen::MatrixXd expectedT(2 * anyDimension, variable2ElementSize);
         expectedT << C, -C;
         REQUIRE(A.isApprox(expectedT));
 
         // check the vector B
-        Eigen::VectorXd expectedB(2*anyDimension);
+        Eigen::VectorXd expectedB(2 * anyDimension);
         expectedB << u, -l;
         REQUIRE(B.isApprox(expectedB));
     }

From 122092be8c7c4f026783858985fc2ccd4c76ff6a Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Fri, 17 Jan 2025 18:16:37 +0100
Subject: [PATCH 14/20] added #include in 'Module.cpp'

---
 bindings/python/TSID/src/Module.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/bindings/python/TSID/src/Module.cpp b/bindings/python/TSID/src/Module.cpp
index 881585bf40..fa7fc1754f 100644
--- a/bindings/python/TSID/src/Module.cpp
+++ b/bindings/python/TSID/src/Module.cpp
@@ -21,6 +21,7 @@
 #include <BipedalLocomotion/bindings/TSID/TSIDLinearTask.h>
 #include <BipedalLocomotion/bindings/TSID/TaskSpaceInverseDynamics.h>
 #include <BipedalLocomotion/bindings/TSID/VariableRegularizationTask.h>
+#include <BipedalLocomotion/bindings/TSID/VariableFeasibleRegionTask.h>
 
 namespace BipedalLocomotion
 {

From d7f3575852d83eef1b56f4377d0d01a4b47a61ed Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Mon, 20 Jan 2025 15:16:10 +0100
Subject: [PATCH 15/20] minor changes

---
 bindings/python/TSID/src/VariableFeasibleRegionTask.cpp   | 2 +-
 .../BipedalLocomotion/TSID/VariableFeasibleRegionTask.h   | 8 ++++----
 src/TSID/src/VariableFeasibleRegionTask.cpp               | 5 ++---
 3 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/bindings/python/TSID/src/VariableFeasibleRegionTask.cpp b/bindings/python/TSID/src/VariableFeasibleRegionTask.cpp
index 9583892547..52bbbdf6fb 100644
--- a/bindings/python/TSID/src/VariableFeasibleRegionTask.cpp
+++ b/bindings/python/TSID/src/VariableFeasibleRegionTask.cpp
@@ -20,7 +20,7 @@ namespace bindings
 namespace TSID
 {
 
-void CreateVariableRegularizationTask(pybind11::module& module)
+void CreateVariableFeasibleRegionTask(pybind11::module& module)
 {
     namespace py = ::pybind11;
     using namespace ::BipedalLocomotion::TSID;
diff --git a/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h b/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
index e25b006dd7..9f5d44b43a 100644
--- a/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
+++ b/src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h
@@ -5,8 +5,8 @@
  * distributed under the terms of the BSD-3-Clause license.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_TSID_VARIABLE_LINEAR_TASK_H
-#define BIPEDAL_LOCOMOTION_TSID_VARIABLE_LINEAR_TASK_H
+#ifndef BIPEDAL_LOCOMOTION_TSID_VARIABLE_FEASIBLE_REGION_TASK_H
+#define BIPEDAL_LOCOMOTION_TSID_VARIABLE_FEASIBLE_REGION_TASK_H
 
 #include <BipedalLocomotion/TSID/TSIDLinearTask.h>
 
@@ -19,7 +19,7 @@ namespace TSID
  * you want to create a inequality linear constraint on a subset of the optimization variable. The
  * task represents the following equation:
  * \f[
- * l < Cx < u
+ * l \ne Cx \ne u
  * \f]
  * where \f$C\f$ is a generic transformation matrix (m x n), \f$l\f$ is a vector of lower bounds (m
  * x 1), \f$u\f$ is a vector of upper bounds (m x 1), and \f$x\f$ are the elements of the variable
@@ -99,4 +99,4 @@ class VariableFeasibleRegionTask : public TSIDLinearTask
 } // namespace TSID
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_TSID_VARIABLE_LINEAR_TASK_H
+#endif // BIPEDAL_LOCOMOTION_TSID_VARIABLE_FEASIBLE_REGION_TASK_H
diff --git a/src/TSID/src/VariableFeasibleRegionTask.cpp b/src/TSID/src/VariableFeasibleRegionTask.cpp
index eb8b9a7201..97f76b3c06 100644
--- a/src/TSID/src/VariableFeasibleRegionTask.cpp
+++ b/src/TSID/src/VariableFeasibleRegionTask.cpp
@@ -80,9 +80,8 @@ bool VariableFeasibleRegionTask::setVariablesHandler(const VariablesHandler& var
             return false;
         }
         // if the size of the m_controlledElements vector is zero, this means that the entire
-        // variable is regularized
-        // iDynTree::toEigen(this->subA(variable)).setIdentity(); // devo sostituirlo col comando
-        // sotto?
+        // variable is considered
+        // iDynTree::toEigen(this->subA(variable)).setIdentity();
         m_S = Eigen::MatrixXd::Identity(m_variableSize, m_variableSize);
     }
     return true;

From 3da406afc4b1911316b31d4607beeec80ea01b73 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Mon, 20 Jan 2025 17:12:12 +0100
Subject: [PATCH 16/20] modified the 'CMakeLists.txt' in 'bindigs/python/TSID'

---
 bindings/python/TSID/CMakeLists.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/bindings/python/TSID/CMakeLists.txt b/bindings/python/TSID/CMakeLists.txt
index dd6870b681..3d738b0a41 100644
--- a/bindings/python/TSID/CMakeLists.txt
+++ b/bindings/python/TSID/CMakeLists.txt
@@ -8,8 +8,8 @@ if(TARGET BipedalLocomotion::TSID)
 
   add_bipedal_locomotion_python_module(
     NAME TSIDBindings
-    SOURCES src/BaseDynamicsTask.cpp src/CoMTask.cpp src/FeasibleContactWrenchTask.cpp src/JointDynamicsTask.cpp src/JointTrackingTask.cpp src/Module.cpp src/QPTSID.cpp src/SE3Task.cpp src/SO3Task.cpp src/TaskSpaceInverseDynamics.cpp src/TSIDLinearTask.cpp src/VariableRegularizationTask.cpp src/AngularMomentumTask.cpp src/R3Task.cpp
-    HEADERS ${H_PREFIX}/BaseDynamicsTask.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/FeasibleContactWrenchTask.h ${H_PREFIX}/JointDynamicsTask.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/QPTSID.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/TaskSpaceInverseDynamics.h ${H_PREFIX}/TSIDLinearTask.h ${H_PREFIX}/VariableRegularizationTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/R3Task.h
+    SOURCES src/BaseDynamicsTask.cpp src/CoMTask.cpp src/FeasibleContactWrenchTask.cpp src/JointDynamicsTask.cpp src/JointTrackingTask.cpp src/Module.cpp src/QPTSID.cpp src/SE3Task.cpp src/SO3Task.cpp src/TaskSpaceInverseDynamics.cpp src/TSIDLinearTask.cpp src/VariableRegularizationTask.cpp src/VariableFeasibleRegionTask.cpp src/AngularMomentumTask.cpp src/R3Task.cpp
+    HEADERS ${H_PREFIX}/BaseDynamicsTask.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/FeasibleContactWrenchTask.h ${H_PREFIX}/JointDynamicsTask.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/QPTSID.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/TaskSpaceInverseDynamics.h ${H_PREFIX}/TSIDLinearTask.h ${H_PREFIX}/VariableRegularizationTask.h ${H_PREFIX}/VariableFeasibleRegionTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/R3Task.h
     LINK_LIBRARIES BipedalLocomotion::TSID
     TESTS tests/test_TSID.py
     TESTS_RUNTIME_CONDITIONS FRAMEWORK_USE_icub-models

From b8820acd0fd327ffd967ff91c89df09bea8e9542 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Mon, 20 Jan 2025 17:15:49 +0100
Subject: [PATCH 17/20] fixed the computation of 'm_S' when
 'm_controlledElements.size() == 0'

---
 src/TSID/src/VariableFeasibleRegionTask.cpp       | 7 +++++--
 src/TSID/tests/VariableFeasibleRegionTaskTest.cpp | 9 ++++++---
 2 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/src/TSID/src/VariableFeasibleRegionTask.cpp b/src/TSID/src/VariableFeasibleRegionTask.cpp
index 97f76b3c06..bc93e9960d 100644
--- a/src/TSID/src/VariableFeasibleRegionTask.cpp
+++ b/src/TSID/src/VariableFeasibleRegionTask.cpp
@@ -81,8 +81,11 @@ bool VariableFeasibleRegionTask::setVariablesHandler(const VariablesHandler& var
         }
         // if the size of the m_controlledElements vector is zero, this means that the entire
         // variable is considered
-        // iDynTree::toEigen(this->subA(variable)).setIdentity();
-        m_S = Eigen::MatrixXd::Identity(m_variableSize, m_variableSize);
+        int startIndex = variable.offset;
+        for (int i = 0; i < m_variableSize; i++)
+        {
+            m_S(i, startIndex + i) = 1.0;
+        }
     }
     return true;
 }
diff --git a/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp b/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
index 12a66be420..3c92e7bf60 100644
--- a/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
+++ b/src/TSID/tests/VariableFeasibleRegionTaskTest.cpp
@@ -122,7 +122,7 @@ TEST_CASE("Variable Feasible Region Task")
 
         const int variable2ElementSize = variablesHandler.getVariable(variable2Name).size;
         const int variableTotalSize = variablesHandler.getNumberOfVariables();
-        const int anyDimension = 1234;
+        const int anyDimension = 21;
 
         // the matrix C can have any number of rows
         Eigen::MatrixXd C(anyDimension, variable2ElementSize);
@@ -139,10 +139,13 @@ TEST_CASE("Variable Feasible Region Task")
         Eigen::Ref<const Eigen::MatrixXd> A = task2.getA();
         Eigen::Ref<const Eigen::VectorXd> B = task2.getB();
 
-        // A == expectedT implies S == identity
+        // subA == ExpectedT implies subS == identity
         Eigen::MatrixXd expectedT(2 * anyDimension, variable2ElementSize);
+        Eigen::MatrixXd subA;
+        int variable2ElementOffset = variablesHandler.getVariable(variable2Name).offset;
         expectedT << C, -C;
-        REQUIRE(A.isApprox(expectedT));
+        subA = A.block(0, variable2ElementOffset, 2 * anyDimension, variable2ElementSize);
+        REQUIRE(subA.isApprox(expectedT));
 
         // check the vector B
         Eigen::VectorXd expectedB(2 * anyDimension);

From 51c5cd18c5259741fb59cc63e004b440ef05e44d Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Mon, 20 Jan 2025 18:26:25 +0100
Subject: [PATCH 18/20] added tests in 'test_TSID.py'

---
 bindings/python/TSID/tests/test_TSID.py | 28 +++++++++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/bindings/python/TSID/tests/test_TSID.py b/bindings/python/TSID/tests/test_TSID.py
index 6d51b79b4f..b05060ffb5 100644
--- a/bindings/python/TSID/tests/test_TSID.py
+++ b/bindings/python/TSID/tests/test_TSID.py
@@ -299,3 +299,31 @@ def test_variable_regularization_task():
     regularizer_2 = blf.tsid.VariableRegularizationTask()
     assert regularizer_2.initialize(param_handler=param_handler_2)
     assert regularizer_2.set_variables_handler(variables_handler=var_handler)
+
+def test_variable_feasible_region_task():
+
+    # Set the parameters
+    param_handler_1 = blf.parameters_handler.StdParametersHandler()
+    param_handler_1.set_parameter_string(name="variable_name", value="mysterious_variables")
+    param_handler_1.set_parameter_int(name="variable_size", value=15)
+
+    # Set the parameters
+    param_handler_2 = blf.parameters_handler.StdParametersHandler()
+    param_handler_2.set_parameter_string(name="variable_name", value="torques")
+    param_handler_2.set_parameter_int(name="variable_size", value=3)
+    param_handler_2.set_parameter_vector_string(name="elements_name",
+                                                value = ["roll", "pitch", "yaw"])
+
+    var_handler = blf.system.VariablesHandler()
+    var_handler.add_variable("mysterious_variables", 15)
+    var_handler.add_variable("torques", ["roll", "pitch"])
+
+    # Initialize the task
+    task_1 = blf.tsid.VariableFeasibleRegionTask()
+    assert task_1.initialize(param_handler=param_handler_1)
+    assert task_1.set_variables_handler(variables_handler=var_handler)
+
+    # Initialize the task
+    task_2 = blf.tsid.VariableFeasibleRegionTask()
+    assert task_2.initialize(param_handler=param_handler_2)
+    assert task_2.set_variables_handler(variables_handler=var_handler)

From cc80f581a814ebd1d381e36de2506983ad007d54 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Wed, 22 Jan 2025 16:39:49 +0100
Subject: [PATCH 19/20] fixed and extended tests in 'test_TSID.py'

---
 bindings/python/TSID/tests/test_TSID.py | 30 ++++++++++++++++++++++---
 1 file changed, 27 insertions(+), 3 deletions(-)

diff --git a/bindings/python/TSID/tests/test_TSID.py b/bindings/python/TSID/tests/test_TSID.py
index b05060ffb5..39645ae9f9 100644
--- a/bindings/python/TSID/tests/test_TSID.py
+++ b/bindings/python/TSID/tests/test_TSID.py
@@ -310,13 +310,13 @@ def test_variable_feasible_region_task():
     # Set the parameters
     param_handler_2 = blf.parameters_handler.StdParametersHandler()
     param_handler_2.set_parameter_string(name="variable_name", value="torques")
-    param_handler_2.set_parameter_int(name="variable_size", value=3)
+    param_handler_2.set_parameter_int(name="variable_size", value=2)
     param_handler_2.set_parameter_vector_string(name="elements_name",
-                                                value = ["roll", "pitch", "yaw"])
+                                                value = ["roll", "pitch"])
 
     var_handler = blf.system.VariablesHandler()
     var_handler.add_variable("mysterious_variables", 15)
-    var_handler.add_variable("torques", ["roll", "pitch"])
+    var_handler.add_variable("torques", ["roll", "pitch", "yaw"])
 
     # Initialize the task
     task_1 = blf.tsid.VariableFeasibleRegionTask()
@@ -327,3 +327,27 @@ def test_variable_feasible_region_task():
     task_2 = blf.tsid.VariableFeasibleRegionTask()
     assert task_2.initialize(param_handler=param_handler_2)
     assert task_2.set_variables_handler(variables_handler=var_handler)
+
+    # Test set_feasible_region (correct case)
+    C = np.array([[1, 2], [0, 1]])
+    l = np.array([0, 0])
+    u = np.array([1, 1])
+    assert task_2.set_feasible_region(C, l, u)
+
+    # Test infinite bounds (correct case)
+    C = np.array([[1, 2], [0, 1]])
+    l = np.array([-np.infty, 0])
+    u = np.array([1, np.infty])
+    assert task_2.set_feasible_region(C, l, u)
+
+    # Test wrong dimensions (incorrect case)
+    C = np.array([[1, 2], [0, 1]])
+    l = np.array([0, 0, 0])
+    u = np.array([1, 1, 1])
+    assert not task_2.set_feasible_region(C, l, u)
+
+    # Test lower bound greater than upper bound (incorrect case)
+    C = np.array([[1, 2], [0, 1]])
+    l = np.array([0, 2])
+    u = np.array([1, 1])
+    assert not task_2.set_feasible_region(C, l, u)

From 37c3227859af6a8182fd980e63be41af9a56cad7 Mon Sep 17 00:00:00 2001
From: Roberto Mauceri <rob_mau@hotmail.it>
Date: Wed, 22 Jan 2025 16:50:50 +0100
Subject: [PATCH 20/20] added test in 'test_TSID.py'

---
 bindings/python/TSID/tests/test_TSID.py | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/bindings/python/TSID/tests/test_TSID.py b/bindings/python/TSID/tests/test_TSID.py
index 39645ae9f9..ff34a0e5f6 100644
--- a/bindings/python/TSID/tests/test_TSID.py
+++ b/bindings/python/TSID/tests/test_TSID.py
@@ -340,7 +340,13 @@ def test_variable_feasible_region_task():
     u = np.array([1, np.infty])
     assert task_2.set_feasible_region(C, l, u)
 
-    # Test wrong dimensions (incorrect case)
+    # Test inconsistency with the variable size = 2 (incorrect case)
+    C = np.array([[1, 2, 0], [0, 1, 2], [0, 0, 1]])
+    l = np.array([0, 0, 0])
+    u = np.array([1, 1, 1])
+    assert not task_2.set_feasible_region(C, l, u)
+
+    # Test inconsistent dimensions among C, l, and u (incorrect case)
     C = np.array([[1, 2], [0, 1]])
     l = np.array([0, 0, 0])
     u = np.array([1, 1, 1])