It is now possible to specify via the parameter 'useTorquesForGrasping'

whether you want to close the hand based on torques
(useTorquesForGrasping=1) or based on joint angles
(useTorquesForGrasping=0).

source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp

@@ -46,6 +46,7 @@ namespace armarx
         addToInput("jointAnglesPreshape", VariantType::List(VariantType::Float), false);
         //how long to wait before switching from preshape to grasp
         addToInput("timeoutPreshape", VariantType::Float, false);
+        addToInput("useTorquesForGrasping", VariantType::Int, false);
 
         //closeHandWithTorques settings:
         addToInput("jointTorquesGrasp", VariantType::List(VariantType::Float), false);        
@@ -53,6 +54,9 @@ namespace armarx
         //low joint velocities as a termination criterion.
         addToInput("timeoutMinExecutionTime", VariantType::Float, false);
 
+        //closeHandWithJointAngles settings:
+        addToInput("jointAnglesGrasp", VariantType::List(VariantType::Float), false);
+
         //installTerminateConditions settings:
         //how long to wait until you think the hand is closed and grasping is completed
         addToInput("timeoutGrasp", VariantType::Float, false);
@@ -73,6 +77,7 @@ namespace armarx
     {
         StatePtr statePreshape = addState<StatePreshape>("statePreshape");
         StatePtr stateCloseHandWithTorques = addState<StateCloseHandWithTorques>("stateCloseHandWithTorques");
+        StatePtr stateCloseHandWithJointAngles = addState<StateCloseHandWithJointAngles>("stateCloseHandWithJointAngles");
         StatePtr stateInstallTerminateConditions = addState<StateInstallTerminateConditions>("stateInstallTerminateConditions");
         StatePtr stateSuccess = addState<SuccessState>("stateSuccess");
         StatePtr stateFailure = addState<FailureState>("stateFailure");
@@ -80,13 +85,19 @@ namespace armarx
         ParameterMappingPtr mapPreshapeInfo = ParameterMapping::createMapping()
                 ->mapFromParent("jointAnglesPreshape", "jointAnglesPreshape")
                 ->mapFromParent("timeoutPreshape", "timeoutPreshape")
-                ->mapFromParent("jointNames", "jointNames");
+                ->mapFromParent("jointNames", "jointNames")
+                ->mapFromParent("useTorquesForGrasping", "useTorquesForGrasping");
 
         ParameterMappingPtr mapCloseHandWithTorquesInfo = ParameterMapping::createMapping()
                 ->mapFromParent("jointTorquesGrasp", "jointTorquesGrasp")
                 ->mapFromParent("timeoutMinExecutionTime", "timeoutMinExecutionTime")
                 ->mapFromParent("jointNames", "jointNames");
 
+        ParameterMappingPtr mapCloseHandWithJointAnglesInfo = ParameterMapping::createMapping()
+                ->mapFromParent("jointAnglesGrasp", "jointAnglesGrasp")
+                ->mapFromParent("timeoutGrasp", "timeoutGrasp")
+                ->mapFromParent("jointNames", "jointNames");
+
         ParameterMappingPtr mapInstallTerminateConditionsInfo = ParameterMapping::createMapping()
                 ->mapFromParent("timeoutGrasp", "timeoutGrasp")
                 ->mapFromParent("thresholdVelocity", "thresholdVelocity")
@@ -97,10 +108,12 @@ namespace armarx
         setInitState(statePreshape, mapPreshapeInfo);
 
         //transitions
-        addTransition<EvPreshapeTimeout>(statePreshape, stateCloseHandWithTorques, mapCloseHandWithTorquesInfo);
+        addTransition<EvPreshapeTimeout_ToCloseWithTorques>(statePreshape, stateCloseHandWithTorques, mapCloseHandWithTorquesInfo);
+        addTransition<EvPreshapeTimeout_ToCloseWithJointAngles>(statePreshape, stateCloseHandWithJointAngles, mapCloseHandWithJointAnglesInfo);
         addTransition<EvMinExecutionTimeout>(stateCloseHandWithTorques, stateInstallTerminateConditions, mapInstallTerminateConditionsInfo);
-        addTransition<EvGraspTimeout>(stateInstallTerminateConditions, stateFailure);
+        addTransition<EvGraspWithTorquesTimeout>(stateInstallTerminateConditions, stateFailure);
         addTransition<EvAllJointVelocitiesLow>(stateInstallTerminateConditions, stateSuccess);
+        addTransition<EvGraspWithJointAnglesTimeout>(stateCloseHandWithJointAngles, stateSuccess);
 
     }
 
@@ -124,7 +137,7 @@ namespace armarx
             dataFields.addVariant(context->getDatafieldRef(tempChannelRef, nodeSet->getNode(i)->getName()));
         }
 
-        setLocal("jointNames", jointNames);      //befüllen...!?
+        setLocal("jointNames", jointNames);
         setLocal("jointVelocitiesDatafields", dataFields);
 
     }
