added interface to get base position and orientation

ami-iit · Dec 6, 2023 · 50d62e2 · 50d62e2
1 parent c34dc3c
commit 50d62e2
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 0 deletions.
diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
@@ -29,6 +29,10 @@ class HumanIK
     // Joint positions and velocities
     Eigen::VectorXd m_jointPositions;
     Eigen::VectorXd m_jointVelocities;
+    Eigen::Vector3d m_basePosition;
+    Eigen::Vector3d m_baseVelocity;
+    manif::SO3d m_baseOrientation;
+    Eigen::Vector3d m_baseAngularVelocity;
 
     // Number of Joint Degrees of Freedom
     int m_nrDoFs;
@@ -70,6 +74,18 @@ class HumanIK
 
     // get the joint velocities
     bool getJointVelocities(Eigen::Ref<Eigen::VectorXd> jointVelocities) const;
+
+    // get the base position
+    bool getBasePosition(Eigen::Ref<Eigen::Vector3d> basePosition) const;
+
+    // get the base velocity
+    bool getBaseVelocity(Eigen::Ref<Eigen::Vector3d> baseVelocity) const;
+
+    // get the base orientation
+    bool getBaseOrientation(manif::SO3d& baseOrientation) const;
+
+    // get the base angular velocity
+    bool getBaseAngularVelocity(Eigen::Ref<Eigen::Vector3d> baseAngularVelocity) const;
 };
 
 } // namespace IK

diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp
@@ -79,3 +79,31 @@ bool HumanIK::getJointVelocities(Eigen::Ref<Eigen::VectorXd> jointVelocities) co
 
     return true;
 }
+
+bool HumanIK::getBasePosition(Eigen::Ref<Eigen::Vector3d> basePosition) const
+{
+    basePosition = m_basePosition;
+
+    return true;
+}
+
+bool HumanIK::getBaseVelocity(Eigen::Ref<Eigen::Vector3d> baseVelocity) const
+{
+    baseVelocity = m_baseVelocity;
+
+    return true;
+}
+
+bool HumanIK::getBaseOrientation(manif::SO3d& baseOrientation) const
+{
+    baseOrientation = m_baseOrientation;
+
+    return true;
+}
+
+bool HumanIK::getBaseAngularVelocity(Eigen::Ref<Eigen::Vector3d> baseAngularVelocity) const
+{
+    baseAngularVelocity = m_baseAngularVelocity;
+
+    return true;
+}