From ac16076f5c4c5508c280cb735236e9aaeef9cb4a Mon Sep 17 00:00:00 2001
From: Patrick Dormanns <patrick.dormanns@student.kit.edu>
Date: Mon, 26 Feb 2024 14:43:34 +0100
Subject: [PATCH] get endeffector from HumanMapping
---
.../armarx/control/pointing/core/Pointing.cpp | 18 ++++++++++++++----
1 file changed, 14 insertions(+), 4 deletions(-)
diff --git a/source/armarx/control/pointing/core/Pointing.cpp b/source/armarx/control/pointing/core/Pointing.cpp
index ee29a875..e9824dac 100644
--- a/source/armarx/control/pointing/core/Pointing.cpp
+++ b/source/armarx/control/pointing/core/Pointing.cpp
@@ -137,13 +137,23 @@ namespace armarx::control::pointing::core
Side side,
const std::string& shapeName)
{
- auto eefName = std::string("Hand_") + (side == Side::LEFT ? "L" : "R") + "_EEF"; // TODO
- auto eef = robot->getEndEffector(eefName);
+ auto humanMapping = robot->getHumanMapping();
+ ARMARX_CHECK(humanMapping) << "Robot '" << robot->getName() << "' has no human mapping";
+ auto armDescription = (side == LEFT) ? humanMapping->leftArm : humanMapping->rightArm;
+ auto tcp = armDescription.segments.tcp.nodeName;
+
+ // get endeffector whichs tcp-name matches the tcp-name of the according hand
+ auto eefs = robot->getEndEffectors();
+ auto eef = std::find_if(eefs.begin(),
+ eefs.end(),
+ [&tcp](VirtualRobot::EndEffectorPtr eef)
+ { return eef->getTcpName() == tcp; });
+ ARMARX_CHECK(eef != eefs.end());
- auto shape = eef->getPreshape(shapeName);
+ auto shape = (*eef)->getPreshape(shapeName);
if (not shape)
{
- ARMARX_ERROR << "Hand '" << eefName << "' has no shape '" << shape << "'";
+ ARMARX_ERROR << "Hand '" << (*eef)->getName() << "' has no shape '" << shapeName << "'";
throw armarx::exceptions::local::PointingFailedException();
}
return shape->getRobotNodeJointValueMap();
--
GitLab