Merge branch 'master' of https://i61wiki.itec.uka.de/git/robot-api

Conflicts:
	source/RobotAPI/CMakeLists.txt

source/RobotAPI/CMakeLists.txt

@@ -4,3 +4,5 @@ add_subdirectory(applications)
 add_subdirectory(units)
 add_subdirectory(GraspingWithTorques)
 add_subdirectory(armarx-objects)
+add_subdirectory(OpenHand)
+

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>
@@ -48,6 +46,7 @@ namespace armarx
         addToInput("jointAnglesPreshape", VariantType::List(VariantType::Float), false);
         //how long to wait before switching from preshape to grasp
         addToInput("timeoutPreshape", VariantType::Float, false);
+        addToInput("useTorquesForGrasping", VariantType::Int, false);
 
         //closeHandWithTorques settings:
         addToInput("jointTorquesGrasp", VariantType::List(VariantType::Float), false);        
@@ -55,19 +54,22 @@ namespace armarx
         //low joint velocities as a termination criterion.
         addToInput("timeoutMinExecutionTime", VariantType::Float, false);
 
+        //closeHandWithJointAngles settings:
+        addToInput("jointAnglesGrasp", VariantType::List(VariantType::Float), false);
+
         //installTerminateConditions settings:
         //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("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));
 
     }
 
@@ -75,6 +77,7 @@ namespace armarx
     {
         StatePtr statePreshape = addState<StatePreshape>("statePreshape");
         StatePtr stateCloseHandWithTorques = addState<StateCloseHandWithTorques>("stateCloseHandWithTorques");
+        StatePtr stateCloseHandWithJointAngles = addState<StateCloseHandWithJointAngles>("stateCloseHandWithJointAngles");
         StatePtr stateInstallTerminateConditions = addState<StateInstallTerminateConditions>("stateInstallTerminateConditions");
         StatePtr stateSuccess = addState<SuccessState>("stateSuccess");
         StatePtr stateFailure = addState<FailureState>("stateFailure");
@@ -82,26 +85,35 @@ namespace armarx
         ParameterMappingPtr mapPreshapeInfo = ParameterMapping::createMapping()
                 ->mapFromParent("jointAnglesPreshape", "jointAnglesPreshape")
                 ->mapFromParent("timeoutPreshape", "timeoutPreshape")
-                ->mapFromParent("jointNames", "jointNames");
+                ->mapFromParent("jointNames", "jointNames")
+                ->mapFromParent("useTorquesForGrasping", "useTorquesForGrasping");
 
         ParameterMappingPtr mapCloseHandWithTorquesInfo = ParameterMapping::createMapping()
                 ->mapFromParent("jointTorquesGrasp", "jointTorquesGrasp")
                 ->mapFromParent("timeoutMinExecutionTime", "timeoutMinExecutionTime")
                 ->mapFromParent("jointNames", "jointNames");
 
+        ParameterMappingPtr mapCloseHandWithJointAnglesInfo = ParameterMapping::createMapping()
+                ->mapFromParent("jointAnglesGrasp", "jointAnglesGrasp")
+                ->mapFromParent("timeoutGrasp", "timeoutGrasp")
+                ->mapFromParent("jointNames", "jointNames");
+
         ParameterMappingPtr mapInstallTerminateConditionsInfo = ParameterMapping::createMapping()
                 ->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);
 
         //transitions
-        addTransition<EvPreshapeTimeout>(statePreshape, stateCloseHandWithTorques, mapCloseHandWithTorquesInfo);
+        addTransition<EvPreshapeTimeout_ToCloseWithTorques>(statePreshape, stateCloseHandWithTorques, mapCloseHandWithTorquesInfo);
+        addTransition<EvPreshapeTimeout_ToCloseWithJointAngles>(statePreshape, stateCloseHandWithJointAngles, mapCloseHandWithJointAnglesInfo);
         addTransition<EvMinExecutionTimeout>(stateCloseHandWithTorques, stateInstallTerminateConditions, mapInstallTerminateConditionsInfo);
-        addTransition<EvGraspTimeout>(stateInstallTerminateConditions, stateFailure);
+        addTransition<EvGraspWithTorquesTimeout>(stateInstallTerminateConditions, stateFailure);
         addTransition<EvAllJointVelocitiesLow>(stateInstallTerminateConditions, stateSuccess);
+        addTransition<EvGraspWithJointAnglesTimeout>(stateCloseHandWithJointAngles, stateSuccess);
 
     }
 
