From 715d3fe7bdbcbe73cc834edd1c0978eaa4989bf6 Mon Sep 17 00:00:00 2001
From: armar6-demo <armar6-demo@kit.edu>
Date: Fri, 26 Feb 2021 08:53:17 +0100
Subject: [PATCH] Revert "Fixed assumption that grasp candidates are always
provided in root frame"
This reverts commit 6de4c018194d47020d43669d3279b1a0e6ecc309.
---
.../GraspingUtility/GraspCandidateHelper.cpp | 16 +++++-----------
.../GraspingUtility/GraspCandidateHelper.h | 2 --
2 files changed, 5 insertions(+), 13 deletions(-)
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp
index c4f221549..3533eb02c 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp
+++ b/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
{
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
index 6c9ba617b..050958f91 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
+++ b/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;
--
GitLab