Repository: ami-iit/biomechanical-analysis-framework
Pull request #57: Update regularization task: added joint position regularization
Author: login: LudovicaDanovaro
Created: 2025-03-05T09:56:44Z | Merged: 2025-03-10T15:21:38Z

- Added joint position regularization alongside velocity regularization (see https://github.com/ami-iit/element_human-body-estimation/issues/98)

This PR introduces a new overload of `HumanIK::updateJointRegularizationTask()` that takes a `const Eigen::VectorXd& jointPositionSetPoint` as an argument ⮕ `HumanIK::updateJointRegularizationTask(const Eigen::VectorXd& jointPositionSetPoint)`. The original function remains unchanged, so if no joint regularization is needed, simply continue calling `HumanIK::updateJointRegularizationTask()` without any arguments.


Diff:
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index b328933..e135a93 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -67,10 +67,11 @@ jobs:
           if: matrix.os == 'ubuntu-latest'
           shell: bash -l {0}
           run: |
-            # Additional dependencies only useful on Linux
-            # See https://github.com/robotology/robotology-superbuild/issues/477
-            micromamba install expat-cos6-x86_64 libselinux-cos6-x86_64 libxau-cos6-x86_64 libxcb-cos6-x86_64 libxdamage-cos6-x86_64 libxext-cos6-x86_64 libxfixes-cos6-x86_64 libxxf86vm-cos6-x86_64 mesalib mesa-libgl-cos6-x86_64 mesa-libgl-devel-cos6-x86_64
-
+            # Additional dependencies only useful on Linux, both at the conda and the system level,
+            # see https://conda-forge.org/docs/maintainer/maintainer_faq/#how-do-i-fix-the-libglso1-import-error
+            micromamba install libgl-devel
+            sudo apt-get update
+            sudo apt install libgl1-mesa-dri libglx-mesa0 libegl-mesa0
 
         # ===================
         # CMAKE-BASED PROJECT
diff --git a/bindings/python/ID/src/InverseDynamics.cpp b/bindings/python/ID/src/InverseDynamics.cpp
index 5b32ead..981a946 100644
--- a/bindings/python/ID/src/InverseDynamics.cpp
+++ b/bindings/python/ID/src/InverseDynamics.cpp
@@ -31,43 +31,41 @@ void CreateInverseDynamics(pybind11::module& module)
 
     py::class_<HumanID>(module, "HumanID")
         .def(py::init())
-        .def(
-            "initialize",
-            [](HumanID& id, std::shared_ptr<const IParametersHandler> handler, py::object& obj) -> bool {
-                std::shared_ptr<iDynTree::KinDynComputations>* cls
-                    = py::detail::swig_wrapped_pointer_to_pybind<std::shared_ptr<iDynTree::KinDynComputations>>(obj);
+        .def("initialize",
+             [](HumanID& id, std::shared_ptr<const IParametersHandler> handler, py::object& obj) -> bool {
+                 std::shared_ptr<iDynTree::KinDynComputations>* cls
+                     = py::detail::swig_wrapped_pointer_to_pybind<std::shared_ptr<iDynTree::KinDynComputations>>(obj);
 
-                if (cls == nullptr)
-                {
-                    throw ::pybind11::value_error("Invalid input for the function. Please provide "
-                                                  "an iDynTree::KinDynComputations object.");
-                }
+                 if (cls == nullptr)
+                 {
+                     throw ::pybind11::value_error("Invalid input for the function. Please provide "
+                                                   "an iDynTree::KinDynComputations object.");
+                 }
 
-                return id.initialize(handler, *cls);
-            },
-            py::arg("param_handler"),
-            py::arg("kin_dyn"))
-        .def(
-            "updateExtWrenchesMeasurements",
-            [](HumanID& id, const std::unordered_map<std::string, Eigen::VectorXd>& wrenchesEigen) -> bool {
-                std::unordered_map<std::string, iDynTree::Wrench> wrenches;
-                for (const auto& [key, value] : wrenchesEigen)
-                {
-                    if (value.size() != 6)
-                    {
-                        throw ::pybind11::value_error("Invalid input for the function. The wrenches "
-                                                      "must have 6 elements.");
-                    }
+                 return id.initialize(handler, *cls);
+             },
+             py::arg("param_handler"),
+             py::arg("kin_dyn"))
+        .def("updateExtWrenchesMeasurements",
+             [](HumanID& id, const std::unordered_map<std::string, Eigen::VectorXd>& wrenchesEigen) -> bool {
+                 std::unordered_map<std::string, iDynTree::Wrench> wrenches;
+                 for (const auto & [ key, value ] : wrenchesEigen)
+                 {
+                     if (value.size() != 6)
+                     {
+                         throw ::pybind11::value_error("Invalid input for the function. The wrenches "
+                                                       "must have 6 elements.");
+                     }
 
-                    iDynTree::Wrench w;
-                    w.setLinearVec3(iDynTree::GeomVector3(value(0), value(1), value(2)));
-                    w.setAngularVec3(iDynTree::GeomVector3(value(3), value(4), value(5)));
+                     iDynTree::Wrench w;
+                     w.setLinearVec3(iDynTree::GeomVector3(value(0), value(1), value(2)));
+                     w.setAngularVec3(iDynTree::GeomVector3(value(3), value(4), value(5)));
 
-                    wrenches[key] = w;
-                }
-                return id.updateExtWrenchesMeasurements(wrenches);
-            },
-            py::arg("wrenches"))
+                     wrenches[key] = w;
+                 }
+                 return id.updateExtWrenchesMeasurements(wrenches);
+             },
+             py::arg("wrenches"))
         .def("solve", &HumanID::solve)
         .def("getJointTorques",
              [](HumanID& id) -> Eigen::VectorXd {
diff --git a/bindings/python/IK/src/InverseKinematics.cpp b/bindings/python/IK/src/InverseKinematics.cpp
index 8bf3051..f0439ec 100644
--- a/bindings/python/IK/src/InverseKinematics.cpp
+++ b/bindings/python/IK/src/InverseKinematics.cpp
@@ -35,22 +35,21 @@ void CreateInverseKinematics(pybind11::module& module)
 
     py::class_<HumanIK>(module, "HumanIK")
         .def(py::init())
-        .def(
-            "initialize",
-            [](HumanIK& ik, std::shared_ptr<const IParametersHandler> handler, py::object& obj) -> bool {
-                std::shared_ptr<iDynTree::KinDynComputations>* cls
-                    = py::detail::swig_wrapped_pointer_to_pybind<std::shared_ptr<iDynTree::KinDynComputations>>(obj);
+        .def("initialize",
+             [](HumanIK& ik, std::shared_ptr<const IParametersHandler> handler, py::object& obj) -> bool {
+                 std::shared_ptr<iDynTree::KinDynComputations>* cls
+                     = py::detail::swig_wrapped_pointer_to_pybind<std::shared_ptr<iDynTree::KinDynComputations>>(obj);
 
-                if (cls == nullptr)
-                {
-                    throw ::pybind11::value_error("Invalid input for the function. Please provide "
-                                                  "an iDynTree::KinDynComputations object.");
-                }
+                 if (cls == nullptr)
+                 {
+                     throw ::pybind11::value_error("Invalid input for the function. Please provide "
+                                                   "an iDynTree::KinDynComputations object.");
+                 }
 
-                return ik.initialize(handler, *cls);
-            },
-            py::arg("param_handler"),
-            py::arg("kin_dyn"))
+                 return ik.initialize(handler, *cls);
+             },
+             py::arg("param_handler"),
+             py::arg("kin_dyn"))
         .def("setDt", &HumanIK::setDt, py::arg("dt"))
         .def("getDt", &HumanIK::getDt)
         .def("getDoFsNumber", &HumanIK::getDoFsNumber)
@@ -62,7 +61,10 @@ void CreateInverseKinematics(pybind11::module& module)
         .def("calibrateAllWithWorld", &HumanIK::calibrateAllWithWorld, py::arg("nodeStruct"), py::arg("frameName"))
         .def("updateOrientationGravityTasks", &HumanIK::updateOrientationAndGravityTasks, py::arg("nodeStruct"))
         .def("updateFloorContactTasks", &HumanIK::updateFloorContactTasks, py::arg("wrenchMap"), py::arg("linkHeight"))
-        .def("updateJointRegularizationTask", &HumanIK::updateJointRegularizationTask)
+        .def("updateJointRegularizationTask", py::overload_cast<>(&HumanIK::updateJointRegularizationTask))
+        .def("updateJointRegularizationTask",
+             py::overload_cast<const Eigen::VectorXd&>(&HumanIK::updateJointRegularizationTask),
+             py::arg("jointPositionSetPoint"))
         .def("updateJointConstraintsTask", &HumanIK::updateJointConstraintsTask)
         .def("advance", &HumanIK::advance)
         .def("getJointPositions",
diff --git a/src/ID/src/InverseDynamics.cpp b/src/ID/src/InverseDynamics.cpp
index 57394b1..e786e00 100644
--- a/src/ID/src/InverseDynamics.cpp
+++ b/src/ID/src/InverseDynamics.cpp
@@ -735,7 +735,8 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptr<BipedalLocomotio
         switch (berdySensor.type)
         {
 
-        case iDynTree::BerdySensorTypes::NET_EXT_WRENCH_SENSOR: {
+        case iDynTree::BerdySensorTypes::NET_EXT_WRENCH_SENSOR:
+        {
             // Initialize with default covariance
             iDynTree::Vector6 wrenchCovariance;
             for (int i = 0; i < 6; i++)
@@ -754,7 +755,8 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptr<BipedalLocomotio
                     {berdySensor.range.offset + i, berdySensor.range.offset + i, wrenchCovariance[i]});
         }
         break;
-        case iDynTree::BerdySensorTypes::RCM_SENSOR: {
+        case iDynTree::BerdySensorTypes::RCM_SENSOR:
+        {
             auto specificMeasurementsPtr = m_extWrenchesEstimator.params.specificMeasurementsCovariance.find("RCM_"
                                                                                                              "SENS"
                                                                                                              "OR");
diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
index 31ad9a0..7cda879 100644
--- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
+++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
@@ -127,6 +127,9 @@ class HumanIK
 
     Eigen::VectorXd m_calibrationJointPositions; /** Joint positions for calibration */
 
+    Eigen::VectorXd m_jointPositionSetPoint; /** Custom set point for the regularization task */
+    Eigen::VectorXd m_zeroOfDimensionNrDoFs; /** Buffer of dimention nrOfDoFs full of zeros */
+
     /**
      * Struct containing the SO3 task from the BipedalLocomotion IK, the node number and the
      * rotation matrix between the IMU and the link
@@ -398,12 +401,27 @@ class HumanIK
     /**
      * set the setpoint for the joint regularization task.
      * This function is to be called before the advance function to set the joint constraints
+     * This function does not allow to set the position regularization setpoint
      * @param jointPositions joint positions, by defualt it is set to zero
      * @param jointVelocities joint velocities, by defualt it is set to zero
      * @return true if the joint regularization task is set correctly
      */
     bool updateJointRegularizationTask();
 
+    /**
+     * set the setpoint for the joint regularization task.
+     * This function is to be called before the advance function to set the joint constraints
+     * This function allows to set the position regularization setpoint
+     * @param jointPositionSetPoint setpoint for the joint regularization task: it is a vector of the same size of the number of DoFs
+     * @return true if the joint regularization task is set correctly
+     */
+    bool updateJointRegularizationTask(const Eigen::VectorXd& jointPositionSetPoint);
+
+    /**
+     * get the setpoint for the joint regularization task.
+     */
+    Eigen::VectorXd getJointPositionSetPoint();
+
     /**
      * update the joint constraint task.
      * This function is to be called before the advance function to set the joint constraints
diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp
index 8415a34..48e5ab1 100644
--- a/src/IK/src/InverseKinematics.cpp
+++ b/src/IK/src/InverseKinematics.cpp
@@ -38,6 +38,7 @@ bool HumanIK::initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandle
     m_jointPositions.resize(m_kinDyn->getNrOfDegreesOfFreedom());
     m_jointVelocities.resize(m_kinDyn->getNrOfDegreesOfFreedom());
     m_calibrationJointPositions.resize(m_kinDyn->getNrOfDegreesOfFreedom());
+    m_zeroOfDimensionNrDoFs = Eigen::VectorXd::Zero(m_kinDyn->getNrOfDegreesOfFreedom());
 
     // Retrieve the state of the system
     if (!kinDyn->getRobotState(m_basePose, m_jointPositions, m_baseVelocity, m_jointVelocities, m_gravity))
@@ -282,7 +283,7 @@ bool HumanIK::updateFloorContactTask(const int node, const double verticalForce,
     return ok;
 }
 
-bool HumanIK::updateJointRegularizationTask()
+bool HumanIK::updateJointRegularizationTask(const Eigen::VectorXd& jointPositionSetPoint)
 {
     // check if the joint regularization task is initialized
     if (m_jointRegularizationTask == nullptr)
@@ -290,8 +291,15 @@ bool HumanIK::updateJointRegularizationTask()
         BiomechanicalAnalysis::log()->error("[HumanIK::updateJointRegularizationTask] Joint regularization task not initialized.");
         return false;
     }
-    // Set the set point of the joint regularization task to a zero vector of size m_nrDoFs
-    return m_jointRegularizationTask->setSetPoint(Eigen::VectorXd::Zero(m_nrDoFs));
+
+    // Set the set point of the joint regularization task to vector of size m_nrDoFs with the desired joint positions where Kp is different
+    // from zero, otherwise set to zero
+    return m_jointRegularizationTask->setSetPoint(jointPositionSetPoint);
+}
+
+bool HumanIK::updateJointRegularizationTask()
+{
+    return updateJointRegularizationTask(m_zeroOfDimensionNrDoFs);
 }
 
 bool HumanIK::updateJointConstraintsTask()
@@ -309,7 +317,7 @@ bool HumanIK::updateJointConstraintsTask()
 bool HumanIK::updateOrientationAndGravityTasks(const std::unordered_map<int, nodeData>& nodeStruct)
 {
     // Update the orientation and gravity tasks
-    for (const auto& [node, data] : nodeStruct)
+    for (const auto & [ node, data ] : nodeStruct)
     {
         if (m_OrientationTasks.find(node) != m_OrientationTasks.end())
         {
@@ -343,7 +351,7 @@ bool HumanIK::updateOrientationAndGravityTasks(const std::unordered_map<int, nod
 
 bool HumanIK::updateFloorContactTasks(const std::unordered_map<int, Eigen::Matrix<double, 6, 1>>& wrenchMap, const double linkHeight)
 {
-    for (const auto& [node, data] : wrenchMap)
+    for (const auto & [ node, data ] : wrenchMap)
     {
         if (!updateFloorContactTask(node, data(WRENCH_FORCE_Z), linkHeight))
         {
@@ -358,12 +366,12 @@ bool HumanIK::updateFloorContactTasks(const std::unordered_map<int, Eigen::Matri
 
 bool HumanIK::clearCalibrationMatrices()
 {
-    for (auto& [node, data] : m_OrientationTasks)
+    for (auto & [ node, data ] : m_OrientationTasks)
     {
         data.calibrationMatrix = manif::SO3d::Identity();
         data.IMU_R_link = m_OrientationTasks[node].IMU_R_link_init;
     }
-    for (auto& [node, data] : m_GravityTasks)
+    for (auto & [ node, data ] : m_GravityTasks)
     {
         data.calibrationMatrix = manif::SO3d::Identity();
         data.IMU_R_link = m_GravityTasks[node].IMU_R_link_init;
@@ -384,7 +392,7 @@ bool HumanIK::calibrateWorldYaw(std::unordered_map<int, nodeData> nodeStruct)
     baseVelocity.setZero();
     m_kinDyn->setRobotState(basePose, m_calibrationJointPositions, baseVelocity, jointVelocities, m_gravity);
     // Update the orientation and gravity tasks
-    for (const auto& [node, data] : nodeStruct)
+    for (const auto & [ node, data ] : nodeStruct)
     {
         // check if the node number is valid
         if (m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end())
@@ -436,7 +444,7 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map<int, nodeData> nodeStruct
         secondaryCalib = manif::SO3d(refFrameRot.asRPY()(0), refFrameRot.asRPY()(1), refFrameRot.asRPY()(2));
     }
 
-    for (const auto& [node, data] : nodeStruct)
+    for (const auto & [ node, data ] : nodeStruct)
     {
         // check if the node number is valid
         if (m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end())
@@ -513,7 +521,7 @@ bool HumanIK::advance()
     }
 
     // Get the solution (base position, base rotation, joint positions) from the integrator
-    const auto& [basePosition, baseRotation, jointPosition] = m_system.integrator->getSolution();
+    const auto & [ basePosition, baseRotation, jointPosition ] = m_system.integrator->getSolution();
     // Update the base pose and joint positions
     m_basePose.topRightCorner<3, 1>() = basePosition;
     m_basePose.topLeftCorner<3, 3>() = baseRotation.rotation();
@@ -867,7 +875,89 @@ bool HumanIK::initializeJointRegularizationTask(const std::string& taskName,
     // Initialize a vector of proportional gains (kp) with zeros
     std::vector<double> kp(m_kinDyn->getNrOfDegreesOfFreedom(), 0.0);
 
-    // Set the proportional gains (kp) parameter for the task
+    // Retrieve the joints_list_kp parameter from the task handler
+    std::vector<std::string> jointsListKp;
+    if (taskHandler->getParameter("joints_list_kp", jointsListKp))
+    {
+        // Retrieve the joints_kp parameter from the task handler
+        std::vector<double> JointsKp;
+        if (!taskHandler->getParameter("joints_kp", JointsKp))
+        {
+
+            BiomechanicalAnalysis::log()->error("[HumanIK::initializeJointRegularizationTask] "
+                                                "Parameter 'joints_kp' of the {} task is missing; this parameter is only ",
+                                                "required if 'joints_list_kp' is provided. Please provide the 'joints_kp' parameter",
+                                                "or remove the 'joints_list_kp' parameter from the {} task",
+                                                taskName);
+            return false;
+        }
+        if (jointsListKp.size() != JointsKp.size())
+        {
+            BiomechanicalAnalysis::log()->error("[HumanIK::initializeJointRegularizationTask] "
+                                                "The size of the parameter 'joints_kp' of the {} "
+                                                "task is {}, it should be equal to the size of the "
+                                                "parameter 'joints_list_Kp' that is {}",
+                                                taskName,
+                                                JointsKp.size(),
+                                                jointsListKp.size());
+            return false;
+        }
+
+        // Set the kp for the specified joints
+        for (int i = 0; i < jointsListKp.size(); i++)
+        {
+            auto index = m_kinDyn->model().getJointIndex(jointsListKp[i]);
+            if (!m_kinDyn->model().isValidJointIndex(index))
+            {
+                BiomechanicalAnalysis::log()->error("[HumanIK::initializeJointRegularizationTask] "
+                                                    "Joint {} is not present in the model",
+                                                    jointsListKp[i]);
+                return false;
+            }
+
+            if (m_kinDyn->model().getJoint(index)->getNrOfDOFs() != 1)
+            {
+                BiomechanicalAnalysis::log()->error("[HumanIK::initializeJointRegularizationTask] "
+                                                    "Joint {} has a number of dofs different from 1",
+                                                    jointsListKp[i]);
+                return false;
+            }
+            auto dofOffset = m_kinDyn->model().getJoint(index)->getDOFsOffset();
+            kp[dofOffset] = JointsKp[i];
+        }
+
+        // Initialize the desired positions for the joints with Kp != 0
+        Eigen::VectorXd desiredJointsPositionsKp = Eigen::VectorXd::Zero(jointsListKp.size());
+        m_jointPositionSetPoint = Eigen::VectorXd::Zero(m_nrDoFs);
+
+        for (size_t i = 0; i < jointsListKp.size(); ++i)
+        {
+            // Retrieve the index of the joint in the list of joints
+            auto index = m_kinDyn->model().getJointIndex(jointsListKp[i]);
+            // Check if the joint is valid
+            if (!m_kinDyn->model().isValidJointIndex(index))
+            {
+                BiomechanicalAnalysis::log()->error("[HumanIK::initializeJointRegularizationTask] "
+                                                    "Joint {} is not present in the model",
+                                                    jointsListKp[i]);
+                return false;
+            }
+
+            if (m_kinDyn->model().getJoint(index)->getNrOfDOFs() != 1)
+            {
+                BiomechanicalAnalysis::log()->error("[HumanIK::initializeJointRegularizationTask] "
+                                                    "Joint {} has a number of dofs different from 1",
+                                                    jointsListKp[i]);
+                return false;
+            }
+
+            // Retrieve the joint position from the model
+            auto dOffset = m_kinDyn->model().getJoint(index)->getDOFsOffset();
+            m_jointPositionSetPoint(dOffset) = desiredJointsPositionsKp(i);
+        }
+    }
+
+    // Set the kp parameter for the regularization task
     taskHandler->setParameter("kp", kp);
 
     // Retrieve the weight parameter from the task handler
@@ -946,6 +1036,11 @@ bool HumanIK::initializeJointRegularizationTask(const std::string& taskName,
     return ok;
 }
 
+Eigen::VectorXd HumanIK::getJointPositionSetPoint()
+{
+    return m_jointPositionSetPoint;
+}
+
 bool HumanIK::initializeJointConstraintsTask(const std::string& taskName,
                                              const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler)
 {
diff --git a/src/IK/tests/HumanIKTest.cpp b/src/IK/tests/HumanIKTest.cpp
index 0fc96d5..a7389f7 100644
--- a/src/IK/tests/HumanIKTest.cpp
+++ b/src/IK/tests/HumanIKTest.cpp
@@ -10,6 +10,8 @@
 #include <BipedalLocomotion/ParametersHandler/TomlImplementation.h>
 #include <ConfigFolderPath.h>
 
+using namespace BipedalLocomotion::ParametersHandler;
+
 TEST_CASE("InverseKinematics test")
 {
     auto kinDyn = std::make_shared<iDynTree::KinDynComputations>();
@@ -20,11 +22,42 @@ TEST_CASE("InverseKinematics test")
     const iDynTree::Model model = iDynTree::getRandomModel(nrDoFs);
     kinDyn->loadRobotModel(model);
     auto paramHandler = std::make_shared<BipedalLocomotion::ParametersHandler::TomlImplementation>();
-
+    IParametersHandler::shared_ptr handler = paramHandler;
     std::cout << "configPath = " << getConfigPath() + "/configTestIK.toml" << std::endl;
-
     REQUIRE(paramHandler->setFromFile(getConfigPath() + "/configTestIK.toml"));
 
+    /* CREATE THE PARAMETERS FOR THE JOINT_REG TASK*/
+    std::vector<std::string> joints_list_kp, joints_list;
+    std::vector<double> joints_kp, joints_weights;
+
+    // Iter over all the joints of the model
+    for (int i = 0; i < model.getNrOfJoints(); i++)
+    {
+        auto joint = model.getJoint(i);
+
+        // Check if the joint has only one degree of freedom
+        if (joint->getNrOfDOFs() == 1)
+        {
+            std::string jointName = model.getJointName(i);
+            joints_list_kp.push_back(jointName);
+            joints_kp.push_back(10.0);
+            joints_list.push_back(jointName);
+            joints_weights.push_back(0.1);
+        }
+    }
+
+    IParametersHandler::shared_ptr setGroup = std::make_shared<TomlImplementation>();
+    setGroup->setParameter("type", "JointRegularizationTask");
+    setGroup->setParameter("robot_velocity_variable_name", "robot_velocity");
+    setGroup->setParameter("weight", 0.01);
+    setGroup->setParameter("upper_limit", 5.0);
+    setGroup->setParameter("lower_limit", -5.0);
+    setGroup->setParameter("joints_list_kp", joints_list_kp);
+    setGroup->setParameter("joints_kp", joints_kp);
+    setGroup->setParameter("joints_list", joints_list);
+    setGroup->setParameter("joints_weights", joints_weights);
+    REQUIRE(paramHandler->setGroup("JOINT_REG_TASK", setGroup));
+
     // inintialize the joint positions and velocities
     Eigen::VectorXd JointPositions(kinDyn->getNrOfDegreesOfFreedom());
     Eigen::VectorXd JointVelocities(kinDyn->getNrOfDegreesOfFreedom());
@@ -56,7 +89,8 @@ TEST_CASE("InverseKinematics test")
     REQUIRE(ik.updateGravityTask(10, I_R_IMU));
     REQUIRE(ik.updateOrientationAndGravityTasks(mapNodeData));
     REQUIRE(ik.updateJointConstraintsTask());
-    REQUIRE(ik.updateJointRegularizationTask());
+    Eigen::VectorXd jointPositionSetPoint = ik.getJointPositionSetPoint();
+    REQUIRE(ik.updateJointRegularizationTask(jointPositionSetPoint));
     REQUIRE(ik.calibrateWorldYaw(mapNodeData));
     REQUIRE(ik.calibrateAllWithWorld(mapNodeData, "link1"));
     REQUIRE(ik.advance());
diff --git a/src/IK/tests/configTestIK.toml b/src/IK/tests/configTestIK.toml
index 9281540..80a0f78 100644
--- a/src/IK/tests/configTestIK.toml
+++ b/src/IK/tests/configTestIK.toml
@@ -116,13 +116,8 @@ joints_list = ["link0joint", "link1joint"]
 upper_bounds = [1.0, 1.0]
 lower_bounds = [-1.0, -1.0]
 
-[JOINT_REG_TASK]
-type = "JointRegularizationTask"
-robot_velocity_variable_name = "robot_velocity"
-weight = 1.0
-
 [JOINT_VEL_LIMITS_TASK]
 type = "JointVelocityLimitsTask"
 robot_velocity_variable_name = "robot_velocity"
-upper_limit = 1.0
-lower_limit = -1.0
+upper_limit = 5.0
+lower_limit = -5.0
diff --git a/src/Logging/YarpImplementation/src/YarpLogger.cpp b/src/Logging/YarpImplementation/src/YarpLogger.cpp
index 7fd2673..652411c 100644
--- a/src/Logging/YarpImplementation/src/YarpLogger.cpp
+++ b/src/Logging/YarpImplementation/src/YarpLogger.cpp
@@ -7,9 +7,7 @@ using namespace BipedalLocomotion::TextLogging;
 
 void BiomechanicalAnalysis::Logging::useYarpLogger()
 {
-    const static std::shared_ptr<YarpLoggerFactory> _bafYarpLoggerFactoryBasePtr
-        = std::make_shared<YarpLoggerFactory>("baf");
-    const static std::shared_ptr<LoggerFactory> _bafYarpLogFactory
-        = std::dynamic_pointer_cast<LoggerFactory>(_bafYarpLoggerFactoryBasePtr);
+    const static std::shared_ptr<YarpLoggerFactory> _bafYarpLoggerFactoryBasePtr = std::make_shared<YarpLoggerFactory>("baf");
+    const static std::shared_ptr<LoggerFactory> _bafYarpLogFactory = std::dynamic_pointer_cast<LoggerFactory>(_bafYarpLoggerFactoryBasePtr);
     BiomechanicalAnalysis::Logging::setLoggerFactory(_bafYarpLogFactory);
 }
diff --git a/src/examples/IK/exampleIK.cpp b/src/examples/IK/exampleIK.cpp
index d368a74..73b0692 100644
--- a/src/examples/IK/exampleIK.cpp
+++ b/src/examples/IK/exampleIK.cpp
@@ -495,7 +495,9 @@ int main()
 
         // IK Solver: Update joint constraints and regularization tasks
         ik.updateJointConstraintsTask();
-        ik.updateJointRegularizationTask();
+
+        Eigen::VectorXd jointPositionSetPoint = ik.getJointPositionSetPoint();
+        ik.updateJointRegularizationTask(jointPositionSetPoint);
 
         // Advance the inverse kinematics solver: compute solution
         if (!ik.advance())
diff --git a/src/examples/IK/exampleIK.ini b/src/examples/IK/exampleIK.ini
index 0ddf363..6e21a4e 100644
--- a/src/examples/IK/exampleIK.ini
+++ b/src/examples/IK/exampleIK.ini
@@ -162,4 +162,8 @@ lower_bounds                    (-0.001, -0.001, -0.001, -0.001)
 [JOINT_REG_TASK]
 type                            "JointRegularizationTask"
 robot_velocity_variable_name    "robot_velocity"
-weight                          0.000001
+weight                          0.01
+joints_list                     ("jLeftKnee_rotz", "jRightKnee_rotz", "jLeftAnkle_rotz", "jRightAnkle_rotz")
+joints_weights                  (0.1, 0.1, 0.1, 0.1)
+joints_list_kp                  ("jLeftKnee_rotz", "jRightKnee_rotz", "jLeftAnkle_rotz", "jRightAnkle_rotz")
+joints_kp                       (10.0, 10.0, 10.0, 10.0)


Reviews:
user: login: traversaro
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-05T10:04:44Z
user: login: traversaro
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-05T10:20:50Z
user: login: traversaro
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-10T09:30:52Z
user: login: traversaro
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-10T09:31:13Z
user: login: LudovicaDanovaro
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-10T09:52:09Z
user: login: traversaro
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-10T10:08:49Z
user: login: davidegorbani
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-03-10T10:09:48Z
user: login: traversaro
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-10T12:00:16Z
user: login: davidegorbani
body: I left some comments, once the comments are addressed could you please format the files with `clang` format? To do that you can just run the following command line from the root of the repo:
```terminal
git ls-files '*.cpp' '*.h' | xargs clang-format -i
```
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-03-10T13:33:29Z
user: login: LudovicaDanovaro
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-10T13:44:00Z
user: login: davidegorbani
body: Last think is the formatting and then it's good for me!
state: APPROVED
author_association: COLLABORATOR
submitted_at: 2025-03-10T14:23:41Z
user: login: traversaro
body: Thanks @LudovicaDanovaro !
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-03-10T14:40:44Z

Review comments:
diff_hunk: @@ -402,7 +402,7 @@ class HumanIK
      * @param jointVelocities joint velocities, by defualt it is set to zero
      * @return true if the joint regularization task is set correctly
      */
-    bool updateJointRegularizationTask();
+    bool updateJointRegularizationTask(const Eigen::VectorXd& desiredPositionsKp, const std::vector<std::string> jointsListKp);
path: src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
user: login: traversaro
body: This is a breaking change and would break the iFeelBAF internal application, did you coordinated for doing a change there?
created_at: 2025-03-05T10:04:44Z
author_association: CONTRIBUTOR
diff_hunk: @@ -402,7 +402,7 @@ class HumanIK
      * @param jointVelocities joint velocities, by defualt it is set to zero
      * @return true if the joint regularization task is set correctly
      */
-    bool updateJointRegularizationTask();
+    bool updateJointRegularizationTask(const Eigen::VectorXd& desiredPositionsKp, const std::vector<std::string> jointsListKp);
path: src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
user: login: traversaro
body: No problem, I just saw the internal PR. Anyhow, I would clearly document this change in the PR body, for example with this text:

> This PR changes the signature of `HumanIK::updateJointRegularizationTask()` to be `HumanIK::updateJointRegularizationTask(const Eigen::VectorXd& desiredPositionsKp, const std::vector<std::string> jointsListKp)` . If you want to continue to not regularize any joint, change your code from:
> 
> ~~~cpp
> humanIK.updateJointRegularizationTask();
> ~~~
>
> to
> 
> ~~~cpp
> Eigen::VectorXd dummyVec;
> std::vector<std::string> dummyList;
> humanIK.updateJointRegularizationTask(dummyVec, dummyList);
> ~~~
created_at: 2025-03-05T10:20:50Z
author_association: CONTRIBUTOR
diff_hunk: @@ -18,12 +23,105 @@ TEST_CASE("InverseKinematics test")
     int nrDoFs = 20;
 
     const iDynTree::Model model = iDynTree::getRandomModel(nrDoFs);
-    kinDyn->loadRobotModel(model);
+    kinDyn->loadRobotModel(model);   
     auto paramHandler = std::make_shared<BipedalLocomotion::ParametersHandler::TomlImplementation>();
-
+    
     std::cout << "configPath = " << getConfigPath() + "/configTestIK.toml" << std::endl;
 
-    REQUIRE(paramHandler->setFromFile(getConfigPath() + "/configTestIK.toml"));
+
+
+    /* CREATE THE PARAMETERS FOR THE JOINT_REG TASK*/
+    std::vector<std::string> joints_list_kp, joints_list;
+    std::vector<double> joints_kp, joints_weights;
+
+    // Iter over all the joints of the model
+    for (int i = 0; i < model.getNrOfJoints(); i++) {
+        auto joint = model.getJoint(i);
+
+        // Check if the joint has only one degree of freedom
+        if (joint->getNrOfDOFs() == 1) {
+            std::string jointName = model.getJointName(i);
+            joints_list_kp.push_back(jointName);
+            joints_kp.push_back(10.0);
+            joints_list.push_back(jointName);
+            joints_weights.push_back(0.1);
+        }
+    }
+
+    std::string filename = getConfigPath() + "/configTestIK.toml";
+    std::ifstream inputFile(filename);
+    std::string tempFilename = getConfigPath() + "/configTestIKTEST.toml";
+    std::ofstream outputFile(tempFilename);
+    std::string line;
+    bool inTargetSection = false;
+    bool foundType = false;
+    bool foundVelocity = false;
+    bool foundWeight = false;
+
+    while (std::getline(inputFile, line)) {
+        outputFile << line << "\n";
+        
+        if (line == "[JOINT_REG_TASK]") {
+            inTargetSection = true;
+        } else if (inTargetSection) {
+            if (line.find("type") != std::string::npos) {
+                foundType = true;
+            } else if (line.find("robot_velocity_variable_name") != std::string::npos) {
+                foundVelocity = true;
+            } else if (line.find("weight") != std::string::npos) {
+                foundWeight = true;
+            }
+            
+            if (foundType && foundVelocity && foundWeight) {
+                outputFile << "joints_list = [";
+                for (size_t i = 0; i < joints_list.size(); ++i) {
+                    outputFile << "\"" << joints_list[i] << "\"";
+                    if (i != joints_list.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_list line
+                
+                outputFile << "joints_weights = [";
+                for (size_t i = 0; i < joints_weights.size(); ++i) {
+                    outputFile << joints_weights[i];
+                    if (i != joints_weights.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_weights line
+                
+                outputFile << "joints_list_kp = [";
+                for (size_t i = 0; i < joints_list_kp.size(); ++i) {
+                    outputFile << "\"" << joints_list_kp[i] << "\"";
+                    if (i != joints_list_kp.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_list_kp line
+                
+                outputFile << "joints_kp = [";
+                for (size_t i = 0; i < joints_kp.size(); ++i) {
+                    outputFile << std::fixed << std::setprecision(1) << joints_kp[i];
+                    if (i != joints_kp.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_kp line
+    
+                inTargetSection = false;
+            }
+        }
+    }
path: src/IK/tests/HumanIKTest.cpp
user: login: traversaro
body: This part of the code seems quite hard to mantain, as it conflates a TOML parser and reader in an extremely ad-hoc way. Was this LLM-generated? I think the idea of @davidegorbani was to first load a simple `.toml` file in `paramHandler->setFromFile`, and then add missing parameters with `paramHandler->setParameter`. If that is not possible and you want to manually modify the .doml file, I would use the library https://github.com/marzer/tomlplusplus that you should already have installed as it is a dependency of blf to read and write toml files, and I would not try to edit manually TOML files.
created_at: 2025-03-10T09:30:51Z
author_association: CONTRIBUTOR
diff_hunk: @@ -18,12 +23,105 @@ TEST_CASE("InverseKinematics test")
     int nrDoFs = 20;
 
     const iDynTree::Model model = iDynTree::getRandomModel(nrDoFs);
-    kinDyn->loadRobotModel(model);
+    kinDyn->loadRobotModel(model);   
     auto paramHandler = std::make_shared<BipedalLocomotion::ParametersHandler::TomlImplementation>();
-
+    
     std::cout << "configPath = " << getConfigPath() + "/configTestIK.toml" << std::endl;
 
-    REQUIRE(paramHandler->setFromFile(getConfigPath() + "/configTestIK.toml"));
+
+
+    /* CREATE THE PARAMETERS FOR THE JOINT_REG TASK*/
+    std::vector<std::string> joints_list_kp, joints_list;
+    std::vector<double> joints_kp, joints_weights;
+
+    // Iter over all the joints of the model
+    for (int i = 0; i < model.getNrOfJoints(); i++) {
+        auto joint = model.getJoint(i);
+
+        // Check if the joint has only one degree of freedom
+        if (joint->getNrOfDOFs() == 1) {
+            std::string jointName = model.getJointName(i);
+            joints_list_kp.push_back(jointName);
+            joints_kp.push_back(10.0);
+            joints_list.push_back(jointName);
+            joints_weights.push_back(0.1);
+        }
+    }
+
+    std::string filename = getConfigPath() + "/configTestIK.toml";
+    std::ifstream inputFile(filename);
+    std::string tempFilename = getConfigPath() + "/configTestIKTEST.toml";
+    std::ofstream outputFile(tempFilename);
+    std::string line;
+    bool inTargetSection = false;
+    bool foundType = false;
+    bool foundVelocity = false;
+    bool foundWeight = false;
+
+    while (std::getline(inputFile, line)) {
+        outputFile << line << "\n";
+        
+        if (line == "[JOINT_REG_TASK]") {
+            inTargetSection = true;
+        } else if (inTargetSection) {
+            if (line.find("type") != std::string::npos) {
+                foundType = true;
+            } else if (line.find("robot_velocity_variable_name") != std::string::npos) {
+                foundVelocity = true;
+            } else if (line.find("weight") != std::string::npos) {
+                foundWeight = true;
+            }
+            
+            if (foundType && foundVelocity && foundWeight) {
+                outputFile << "joints_list = [";
+                for (size_t i = 0; i < joints_list.size(); ++i) {
+                    outputFile << "\"" << joints_list[i] << "\"";
+                    if (i != joints_list.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_list line
+                
+                outputFile << "joints_weights = [";
+                for (size_t i = 0; i < joints_weights.size(); ++i) {
+                    outputFile << joints_weights[i];
+                    if (i != joints_weights.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_weights line
+                
+                outputFile << "joints_list_kp = [";
+                for (size_t i = 0; i < joints_list_kp.size(); ++i) {
+                    outputFile << "\"" << joints_list_kp[i] << "\"";
+                    if (i != joints_list_kp.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_list_kp line
+                
+                outputFile << "joints_kp = [";
+                for (size_t i = 0; i < joints_kp.size(); ++i) {
+                    outputFile << std::fixed << std::setprecision(1) << joints_kp[i];
+                    if (i != joints_kp.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_kp line
+    
+                inTargetSection = false;
+            }
+        }
+    }
path: src/IK/tests/HumanIKTest.cpp
user: login: traversaro
body: Having said that, if @davidegorbani is ok in merging this it is not a problem for me.
created_at: 2025-03-10T09:31:13Z
author_association: CONTRIBUTOR
diff_hunk: @@ -18,12 +23,105 @@ TEST_CASE("InverseKinematics test")
     int nrDoFs = 20;
 
     const iDynTree::Model model = iDynTree::getRandomModel(nrDoFs);
-    kinDyn->loadRobotModel(model);
+    kinDyn->loadRobotModel(model);   
     auto paramHandler = std::make_shared<BipedalLocomotion::ParametersHandler::TomlImplementation>();
-
+    
     std::cout << "configPath = " << getConfigPath() + "/configTestIK.toml" << std::endl;
 
-    REQUIRE(paramHandler->setFromFile(getConfigPath() + "/configTestIK.toml"));
+
+
+    /* CREATE THE PARAMETERS FOR THE JOINT_REG TASK*/
+    std::vector<std::string> joints_list_kp, joints_list;
+    std::vector<double> joints_kp, joints_weights;
+
+    // Iter over all the joints of the model
+    for (int i = 0; i < model.getNrOfJoints(); i++) {
+        auto joint = model.getJoint(i);
+
+        // Check if the joint has only one degree of freedom
+        if (joint->getNrOfDOFs() == 1) {
+            std::string jointName = model.getJointName(i);
+            joints_list_kp.push_back(jointName);
+            joints_kp.push_back(10.0);
+            joints_list.push_back(jointName);
+            joints_weights.push_back(0.1);
+        }
+    }
+
+    std::string filename = getConfigPath() + "/configTestIK.toml";
+    std::ifstream inputFile(filename);
+    std::string tempFilename = getConfigPath() + "/configTestIKTEST.toml";
+    std::ofstream outputFile(tempFilename);
+    std::string line;
+    bool inTargetSection = false;
+    bool foundType = false;
+    bool foundVelocity = false;
+    bool foundWeight = false;
+
+    while (std::getline(inputFile, line)) {
+        outputFile << line << "\n";
+        
+        if (line == "[JOINT_REG_TASK]") {
+            inTargetSection = true;
+        } else if (inTargetSection) {
+            if (line.find("type") != std::string::npos) {
+                foundType = true;
+            } else if (line.find("robot_velocity_variable_name") != std::string::npos) {
+                foundVelocity = true;
+            } else if (line.find("weight") != std::string::npos) {
+                foundWeight = true;
+            }
+            
+            if (foundType && foundVelocity && foundWeight) {
+                outputFile << "joints_list = [";
+                for (size_t i = 0; i < joints_list.size(); ++i) {
+                    outputFile << "\"" << joints_list[i] << "\"";
+                    if (i != joints_list.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_list line
+                
+                outputFile << "joints_weights = [";
+                for (size_t i = 0; i < joints_weights.size(); ++i) {
+                    outputFile << joints_weights[i];
+                    if (i != joints_weights.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_weights line
+                
+                outputFile << "joints_list_kp = [";
+                for (size_t i = 0; i < joints_list_kp.size(); ++i) {
+                    outputFile << "\"" << joints_list_kp[i] << "\"";
+                    if (i != joints_list_kp.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_list_kp line
+                
+                outputFile << "joints_kp = [";
+                for (size_t i = 0; i < joints_kp.size(); ++i) {
+                    outputFile << std::fixed << std::setprecision(1) << joints_kp[i];
+                    if (i != joints_kp.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_kp line
+    
+                inTargetSection = false;
+            }
+        }
+    }
path: src/IK/tests/HumanIKTest.cpp
user: login: LudovicaDanovaro
body: > This part of the code seems quite hard to mantain, as it conflates a TOML parser and reader in an extremely ad-hoc way. Was this LLM-generated? I think the idea of @davidegorbani was to first load a simple `.toml` file in `paramHandler->setFromFile`, and then add missing parameters with `paramHandler->setParameter`. If that is not possible and you want to manually modify the .doml file, I would use the library https://github.com/marzer/tomlplusplus that you should already have installed as it is a dependency of blf to read and write toml files, and I would not try to edit manually TOML files.

Initially, I tried adding the parameters directly using `paramHandler->setParameter`, but unfortunately, that was not possible because `BipedalLocomotion::ParametersHandler::IParametersHandler::shared_ptr` does not allow calling `setParameter`.  

For this reason, I looked for a way to read and modify `.toml` files manually. However, I can try using the library you suggested to handle this more effectively.

created_at: 2025-03-10T09:52:09Z
author_association: CONTRIBUTOR
diff_hunk: @@ -18,12 +23,105 @@ TEST_CASE("InverseKinematics test")
     int nrDoFs = 20;
 
     const iDynTree::Model model = iDynTree::getRandomModel(nrDoFs);
-    kinDyn->loadRobotModel(model);
+    kinDyn->loadRobotModel(model);   
     auto paramHandler = std::make_shared<BipedalLocomotion::ParametersHandler::TomlImplementation>();
-
+    
     std::cout << "configPath = " << getConfigPath() + "/configTestIK.toml" << std::endl;
 
-    REQUIRE(paramHandler->setFromFile(getConfigPath() + "/configTestIK.toml"));
+
+
+    /* CREATE THE PARAMETERS FOR THE JOINT_REG TASK*/
+    std::vector<std::string> joints_list_kp, joints_list;
+    std::vector<double> joints_kp, joints_weights;
+
+    // Iter over all the joints of the model
+    for (int i = 0; i < model.getNrOfJoints(); i++) {
+        auto joint = model.getJoint(i);
+
+        // Check if the joint has only one degree of freedom
+        if (joint->getNrOfDOFs() == 1) {
+            std::string jointName = model.getJointName(i);
+            joints_list_kp.push_back(jointName);
+            joints_kp.push_back(10.0);
+            joints_list.push_back(jointName);
+            joints_weights.push_back(0.1);
+        }
+    }
+
+    std::string filename = getConfigPath() + "/configTestIK.toml";
+    std::ifstream inputFile(filename);
+    std::string tempFilename = getConfigPath() + "/configTestIKTEST.toml";
+    std::ofstream outputFile(tempFilename);
+    std::string line;
+    bool inTargetSection = false;
+    bool foundType = false;
+    bool foundVelocity = false;
+    bool foundWeight = false;
+
+    while (std::getline(inputFile, line)) {
+        outputFile << line << "\n";
+        
+        if (line == "[JOINT_REG_TASK]") {
+            inTargetSection = true;
+        } else if (inTargetSection) {
+            if (line.find("type") != std::string::npos) {
+                foundType = true;
+            } else if (line.find("robot_velocity_variable_name") != std::string::npos) {
+                foundVelocity = true;
+            } else if (line.find("weight") != std::string::npos) {
+                foundWeight = true;
+            }
+            
+            if (foundType && foundVelocity && foundWeight) {
+                outputFile << "joints_list = [";
+                for (size_t i = 0; i < joints_list.size(); ++i) {
+                    outputFile << "\"" << joints_list[i] << "\"";
+                    if (i != joints_list.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_list line
+                
+                outputFile << "joints_weights = [";
+                for (size_t i = 0; i < joints_weights.size(); ++i) {
+                    outputFile << joints_weights[i];
+                    if (i != joints_weights.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_weights line
+                
+                outputFile << "joints_list_kp = [";
+                for (size_t i = 0; i < joints_list_kp.size(); ++i) {
+                    outputFile << "\"" << joints_list_kp[i] << "\"";
+                    if (i != joints_list_kp.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_list_kp line
+                
+                outputFile << "joints_kp = [";
+                for (size_t i = 0; i < joints_kp.size(); ++i) {
+                    outputFile << std::fixed << std::setprecision(1) << joints_kp[i];
+                    if (i != joints_kp.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_kp line
+    
+                inTargetSection = false;
+            }
+        }
+    }
path: src/IK/tests/HumanIKTest.cpp
user: login: traversaro
body: > but unfortunately, that was not possible because BipedalLocomotion::ParametersHandler::IParametersHandler::shared_ptr does not allow calling setParameter.

What was the exact error? `setParameter` is part of the `IParametersHandler` interface (see https://ami-iit.github.io/bipedal-locomotion-framework/classBipedalLocomotion_1_1ParametersHandler_1_1IParametersHandler.html), so this is a bit surprising.
created_at: 2025-03-10T10:08:49Z
author_association: CONTRIBUTOR
diff_hunk: @@ -18,12 +23,105 @@ TEST_CASE("InverseKinematics test")
     int nrDoFs = 20;
 
     const iDynTree::Model model = iDynTree::getRandomModel(nrDoFs);
-    kinDyn->loadRobotModel(model);
+    kinDyn->loadRobotModel(model);   
     auto paramHandler = std::make_shared<BipedalLocomotion::ParametersHandler::TomlImplementation>();
-
+    
     std::cout << "configPath = " << getConfigPath() + "/configTestIK.toml" << std::endl;
 
-    REQUIRE(paramHandler->setFromFile(getConfigPath() + "/configTestIK.toml"));
+
+
+    /* CREATE THE PARAMETERS FOR THE JOINT_REG TASK*/
+    std::vector<std::string> joints_list_kp, joints_list;
+    std::vector<double> joints_kp, joints_weights;
+
+    // Iter over all the joints of the model
+    for (int i = 0; i < model.getNrOfJoints(); i++) {
+        auto joint = model.getJoint(i);
+
+        // Check if the joint has only one degree of freedom
+        if (joint->getNrOfDOFs() == 1) {
+            std::string jointName = model.getJointName(i);
+            joints_list_kp.push_back(jointName);
+            joints_kp.push_back(10.0);
+            joints_list.push_back(jointName);
+            joints_weights.push_back(0.1);
+        }
+    }
+
+    std::string filename = getConfigPath() + "/configTestIK.toml";
+    std::ifstream inputFile(filename);
+    std::string tempFilename = getConfigPath() + "/configTestIKTEST.toml";
+    std::ofstream outputFile(tempFilename);
+    std::string line;
+    bool inTargetSection = false;
+    bool foundType = false;
+    bool foundVelocity = false;
+    bool foundWeight = false;
+
+    while (std::getline(inputFile, line)) {
+        outputFile << line << "\n";
+        
+        if (line == "[JOINT_REG_TASK]") {
+            inTargetSection = true;
+        } else if (inTargetSection) {
+            if (line.find("type") != std::string::npos) {
+                foundType = true;
+            } else if (line.find("robot_velocity_variable_name") != std::string::npos) {
+                foundVelocity = true;
+            } else if (line.find("weight") != std::string::npos) {
+                foundWeight = true;
+            }
+            
+            if (foundType && foundVelocity && foundWeight) {
+                outputFile << "joints_list = [";
+                for (size_t i = 0; i < joints_list.size(); ++i) {
+                    outputFile << "\"" << joints_list[i] << "\"";
+                    if (i != joints_list.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_list line
+                
+                outputFile << "joints_weights = [";
+                for (size_t i = 0; i < joints_weights.size(); ++i) {
+                    outputFile << joints_weights[i];
+                    if (i != joints_weights.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_weights line
+                
+                outputFile << "joints_list_kp = [";
+                for (size_t i = 0; i < joints_list_kp.size(); ++i) {
+                    outputFile << "\"" << joints_list_kp[i] << "\"";
+                    if (i != joints_list_kp.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_list_kp line
+                
+                outputFile << "joints_kp = [";
+                for (size_t i = 0; i < joints_kp.size(); ++i) {
+                    outputFile << std::fixed << std::setprecision(1) << joints_kp[i];
+                    if (i != joints_kp.size() - 1) {
+                        outputFile << ", ";
+                    }
+                }
+                outputFile << "]\n"; // End of joints_kp line
+    
+                inTargetSection = false;
+            }
+        }
+    }
path: src/IK/tests/HumanIKTest.cpp
user: login: davidegorbani
body: > was to first load a simple `.toml` file in `paramHandler->setFromFile`, and then add missing parameters with `paramHandler->setParameter`

Yes, exactly; my idea was to create the group related to the regularization task and add it to the main parameter handler. [Here](https://github.com/ami-iit/bipedal-locomotion-framework/blob/d6e289c2707d85775f7c49a6f7d646d9e9e16259/src/ParametersHandler/tests/ParametersHandlerTomlTest.cpp#L144-L148) there is an example of how to set a group inside a parameter handler.
created_at: 2025-03-10T10:09:48Z
author_association: COLLABORATOR
diff_hunk: @@ -9,6 +9,14 @@
 #include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
 #include <BipedalLocomotion/ParametersHandler/TomlImplementation.h>
 #include <ConfigFolderPath.h>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <toml++/toml.h>
path: src/IK/tests/HumanIKTest.cpp
user: login: traversaro
body: ```suggestion
```

It seems that these includes were used in previous iterations of this PR, and they are not needed anymore?
created_at: 2025-03-10T12:00:16Z
author_association: CONTRIBUTOR
path: src/IK/tests/configTestIKTEST.toml
user: login: davidegorbani
body: I think this file can be removed.
created_at: 2025-03-10T13:24:08Z
author_association: COLLABORATOR
diff_hunk: @@ -127,6 +127,9 @@ class HumanIK
 
     Eigen::VectorXd m_calibrationJointPositions; /** Joint positions for calibration */
 
+    Eigen::VectorXd jointPositionSetPoint; /** Custom set point for the regularization task */
path: src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
user: login: davidegorbani
body: ```suggestion
    Eigen::VectorXd m_jointPositionSetPoint; /** Custom set point for the regularization task */
```

For this class, a `m_` suffix is added to the object that belongs to the class such that it is easier to recognise them in the code.
created_at: 2025-03-10T13:29:40Z
author_association: COLLABORATOR
diff_hunk: @@ -495,7 +495,15 @@ int main()
 
         // IK Solver: Update joint constraints and regularization tasks
         ik.updateJointConstraintsTask();
-        ik.updateJointRegularizationTask();
+
+        std::vector<std::string> jointsListKp = getJointsList();
+        if (!paramHandler->getParameter("joints_list_kp", jointsListKp))
+        {
+            BiomechanicalAnalysis::log()->error("Cannot find the parameter 'joints_list_kp'");
+            return 1;
+        }
path: src/examples/IK/exampleIK.cpp
user: login: davidegorbani
body: Here I don't understand why you are retrieving the list of the joints in the for cycle
created_at: 2025-03-10T13:31:16Z
author_association: COLLABORATOR
diff_hunk: @@ -495,7 +495,15 @@ int main()
 
         // IK Solver: Update joint constraints and regularization tasks
         ik.updateJointConstraintsTask();
-        ik.updateJointRegularizationTask();
+
+        std::vector<std::string> jointsListKp = getJointsList();
+        if (!paramHandler->getParameter("joints_list_kp", jointsListKp))
+        {
+            BiomechanicalAnalysis::log()->error("Cannot find the parameter 'joints_list_kp'");
+            return 1;
+        }
path: src/examples/IK/exampleIK.cpp
user: login: LudovicaDanovaro
body: These lines were added before the latest modifications to the code. They are no longer needed, so they should be removed. Thanks!

created_at: 2025-03-10T13:44:00Z
author_association: CONTRIBUTOR

PR comments:
user: login: traversaro
created_at: 2025-03-05T10:11:00Z
body: The CI is failing. The linux failure is probably unrelated (see https://github.com/ami-iit/biomechanical-analysis-framework/issues/58), but the macos CI seems to be a real failure:

~~~
1/3 Test #1: HumanIKTestUnitTests .............***Failed    0.18 sec
Randomness seeded to: 857724361
configPath = /Users/runner/work/biomechanical-analysis-framework/biomechanical-analysis-framework/src/IK/tests/configTestIK.toml
Warning: 05 10:00:38.096] [thread: 34070] [blf] [warning] [HumanIK::initialize] Parameter calibration_joint_positions is missing, setting all joints to zero
Warning: 05 10:00:38.097] [thread: 34070] [blf] [warning] [HumanIK::initializeOrientationTask] Parameter rotation_matrix of the PELVIS_TASK task is missing, setting the rotation matrix from the IMU to the frame link0 to identity
Warning: 05 10:00:38.097] [thread: 34070] [blf] [warning] [HumanIK::initializeOrientationTask] Parameter rotation_matrix of the T8_TASK task is missing, setting the rotation matrix from the IMU to the frame link1 to identity
Warning: 05 10:00:38.097] [thread: 34070] [blf] [warning] [HumanIK::initializeOrientationTask] Parameter rotation_matrix of the RIGHT_UPPER_ARM_TASK task is missing, setting the rotation matrix from the IMU to the frame link2 to identity
Warning: 05 10:00:38.097] [thread: 34070] [blf] [warning] [HumanIK::initializeOrientationTask] Parameter rotation_matrix of the RIGHT_FORE_ARM_TASK task is missing, setting the rotation matrix from the IMU to the frame link3 to identity
Warning: 05 10:00:38.097] [thread: 34070] [blf] [warning] [HumanIK::initializeOrientationTask] Parameter rotation_matrix of the LEFT_UPPER_ARM_TASK task is missing, setting the rotation matrix from the IMU to the frame link4 to identity
Warning: 05 10:00:38.097] [thread: 34070] [blf] [warning] [HumanIK::initializeOrientationTask] Parameter rotation_matrix of the LEFT_FORE_ARM_TASK task is missing, setting the rotation matrix from the IMU to the frame link5 to identity
Warning: 05 10:00:38.097] [thread: 34070] [blf] [warning] [HumanIK::initializeOrientationTask] Parameter rotation_matrix of the RIGHT_UPPER_LEG_TASK task is missing, setting the rotation matrix from the IMU to the frame link6 to identity
Warning: 05 10:00:38.097] [thread: 34070] [blf] [warning] [HumanIK::initializeOrientationTask] Parameter rotation_matrix of the RIGHT_LOWER_LEG_TASK task is missing, setting the rotation matrix from the IMU to the frame link7 to identity
Warning: 05 10:00:38.097] [thread: 34070] [blf] [warning] [HumanIK::initializeOrientationTask] Parameter rotation_matrix of the LEFT_UPPER_LEG_TASK task is missing, setting the rotation matrix from the IMU to the frame link8 to identity
Warning: 05 10:00:38.097] [thread: 34070] [blf] [warning] [HumanIK::initializeOrientationTask] Parameter rotation_matrix of the LEFT_LOWER_LEG_TASK task is missing, setting the rotation matrix from the IMU to the frame link9 to identity
Warning: 05 10:00:38.097] [thread: 34070] [blf] [warning] [HumanIK::initializeGravityTask] Parameter rotation_matrix of the GRAVITY_TASK_1 task is missing, setting the rotation matrix from the IMU to the frame  to identity
[2025-03-05 10:00:38.097] [thread: 34070] [blf] [info] [R3Task::initialize]  [IK-R3Task - Frame name:  link10] Unable to find the mask parameter. The default value is used: true true true.
Error: 3-05 10:00:38.097] [thread: 34070] [blf] [error] [HumanIK::initializeJointRegularizationTask] Joint link6joint has a number of dofs different from 1
Error: 3-05 10:00:38.097] [thread: 34070] [blf] [error] [HumanIK::initialize] Error in the initialization of the JOINT_REG_TASK task
~~~

did you run tests locally?
author_association: CONTRIBUTOR
user: login: traversaro
created_at: 2025-03-05T12:44:18Z
body: > The CI is failing. The linux failure is probably unrelated (see #58),

If you merge or rebase on top of https://github.com/ami-iit/biomechanical-analysis-framework/pull/59 also the Linux CI should work fine.
author_association: CONTRIBUTOR
user: login: LudovicaDanovaro
created_at: 2025-03-10T09:18:41Z
body: Could I merge it?
@traversaro @Gianlucamilani @davidegorbani 
author_association: CONTRIBUTOR
user: login: traversaro
created_at: 2025-03-10T09:21:44Z
body: > Could I merge it? @traversaro @Gianlucamilani @davidegorbani

Ok for me. FYI, you can ask for a new review after you addressed some requested changes by clicking on `🔄` button, see point 5 of https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/requesting-a-pull-request-review#requesting-reviews-from-collaborators-and-organization-members for more info.
author_association: CONTRIBUTOR