Variable feasible region task

CHANGELOG.md

@@ -4,6 +4,7 @@ All notable changes to this project are documented in this file.
 ## [unreleased]
 ### Added
 - Add `ZMPgenerator` to `UnicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/916)
+- Add `VariableFeasibleRegionTaskVariableFeasibleRegionTask` 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)

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

bindings/python/TSID/include/BipedalLocomotion/bindings/TSID/VariableFeasibleRegionTask.h

@@ -0,0 +1,26 @@
+/**
+ * @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.
+ */
+
+#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

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
 {
@@ -43,6 +44,7 @@ void CreateModule(pybind11::module& module)
     CreateFeasibleContactWrenchTask(module);
     CreateTaskSpaceInverseDynamics(module);
     CreateVariableRegularizationTask(module);
+    CreateVariableFeasibleRegionTask(module);
     CreateAngularMomentumTask(module);
     CreateQPTSID(module);
     CreateQPFixedBaseTSID(module);

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 CreateVariableFeasibleRegionTask(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

bindings/python/TSID/tests/test_TSID.py

@@ -299,3 +299,61 @@ 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=2)
+    param_handler_2.set_parameter_vector_string(name="elements_name",
+                                                value = ["roll", "pitch"])
+
+    var_handler = blf.system.VariablesHandler()
+    var_handler.add_variable("mysterious_variables", 15)
+    var_handler.add_variable("torques", ["roll", "pitch", "yaw"])
+
+    # 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)
+
+    # 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 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])
+    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)

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

src/TSID/include/BipedalLocomotion/TSID/VariableFeasibleRegionTask.h

@@ -0,0 +1,102 @@
+/**
+ * @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.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_TSID_VARIABLE_FEASIBLE_REGION_TASK_H
+#define BIPEDAL_LOCOMOTION_TSID_VARIABLE_FEASIBLE_REGION_TASK_H
+
+#include <BipedalLocomotion/TSID/TSIDLinearTask.h>
+
+namespace BipedalLocomotion
+{
+namespace TSID
+{
+/**
+ * VariableFeasibleRegionTask is a concrete implementation of the Task. Please use this element if
+ * you want to create a inequality linear constraint on a subset of the optimization variable. The
+ * task represents the following equation:
+ * \f[
+ * 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
+ * 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. */
+    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_T; /**< Transformation Matrix. */
+
+public:
+    // clang-format off
+    /**
+     * 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;
+    // clang-format on
+
+    /**
+     * 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 region of feasibility for the desired elements of the variable.
+     * @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);
+
+    /**
+     * 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 VariableFeasibleRegionTask 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_FEASIBLE_REGION_TASK_H