Revert "Fixed assumption that grasp candidates are always provided in root frame"

This reverts commit 6de4c018.

source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp

@@ -38,7 +38,8 @@ GraspCandidateHelper::GraspCandidateHelper(const grasping::GraspCandidatePtr& ca
 
 Eigen::Matrix4f GraspCandidateHelper::getGraspPoseInRobotRoot() const
 {
-    const Eigen::Matrix4f graspPose = getFramedPose().toRootEigen(robot);
+    const Eigen::Matrix4f curRobotPose = robot->getGlobalPose();
+    const Eigen::Matrix4f graspPose = curRobotPose.inverse() * getGraspPoseInGlobal();
     return graspPose;
 }
 
@@ -57,18 +58,11 @@ Eigen::Vector3f GraspCandidateHelper::getGraspPositionInRobotRoot() const
     return math::Helpers::GetPosition(getGraspPoseInRobotRoot());
 }
 
-armarx::FramedPose GraspCandidateHelper::getFramedPose() const
-{
-    return armarx::FramedPose(candidate->graspPose->position,
-                              candidate->graspPose->orientation,
-                              candidate->sourceFrame,
-                              robot->getName());
-}
-
 Eigen::Matrix4f GraspCandidateHelper::getGraspPoseInGlobal() const
 {
-    auto grasp_pose = getFramedPose();
-    return grasp_pose.toGlobalEigen(robot);
+    const Eigen::Matrix4f oldGraspPose = defrost(candidate->graspPose);
+    const Eigen::Matrix4f oldRobotPose = defrost(candidate->robotPose);
+    return oldRobotPose * oldGraspPose;
 }
 Eigen::Matrix3f GraspCandidateHelper::getGraspOrientationInGlobal() const
 {

source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h

@@ -30,7 +30,6 @@
 #include <VirtualRobot/Robot.h>
 
 #include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
 
 namespace armarx
 {
@@ -51,7 +50,6 @@ namespace armarx
         Eigen::Matrix3f getGraspOrientationInRobotRoot() const;
         Eigen::Vector3f getGraspPositionInRobotRoot() const;
 
-        armarx::FramedPose getFramedPose() const;
         Eigen::Matrix4f getGraspPoseInGlobal() const;
         Eigen::Matrix3f getGraspOrientationInGlobal() const;
         Eigen::Vector3f getGraspPositionInGlobal() const;