added a new method to update all the floor contact tasks

ami-iit · Jun 12, 2024 · ff81acc · ff81acc
1 parent cafbce7
commit ff81acc
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 12 deletions.
diff --git 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
@@ -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
@@ -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());