Worked on Pick and Place Demo

source/RobotAPI/core/RobotStatechartContext.cpp

@@ -68,10 +68,15 @@ namespace armarx
         ARMARX_LOG << eINFO << "Starting RobotStatechartContext" << flush;
 
         // retrieve proxies
-        robotStateComponent = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
-        kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
+        std::string rbStateName = "RobotStateComponent";
+        robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(rbStateName);
+        std::string kinUnitName = getProperty<std::string>("KinematicUnitName").getValue();
+        kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinUnitName);
         kinematicUnitObserverPrx = getProxy<KinematicUnitObserverInterfacePrx>(kinematicUnitObserverName);
-        ARMARX_LOG << eINFO << "Fetched proxies" <<  kinematicUnitPrx << " " << robotStateComponent << flush;
+
+        ARMARX_LOG << eINFO << "Fetched proxies " << kinUnitName << ":" << kinematicUnitPrx << ", " << rbStateName << ": " << robotStateComponent << flush;
+
+
 
         if( !getProperty<std::string>("HandUnits").getValue().empty())
         {
@@ -81,7 +86,9 @@ namespace armarx
             for(size_t i = 0; i < handUnitList.size(); i++)
             {
                 boost::algorithm::trim(handUnitList.at(i));
-                handUnits[handUnitList.at(i)] = getProxy<HandUnitInterfacePrx>(handUnitList.at(i));
+                HandUnitInterfacePrx handPrx = getProxy<HandUnitInterfacePrx>(handUnitList.at(i));
+                handUnits[handUnitList.at(i)] = handPrx;
+                ARMARX_LOG << eINFO << "Fetched handUnit proxy " << handUnitList.at(i) << ": " << handPrx << flush;
             }
         }
 		// initialize remote robot
@@ -92,7 +99,24 @@ namespace armarx
     PropertyDefinitionsPtr RobotStatechartContext::createPropertyDefinitions()
     {
         return PropertyDefinitionsPtr(new RobotStatechartContextProperties(
-                                               getConfigIdentifier()));
+                                          getConfigIdentifier()));
+    }
+
+    HandUnitInterfacePrx RobotStatechartContext::getHandUnit(const std::string &handUnitName)
+    {
+        if (handUnits.find(handUnitName)!=handUnits.end())
+        {
+            ARMARX_LOG << eINFO << "Found proxy of hand unit with name  " << handUnitName << flush;
+            return handUnits[handUnitName];
+        }
+        ARMARX_LOG << eINFO << "Do not know proxy of hand unit with name  " << handUnitName << flush;
+        std::map<std::string, HandUnitInterfacePrx>::iterator it = handUnits.begin();
+        while (it!=handUnits.end())
+        {
+            ARMARX_LOG << eINFO << "************ Known hand units: " << it->first << ":" << it->second << flush;
+            it++;
+        }
+        return HandUnitInterfacePrx();
     }
 
    /* const VirtualRobot::RobotPtr armarx::Armar4Context::getRobot()

source/RobotAPI/core/RobotStatechartContext.h

@@ -54,7 +54,6 @@ namespace armarx
             defineRequiredProperty<std::string>("KinematicUnitObserverName", "Name of the kinematic unit observer that should be used");
             //HandUnits should only be changed via config file and default parameter should remain empty
             defineOptionalProperty<std::string>("HandUnits", "", "Name of the comma-seperated hand units that should be used. Unitname for left hand should be LeftHandUnit, and for right hand RightHandUnit");
-
         }
     };
 
@@ -83,6 +82,8 @@ namespace armarx
 
         std::string getKinematicUnitObserverName() { return kinematicUnitObserverName; }
 
+        HandUnitInterfacePrx getHandUnit(const std::string &handUnitName);
+
         //! Prx for the RobotState
 		RobotStateComponentInterfacePrx robotStateComponent;
 		KinematicUnitInterfacePrx kinematicUnitPrx;
@@ -90,6 +91,7 @@ namespace armarx
         TCPControlUnitInterfacePrx tcpControlPrx;
         VirtualRobot::RobotPtr remoteRobot;
         std::map<std::string, HandUnitInterfacePrx> handUnits;
+
     private:
         std::string kinematicUnitObserverName;
     };

source/RobotAPI/interface/units/HandUnitInterface.ice

@@ -41,6 +41,19 @@ module armarx
         void preshape(string preshapeName);
         SingleTypeVariantListBase getPreshapeNames();
         NameValueMap getPreshapeJointValues(string preshapeName);
+
+        /*!
+         * \brief Inform the hand unit that an object has been successfully grasped.
+         * \param objectName The name of the object
+        *  E.g. a state that verifies if a grasp was successful can call this routine.
+        */
+        void setObjectGrasped(string objectName);
+
+        /*!
+         * \brief setObjectReleased Inform the hand unit that an object has been released.
+         * \param objectName The name of the object
+         */
+        void setObjectReleased(string objectName);
     };
 
     interface HandUnitListener
