diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index e84e140..cce0591 100644 --- 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 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 index cfedef9..0c216df 100644 --- 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 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 taskHandler) {