Added headIK to RobotContext (optional)

Changed headIK, so that the remoteRobot is cloned to get a local robot (instead of loading a complete robot instance)
Worked on ForceTorqueUnit
Added mutex to RemoteRobot->clone

source/RobotAPI/core/RobotStatechartContext.cpp

@@ -59,6 +59,12 @@ namespace armarx
                 usingProxy(handUnitList.at(i));
             }
         }
+
+        // headIKUnit
+        headIKUnitName = getProperty<std::string>("HeadIKUnitName").getValue();
+        headIKKinematicChainName = getProperty<std::string>("HeadIKKinematicChainName").getValue();
+        if (!headIKUnitName.empty())
+            usingProxy(headIKUnitName);
     }
 
 
@@ -76,6 +82,11 @@ namespace armarx
 
         ARMARX_LOG << eINFO << "Fetched proxies " << kinUnitName << ":" << kinematicUnitPrx << ", " << rbStateName << ": " << robotStateComponent << flush;
 
+        if (!headIKUnitName.empty())
+        {
+            headIKUnitPrx = getProxy<HeadIKUnitInterfacePrx>(headIKUnitName);
+            ARMARX_LOG << eINFO << "Fetched headIK proxy " << headIKUnitName << ":" << headIKUnitPrx << ", head IK kin chain:" << headIKKinematicChainName << flush;
+        }
 
 
         if( !getProperty<std::string>("HandUnits").getValue().empty())

source/RobotAPI/core/RobotStatechartContext.h

@@ -33,6 +33,7 @@
 #include <RobotAPI/interface/units/HandUnitInterface.h>
 
 #include <RobotAPI/interface/units/TCPControlUnit.h>
+#include <RobotAPI/interface/units/HeadIKUnit.h>
 
 #include <RobotAPI/units/KinematicUnitObserver.h>
 //#include <VirtualRobot/VirtualRobot.h>
@@ -54,6 +55,8 @@ 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");
+            defineOptionalProperty<std::string>("HeadIKUnitName", "", "Name of the head unit that should be used.");
+            defineOptionalProperty<std::string>("HeadIKKinematicChainName", "", "Name of the inematic chain that should be used for head IK.");
         }
     };
 
@@ -92,9 +95,13 @@ namespace armarx
         VirtualRobot::RobotPtr remoteRobot;
         std::map<std::string, HandUnitInterfacePrx> handUnits;
 
+        HeadIKUnitInterfacePrx headIKUnitPrx;
+        std::string headIKKinematicChainName;
+
     private:
         std::string kinematicUnitObserverName;
-    };
+        std::string headIKUnitName;
+     };
 }
 
 #endif

source/RobotAPI/robotstate/remote/RemoteRobot.cpp

