hand unit stops hand after setObjectGrasped was called

source/ArmarXSimulation/components/HandUnitDynamicSimulation/HandUnitDynamicSimulation.cpp

@@ -54,7 +54,7 @@ void HandUnitDynamicSimulation::onStartHandUnit()
 
     kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
 
-
+    robotName = getProperty<std::string>("RobotName");
 }
 
 
@@ -100,6 +100,15 @@ void HandUnitDynamicSimulation::setObjectGrasped(const std::string& objectName,
     {
         ARMARX_INFO << "Simulator call objectGrasped: " << robotName << "," << tcpName << "," << objectName << flush;
         simulatorPrx->objectGrasped(robotName, tcpName, objectName);
+        NameControlModeMap modes;
+        auto joints = getCurrentJointValues();
+        for (auto & joint : joints)
+        {
+            modes[joint.first] = ePositionControl;
+        }
+        kinematicUnitPrx->switchControlMode(modes);
+        kinematicUnitPrx->setJointAngles(joints);
+
     }
     else
     {
@@ -126,17 +135,17 @@ void HandUnitDynamicSimulation::setObjectReleased(const std::string& objectName,
 NameValueMap HandUnitDynamicSimulation::getCurrentJointValues(const Ice::Current& c)
 {
     NameValueMap result;
-
     try
     {
         for (auto j : handJoints)
         {
             result[j.first] = simulatorPrx->getRobotJointAngle(robotName, j.first);
+            ARMARX_INFO_S << "jointvalue: " << j.first << result[j.first];
         }
     }
     catch (...)
     {
-        ARMARX_WARNING << "Could not get joint angles fvrom simulator...";
+        ARMARX_WARNING << "Could not get joint angles from simulator...";
     }
 
     return result;
@@ -147,7 +156,7 @@ void armarx::HandUnitDynamicSimulation::setShape(const std::string& shapeName, c
 {
     std::string myShapeName = shapeName;
     ARMARX_INFO << "Setting shape " << myShapeName;
-
+    getCurrentJointValues();
     if (!eef)
     {
         ARMARX_WARNING << "No EEF";
@@ -183,14 +192,46 @@ void armarx::HandUnitDynamicSimulation::setShape(const std::string& shapeName, c
     std::map < std::string, float > jointAngles = config->getRobotNodeJointValueMap();
 
     NameControlModeMap controlModes;
-
-    for (std::pair<std::string, float> pair : jointAngles)
+    if (getProperty<bool>("UseTorques"))
     {
-        controlModes.insert(std::make_pair(pair.first, ePositionControl));
+        auto currentJointAngles = simulatorPrx->getRobotJointAngles(robotName);
+        float torque = getProperty<float>("TorqueValue");
+        StringFloatDictionary torques;
+        for (std::pair<std::string, float> pair : jointAngles)
+        {
+            controlModes.insert(std::make_pair(pair.first, eTorqueControl));
+            auto sign = pair.second > currentJointAngles[pair.first] ? 1 : -1;
+            torques[pair.first] = torque * sign;
+        }
+        kinematicUnitPrx->switchControlMode(controlModes);
+        kinematicUnitPrx->setJointTorques(torques);
+    }
+    else
+    {
+        auto oldJointAngles = simulatorPrx->getRobotJointAngles(robotName);
+        for (std::pair<std::string, float> pair : jointAngles)
+        {
+            controlModes.insert(std::make_pair(pair.first, ePositionControl));
+        }
+        kinematicUnitPrx->switchControlMode(controlModes);
+        kinematicUnitPrx->setJointAngles(jointAngles);
+        ARMARX_INFO << VAROUT(jointAngles);
+        sleep(2);
+        StringFloatDictionary holdPositionTargets;
+        auto currentJointAngles = simulatorPrx->getRobotJointAngles(robotName);
+        for (std::pair<std::string, float> pair : jointAngles)
+        {
+            auto sign = pair.second > oldJointAngles[pair.first] ? 1 : -1;
+
+            holdPositionTargets[pair.first] = currentJointAngles[pair.first] + sign * 10.0f / 180.f * M_PI;
+        }
+        ARMARX_INFO << VAROUT(currentJointAngles);
+        ARMARX_INFO << VAROUT(holdPositionTargets);
+        kinematicUnitPrx->switchControlMode(controlModes);
+        kinematicUnitPrx->setJointAngles(holdPositionTargets);
+
     }
 
-    kinematicUnitPrx->switchControlMode(controlModes);
-    kinematicUnitPrx->setJointAngles(jointAngles);
 }
 
 

source/ArmarXSimulation/components/HandUnitDynamicSimulation/HandUnitDynamicSimulation.h

@@ -54,6 +54,9 @@ namespace armarx
         {
             defineOptionalProperty<std::string>("SimulatorProxyName", "Simulator", "Name of the simulator proxy to use.");
             defineRequiredProperty<std::string>("KinematicUnitName", "Name of the kinematic unit that should be used");
+            defineRequiredProperty<std::string>("RobotName", "Name of the robot that should be used");
+            defineOptionalProperty<bool>("UseTorques", true, "Just apply torques on fingers for shapes in the correct direction (i.e. closing opening of fingers)");
+            defineOptionalProperty<float>("TorqueValue", 2.0f, "Torque applied on fingers");
         }
     };
 
@@ -93,7 +96,7 @@ namespace armarx
         void setObjectGrasped(const std::string& objectName, const Ice::Current&);
         void setObjectReleased(const std::string& objectName, const Ice::Current&);
 
-        virtual NameValueMap getCurrentJointValues(const Ice::Current& c = ::Ice::Current());
+        NameValueMap getCurrentJointValues(const Ice::Current& c = ::Ice::Current()) override;