From 2e31ebc031caadf68e36ba84790d4b4e21c778c6 Mon Sep 17 00:00:00 2001
From: patrickhegemann <p.hegemann@mailbox.org>
Date: Thu, 14 May 2020 00:34:08 +0200
Subject: [PATCH] Add method to get grasp TCP pose relative to robot root
---
VirtualRobot/Grasping/Grasp.cpp | 5 +++++
VirtualRobot/Grasping/Grasp.h | 5 +++++
2 files changed, 10 insertions(+)
diff --git a/VirtualRobot/Grasping/Grasp.cpp b/VirtualRobot/Grasping/Grasp.cpp
index e4c3103bf..4fd984cf2 100644
--- a/VirtualRobot/Grasping/Grasp.cpp
+++ b/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;
diff --git a/VirtualRobot/Grasping/Grasp.h b/VirtualRobot/Grasping/Grasp.h
index b3fa57fa0..942612ca9 100644
--- a/VirtualRobot/Grasping/Grasp.h
+++ b/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.
*/
--
GitLab