Version of GraspingWithTorques where joint torques are interpreted

as joint velocities... (because setting torques directly to the
joints does not work in Bullet so far). Joints move, but motion will not
stop (no self collision detection).

source/RobotAPI/GraspingWithTorques/GraspingWithTorques.cpp

@@ -24,9 +24,7 @@
 
 #include "GraspingWithTorques.h"
 //#include "Armar3GraspContext.h"   //MP: Maybe necessary again later?
-#include "../core/RobotStatechartContext.h" //MP 2014-01-21
-
-//#include "../../units/KinematicUnitHand/KinematicUnitHand.h"
+#include "../core/RobotStatechartContext.h"
 
 #include <Core/observers/variant/ChannelRef.h>
 #include <Core/observers/variant/SingleTypeVariantList.h>
@@ -61,13 +59,13 @@ namespace armarx
         addToInput("thresholdVelocity", VariantType::Float, false);
 
 
-        addToInput("robotNodeSetName", VariantType::String, false);        //RobotNodeSet: LeftHand (auskommentarisiert MP 2014-01-21)
+        addToInput("robotNodeSetName", VariantType::String, false);
 
         addToLocal("jointNames", VariantType::List(VariantType::String));
-        //addToLocal("jointNames", SingleTypeVariantList(VariantType::String));   //Geht das? (2014-01-21)
 
-        addToLocal("jointVelocityChannel", VariantType::ChannelRef);
-        //addToLocal("jointVelocityChannel", VariantType::ChannelRef, false);
+        //addToLocal("jointVelocityChannel", VariantType::ChannelRef);  //first try ...
+
+        addToLocal("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef));
 
     }
 
@@ -93,7 +91,8 @@ namespace armarx
                 ->mapFromParent("timeoutGrasp", "timeoutGrasp")
                 ->mapFromParent("thresholdVelocity", "thresholdVelocity")
                 ->mapFromParent("jointNames")
-                ->mapFromParent("jointVelocityChannel"); //bei gleichem Namen muss man ihn nur einmal angeben
+                ->mapFromParent("jointVelocitiesDatafields");
+                //->mapFromParent("jointVelocityChannel"); //first try...
 
         setInitState(statePreshape, mapPreshapeInfo);
 
@@ -110,18 +109,24 @@ namespace armarx
         ARMARX_LOG << eINFO << "Entering StatechartGraspingWithTorques";
 
         RobotStatechartContext* context = getContext<RobotStatechartContext>();
-        setLocal("jointVelocityChannel", context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities"));
+        //setLocal("jointVelocityChannel", context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities"));
+        ChannelRefPtr tempChannelRef = context->getChannelRef(context->getKinematicUnitObserverName(),"jointvelocities");
 
         VirtualRobot::RobotNodeSetPtr nodeSet = context->remoteRobot->getRobotNodeSet(getInput<std::string>("robotNodeSetName"));    //woher robotNodeSetName holen? (Argument für getRobotNodeSet())
 
         SingleTypeVariantList jointNames(VariantType::String);
+        SingleTypeVariantList dataFields(VariantType::DatafieldRef);
+
 
         for (int i=0; i<nodeSet->getSize(); i++)
         {
             jointNames.addVariant(nodeSet->getNode(i)->getName());      //nodes = joints
+            dataFields.addVariant(context->getDatafieldRef(tempChannelRef, nodeSet->getNode(i)->getName()));
         }
 
-        setLocal("jointNames",jointNames);      //befüllen...!?
+        setLocal("jointNames", jointNames);      //befüllen...!?
+        setLocal("jointVelocitiesDatafields", dataFields);
+
     }
 
     void StatechartGraspingWithTorques::onExit()
@@ -140,8 +145,8 @@ namespace armarx
         addToInput("jointAnglesPreshape", VariantType::List(VariantType::Float), false);
         addToInput("timeoutPreshape", VariantType::Float, false);
 
-        //addToLocal("jointNames", VariantType::List(VariantType::String));   //ACHTUNG: addToLocal() oder addToInput()? (2013-01-21)
-        addToInput("jointNames", VariantType::List(VariantType::String), false);   //ACHTUNG: addToLocal() oder addToInput()? (2013-01-21)
+        //addToLocal("jointNames", VariantType::List(VariantType::String));
+        addToInput("jointNames", VariantType::List(VariantType::String), false);
     }
 
     void StatePreshape::onEnter()
@@ -151,7 +156,6 @@ namespace armarx
         RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
         SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
         SingleTypeVariantListPtr jointAnglesPreshapeList = getInput<SingleTypeVariantList>("jointAnglesPreshape");
-        //NameValueMap jointAnglesPreshapeMap;
         NameValueMap jointNamesAndValues;
 
         if (jointNames->getSize() == jointAnglesPreshapeList->getSize())
@@ -164,9 +168,6 @@ namespace armarx
         else
             throw LocalException("StatePreshape: List lengths do not match!");
 
