Implemented HandUnitSimulation

source/RobotAPI/components/units/HandUnitSimulation.cpp

 /*
  * This file is part of ArmarX.
  *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ * Copyright (C) 2013-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
  *
  * ArmarX is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -16,8 +16,8 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
  * @package    ArmarXCore::units
- * @author     David Schiebener (schiebener at kit dot edu)
- * @date       2014
+ * @author     Peter Kaiser (peter dot kaiser at kit dot edu)
+ * @date       2017
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
@@ -25,40 +25,99 @@
 
 #include "HandUnitSimulation.h"
 
+#include <Eigen/Geometry>
 
-using namespace armarx;
+#include <cmath>
 
+#include <VirtualRobot/EndEffector/EndEffector.h>
+#include <VirtualRobot/RobotConfig.h>
 
+using namespace armarx;
+using namespace std;
 
 void HandUnitSimulation::onInitHandUnit()
 {
-
+    kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
+    usingProxy(kinematicUnitName);
 }
 
-
-
 void HandUnitSimulation::onStartHandUnit()
 {
-
+    kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+    if (!kinematicUnitPrx)
+    {
+        ARMARX_ERROR << "Failed to obtain kinematic unit proxy";
+    }
 }
 
-
-
 void HandUnitSimulation::onExitHandUnit()
 {
-
 }
 
-
-
-
-void HandUnitSimulation::setShape(const std::string& shapeName, const Ice::Current& c)
+PropertyDefinitionsPtr HandUnitSimulation::createPropertyDefinitions()
 {
+    return PropertyDefinitionsPtr(new HandUnitSimulationPropertyDefinitions(getConfigIdentifier()));
 }
 
-
+void armarx::HandUnitSimulation::setShape(const std::string& shapeName, const Ice::Current&)
+{
+    std::string myShapeName = shapeName;
+    ARMARX_INFO << "Setting shape " << myShapeName;
+
+    if (!eef)
+    {
+        ARMARX_WARNING << "No EEF";
+        return;
+    }
+
+
+    if (!eef->hasPreshape(myShapeName))
+    {
+        ARMARX_INFO << "Shape with name " << myShapeName << " not known in eef " << eef->getName() << ". Looking for partial match";
+
+        bool foundMatch = false;
+
+        for (std::string name : eef->getPreshapes())
+        {
+            if (name.find(myShapeName) != std::string::npos)
+            {
+                foundMatch = true;
+                myShapeName = name;
+                ARMARX_INFO << "Using matching shape: " << name;
+                break;
+            }
+        }
+
+        if (!foundMatch)
+        {
+            ARMARX_WARNING << "No match found for " << myShapeName << " in eef " << eef->getName() << " available shapes: " << eef->getPreshapes();
+            return;
+        }
+    }
+
+    VirtualRobot::RobotConfigPtr config = eef->getPreshape(myShapeName);
+    std::map < std::string, float > jointAngles = config->getRobotNodeJointValueMap();
+
+    NameControlModeMap controlModes;
+
+    for (std::pair<std::string, float> pair : jointAngles)
+    {
+        controlModes.insert(std::make_pair(pair.first, ePositionControl));
+    }
+
+    kinematicUnitPrx->switchControlMode(controlModes);
+    kinematicUnitPrx->setJointAngles(jointAngles);
+}
 
 void armarx::HandUnitSimulation::setJointAngles(const NameValueMap& jointAngles, const Ice::Current&)
 {
+    NameControlModeMap controlModes;
+
+    for (std::pair<std::string, float> pair : jointAngles)
+    {
+        controlModes.insert(std::make_pair(pair.first, ePositionControl));
+    }
 
+    kinematicUnitPrx->switchControlMode(controlModes);
+    kinematicUnitPrx->setJointAngles(jointAngles);
 }

source/RobotAPI/components/units/HandUnitSimulation.h

@@ -16,8 +16,8 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
  * @package    ArmarXCore::units
- * @author     David Schiebener (schiebener at kit dot edu)
- * @date       2014
+ * @author     Peter Kaiser (peter dot kaiser at kit dot edu)
+ * @date       2017
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
@@ -35,14 +35,13 @@ namespace armarx
      * \brief Defines all necessary properties for armarx::HandUnitSimulation
      */
     class HandUnitSimulationPropertyDefinitions:
-        public ComponentPropertyDefinitions
+        public HandUnitPropertyDefinitions
     {
     public:
         HandUnitSimulationPropertyDefinitions(std::string prefix):
-            ComponentPropertyDefinitions(prefix)
+            HandUnitPropertyDefinitions(prefix)
         {
-            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')");
+            defineRequiredProperty<std::string>("KinematicUnitName", "Name of the kinematic unit that should be used");
         }
     };
 
@@ -54,8 +53,6 @@ namespace armarx
      *
      * An instance of a HandUnitSimulation provides means to open, close, and shape hands.
      * It uses the HandUnitListener Ice interface to report updates of its current state
-     *
-     * \warning This component is not yet fully functional
      */
     class HandUnitSimulation :
         virtual public HandUnit
@@ -82,6 +79,14 @@ namespace armarx
          * \warning Not implemented yet!
          */
         virtual void setJointAngles(const NameValueMap& jointAngles, const Ice::Current& c = ::Ice::Current());
+
+        /**
+         * \see armarx::PropertyUser::createPropertyDefinitions()
+         */
+        virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+    protected:
+        KinematicUnitInterfacePrx kinematicUnitPrx;
     };
 }