@@ -48,6 +61,9 @@ module armarx
         void reportHandClosed();
         void reportHandOpened();
         void reportHandPreshaped();
+
+        // informs all listeners that we grasped an object
+        void reportObjectGrasped(string robotName, string robotNodeName, string objectName);
     };
 
 };

source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.cpp

@@ -32,6 +32,7 @@
 #include <VirtualRobot/IK/DifferentialIK.h>
 
 #include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/interface/units/HandUnitInterface.h>
 
 #include <RobotAPI/core/RobotStatechartContext.h>
 
@@ -61,6 +62,7 @@ namespace armarx
         //how long to wait until you think the hand is closed and grasping is completed
         addToInput("timeoutGrasp", VariantType::Float, false);
         addToInput("thresholdVelocity", VariantType::Float, false);
+        addToInput("handUnitName", VariantType::String, false);
 
 
         addToInput("robotNodeSetName", VariantType::String, false);
@@ -102,6 +104,7 @@ namespace armarx
                 ->mapFromParent("timeoutGrasp", "timeoutGrasp")
                 ->mapFromParent("thresholdVelocity", "thresholdVelocity")
                 ->mapFromParent("jointNames")
+                ->mapFromParent("handUnitName", "handUnitName")
                 ->mapFromParent("jointVelocitiesDatafields");
                 //->mapFromParent("jointVelocityChannel"); //first try...
 
@@ -111,7 +114,8 @@ namespace armarx
         addTransition<EvPreshapeTimeout_ToCloseWithTorques>(statePreshape, stateCloseHandWithTorques, mapCloseHandWithTorquesInfo);
         addTransition<EvPreshapeTimeout_ToCloseWithJointAngles>(statePreshape, stateCloseHandWithJointAngles, mapCloseHandWithJointAnglesInfo);
         addTransition<EvMinExecutionTimeout>(stateCloseHandWithTorques, stateInstallTerminateConditions, mapInstallTerminateConditionsInfo);
-        addTransition<EvGraspWithTorquesTimeout>(stateInstallTerminateConditions, stateFailure);
+        //addTransition<EvGraspWithTorquesTimeout>(stateInstallTerminateConditions, stateSuccess);
+        addTransition<EvMinExecutionTimeout>(stateInstallTerminateConditions, stateSuccess); // timeout is ok for now....
         addTransition<EvAllJointVelocitiesLow>(stateInstallTerminateConditions, stateSuccess);
         addTransition<EvGraspWithJointAnglesTimeout>(stateCloseHandWithJointAngles, stateSuccess);
 
@@ -314,7 +318,7 @@ namespace armarx
         //addToInput("jointVelocityChannel", VariantType::ChannelRef, false);   //first try ...
 
         addToInput("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef), false);
-
+        addToInput("handUnitName", VariantType::String, false);
     }
 
     void StateInstallTerminateConditions::onEnter()
@@ -325,8 +329,8 @@ namespace armarx
         float timeoutGrasp = getInput<float>("timeoutGrasp");
         condGraspTimeout = setTimeoutEvent(timeoutGrasp, createEvent<EvMinExecutionTimeout>());
 
-        ARMARX_LOG << eINFO << "Installing allJointVelocitiesLow condition";
         float thresholdVelocity = getInput<float>("thresholdVelocity");
+        ARMARX_LOG << eINFO << "Installing allJointVelocitiesLow condition, threshold: " << thresholdVelocity << flush;
 
         //=======
         //first try ... (with ChannelRef)
@@ -372,7 +376,16 @@ namespace armarx
         //set joint velocities manually to zero (DEBUG)
         //---------
 
+        std::string handUnitName = getInput<std::string>("handUnitName");
+        ARMARX_LOG << eIMPORTANT << "xx..................SENDING OBJECTGRASPED TO hand unit name " << handUnitName << "...................";
+
         RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+        if (rsContext->getHandUnit(handUnitName))
+        {
+            ARMARX_LOG << eIMPORTANT << "xx..................SENDING OBJECTGRASPED TO hand unit name " << handUnitName << ".......OK: sending Vitalis............";
+            rsContext->getHandUnit(handUnitName)->setObjectGrasped("Vitalis");
+        }
+
         SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
         //SingleTypeVariantListPtr jointTorquesGraspList = getInput<SingleTypeVariantList>("jointTorquesGrasp");
         NameValueMap jointNamesAndValues;
@@ -380,8 +393,8 @@ namespace armarx
 
         for (int j=0; j<jointNames->getSize(); j++)
         {
-			// we want to ensure that the object stayes grasped -> apply a small velocity (todo: find a better solution!)
-            jointNamesAndValues[jointNames->getVariant(j)->getString()] = 0.01f;  //set everything to zero
+            // no, now we set it to zero.... we want to ensure that the object stayes grasped -> apply a small velocity (todo: find a better solution!)
+            jointNamesAndValues[jointNames->getVariant(j)->getString()] = 0.0;//1f;  //set everything to zero
             controlModes[jointNames->getVariant(j)->getString()] = eVelocityControl;
         }
 