@@ -110,18 +122,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);
+        setLocal("jointVelocitiesDatafields", dataFields);
+
     }
 
     void StatechartGraspingWithTorques::onExit()
@@ -140,8 +158,9 @@ 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);
+        addToInput("useTorquesForGrasping", VariantType::Int, false);
     }
 
     void StatePreshape::onEnter()
@@ -151,7 +170,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,14 +182,16 @@ 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";
         float timeoutPreshape = getInput<float>("timeoutPreshape");
-        condPreshapeTimeout = setTimeoutEvent(timeoutPreshape, createEvent<EvPreshapeTimeout>());
+
+        int useTorquesForGrasping = getInput<int>("useTorquesForGrasping");
+        if (useTorquesForGrasping > 0)
+            condPreshapeTimeout = setTimeoutEvent(timeoutPreshape, createEvent<EvPreshapeTimeout_ToCloseWithTorques>());
+        else
+            condPreshapeTimeout = setTimeoutEvent(timeoutPreshape, createEvent<EvPreshapeTimeout_ToCloseWithJointAngles>());
 
         ARMARX_LOG << eINFO << "Done onEnter()";
     }
@@ -228,6 +248,53 @@ namespace armarx
         removeTimeoutEvent(condMinimumExecutionTimeout);
     }
 
+    // ****************************************************************
+    // Implementation of StateCloseHandWithJointAngles
+    // ****************************************************************
+
+    void StateCloseHandWithJointAngles::defineParameters()
+    {
+        //closeHandWithJointAngles settings:
+        addToInput("jointAnglesGrasp", VariantType::List(VariantType::Float), false);
+        addToInput("timeoutGrasp", VariantType::Float, false);
+
+        addToInput("jointNames", VariantType::List(VariantType::String), false);
+    }
+
+    void StateCloseHandWithJointAngles::onEnter()
+    {
+        ARMARX_LOG << eIMPORTANT << "Entering stateCloseHandWithJointAngles !!!";
+
+        RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+        SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
+        SingleTypeVariantListPtr jointAnglesGraspList = getInput<SingleTypeVariantList>("jointAnglesGrasp");
+        NameValueMap jointNamesAndValues;
+
+        if (jointNames->getSize() == jointAnglesGraspList->getSize())
+        {
+            for (int j=0; j<jointNames->getSize(); j++)
+            {
+                jointNamesAndValues[jointNames->getVariant(j)->getString()]=jointAnglesGraspList->getVariant(j)->getFloat();
+            }
+        }
+        else
+            throw LocalException("stateCloseHandWithJointAngles: List lengths do not match!");
+
+        rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
+
+        ARMARX_LOG << eINFO << "Installing timeoutGrasp condition";
+        float timeoutGrasp = getInput<float>("timeoutGrasp");
+        condGraspWithJointAnglesTimeout = setTimeoutEvent(timeoutGrasp, createEvent<EvGraspWithJointAnglesTimeout>());
+
+        ARMARX_LOG << eIMPORTANT << "Done onEnter()";
+    }
+
+    void StateCloseHandWithJointAngles::onExit()
+    {
+        removeTimeoutEvent(condGraspWithJointAnglesTimeout);
+    }
+
+
     // ****************************************************************
     // Implementation of StateInstallTerminateConditions
     // ****************************************************************
@@ -241,8 +308,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 +325,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 +337,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 +365,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);
+
+
+
     }
 
 }

source/RobotAPI/GraspingWithTorques/GraspingWithTorques.h

@@ -34,9 +34,11 @@ namespace armarx
     // ****************************************************************
 
     //only for first tests
-    DEFINEEVENT(EvPreshapeTimeout)
+    DEFINEEVENT(EvPreshapeTimeout_ToCloseWithTorques)
+    DEFINEEVENT(EvPreshapeTimeout_ToCloseWithJointAngles)
     DEFINEEVENT(EvMinExecutionTimeout)
-    DEFINEEVENT(EvGraspTimeout)
+    DEFINEEVENT(EvGraspWithJointAnglesTimeout)
+    DEFINEEVENT(EvGraspWithTorquesTimeout)
     DEFINEEVENT(EvAllJointVelocitiesLow)
 
     // ****************************************************************
@@ -90,6 +92,26 @@ namespace armarx
         StateUtility::ActionEventIdentifier condMinimumExecutionTimeout;
     };
 
