From 45e42ce398c72cdfb281b495bb113c0f6620ce62 Mon Sep 17 00:00:00 2001 From: Davide Date: Thu, 14 Dec 2023 17:01:21 +0100 Subject: [PATCH] removed not necessary variables --- .../BiomechanicalAnalysis/IK/InverseKinematics.h | 7 ++----- src/IK/src/InverseKinematics.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index 5803567..0564785 100644 --- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h +++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h @@ -43,10 +43,7 @@ class HumanIK Eigen::VectorXd m_jointPositions; Eigen::VectorXd m_jointVelocities; Eigen::Matrix4d m_basePose; - Eigen::Vector3d m_baseLinearPosition; Eigen::Matrix m_baseVelocity; - manif::SO3d m_baseOrientation; - Eigen::Vector3d m_baseAngularVelocity; Eigen::Vector3d m_gravity; // tasks @@ -100,10 +97,10 @@ class HumanIK bool getBasePosition(Eigen::Ref basePosition) const; // get the base velocity - bool getBaseVelocity(manif::SE3Tangentd & baseVelocity) const; + bool getBaseLinearVelocity(Eigen::Ref baseVelocity) const; // get the base orientation - bool getBaseOrientation(manif::SO3d& baseOrientation) const; + bool getBaseOrientation(Eigen::Ref baseOrientation) const; // get the base angular velocity bool getBaseAngularVelocity(Eigen::Ref baseAngularVelocity) const; diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index 4ccc0b1..cbb2609 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -151,28 +151,28 @@ bool HumanIK::getJointVelocities(Eigen::Ref jointVelocities) co bool HumanIK::getBasePosition(Eigen::Ref basePosition) const { - basePosition = m_baseLinearPosition; + basePosition = m_basePose.topRightCorner<3, 1>(); return true; } -bool HumanIK::getBaseVelocity(manif::SE3Tangentd &baseVelocity) const +bool HumanIK::getBaseLinearVelocity(Eigen::Ref baseVelocity) const { - baseVelocity = m_baseVelocity; + baseVelocity = m_baseVelocity.topRows<3>(); return true; } -bool HumanIK::getBaseOrientation(manif::SO3d& baseOrientation) const +bool HumanIK::getBaseOrientation(Eigen::Ref baseOrientation) const { - baseOrientation = m_baseOrientation; + baseOrientation = m_basePose.topLeftCorner<3, 3>(); return true; } bool HumanIK::getBaseAngularVelocity(Eigen::Ref baseAngularVelocity) const { - baseAngularVelocity = m_baseAngularVelocity; + baseAngularVelocity = m_baseVelocity.bottomRows<3>(); return true; }