get W R Link rotation

ami-iit · Jan 24, 2025 · cd6be60 · cd6be60
1 parent d955176
commit cd6be60
Show file tree

Hide file tree

Showing 2 changed files with 64 additions and 5 deletions.
diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
@@ -141,6 +141,7 @@ class HumanIK
                                                                  // to the World of the IMU, which
                                                                  // will be calibrated using Tpose
                                                                  // script
+        manif::SO3d W_R_link; // Calibrated orientation of the link in the inertial frame
         Eigen::Vector3d weight; // Weight of the task
         std::string frameName; // Name of the frame in which the task is expressed
     };
@@ -156,6 +157,7 @@ class HumanIK
         manif::SO3d IMU_R_link_init; // Initial value of the rotation matrix from the IMU to related link, set through config
                                      // file
         manif::SO3d calibrationMatrix = manif::SO3d::Identity();
+        manif::SO3d W_R_link; // Calibrated orientation of the link in the inertial frame
         Eigen::Vector2d weight;
         int nodeNumber;
         std::string taskName;
@@ -500,7 +502,22 @@ class HumanIK
      * @return true if the base angular velocity is retrieved correctly
      */
     bool getBaseAngularVelocity(Eigen::Ref<Eigen::Vector3d> baseAngularVelocity) const;
-};
+
+    /**
+     * get the calibration matrix between the world and the link
+     * @param node node number
+     * @return calibration matrix
+     */
+    const manif::SO3d& getCalibratedIMURotation(int node) const;
+
+    /**
+     * get the frame name of the orientation task
+     * @param node node number
+     * @return frame name
+     */
+    std::string getNodeFrameName(int node) const;
+
+};          
 
 } // namespace IK
 } // namespace BiomechanicalAnalysis

diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp
@@ -186,10 +186,10 @@ bool HumanIK::updateOrientationTask(const int node, const manif::SO3d& I_R_IMU,
 
     // Compute the rotation matrix from the world to the link frame as:
     // W_R_link = W_R_WIMU * WIMU_R_IMU * IMU_R_link
-    I_R_link = m_OrientationTasks[node].calibrationMatrix * I_R_IMU * m_OrientationTasks[node].IMU_R_link;
+    m_OrientationTasks[node].W_R_link = m_OrientationTasks[node].calibrationMatrix * I_R_IMU * m_OrientationTasks[node].IMU_R_link;
 
     // Set the setpoint for the orientation task of the node
-    return m_OrientationTasks[node].task->setSetPoint(I_R_link,
+    return m_OrientationTasks[node].task->setSetPoint(m_OrientationTasks[node].W_R_link,
                                                       m_OrientationTasks[node].calibrationMatrix.rotation() * I_omega_IMU.coeffs());
 }
 
@@ -204,11 +204,11 @@ bool HumanIK::updateGravityTask(const int node, const manif::SO3d& I_R_IMU)
 
     // compute the rotation matrix from the world to the link frame as:
     // W_R_link = W_R_WIMU * WIMU_R_IMU * IMU_R_link
-    I_R_link = m_GravityTasks[node].calibrationMatrix * I_R_IMU * m_GravityTasks[node].IMU_R_link;
+    m_GravityTasks[node].W_R_link = m_GravityTasks[node].calibrationMatrix * I_R_IMU * m_GravityTasks[node].IMU_R_link;
 
     // set the set point of the gravity task choosing the z direction of the link_R_W rotation
     // matrix
-    return m_GravityTasks[node].task->setSetPoint((I_R_link.rotation().transpose().rightCols(1)));
+    return m_GravityTasks[node].task->setSetPoint((m_GravityTasks[node].W_R_link.rotation().transpose().rightCols(1)));
 }
 
 bool HumanIK::updateFloorContactTask(const int node, const double verticalForce)
@@ -562,6 +562,48 @@ bool HumanIK::getBaseAngularVelocity(Eigen::Ref<Eigen::Vector3d> baseAngularVelo
     return true;
 }
 
+const manif::SO3d& HumanIK::getCalibratedIMURotation(int node) const
+{
+    // Check if the node exists in the orientation tasks
+    if (m_OrientationTasks.find(node) != m_OrientationTasks.end())
+    {
+        return m_OrientationTasks.at(node).W_R_link;
+    }
+    // Check if the node exists in the gravity tasks
+    else if (m_GravityTasks.find(node) != m_GravityTasks.end())
+    {
+        return m_GravityTasks.at(node).W_R_link;
+    }
+    else
+    {
+        // If the node is not defined in any of the tasks, generate an error
+        BiomechanicalAnalysis::log()->error("[HumanIK::getCalibratedIMURotationMatrix] Invalid node number {}.", node);
+        throw std::out_of_range("Invalid node number");
+    }
+}
+
+
+std::string HumanIK::getNodeFrameName(int node) const
+{
+    // Check if the node exists in the orientation tasks
+    if (m_OrientationTasks.find(node) != m_OrientationTasks.end())
+    {
+        return m_OrientationTasks.at(node).frameName;
+    }
+    // Check if the node exists in the gravity tasks
+    else if (m_GravityTasks.find(node) != m_GravityTasks.end())
+    {
+        return m_GravityTasks.at(node).frameName;
+    }
+    else
+    {
+        // If the node is not defined in any of the tasks, generate an error
+        BiomechanicalAnalysis::log()->error("[HumanIK::getFrameNameOrientationTask] Invalid node number {}.", node);
+        throw std::out_of_range("Invalid node number");
+    }
+}
+
+
 bool HumanIK::initializeOrientationTask(const std::string& taskName,
                                         const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler)
 {