+    // ****************************************************************
+    // Definition of StateCloseHandWithJointAngles
+    // ****************************************************************
+
+    /**
+     * StateCloseHandWithJointAngles: Close the hand using a target joint angle configuration.
+     */
+    struct StateCloseHandWithJointAngles :
+        StateTemplate<StateCloseHandWithJointAngles>
+    {
+        void defineParameters();
+        void onEnter();
+        void onExit();
+
+        //Make sure that the joints have time to start moving before we check
+        //low joint velocities as a termination criterion.
+        StateUtility::ActionEventIdentifier condGraspWithJointAnglesTimeout;
+    };
+
+
     // ****************************************************************
     // Definition of StateInstallTerminateConditions
     // ****************************************************************

source/RobotAPI/OpenHand/CMakeLists.txt

+armarx_set_target("RobotAPI Library: OpenHand")
+
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+if (ARMARX_BUILD)
+    include_directories(${Eigen3_INCLUDE_DIR})
+    include_directories(${Simox_INCLUDE_DIRS})
+
+    set(LIB_NAME       OpenHand)
+    set(LIB_VERSION    0.1.0)
+    set(LIB_SOVERSION  0)
+
+    set(LIBS RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers)
+
+    set(LIB_FILES OpenHand.cpp
+         )
+    set(LIB_HEADERS OpenHand.h
+         )
+
+    add_library(${LIB_NAME} SHARED ${LIB_FILES} ${LIB_HEADERS})
+
+    library_settings("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_HEADERS}")
+    target_link_ice(${LIB_NAME})
+    target_link_libraries(${LIB_NAME} ${LIBS})
+endif()
+

source/RobotAPI/OpenHand/OpenHand.cpp

+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package    RobotAPI::OpenHand
+* @author     Markus Przybylski
+* @date       2014 Markus Przybylski
+
+* @copyright  http://www.gnu.org/licenses/gpl.txt
+*             GNU General Public License
+*/
+
+#include "OpenHand.h"
+//#include "Armar3GraspContext.h"   //MP: Maybe necessary again later?
+#include "../core/RobotStatechartContext.h"
+
+#include <Core/observers/variant/ChannelRef.h>
+#include <Core/observers/variant/SingleTypeVariantList.h>
+#include <Core/robotstate/remote/ArmarPose.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
+#include <Core/interface/units/KinematicUnitInterface.h>
+
+#include <RobotAPI/core/RobotStatechartContext.h>
+
+namespace armarx
+{
+    // ****************************************************************
+    // Implementation of StatechartOpenHand
+    // ****************************************************************
+    void StatechartOpenHand::defineParameters()     //(OK)TODO: Anpassen!
+    {
+        //openHand settings:
+        addToInput("jointAnglesOpenHand", VariantType::List(VariantType::Float), false);
+        addToInput("timeoutOpenHandSuccess", VariantType::Float, false);
+        addToInput("timeoutOpenHandDummyFailure", VariantType::Float, false);
+
+        addToInput("robotNodeSetName", VariantType::String, false);
+        addToLocal("jointNames", VariantType::List(VariantType::String));
+
+        //addToLocal("jointVelocitiesDatafields", VariantType::List(VariantType::DatafieldRef));
+
+    }
+
+    void StatechartOpenHand::defineSubstates()     //(OK)TODO: Anpassen!
+    {
+        StatePtr stateOpenHand = addState<StateOpenHand>("stateOpenHand");
+        StatePtr stateSuccess = addState<SuccessState>("stateSuccess");
+        StatePtr stateFailure = addState<FailureState>("stateFailure");
+
+        ParameterMappingPtr mapOpenHandInfo = ParameterMapping::createMapping()
+                ->mapFromParent("jointAnglesOpenHand", "jointAnglesOpenHand")
+                ->mapFromParent("timeoutOpenHandSuccess", "timeoutOpenHandSuccess")
+                ->mapFromParent("timeoutOpenHandDummyFailure", "timeoutOpenHandDummyFailure")
+                ->mapFromParent("jointNames", "jointNames");
+
+        setInitState(stateOpenHand, mapOpenHandInfo);
+
+        //transitions
+        addTransition<EvOpenHandSuccessTimeout>(stateOpenHand, stateSuccess);
+        addTransition<EvOpenHandDummyFailureTimeout>(stateOpenHand, stateFailure);  //Do we need to include a failure state?
+
+    }
+
+    void StatechartOpenHand::onEnter()     //TODO: Anpassen! ???
+    {
+        ARMARX_LOG << eINFO << "Entering StatechartOpenHand";
+
+        RobotStatechartContext* context = getContext<RobotStatechartContext>();
+        //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);
+        setLocal("jointVelocitiesDatafields", dataFields);
+
+    }
+
+    void StatechartOpenHand::onExit()
+    {
+
+        ARMARX_LOG << eINFO << "Exiting StatechartOpenHand...";
+
+    }
+
+    // ****************************************************************
+    // Implementation of StateOpenHand
+    // ****************************************************************
+
+    void StateOpenHand::defineParameters()      //(OK)TODO: Anpassen!
+    {
+        addToInput("jointAnglesOpenHand", VariantType::List(VariantType::Float), false);
+        addToInput("timeoutOpenHandSuccess", VariantType::Float, false);
+
+        addToInput("jointNames", VariantType::List(VariantType::String), false);
+        //addToInput("useTorquesForGrasping", VariantType::Int, false);
+    }
+
+    void StateOpenHand::onEnter()      //(OK)TODO: Anpassen!
+    {
+        ARMARX_LOG << eIMPORTANT << "Entering StateOpenHand";
+
+        RobotStatechartContext* rsContext = getContext<RobotStatechartContext>();
+        SingleTypeVariantListPtr jointNames = getInput<SingleTypeVariantList>("jointNames");
+        SingleTypeVariantListPtr jointAnglesOpenHandList = getInput<SingleTypeVariantList>("jointAnglesOpenHand");
+        NameValueMap jointNamesAndValues;
+
+        if (jointNames->getSize() == jointAnglesOpenHandList->getSize())
+        {
+            for (int j=0; j<jointNames->getSize(); j++)
+            {
+                jointNamesAndValues[jointNames->getVariant(j)->getString()]=jointAnglesOpenHandList->getVariant(j)->getFloat();
+            }
+        }
+        else
+            throw LocalException("StateOpenHand: List lengths do not match!");
+
+        rsContext->kinematicUnitPrx->setJointAngles(jointNamesAndValues);
+
+        ARMARX_LOG << eINFO << "Installing openHand timeout condition";
+        float timeoutOpenHandSuccess = getInput<float>("timeoutOpenHandSuccess");
+
+        condOpenHandSuccessTimeout = setTimeoutEvent(timeoutOpenHandSuccess, createEvent<EvOpenHandSuccessTimeout>());
+        condOpenHandDummyFailureTimeout = setTimeoutEvent(timeoutOpenHandSuccess, createEvent<EvOpenHandDummyFailureTimeout>());
+
+        ARMARX_LOG << eINFO << "StateOpenHand: Done onEnter()";
+    }
+
+    void StateOpenHand::onExit()      //(OK)TODO: Anpassen!
+    {
+        removeTimeoutEvent(condOpenHandSuccessTimeout);
+        removeTimeoutEvent(condOpenHandDummyFailureTimeout);
+        ARMARX_LOG << eIMPORTANT << "StateOpenHand: Done onExit()";
+    }
+
+
+
+}
+

