removed not necessary variables

ami-iit · Dec 14, 2023 · 45e42ce · 45e42ce
1 parent 406d2e0
commit 45e42ce
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 11 deletions.
diff --git 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<double, 6, 1> 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<Eigen::Vector3d> basePosition) const;
 
     // get the base velocity
-    bool getBaseVelocity(manif::SE3Tangentd & baseVelocity) const;
+    bool getBaseLinearVelocity(Eigen::Ref<Eigen::Vector3d> baseVelocity) const;
 
     // get the base orientation
-    bool getBaseOrientation(manif::SO3d& baseOrientation) const;
+    bool getBaseOrientation(Eigen::Ref<Eigen::Matrix3d> baseOrientation) const;
 
     // get the base angular velocity
     bool getBaseAngularVelocity(Eigen::Ref<Eigen::Vector3d> baseAngularVelocity) const;

diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp
@@ -151,28 +151,28 @@ bool HumanIK::getJointVelocities(Eigen::Ref<Eigen::VectorXd> jointVelocities) co
 
 bool HumanIK::getBasePosition(Eigen::Ref<Eigen::Vector3d> 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<Eigen::Vector3d> baseVelocity) const
 {
-    baseVelocity = m_baseVelocity;
+    baseVelocity = m_baseVelocity.topRows<3>();
 
     return true;
 }
 
-bool HumanIK::getBaseOrientation(manif::SO3d& baseOrientation) const
+bool HumanIK::getBaseOrientation(Eigen::Ref<Eigen::Matrix3d> baseOrientation) const
 {
-    baseOrientation = m_baseOrientation;
+    baseOrientation = m_basePose.topLeftCorner<3, 3>();
 
     return true;
 }
 
 bool HumanIK::getBaseAngularVelocity(Eigen::Ref<Eigen::Vector3d> baseAngularVelocity) const
 {
-    baseAngularVelocity = m_baseAngularVelocity;
+    baseAngularVelocity = m_baseVelocity.bottomRows<3>();
 
     return true;
 }