@@ -147,6 +160,7 @@ namespace armarx
 
         //addToLocal("jointNames", VariantType::List(VariantType::String));
         addToInput("jointNames", VariantType::List(VariantType::String), false);
+        addToInput("useTorquesForGrasping", VariantType::Int, false);
     }
 
     void StatePreshape::onEnter()
@@ -172,7 +186,12 @@ namespace armarx
 
         ARMARX_LOG << eINFO << "Installing preshape timeout condition";
         float timeoutPreshape = getInput<float>("timeoutPreshape");
-        condPreshapeTimeout = setTimeoutEvent(timeoutPreshape, createEvent<EvPreshapeTimeout>());
+
+        int useTorquesForGrasping = getInput<int>("useTorquesForGrasping");
+        if (useTorquesForGrasping > 0)
+            condPreshapeTimeout = setTimeoutEvent(timeoutPreshape, createEvent<EvPreshapeTimeout_ToCloseWithTorques>());
+        else
+            condPreshapeTimeout = setTimeoutEvent(timeoutPreshape, createEvent<EvPreshapeTimeout_ToCloseWithJointAngles>());
 
         ARMARX_LOG << eINFO << "Done onEnter()";
     }
@@ -229,6 +248,53 @@ namespace armarx
         removeTimeoutEvent(condMinimumExecutionTimeout);
     }
 
+    // ****************************************************************
+    // Implementation of StateCloseHandWithJointAngles
+    // ****************************************************************
+
+    void StateCloseHandWithJointAngles::defineParameters()
+    {
+        //closeHandWithJointAngles settings:
+        addToInput("jointAnglesGrasp", VariantType::List(VariantType::Float), false);
+        addToInput("timeoutGrasp", VariantType::Float, false);
+
+        addToInput("jointNames", VariantType::List(VariantType::String), false);
+    }
+
+    void StateCloseHandWithJointAngles::onEnter()
+    {
+        ARMARX_LOG << eIMPORTANT << "Entering stateCloseHandWithJointAngles !!!";
+
+        RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+        SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
+        SingleTypeVariantListPtr jointAnglesGraspList = getInput<SingleTypeVariantList>("jointAnglesGrasp");
+        NameValueMap jointNamesAndValues;
+
+        if (jointNames->getSize() == jointAnglesGraspList->getSize())
+        {
+            for (int j=0; j<jointNames->getSize(); j++)
+            {
+                jointNamesAndValues[jointNames->getVariant(j)->getString()]=jointAnglesGraspList->getVariant(j)->getFloat();
+            }
+        }
+        else
+            throw LocalException("stateCloseHandWithJointAngles: List lengths do not match!");
+
+        rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
+
+        ARMARX_LOG << eINFO << "Installing timeoutGrasp condition";
+        float timeoutGrasp = getInput<float>("timeoutGrasp");
+        condGraspWithJointAnglesTimeout = setTimeoutEvent(timeoutGrasp, createEvent<EvGraspWithJointAnglesTimeout>());
+
+        ARMARX_LOG << eIMPORTANT << "Done onEnter()";
+    }
+
+    void StateCloseHandWithJointAngles::onExit()
+    {
+        removeTimeoutEvent(condGraspWithJointAnglesTimeout);
+    }
+
+
     // ****************************************************************
     // Implementation of StateInstallTerminateConditions
     // ****************************************************************

source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h

@@ -34,9 +34,11 @@ namespace armarx
     // ****************************************************************
 
     //only for first tests
-    DEFINEEVENT(EvPreshapeTimeout)
+    DEFINEEVENT(EvPreshapeTimeout_ToCloseWithTorques)
+    DEFINEEVENT(EvPreshapeTimeout_ToCloseWithJointAngles)
     DEFINEEVENT(EvMinExecutionTimeout)
-    DEFINEEVENT(EvGraspTimeout)
+    DEFINEEVENT(EvGraspWithJointAnglesTimeout)
+    DEFINEEVENT(EvGraspWithTorquesTimeout)
     DEFINEEVENT(EvAllJointVelocitiesLow)
 
     // ****************************************************************
@@ -90,6 +92,26 @@ namespace armarx
         StateUtility::ActionEventIdentifier condMinimumExecutionTimeout;
     };
 
+    // ****************************************************************
+    // Definition of StateCloseHandWithJointAngles
+    // ****************************************************************
+
+    /**
+     * StateCloseHandWithJointAngles: Close the hand using a target joint angle configuration.
+     */
+    struct StateCloseHandWithJointAngles :
+        StateTemplate<StateCloseHandWithJointAngles>
+    {
+        void defineParameters();
+        void onEnter();
+        void onExit();
+
+        //Make sure that the joints have time to start moving before we check
+        //low joint velocities as a termination criterion.
+        StateUtility::ActionEventIdentifier condGraspWithJointAnglesTimeout;
+    };
+
+
     // ****************************************************************
     // Definition of StateInstallTerminateConditions
     // ****************************************************************