Add method to get grasp TCP pose relative to robot root

VirtualRobot/Grasping/Grasp.cpp

@@ -150,6 +150,11 @@ namespace VirtualRobot
         return result;
     }
 
+    Eigen::Matrix4f Grasp::getTcpPoseRobotRoot(const Eigen::Matrix4f& objectPose, const RobotPtr& robot) const
+    {
+        return robot->getGlobalPose().inverse() * getTcpPoseGlobal(objectPose);
+    }
+
     Eigen::Matrix4f Grasp::getObjectTargetPoseGlobal(const Eigen::Matrix4f& graspingPose) const
     {
         Eigen::Matrix4f result = graspingPose * poseTcp;

VirtualRobot/Grasping/Grasp.h

@@ -74,6 +74,11 @@ namespace VirtualRobot
         */
         Eigen::Matrix4f getTcpPoseGlobal(const Eigen::Matrix4f& objectPose) const;
 
+        /*!
+            Get the pose (relative to the robot root) that has to be achieved by the tcp in order to apply the grasp on the object at position objectPose.
+        */
+        Eigen::Matrix4f getTcpPoseRobotRoot(const Eigen::Matrix4f& objectPose, const RobotPtr& robot) const;
+
         /*!
             The returned pose is the object pose that have to be set in order to apply a grasp at pose graspingPose.
         */