Added interface method for retrieving current hand joint angles

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;

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 &);

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());
-
     };
 }
 

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;

source/RobotAPI/interface/units/HandUnitInterface.ice

@@ -51,6 +51,7 @@ module armarx
          */
         SingleTypeVariantListBase getShapeNames();
         NameValueMap getShapeJointValues(string shapeName);
+        NameValueMap getCurrentJointValues();
         string getHandName();