From f23677f478b514a6f2d8cc601cc2cca09915b930 Mon Sep 17 00:00:00 2001
From: Nikolaus Vahrenkamp <vahrenkamp@kit.edu>
Date: Mon, 27 Jul 2015 11:26:11 +0200
Subject: [PATCH] Added interface method for retrieving current hand joint
angles
---
source/RobotAPI/components/units/HandUnit.cpp | 10 ++++++++++
source/RobotAPI/components/units/HandUnit.h | 1 +
source/RobotAPI/components/units/HandUnitSimulation.h | 1 -
source/RobotAPI/components/units/TCPControlUnit.cpp | 5 +++--
source/RobotAPI/interface/units/HandUnitInterface.ice | 1 +
5 files changed, 15 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/components/units/HandUnit.cpp b/source/RobotAPI/components/units/HandUnit.cpp
index 907b0990c..8c8eb50c0 100644
--- a/source/RobotAPI/components/units/HandUnit.cpp
+++ b/source/RobotAPI/components/units/HandUnit.cpp
@@ -150,6 +150,16 @@ NameValueMap HandUnit::getShapeJointValues(const std::string &shapeName, const I
return rc->getRobotNodeJointValueMap();
}
+NameValueMap HandUnit::getCurrentJointValues(const Ice::Current &c)
+{
+ NameValueMap result;
+ for (auto j : handJoints)
+ {
+ result[j.first] = 0.0f;
+ }
+ return result;
+}
+
void HandUnit::setObjectGrasped(const std::string &objectName, const Ice::Current &)
{
ARMARX_INFO << "Object grasped " << objectName << flush;
diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h
index db2c3d6b1..3df2d52bc 100644
--- a/source/RobotAPI/components/units/HandUnit.h
+++ b/source/RobotAPI/components/units/HandUnit.h
@@ -121,6 +121,7 @@ namespace armarx
virtual SingleTypeVariantListBasePtr getShapeNames(const Ice::Current& c = ::Ice::Current());
NameValueMap getShapeJointValues(const std::string &shapeName, const Ice::Current & c = ::Ice::Current());
+ virtual NameValueMap getCurrentJointValues(const Ice::Current & c = ::Ice::Current());
void setObjectGrasped(const std::string& objectName, const Ice::Current &);
void setObjectReleased(const std::string& objectName, const Ice::Current &);
diff --git a/source/RobotAPI/components/units/HandUnitSimulation.h b/source/RobotAPI/components/units/HandUnitSimulation.h
index e37f66e51..36a4d8a83 100644
--- a/source/RobotAPI/components/units/HandUnitSimulation.h
+++ b/source/RobotAPI/components/units/HandUnitSimulation.h
@@ -81,7 +81,6 @@ namespace armarx
* \warning Not implemented yet!
*/
virtual void setJointAngles(const NameValueMap& jointAngles, const Ice::Current& c = ::Ice::Current());
-
};
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index aa6741a0a..07ebe712c 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -164,10 +164,11 @@ namespace armarx
ScopedLock lock(dataMutex);
ARMARX_CHECK_EXPRESSION_W_HINT(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
+
if(translationVelocity)
- ARMARX_VERBOSE << "Setting new Velocity for " << nodeSetName << " in frame " << translationVelocity->frame << ":\n" << FramedDirectionPtr::dynamicCast(translationVelocity)->toEigen();
+ ARMARX_DEBUG << "Setting new Velocity for " << nodeSetName << " in frame " << translationVelocity->frame << ":\n" << FramedDirectionPtr::dynamicCast(translationVelocity)->toEigen();
if(orientationVelocityRPY)
- ARMARX_VERBOSE << "Orientation Velo in frame " << orientationVelocityRPY->frame << ": \n" << FramedDirectionPtr::dynamicCast(orientationVelocityRPY)->toEigen();
+ ARMARX_DEBUG << "Orientation Velo in frame " << orientationVelocityRPY->frame << ": \n" << FramedDirectionPtr::dynamicCast(orientationVelocityRPY)->toEigen();
TCPVelocityData data;
data.nodeSetName = nodeSetName;
diff --git a/source/RobotAPI/interface/units/HandUnitInterface.ice b/source/RobotAPI/interface/units/HandUnitInterface.ice
index 8f989f0cf..0f6d1a861 100644
--- a/source/RobotAPI/interface/units/HandUnitInterface.ice
+++ b/source/RobotAPI/interface/units/HandUnitInterface.ice
@@ -51,6 +51,7 @@ module armarx
*/
SingleTypeVariantListBase getShapeNames();
NameValueMap getShapeJointValues(string shapeName);
+ NameValueMap getCurrentJointValues();
string getHandName();
--
GitLab