source/RobotAPI/statecharts/GraspingWithTorques/GraspingWithTorques.h

@@ -38,7 +38,7 @@ namespace armarx
     DEFINEEVENT(EvPreshapeTimeout_ToCloseWithJointAngles)
     DEFINEEVENT(EvMinExecutionTimeout)
     DEFINEEVENT(EvGraspWithJointAnglesTimeout)
-    DEFINEEVENT(EvGraspWithTorquesTimeout)
+    //DEFINEEVENT(EvGraspWithTorquesTimeout)
     DEFINEEVENT(EvAllJointVelocitiesLow)
 
     // ****************************************************************

source/RobotAPI/units/HandUnit.cpp

@@ -37,7 +37,7 @@ using namespace VirtualRobot;
 
 void HandUnit::onInitComponent()
 {
-    std::string endeffectorFile = getProperty<std::string>("EndeffectorFile").getValue();
+    std::string endeffectorFile = getProperty<std::string>("RobotFileName").getValue();
     std::string endeffectorName = getProperty<std::string>("EndeffectorName").getValue();
 
     if (!ArmarXDataPath::getAbsolutePath(endeffectorFile, endeffectorFile))
@@ -63,6 +63,29 @@ void HandUnit::onInitComponent()
     {
         throw UserException("Robot does not contain an endeffector with name: " + endeffectorName);
     }
+    eef = robot->getEndEffector(endeffectorName);
+    robotName = robot->getName();
+    if (eef->getTcp())
+        tcpName = robot->getEndEffector(endeffectorName)->getTcp()->getName();
+    else
+        throw UserException("Endeffector without TCP: " + endeffectorName);
+
+    // get all joints
+    std::vector<EndEffectorActorPtr> actors;
+    eef->getActors(actors);
+    for (size_t i=0;i<actors.size();i++)
+    {
+        EndEffectorActorPtr a = actors[i];
+        std::vector<EndEffectorActor::ActorDefinition> ads = a->getDefinition();
+        for (size_t j=0;j<ads.size();j++)
+        {
+            EndEffectorActor::ActorDefinition ad = ads[j];
+            if (ad.directionAndSpeed!=0 && ad.robotNode)
+            {
+                handJoints[ad.robotNode->getName()] = ad.directionAndSpeed;
+            }
+        }
+    }
 
     const std::vector<std::string> endeffectorPreshapes = robot->getEndEffector(endeffectorName)->getPreshapes();
 
@@ -134,3 +157,16 @@ NameValueMap HandUnit::getPreshapeJointValues(const std::string &preshapeName, c
     RobotConfigPtr rc = efp->getPreshape(preshapeName);
     return rc->getRobotNodeJointValueMap();
 }
+
+void HandUnit::setObjectGrasped(const std::string &objectName, const Ice::Current &)
+{
+   ARMARX_INFO << "Object grasped " << objectName << flush;
+   graspedObject = objectName;
+}
+
+void HandUnit::setObjectReleased(const std::string &objectName, const Ice::Current &)
+{
+    ARMARX_INFO << "Object released " << objectName << flush;
+    graspedObject = "";
+}
+

source/RobotAPI/units/HandUnit.h

@@ -32,6 +32,7 @@
 #include <Core/core/system/ImportExportComponent.h>
 #include <Core/observers/variant/SingleTypeVariantList.h>
 
+#include <VirtualRobot/EndEffector/EndEffector.h>
 
 namespace VirtualRobot
 {
@@ -52,7 +53,7 @@ namespace armarx
         HandUnitPropertyDefinitions(std::string prefix):
             ComponentPropertyDefinitions(prefix)
         {
-            defineRequiredProperty<std::string>("EndeffectorFile","VirtualRobot XML file in which the endeffector is is stored.");
+            defineRequiredProperty<std::string>("RobotFileName","VirtualRobot XML file in which the endeffector is is stored.");
             defineRequiredProperty<std::string>("EndeffectorName","Name of the endeffector as stored in the XML file (will publish values on EndeffectorName + 'State')");
         }
     };
@@ -130,6 +131,9 @@ namespace armarx
 
         NameValueMap getPreshapeJointValues(const std::string &preshapeName, const Ice::Current &);
 
+         void setObjectGrasped(const std::string &objectName, const Ice::Current &);
+         void setObjectReleased(const std::string &objectName, const Ice::Current &);
+
 
         /**
          * @see armarx::PropertyUser::createPropertyDefinitions()
@@ -151,8 +155,14 @@ namespace armarx
         SingleTypeVariantListPtr preshapeNames;
 
         VirtualRobot::RobotPtr robot;
+        VirtualRobot::EndEffectorPtr eef;
+
+        std::string robotName;
+        std::string tcpName;
 
+        std::map <std::string, float> handJoints;
 
+        std::string graspedObject;
 
     };
 }