@@ -19,6 +19,8 @@ using namespace Eigen;
 
 namespace armarx{
 
+boost::recursive_mutex RemoteRobot::m;
+
 RemoteRobot::RemoteRobot(SharedRobotInterfacePrx robot) :
     Robot(),
     _robot(robot)
@@ -142,6 +144,7 @@ string RemoteRobot::getName()
 
 VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > &childrenMap, RobotPtr robo)
 {
+    boost::recursive_mutex::scoped_lock cloneLock(m);
     static int nonameCounter = 0;
     if (!remoteNode || !robo)
     {
@@ -252,6 +255,7 @@ VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterface
 
 VirtualRobot::RobotPtr RemoteRobot::createLocalClone()
 {
+    boost::recursive_mutex::scoped_lock cloneLock(m);
     std::string robotType = getName();
     std::string robotName = getName();
     VirtualRobot::RobotPtr robo(new VirtualRobot::LocalRobot(robotName,robotType));
@@ -285,6 +289,7 @@ VirtualRobot::RobotPtr RemoteRobot::createLocalClone()
 
 VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string &filename)
 {
+    boost::recursive_mutex::scoped_lock cloneLock(m);
     ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
     VirtualRobot::RobotPtr result;
 

source/RobotAPI/robotstate/remote/RemoteRobot.h

@@ -38,6 +38,7 @@
 #include <RobotAPI/interface/robotstate/RobotState.h>
 
 // boost
+#include <boost/thread/mutex.hpp>
 #include <boost/utility/enable_if.hpp>
 #include <boost/type_traits/is_base_of.hpp>
 
@@ -223,6 +224,8 @@ namespace armarx
             std::map<std::string, VirtualRobot::RobotNodePtr> _cachedNodes;
             VirtualRobot::RobotNodePtr _root;
 
+            static boost::recursive_mutex m;
+
             static VirtualRobot::RobotNodePtr createRemoteRobotNode(SharedRobotNodeInterfacePrx, VirtualRobot::RobotPtr);
     };
 

source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp

@@ -111,14 +111,22 @@ namespace armarx
 
     void StateMoveToNext::onEnter()
     {
-        ARMARX_LOG << eVERBOSE << "Entering StateMoveToNext::onEnter";
+        ARMARX_LOG << eVERBOSE << "Entering StateMoveToNext::onEnter" << flush;
         PlatformContext* context = getContext<PlatformContext>();
         ChannelRefPtr counter = getInput<ChannelRef>("positionCounter");
         int positionIndex = counter->getDataField("value")->getInt();
-
+        ARMARX_DEBUG << "Entering positionIndex:" << positionIndex << flush;
         SingleTypeVariantListPtr points = getInput<SingleTypeVariantList>("targetPositions");
+        ARMARX_DEBUG << "points->getSize:" << points->getSize() << flush;
         if (positionIndex < points->getSize()) {
-            Vector3Ptr currentTarget = getInput<SingleTypeVariantList>("targetPositions")->getVariant(positionIndex)->get<Vector3>();
+            SingleTypeVariantListPtr list = getInput<SingleTypeVariantList>("targetPositions");
+
+            ARMARX_DEBUG << "list->getSize:" << list->getSize() << flush;
+            VariantPtr v = list->getVariant(positionIndex);
+            ARMARX_DEBUG << "v->getTypeName():" << v->getTypeName() << flush;
+
+            Vector3Ptr currentTarget = v->get<Vector3>();
+            ARMARX_DEBUG << "currentTarget:" << currentTarget->toEigen().transpose() << flush;
             float positionalAccuracy = getInput<float>("positionalAccuracy");
             float orientationalAccuracy = getInput<float>("orientationalAccuracy");
             context->platformUnitPrx->moveTo(currentTarget->x, currentTarget->y, currentTarget->z, positionalAccuracy, orientationalAccuracy);

source/RobotAPI/units/ForceTorqueUnit.cpp

@@ -34,6 +34,7 @@ void ForceTorqueUnit::onInitComponent()
 
 void ForceTorqueUnit::onConnectComponent()
 {
+    ARMARX_INFO << "setting report topic to " << listenerName << flush;
     listenerPrx = getTopic<ForceTorqueUnitListenerPrx>(listenerName);
     onStartForceTorqueUnit();
 }

source/RobotAPI/units/HeadIKUnit.cpp

@@ -38,14 +38,15 @@ namespace armarx
         kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
         robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
 
-        remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
+        //remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
+        localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
 
-        // TODO!
-        std::string robotModelFile;
-        ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
-        localRobot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
-        VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
-        localRobot->setConfig(robotSnapshot->getConfig());
+
+        //std::string robotModelFile;
+        //ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
+        //localRobot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
+        //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
+        //localRobot->setConfig(robotSnapshot->getConfig());
 
         requested = false;
         if (execTask) execTask->stop();
@@ -166,8 +167,9 @@ namespace armarx
 
         if (doCalculation)
         {
-            VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
-            localRobot->setConfig(robotSnapshot->getConfig());
+            RemoteRobot::synchronizeLocalClone(localRobot,robotStateComponentPrx);
+            //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx));
+            //localRobot->setConfig(robotSnapshot->getConfig());
 
             VirtualRobot::RobotNodeSetPtr kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName);
             VirtualRobot::RobotNodePrismaticPtr virtualJoint;

source/RobotAPI/units/HeadIKUnit.h

@@ -87,7 +87,7 @@ namespace armarx
 
         RobotStateComponentInterfacePrx robotStateComponentPrx;
         KinematicUnitInterfacePrx kinematicUnitPrx;
-        SharedRobotInterfacePrx remoteRobotPrx;
+        //SharedRobotInterfacePrx remoteRobotPrx;
         VirtualRobot::RobotPtr localRobot;
 
         std::string robotNodeSetName;