From 6663aa5ba52fb5dafdbedd9a75f69bd3cfb88b94 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Thu, 3 Jul 2014 14:17:21 +0200
Subject: [PATCH] Open/Close Hand states now use HandUnit instead of setting
 armar3 specific joint values

---
 .../RobotAPI/core/RobotStatechartContext.cpp  | 23 +++++++++++++
 source/RobotAPI/core/RobotStatechartContext.h |  3 ++
 .../RobotAPI/motioncontrol/MotionControl.cpp  | 34 ++++++++++++++-----
 3 files changed, 52 insertions(+), 8 deletions(-)

diff --git a/source/RobotAPI/core/RobotStatechartContext.cpp b/source/RobotAPI/core/RobotStatechartContext.cpp
index 9ab326179..5570d7d06 100644
--- a/source/RobotAPI/core/RobotStatechartContext.cpp
+++ b/source/RobotAPI/core/RobotStatechartContext.cpp
@@ -47,6 +47,18 @@ namespace armarx
         usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
         usingProxy("RobotStateComponent");
         usingProxy(kinematicUnitObserverName);
+
+        if( getProperty<std::string>("HandUnits").isSet())
+        {
+            std::string handUnitsProp = getProperty<std::string>("HandUnits").getValue();
+            std::vector<std::string> handUnitList;
+            boost::split(handUnitList, handUnitsProp, boost::is_any_of(","));
+            for(size_t i = 0; i < handUnitList.size(); i++)
+            {
+                boost::algorithm::trim(handUnitList.at(i));
+                usingProxy(handUnitList.at(i));
+            }
+        }
     }
 
 
@@ -61,6 +73,17 @@ namespace armarx
         kinematicUnitObserverPrx = getProxy<KinematicUnitObserverInterfacePrx>(kinematicUnitObserverName);
         ARMARX_LOG << eINFO << "Fetched proxies" <<  kinematicUnitPrx << " " << robotStateComponent << flush;
 
+        if( getProperty<std::string>("HandUnits").isSet())
+        {
+            std::string handUnitsProp = getProperty<std::string>("HandUnits").getValue();
+            std::vector<std::string> handUnitList;
+            boost::split(handUnitList, handUnitsProp, boost::is_any_of(","));
+            for(size_t i = 0; i < handUnitList.size(); i++)
+            {
+                boost::algorithm::trim(handUnitList.at(i));
+                handUnits[handUnitList.at(i)] = getProxy<HandUnitInterfacePrx>(handUnitList.at(i));
+            }
+        }
 		// initialize remote robot
         remoteRobot.reset(new RemoteRobot(robotStateComponent->getSynchronizedRobot()));
         ARMARX_LOG << eINFO << "Created remote robot" << flush;
diff --git a/source/RobotAPI/core/RobotStatechartContext.h b/source/RobotAPI/core/RobotStatechartContext.h
index 845f84a9f..1596eb373 100644
--- a/source/RobotAPI/core/RobotStatechartContext.h
+++ b/source/RobotAPI/core/RobotStatechartContext.h
@@ -30,6 +30,7 @@
 #include <Core/statechart/StatechartContext.h>
 #include <Core/robotstate/remote/RemoteRobot.h>
 #include <Core/interface/units/KinematicUnitInterface.h>
+#include <Core/interface/units/HandUnitInterface.h>
 
 #include <RobotAPI/interface/units/TCPControlUnit.h>
 
@@ -51,6 +52,7 @@ namespace armarx
         {
             defineRequiredProperty<std::string>("KinematicUnitName", "Name of the kinematic unit that should be used");
             defineRequiredProperty<std::string>("KinematicUnitObserverName", "Name of the kinematic unit observer that should be used");
+            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");
 
         }
     };
@@ -86,6 +88,7 @@ namespace armarx
         KinematicUnitObserverInterfacePrx kinematicUnitObserverPrx;
         TCPControlUnitInterfacePrx tcpControlPrx;
         VirtualRobot::RobotPtr remoteRobot;
+        std::map<std::string, HandUnitInterfacePrx> handUnits;
     private:
         std::string kinematicUnitObserverName;
     };
diff --git a/source/RobotAPI/motioncontrol/MotionControl.cpp b/source/RobotAPI/motioncontrol/MotionControl.cpp
index 4ef098ce9..6eded6e89 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/RobotAPI/motioncontrol/MotionControl.cpp
@@ -711,17 +711,27 @@ void CloseHand::onEnter()
 {
     RobotStatechartContext* context = getContext<RobotStatechartContext>();
     bool useLeftHand = getInput<bool>("useLeftHand");
-    NameValueMap handConfig;
+    std::string handUnitName;
     if (useLeftHand)
     {
-        handConfig["left_hand_configuration_actual_float"] = 4;
+        handUnitName = "LeftHandUnit";
     }
     else
     {
-        handConfig["right_hand_configuration_actual_float"] = 4;
+        handUnitName = "RightHandUnit";
+    }
+
+    std::map<std::string, HandUnitInterfacePrx>::iterator it = context->handUnits.find(handUnitName);
+    if(it != context->handUnits.end())
+    {
+        it->second->close();
+    }
+    else
+    {
+        ARMARX_ERROR  << "No Proxy for HandUnit with Name "  << handUnitName << " known";
     }
 
-    context->kinematicUnitPrx->setJointAngles(handConfig);
+
 
     sendEvent<EvSuccess>();
 }
@@ -739,17 +749,25 @@ void OpenHand::onEnter()
 {
     RobotStatechartContext* context = getContext<RobotStatechartContext>();
     bool useLeftHand = getInput<bool>("useLeftHand");
-    NameValueMap handConfig;
+    std::string handUnitName;
     if (useLeftHand)
     {
-        handConfig["left_hand_configuration_actual_float"] = 1;
+        handUnitName = "LeftHandUnit";
     }
     else
     {
-        handConfig["right_hand_configuration_actual_float"] = 1;
+        handUnitName = "RightHandUnit";
     }
 
-    context->kinematicUnitPrx->setJointAngles(handConfig);
+    std::map<std::string, HandUnitInterfacePrx>::iterator it = context->handUnits.find(handUnitName);
+    if(it != context->handUnits.end())
+    {
+        it->second->open();
+    }
+    else
+    {
+        ARMARX_ERROR  << "No Proxy for HandUnit with Name "  << handUnitName << " known";
+    }
 
     sendEvent<EvSuccess>();
 }
-- 
GitLab