source/RobotAPI/OpenHand/OpenHand.h

+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package    RobotAPI::OpenHand
+* @author     Markus Przybylski
+* @date       2014 Markus Przybylski
+* @copyright  http://www.gnu.org/licenses/gpl.txt
+*             GNU General Public License
+*/
+
+#ifndef ARMARX_COMPONENT_OPEN_HAND_H
+#define ARMARX_COMPONENT_OPEN_HAND_H
+
+#include <Core/statechart/Statechart.h>
+
+
+namespace armarx
+{
+    // ****************************************************************
+    // Events
+    // ****************************************************************
+
+    //only for first tests
+    DEFINEEVENT(EvOpenHandSuccessTimeout)
+    DEFINEEVENT(EvOpenHandDummyFailureTimeout)
+
+
+    // ****************************************************************
+    // Definition of StatechartOpenHand
+    // ****************************************************************
+    struct StatechartOpenHand :
+        StateTemplate<StatechartOpenHand>
+    {
+        void defineParameters();
+        void defineSubstates();
+        void onEnter();
+        void onExit();
+
+    };
+
+
+
+    // ****************************************************************
+    // Definition of StateOpenHand
+    // ****************************************************************
+    /**
+     * StateOpenHand: Move the fingers to a open configuration, then wait a little.
+     */
+
+    struct StateOpenHand :
+        StateTemplate<StateOpenHand>
+    {
+        void defineParameters();
+        void onEnter();
+        void onExit();
+
+        StateUtility::ActionEventIdentifier condOpenHandSuccessTimeout;
+        StateUtility::ActionEventIdentifier condOpenHandDummyFailureTimeout;
+    };
+
+
+
+
+
+
+
+
+
+}
+
+#endif

