reset the IMU_R_link matrix to the one read by config file

ami-iit · Sep 20, 2024 · 987dcc8 · 987dcc8
1 parent f62f60d
commit 987dcc8
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 7 deletions.
diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
@@ -135,6 +135,8 @@ class HumanIK
         int nodeNumber;
         manif::SO3d IMU_R_link; // Rotation matrix from the IMU to related link, set through config
                                 // file
+        manif::SO3d IMU_R_link_init; // Rotation matrix from the IMU to related link, set through config
+                                     // file
         manif::SO3d calibrationMatrix = manif::SO3d::Identity(); // Initialization (to Identity) of
                                                                  // Rotation matrix from the World
                                                                  // to the World of the IMU, which
@@ -152,6 +154,8 @@ class HumanIK
     {
         std::shared_ptr<BipedalLocomotion::IK::GravityTask> task;
         manif::SO3d IMU_R_link;
+        manif::SO3d IMU_R_link_init; // Rotation matrix from the IMU to related link, set through config
+                                     // file
         manif::SO3d calibrationMatrix = manif::SO3d::Identity();
         Eigen::Vector2d weight;
         int nodeNumber;

diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp
@@ -327,17 +327,16 @@ bool HumanIK::clearCalibrationMatrices()
     for (auto& [node, data] : m_OrientationTasks)
     {
         data.calibrationMatrix = manif::SO3d::Identity();
-        data.IMU_R_link = manif::SO3d::Identity();
+        data.IMU_R_link = m_OrientationTasks[node].IMU_R_link_init;
     }
     for (auto& [node, data] : m_GravityTasks)
     {
         data.calibrationMatrix = manif::SO3d::Identity();
-        data.IMU_R_link = manif::SO3d::Identity();
+        data.IMU_R_link = m_GravityTasks[node].IMU_R_link_init;
     }
     return true;
 }
 
-
 bool HumanIK::calibrateWorldYaw(std::unordered_map<int, nodeData> nodeStruct)
 {
     // reset the robot state
@@ -624,7 +623,7 @@ bool HumanIK::initializeOrientationTask(const std::string& taskName,
     if (taskHandler->getParameter("rotation_matrix", rotation_matrix))
     {
         // Convert rotation matrix to ManifRot and assign it to IMU_R_link
-        m_OrientationTasks[nodeNumber].IMU_R_link
+        m_OrientationTasks[nodeNumber].IMU_R_link_init
             = BipedalLocomotion::Conversions::toManifRot(Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(rotation_matrix.data()));
     } else
     {
@@ -637,8 +636,9 @@ bool HumanIK::initializeOrientationTask(const std::string& taskName,
                                            logPrefix,
                                            taskName,
                                            frame_name);
-        m_OrientationTasks[nodeNumber].IMU_R_link.setIdentity();
+        m_OrientationTasks[nodeNumber].IMU_R_link_init.setIdentity();
     }
+    m_OrientationTasks[nodeNumber].IMU_R_link = m_OrientationTasks[nodeNumber].IMU_R_link_init;
     //*****************************************************************************************************
 
     // Initialize the SO3Task object
@@ -714,7 +714,7 @@ bool HumanIK::initializeGravityTask(const std::string& taskName,
     if (taskHandler->getParameter("rotation_matrix", rotation_matrix))
     {
         // Convert rotation matrix to ManifRot and assign it to IMU_R_link
-        m_GravityTasks[nodeNumber].IMU_R_link
+        m_GravityTasks[nodeNumber].IMU_R_link_init
             = BipedalLocomotion::Conversions::toManifRot(Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(rotation_matrix.data()));
     } else
     {
@@ -727,8 +727,9 @@ bool HumanIK::initializeGravityTask(const std::string& taskName,
                                            logPrefix,
                                            taskName,
                                            frame_name);
-        m_GravityTasks[nodeNumber].IMU_R_link.setIdentity();
+        m_GravityTasks[nodeNumber].IMU_R_link_init.setIdentity();
     }
+    m_GravityTasks[nodeNumber].IMU_R_link = m_GravityTasks[nodeNumber].IMU_R_link_init;
 
     //*****************************************************************************************************