-        //ARMARX_INFO << "node.size()=" << nodes.size() << flush;
-        //ARMARX_INFO << "jointAnglesPreshapeList.size()=" << jointAnglesPreshapeList->getSize() << flush;
-
         rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
 
         ARMARX_LOG << eINFO << "Installing preshape timeout condition";
@@ -241,8 +242,10 @@ namespace armarx
 
         addToInput("jointNames", VariantType::List(VariantType::String), false);
 
-        //addToLocal("jointVelocityChannel", VariantType::ChannelRef);
-        addToInput("jointVelocityChannel", VariantType::ChannelRef, false);
+        //addToInput("jointVelocityChannel", VariantType::ChannelRef, false);   //first try ...
+
+        addToInput("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef), false);
+
     }
 
     void StateInstallTerminateConditions::onEnter()
@@ -256,20 +259,11 @@ namespace armarx
         ARMARX_LOG << eINFO << "Installing allJointVelocitiesLow condition";
         float thresholdVelocity = getInput<float>("thresholdVelocity");
 
-        /*
-        Literal JointVelocityLowJoint0(getInput<ChannelRef>("jointVelocityChannel")
-                                       ->getDataFieldIdentifier(getVariant->getString()), "smaller", Literal::createParameterList(thresholdVelocity));  //siehe SingleTypeVariantList, Auslesen mit getInput()
-        Literal JointVelocityLowJoint1(getInput<ChannelRef>(markerChannelName)
-                                      ->getDataFieldIdentifier("localized"), "smaller", Literal::createParameterList(thresholdVelocity));
-        */
-
-        //Term allJointVelocitiesLow = JointVelocityLowJoint0 && JointVelocityLowJoint1;  //TODO: weitere Literale
-        //condAllJointVelocitiesLow = installCondition(allJointVelocitiesLow, createEvent<EvAllJointVelocitiesLow>());
-
-        //TODO: korrekter ChannelName für Hand-/Fingergelenke? Korrekter DatafieldIdentifier?
-        //TODO: Condition zusammenbauen in einer for-Schleife (siehe ArmarX-Doku zu Conditions...)
-        //TODO: korrekte Parametrierung von getDataFieldIdentifier(), mit getInput()...
+        //=======
+        //first try ... (with ChannelRef)
+        //=======
 
+        /*
         Term allJointVelocitiesLow;
         SingleTypeVariantListPtr jointNamesList = getInput<SingleTypeVariantList>("jointNames");
         for (int i=0; i<jointNamesList->getSize(); i++)
@@ -277,8 +271,25 @@ namespace armarx
            allJointVelocitiesLow = allJointVelocitiesLow && Literal(getInput<ChannelRef>("jointVelocityChannel")
                          ->getDataFieldIdentifier(jointNamesList->getVariant(i)->getString()), "smaller", Literal::createParameterList(thresholdVelocity));
         }
-
         condAllJointVelocitiesLow = installCondition(allJointVelocitiesLow, createEvent<EvAllJointVelocitiesLow>());
+        */
+
+        //=======
+        //second try ... (with DatafieldRef)
+        //=======
+        Term allJointVelocitiesLow_NEW;
+        //SingleTypeVariantListPtr jointNamesList = getInput<SingleTypeVariantList>("jointNames");
+        SingleTypeVariantListPtr dataFieldsList = getInput<SingleTypeVariantList>("jointVelocitiesDatafields");
+
+        //for (int i=0; i<jointNamesList->getSize(); i++)
+        for (int i=0; i<dataFieldsList->getSize(); i++)
+        {
+            allJointVelocitiesLow_NEW = allJointVelocitiesLow_NEW &&
+                    Literal(dataFieldsList->getVariant(i)->get<DatafieldRef>()->getDataFieldIdentifier(),
+                            "inrange", Literal::createParameterList(-thresholdVelocity, thresholdVelocity));
+        }
+
+        condAllJointVelocitiesLow = installCondition(allJointVelocitiesLow_NEW, createEvent<EvAllJointVelocitiesLow>());
 
     }
 
@@ -288,6 +299,26 @@ namespace armarx
         removeTimeoutEvent(condGraspTimeout);
         removeCondition(condAllJointVelocitiesLow);
 
+        //---------
+        //set joint velocities manually to zero (DEBUG)
+        //---------
+
+        RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+        SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
+        //SingleTypeVariantListPtr jointTorquesGraspList = getInput<SingleTypeVariantList>("jointTorquesGrasp");
+        NameValueMap jointNamesAndValues;
+
+        for (int j=0; j<jointNames->getSize(); j++)
+        {
+            jointNamesAndValues[jointNames->getVariant(j)->getString()]=0.0f;  //set everything to zero
+        }
+
+
+        //rsContext->kinematicUnitPrx->setJointTorques(jointNamesAndValues);
+        rsContext->kinematicUnitPrx->setJointVelocities(jointNamesAndValues);
+
+
+
     }
 
 }