source/RobotAPI/applications/ForceTorqueObserver/CMakeLists.txt

@@ -20,9 +20,7 @@ armarx_build_if(ArmarXCoreObservers_ENABLED "ArmarXCoreObservers library disable
 if (ARMARX_BUILD)
     set(COMPONENT_LIBS RobotAPIUnits RobotAPICore RobotAPIInterfaces  ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRemoteRobot)
 
-    set(SOURCES "")
-    set(HEADERS ForceTorqueObserverApp.h)
-
-    armarx_component_add_executable("${SOURCES}" "${HEADERS}")
+    set(SOURCES main.cpp ForceTorqueObserverApp.h)
 
+    armarx_add_component_executable("${SOURCES}")
 endif()

source/RobotAPI/applications/ForceTorqueObserver/main.cpp

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::application::ForceTorqueObserver
+ * @author     Manfred Kroehnert
+ * @date       2014
+ * @copyright  http://www.gnu.org/licenses/gpl.txt
+ *             GNU General Public License
+ */
+
+#include "ForceTorqueObserverApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+    armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::ForceTorqueObserverApp>();
+    app->setName("ForceTorqueObserver");
+
+    return app->main(argc, argv);
+}

source/RobotAPI/applications/MotionControl/CMakeLists.txt

@@ -20,9 +20,7 @@ armarx_build_if(ArmarXCoreObservers_ENABLED "ArmarXCoreObservers library disable
 if (ARMARX_BUILD)
     set(COMPONENT_LIBS MotionControl RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations)
 
-    set(SOURCES "")
-    set(HEADERS MotionControlApp.h)
-
-    armarx_component_add_executable("${SOURCES}" "${HEADERS}")
+    set(SOURCES main.cpp MotionControlApp.h)
 
+    armarx_add_component_executable("${SOURCES}")
 endif()

source/RobotAPI/applications/MotionControl/main.cpp

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::application::MotionControl
+ * @author     Manfred Kroehnert
+ * @date       2014
+ * @copyright  http://www.gnu.org/licenses/gpl.txt
+ *             GNU General Public License
+ */
+
+#include "MotionControlApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+    armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::MotionControlApp>();
+    app->setName("MotionControl");
+
+    return app->main(argc, argv);
+}

source/RobotAPI/applications/MotionControlTest/CMakeLists.txt

@@ -22,9 +22,7 @@ if (ARMARX_BUILD)
     include_directories(${Simox_INCLUDE_DIRS})
     set(COMPONENT_LIBS MotionControl RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations ${Simox_LIBRARIES})
 
-    set(SOURCES "")
-    set(HEADERS MotionControlTestApp.h)
-
-    armarx_component_add_executable("${SOURCES}" "${HEADERS}")
+    set(SOURCES main.cpp MotionControlTestApp.h)
 
+    armarx_add_component_executable("${SOURCES}")
 endif()

source/RobotAPI/applications/MotionControlTest/main.cpp

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::application::MotionControlTest
+ * @author     Manfred Kroehnert
+ * @date       2014
+ * @copyright  http://www.gnu.org/licenses/gpl.txt
+ *             GNU General Public License
+ */
+
+#include "MotionControlTestApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+    armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::MotionControlTestApp>();
+    app->setName("MotionControlTest");
+
+    return app->main(argc, argv);
+}

source/RobotAPI/applications/TCPControlUnit/CMakeLists.txt

@@ -22,9 +22,7 @@ if (ARMARX_BUILD)
     include_directories(${Simox_INCLUDE_DIRS})
     set(COMPONENT_LIBS RobotAPIUnits RobotAPICore RobotAPIInterfaces  ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRemoteRobot ${Simox_LIBRARIES})
 
-    set(SOURCES "")
-    set(HEADERS TCPControlUnitApp.h)
-
-    armarx_component_add_executable("${SOURCES}" "${HEADERS}")
+    set(SOURCES main.cpp TCPControlUnitApp.h)
 
+    armarx_add_component_executable("${SOURCES}")
 endif()

source/RobotAPI/applications/TCPControlUnit/main.cpp

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::application::TCPControlUnit
+ * @author     Manfred Kroehnert
+ * @date       2014
+ * @copyright  http://www.gnu.org/licenses/gpl.txt
+ *             GNU General Public License
+ */
+
+#include "TCPControlUnitApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+    armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::TCPControlUnitApp>();
+    app->setName("TCPControlUnit");
+
+    return app->main(argc, argv);
+}