From ff81acc731162f8532b517c20058634cbea3bde5 Mon Sep 17 00:00:00 2001
From: Davide <davidegorbani@gmail.com>
Date: Wed, 12 Jun 2024 09:03:48 +0200
Subject: [PATCH] added a new method to update all the floor contact tasks

---
 .../IK/InverseKinematics.h                    | 14 ++++++++++-
 src/IK/src/InverseKinematics.cpp              | 23 +++++++++++++++----
 src/IK/tests/HumanIKTest.cpp                  | 11 ++++-----
 3 files changed, 36 insertions(+), 12 deletions(-)

diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
index 4d6ae61..8760662 100644
--- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
+++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
@@ -389,7 +389,19 @@ class HumanIK
      */
     bool updateJointConstraintsTask();
 
-    bool updateOrientationGravityTasks(std::unordered_map<int, nodeData> nodeStruct);
+    /**
+     * update the orientation for all the nodes of the SO3 and gravity tasks
+     * @param nodeStruct unordered map containing the node number and the calibration matrix
+     * @return true if the calibration matrix is set correctly
+     */
+    bool updateOrientationAndGravityTasks(const std::unordered_map<int, nodeData>& nodeStruct);
+
+    /**
+     * update the floor contact task for all the nodes
+     * @param footInContact unordered map containing the node number and the vertical force
+     * @return true if the calibration matrix is set correctly
+     */
+    bool updateFloorContactTasks(const std::unordered_map<int, Eigen::Matrix<double, 6, 1>>& wrenchMap);
 
     /**
      * set the calibration matrix between the IMU and the link
diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp
index 5e842c2..0ec48a7 100644
--- a/src/IK/src/InverseKinematics.cpp
+++ b/src/IK/src/InverseKinematics.cpp
@@ -245,10 +245,10 @@ bool HumanIK::updateJointConstraintsTask()
     return m_jointConstraintsTask->update();
 }
 
-bool HumanIK::updateOrientationGravityTasks(std::unordered_map<int, nodeData> nodeStruct)
+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())
         {
@@ -280,6 +280,21 @@ bool HumanIK::updateOrientationGravityTasks(std::unordered_map<int, nodeData> no
     return true;
 }
 
+bool HumanIK::updateFloorContactTasks(const std::unordered_map<int, Eigen::Matrix<double, 6, 1>>& wrenchMap)
+{
+    for (const auto& [node, data] : wrenchMap)
+    {
+        if (!updateFloorContactTask(node, data(5)))
+        {
+            BiomechanicalAnalysis::log()->error("[HumanIK::updateFloorContactTasks] Error in updating "
+                                                "the floor contact task of node {}",
+                                                node);
+            return false;
+        }
+    }
+    return true;
+}
+
 bool HumanIK::TPoseCalibrationNode(const int node, const manif::SO3d& I_R_IMU)
 {
     m_tPose = true;
@@ -307,7 +322,7 @@ bool HumanIK::TPoseCalibrationNode(const int node, const manif::SO3d& I_R_IMU)
 bool HumanIK::TPoseCalibrationNodes(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 (!TPoseCalibrationNode(node, data.I_R_IMU))
         {
@@ -365,7 +380,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();
diff --git a/src/IK/tests/HumanIKTest.cpp b/src/IK/tests/HumanIKTest.cpp
index 2850f9a..b98f669 100644
--- a/src/IK/tests/HumanIKTest.cpp
+++ b/src/IK/tests/HumanIKTest.cpp
@@ -19,12 +19,9 @@ TEST_CASE("InverseKinematics test")
 
     const iDynTree::Model model = iDynTree::getRandomModel(nrDoFs);
     kinDyn->loadRobotModel(model);
-    auto paramHandler
-        = std::make_shared<BipedalLocomotion::ParametersHandler::YarpImplementation>();
-    auto gravityTaskParamHandler
-        = std::make_shared<BipedalLocomotion::ParametersHandler::YarpImplementation>();
-    auto FloorContactTaskParamHandler
-        = std::make_shared<BipedalLocomotion::ParametersHandler::YarpImplementation>();
+    auto paramHandler = std::make_shared<BipedalLocomotion::ParametersHandler::YarpImplementation>();
+    auto gravityTaskParamHandler = std::make_shared<BipedalLocomotion::ParametersHandler::YarpImplementation>();
+    auto FloorContactTaskParamHandler = std::make_shared<BipedalLocomotion::ParametersHandler::YarpImplementation>();
 
     REQUIRE(paramHandler->setFromFile(getConfigPath() + "/configTestIK.ini"));
 
@@ -57,7 +54,7 @@ TEST_CASE("InverseKinematics test")
     REQUIRE(ik.updateOrientationTask(3, I_R_IMU, I_omega_IMU));
     REQUIRE(ik.updateFloorContactTask(10, 11.0));
     REQUIRE(ik.updateGravityTask(10, I_R_IMU));
-    REQUIRE(ik.updateOrientationGravityTasks(mapNodeData));
+    REQUIRE(ik.updateOrientationAndGravityTasks(mapNodeData));
     REQUIRE(ik.updateJointConstraintsTask());
     REQUIRE(ik.updateJointRegularizationTask());
     REQUIRE(ik.advance());