Align the T Pose to HDE

src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h

@@ -29,6 +29,9 @@ namespace BiomechanicalAnalysis
 namespace IK
 {
 
+/**
+ * @brief Struct containing the orientation and the angular velocity of an IMU
+ */
 struct nodeData
 {
     manif::SO3d I_R_IMU;
@@ -137,7 +140,8 @@ class HumanIK
                                                                  // to the World of the IMU, which
                                                                  // will be calibrated using Tpose
                                                                  // script
-        Eigen::Vector3d weight;
+        Eigen::Vector3d weight; // Weight of the task
+        std::string frameName; // Name of the frame in which the task is expressed
     };
 
     /**
@@ -152,6 +156,7 @@ class HumanIK
         Eigen::Vector2d weight;
         int nodeNumber;
         std::string taskName;
+        std::string frameName;
     };
 
     /**
@@ -403,7 +408,9 @@ class HumanIK
 
     /**
      * 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
+     * @param nodeStruct unordered map containing the struct node data (see
+     * https://github.com/ami-iit/biomechanical-analysis-framework/blob/338129086dca24989552a20ecc1c9dec0492806a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h#L32)
+     * containing the orientation and the angular velocity of an IMU, associated to the node number
      * @return true if the calibration matrix is set correctly
      */
     bool updateOrientationAndGravityTasks(const std::unordered_map<int, nodeData>& nodeStruct);
@@ -416,19 +423,24 @@ class HumanIK
     bool updateFloorContactTasks(const std::unordered_map<int, Eigen::Matrix<double, 6, 1>>& wrenchMap);
 
     /**
-     * set the calibration matrix between the IMU and the link
-     * @param node node number
-     * @param I_R_IMU calibration matrix
+     * remove the offset on the yaw of the IMUs world
+     * @param nodeStruct unordered map containing the struct node data (see
+     * https://github.com/ami-iit/biomechanical-analysis-framework/blob/338129086dca24989552a20ecc1c9dec0492806a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h#L32)
+     * containing the orientation and the angular velocity of an IMU, associated to the node number
      * @return true if the calibration matrix is set correctly
+     * @note gravity is expected to be aligned with the z-axis of the IMU frame
      */
-    bool TPoseCalibrationNode(const int node, const manif::SO3d& I_R_IMU);
+    bool calibrateWorldYaw(std::unordered_map<int, nodeData> nodeStruct);
 
     /**
-     * set the calibration matrix between the IMU and the link for all the nodes
-     * @param nodeStruct unordered map containing the node number and the calibration matrix
+     * compute the calibration matrix between the IMU frame and the associated link frame
+     * @param nodeStruct unordered map containing the struct node data (see
+     * https://github.com/ami-iit/biomechanical-analysis-framework/blob/338129086dca24989552a20ecc1c9dec0492806a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h#L32)
+     * containing the orientation and the angular velocity of an IMU, associated to the node number
+     * @param frameRef reference frame used as world
      * @return true if the calibration matrix is set correctly
      */
-    bool TPoseCalibrationNodes(std::unordered_map<int, nodeData> nodeStruct);
+    bool calibrateAllWithWorld(std::unordered_map<int, nodeData> nodeStruct, std::string frameRef = "");
 
     /**
      * this function solves the inverse kinematics problem and integrate the joint velocity to

src/IK/src/InverseKinematics.cpp

@@ -322,43 +322,107 @@ bool HumanIK::updateFloorContactTasks(const std::unordered_map<int, Eigen::Matri
     return true;
 }
 
-bool HumanIK::TPoseCalibrationNode(const int node, const manif::SO3d& I_R_IMU)
+bool HumanIK::calibrateWorldYaw(std::unordered_map<int, nodeData> nodeStruct)
 {
-    m_tPose = true;
-    // check if the node number is valid
-    if ((m_OrientationTasks.find(node) == m_OrientationTasks.end()) && (m_GravityTasks.find(node) == m_GravityTasks.end()))
+    // reset the robot state
+    Eigen::VectorXd jointPositions;
+    Eigen::VectorXd jointVelocities;
+    jointPositions.resize(this->getDoFsNumber());
+    jointPositions.setZero();
+    jointVelocities.resize(this->getDoFsNumber());
+    jointVelocities.setZero();
+    Eigen::Matrix4d basePose;
+    basePose.setIdentity();
+    Eigen::VectorXd baseVelocity;
+    baseVelocity.resize(6);
+    baseVelocity.setZero();
+    m_kinDyn->setRobotState(basePose, jointPositions, baseVelocity, jointVelocities, m_gravity);
+    // Update the orientation and gravity tasks
+    for (const auto& [node, data] : nodeStruct)
     {
-        BiomechanicalAnalysis::log()->error("[HumanIK::TPoseCalibrationNode] Invalid node number.");
-        return false;
-    }
+        // check if the node number is valid
+        if (m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end())
+        {
+            BiomechanicalAnalysis::log()->error("[HumanIK::calibrateWorldYaw] Invalid node number.");
+            return false;
+        }
 
-    // compute the rotation matrix from the world to the world of the IMU as:
-    // W_R_WIMU = R_calib * (WIMU_R_IMU * IMU_R_link)^{T}
-    // where R_calib is assumed to be the identity
-    // The if condition checks whether the task is m_OrientationTasks or m_GravityTasks
-    if (m_OrientationTasks.find(node) != m_OrientationTasks.end())
-    {
-        m_OrientationTasks[node].calibrationMatrix = calib_W_R_link * (I_R_IMU * m_OrientationTasks[node].IMU_R_link).inverse();
-    } else
-    {
-        m_GravityTasks[node].calibrationMatrix = calib_W_R_link * (I_R_IMU * m_GravityTasks[node].IMU_R_link).inverse();
+        iDynTree::Rotation rpyOffset;
+        if (m_OrientationTasks.find(node) != m_OrientationTasks.end())
+        {
+            // compute the offset between the world and the IMU world as:
+            // W_R_WIMU = W_R_link * (WIMU_R_IMU * IMU_R_link)^{T}
+            iDynTree::toEigen(rpyOffset) = iDynTree::toEigen(m_kinDyn->getWorldTransform(m_OrientationTasks[node].frameName).getRotation())
+                                           * ((data.I_R_IMU * m_OrientationTasks[node].IMU_R_link).inverse()).rotation();
+            // set the calibration matrix of the orientation task to the offset in yaw
+            m_OrientationTasks[node].calibrationMatrix = manif::SO3d(0, 0, rpyOffset.asRPY()(2));
+        } else
+        {
+            // compute the offset between the world and the IMU world as:
+            // W_R_WIMU = W_R_link * (WIMU_R_IMU * IMU_R_link)^{T}
+            iDynTree::toEigen(rpyOffset) = iDynTree::toEigen(m_kinDyn->getWorldTransform(m_GravityTasks[node].frameName).getRotation())
+                                           * ((data.I_R_IMU * m_GravityTasks[node].IMU_R_link).inverse()).rotation();
+            // set the calibration matrix of the orientation task to the offset in yaw
+            m_GravityTasks[node].calibrationMatrix = manif::SO3d(0, 0, rpyOffset.asRPY()(2));
+        }
     }
     return true;
 }
 
-bool HumanIK::TPoseCalibrationNodes(std::unordered_map<int, nodeData> nodeStruct)
+bool HumanIK::calibrateAllWithWorld(std::unordered_map<int, nodeData> nodeStruct, std::string refFrame)
 {
-    // Update the orientation and gravity tasks
+    // reset the robot state
+    Eigen::VectorXd jointPositions;
+    Eigen::VectorXd jointVelocities;
+    jointPositions.resize(this->getDoFsNumber());
+    jointPositions.setZero();
+    jointVelocities.resize(this->getDoFsNumber());
+    jointVelocities.setZero();
+    Eigen::Matrix4d basePose;
+    basePose.setIdentity();
+    Eigen::VectorXd baseVelocity;
+    baseVelocity.resize(6);
+    baseVelocity.setZero();
+    m_kinDyn->setRobotState(basePose, jointPositions, baseVelocity, jointVelocities, m_gravity);
+
+    manif::SO3d secondaryCalib = manif::SO3d::Identity();
+    // if a reference frame is provided, compute the world rotation matrix of the reference frame
+    if (refFrame != "")
+    {
+        iDynTree::Rotation refFrameRot = m_kinDyn->getWorldTransform(refFrame).getRotation().inverse();
+        secondaryCalib = manif::SO3d(refFrameRot.asRPY()(0), refFrameRot.asRPY()(1), refFrameRot.asRPY()(2));
+    }
+
     for (const auto& [node, data] : nodeStruct)
     {
-        if (!TPoseCalibrationNode(node, data.I_R_IMU))
+        // check if the node number is valid
+        if (m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end())
         {
-            BiomechanicalAnalysis::log()->error("[HumanIK::TPoseCalibrationNodes] Error in "
-                                                "calibrating the node {}",
-                                                node);
+            BiomechanicalAnalysis::log()->error("[HumanIK::calibrateWorldYaw] Invalid node number.");
             return false;
         }
+
+        if (m_OrientationTasks.find(node) != m_OrientationTasks.end())
+        {
+            // compute the rotation matrix from the IMU to the link frame as:
+            // IMU_R_link = (W_R_WIMU * WIMU_R_IMU)^{T} * W_R_link
+            Eigen::Matrix3d IMU_R_link = (m_OrientationTasks[node].calibrationMatrix * data.I_R_IMU).rotation().transpose()
+                                         * iDynTree::toEigen(m_kinDyn->getWorldTransform(m_OrientationTasks[node].frameName).getRotation());
+            m_OrientationTasks[node].IMU_R_link = BipedalLocomotion::Conversions::toManifRot(IMU_R_link);
+            m_OrientationTasks[node].calibrationMatrix = secondaryCalib * m_OrientationTasks[node].calibrationMatrix;
+        } else
+        {
+            // compute the rotation matrix from the IMU to the link frame as:
+            // IMU_R_link = (W_R_WIMU * WIMU_R_IMU)^{T} * W_R_link
+            Eigen::Matrix3d IMU_R_link = (m_GravityTasks[node].calibrationMatrix * data.I_R_IMU).rotation().transpose()
+                                         * iDynTree::toEigen(m_kinDyn->getWorldTransform(m_GravityTasks[node].frameName).getRotation());
+            m_GravityTasks[node].IMU_R_link = BipedalLocomotion::Conversions::toManifRot(IMU_R_link);
+            m_GravityTasks[node].calibrationMatrix = secondaryCalib * m_GravityTasks[node].calibrationMatrix;
+        }
     }
+    // set the flag to true to reset the integration
+    m_tPose = true;
+
     return true;
 }
 
@@ -501,6 +565,13 @@ bool HumanIK::initializeOrientationTask(const std::string& taskName,
         return false;
     }
 
+    // Retrieve frame name parameter from config file, using the task handler
+    if (!taskHandler->getParameter("frame_name", m_OrientationTasks[nodeNumber].frameName))
+    {
+        BiomechanicalAnalysis::log()->error("{} Parameter frame_name of the {} task is missing", logPrefix, taskName);
+        return false;
+    }
+
     // Retrieve weight parameter from config file, using the task handler
     std::vector<double> weight;
     if (!taskHandler->getParameter("weight", weight))
@@ -593,6 +664,13 @@ bool HumanIK::initializeGravityTask(const std::string& taskName,
         return false;
     }
 
+    // Retrieve frame name parameter from the config file, using the task handler
+    if (!taskHandler->getParameter("target_frame_name", m_GravityTasks[nodeNumber].frameName))
+    {
+        BiomechanicalAnalysis::log()->error("{} Parameter frame_name of the {} task is missing", logPrefix, taskName);
+        return false;
+    }
+
     // Retrieve weight parameter from the config file, using the task handler
     std::vector<double> weight;
     if (!taskHandler->getParameter("weight", weight))

src/IK/tests/HumanIKTest.cpp

@@ -57,6 +57,8 @@ TEST_CASE("InverseKinematics test")
     REQUIRE(ik.updateOrientationAndGravityTasks(mapNodeData));
     REQUIRE(ik.updateJointConstraintsTask());
     REQUIRE(ik.updateJointRegularizationTask());
+    REQUIRE(ik.calibrateWorldYaw(mapNodeData));
+    REQUIRE(ik.calibrateAllWithWorld(mapNodeData, "link1"));
     REQUIRE(ik.advance());
     REQUIRE(ik.getJointPositions(JointPositions));
     REQUIRE(ik.getJointVelocities(JointVelocities));

src/examples/IK/exampleIK.cpp

@@ -16,6 +16,7 @@
 #include <BiomechanicalAnalysis/Conversions/CommonConversions.h> //conversion of rotation from iDyntree to Eigen
 #include <BiomechanicalAnalysis/IK/InverseKinematics.h> // declaration of classes for IK inherited from iDyntree
 #include <BiomechanicalAnalysis/Logging/Logger.h> //handle logging
+#include <BipedalLocomotion/Conversions/ManifConversions.h>
 #include <BipedalLocomotion/ParametersHandler/YarpImplementation.h> //handle parameters for Yarp
 
 #include <ConfigFolderPath.h>
@@ -417,14 +418,15 @@ int main()
         // Perform the T-pose calibration if the user inputs 'calib'. In that case, calibration matrices used by the tasks are updated
         if (tPoseFlag)
         {
+            std::unordered_map<int, BiomechanicalAnalysis::IK::nodeData> nodeStruct; // Create a map to store node data
             // Span nodes belonging to orientationNodes list
             for (auto& node : orientationNodes)
             {
                 // Retrieve orientation and angular velocity for each node
                 getNodeOrientation(ifeel_data, node, ii, I_R_IMU, I_omega_IMU);
 
                 // Perform T-Pose calibration for each node
-                ik.TPoseCalibrationNode(node, manif::SO3d(fromiDynTreeToEigenQuatConversion(I_R_IMU)));
+                nodeStruct[node].I_R_IMU = BipedalLocomotion::Conversions::toManifRot(I_R_IMU);
             }
 
             // Span nodes belonging to floorContactNodes list - shoes nodes
@@ -434,8 +436,10 @@ int main()
                 getNodeOrientation(ifeel_data, node, ii, I_R_IMU, I_omega_IMU);
 
                 // Perform T-Pose calibration for each node
-                ik.TPoseCalibrationNode(node, manif::SO3d(fromiDynTreeToEigenQuatConversion(I_R_IMU)));
+                nodeStruct[node].I_R_IMU = BipedalLocomotion::Conversions::toManifRot(I_R_IMU);
             }
+            ik.calibrateWorldYaw(nodeStruct); // Calibrate the world yaw
+            ik.calibrateAllWithWorld(nodeStruct, "LeftFoot"); // Calibrate all with world
 
             BiomechanicalAnalysis::log()->info("T-pose calibration done");
             tPoseFlag = false;