Initial import

git-svn-id: https://svn.sfb588.uni-karlsruhe.de/svn/HumanoidRobotAPI@1 e7ad446d-f7d8-486a-9efb-df2f5bbea5c6

CMakeLists.txt

+# HumanoidRobotAPI
+
+cmake_minimum_required(VERSION 2.8)
+if (COMMAND cmake_policy)
+    if (POLICY CMP0011)
+        cmake_policy(SET CMP0011 NEW)
+    endif()
+endif()
+
+find_package("ArmarXCore" REQUIRED
+    PATHS "$ENV{HOME}/armarx/Core/build/cmake"
+        "$ENV{HOME}/armarx-install/share/cmake/ArmarXCore"
+        "/org/share/archive/SFB588_RefDist/armarx/share/cmake/ArmarXCore"
+)
+
+include(${ArmarXCore_CMAKE_DIR}/ArmarXProject.cmake)
+
+armarx_project("HumanoidRobotAPI")
+
+
+add_subdirectory(core)
+add_subdirectory(motioncontrol)
+add_subdirectory(applications)
+
+
+
+install_project()
+

applications/CMakeLists.txt

+
+
+add_subdirectory(MotionControlTest)
+

applications/MotionControlTest/.goutputstream-4MXZKW

+
+armarx_component_set_name(MotionControlTest)
+
+set(COMPONENT_BUILD TRUE)
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+armarx_build_if(COMPONENT_BUILD "component disabled")
+
+# check if ArmarXCoreUnits library gets built
+# LOCATION is NOT-FOUND (equal to FALSE) if library is disabled
+GET_TARGET_PROPERTY(ArmarXCoreUnits_ENABLED ArmarXCoreUnits LOCATION)
+armarx_build_if(ArmarXCoreUnits_ENABLED "ArmarXCoreUnits library disabled")
+
+GET_TARGET_PROPERTY(ArmarXCoreObservers_ENABLED ArmarXCoreObservers LOCATION)
+armarx_build_if(ArmarXCoreObservers_ENABLED "ArmarXCoreObservers library disabled")
+
+
+if (ARMARX_BUILD)
+    set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations)
+
+    set(SOURCES "")
+    set(HEADERS MotionControlTestApp.h)
+
+    armarx_component_add_executable("${SOURCES}" "${HEADERS}")
+
+endif()

applications/MotionControlTest/.goutputstream-ABGNKW

+
+armarx_component_set_name(MotionControlTest)
+
+set(COMPONENT_BUILD TRUE)
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+armarx_build_if(COMPONENT_BUILD "component disabled")
+
+# check if ArmarXCoreUnits library gets built
+# LOCATION is NOT-FOUND (equal to FALSE) if library is disabled
+GET_TARGET_PROPERTY(ArmarXCoreUnits_ENABLED ArmarXCoreUnits LOCATION)
+armarx_build_if(ArmarXCoreUnits_ENABLED "ArmarXCoreUnits library disabled")
+
+GET_TARGET_PROPERTY(ArmarXCoreObservers_ENABLED ArmarXCoreObservers LOCATION)
+armarx_build_if(ArmarXCoreObservers_ENABLED "ArmarXCoreObservers library disabled")
+
+
+if (ARMARX_BUILD)
+    set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations)
+
+    set(SOURCES "")
+    set(HEADERS MotionControlTestApp.h)
+
+    armarx_component_add_executable("${SOURCES}" "${HEADERS}")
+
+endif()

applications/MotionControlTest/CMakeLists.txt

+
+armarx_component_set_name(MotionControlTest)
+
+set(COMPONENT_BUILD TRUE)
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+armarx_build_if(COMPONENT_BUILD "component disabled")
+
+# check if ArmarXCoreUnits library gets built
+# LOCATION is NOT-FOUND (equal to FALSE) if library is disabled
+GET_TARGET_PROPERTY(ArmarXCoreUnits_ENABLED ArmarXCoreUnits LOCATION)
+armarx_build_if(ArmarXCoreUnits_ENABLED "ArmarXCoreUnits library disabled")
+
+GET_TARGET_PROPERTY(ArmarXCoreObservers_ENABLED ArmarXCoreObservers LOCATION)
+armarx_build_if(ArmarXCoreObservers_ENABLED "ArmarXCoreObservers library disabled")
+
+
+if (ARMARX_BUILD)
+    set(COMPONENT_LIBS MotionControl HumanoidRobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations)
+
+    set(SOURCES "")
+    set(HEADERS MotionControlTestApp.h)
+
+    armarx_component_add_executable("${SOURCES}" "${HEADERS}")
+
+endif()

applications/MotionControlTest/MotionControlTestApp.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    ArmarXCore::applications
+ * @author     Kai Welke (weöle dot at kit dot edu)
+ * @date       2012
+ * @copyright  http://www.gnu.org/licenses/gpl.txt
+ *             GNU General Public License
+ */
+
+
+#include <Core/core/application/Application.h>
+#include <motioncontrol/MotionControl.h>
+
+namespace armarx
+{
+    class MotionControlTestApp :
+		virtual public armarx::Application
+	{
+        void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties) 
+        { 
+            registry->addObject( Component::create<MoveHelpers::MotionControl>(properties) );
+        };
+    };
+};
+
+
+

core/CMakeLists.txt

+armarx_set_target("HumanoidRobotAPI Core Library: HumanoidRobotAPICore")
+
+
+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(${VirtualRobot_INCLUDE_DIRS})
+
+    set(LIB_NAME       HumanoidRobotAPICore)
+    set(LIB_VERSION    0.1.0)
+    set(LIB_SOVERSION  0)
+
+    set(LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRobotStateComponent)
+
+    set(LIB_FILES RobotStatechartContext.cpp)
+    set(LIB_HEADERS RobotStatechartContext.h)
+
+    include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../..)
+    use_ice()
+
+    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()
+

core/RobotStatechartContext.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    Armar4::api
+* @author     Nikolaus Vahrenkamp
+* @date       2012 Nikolaus Vahrenkamp
+* @copyright  http://www.gnu.org/licenses/gpl.txt
+*             GNU General Public License
+*/
+
+#include <Core/core/Component.h>
+#include <Core/core/system/ImportExportComponent.h>
+#include <Core/statechart/Statechart.h>
+#include <Core/robotstate/remote/RemoteRobot.h>
+#include <Core/robotstate/remote/ArmarPose.h>
+#include <Core/robotstate/RobotStateObjectFactories.h>
+
+
+//#include <VirtualRobot/VirtualRobot.h>
+
+#include "RobotStatechartContext.h"
+
+namespace armarx
+{
+    // ****************************************************************
+    // Implementation of Component
+    // ****************************************************************
+    void RobotStatechartContext::onInitComponent()
+    {
+        StatechartContext::onInitComponent();
+        ARMARX_LOG << eINFO << "Init Armar4Context" << flush;
+
+        RobotStateObjectFactories::addFactories(getIceManager()->getCommunicator());
+        kinematicUnitObserverName = getProperty<std::string>("KinematicUnitObserverName").getValue();
+
+
+        usingProxy("RobotStateComponent");
+        usingProxy(kinematicUnitObserverName);
+    }
+
+
+    void RobotStatechartContext::onConnectComponent()
+    {
+        StatechartContext::onConnectComponent();
+        ARMARX_LOG << eINFO << "Starting Armar4Context" << flush;
+
+        // retrieve proxies
+        robotStateComponent = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
+        kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
+        kinematicUnitObserverPrx = getProxy<KinematicUnitObserverInterfacePrx>(kinematicUnitObserverName);
+        ARMARX_LOG << eINFO << "Fetched proxies" <<  kinematicUnitPrx << " " << robotStateComponent << flush;
+
+		// initialize remote robot	
+        remoteRobot.reset(new RemoteRobot(robotStateComponent->getSynchronizedRobot()));
+        ARMARX_LOG << eINFO << "Created remote robot" << flush;
+    }
+
+    PropertyDefinitionsPtr RobotStatechartContext::createPropertyDefinitions()
+    {
+        return PropertyDefinitionsPtr(new RobotStatechartContextProperties(
+                                               getConfigIdentifier()));
+    }
+
+   /* const VirtualRobot::RobotPtr armarx::Armar4Context::getRobot()
+    {
+        return remoteRobot;
+    }*/
+
+}
+

core/RobotStatechartContext.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    Armar4::api
+* @author     Nikolaus Vahrenkamp
+* @date       2012 Nikolaus Vahrenkamp
+* @copyright  http://www.gnu.org/licenses/gpl.txt
+*             GNU General Public License
+*/
+
+
+#ifndef ARMARX_COMPONENT_Armar4Api_Armar4Context_H
+#define ARMARX_COMPONENT_Armar4Api_Armar4Context_H
+
+#include <Core/core/Component.h>
+#include <Core/core/system/ImportExportComponent.h>
+#include <Core/statechart/StatechartContext.h>
+#include <Core/robotstate/remote/RemoteRobot.h>
+#include <Core/interface/cpp/units/KinematicUnitInterface.h>
+
+#include <Core/units/KinematicUnitObserver.h>
+//#include <VirtualRobot/VirtualRobot.h>
+#include <IceUtil/Time.h>
+
+namespace armarx
+{
+
+    // ****************************************************************
+    // Component and context
+    // ****************************************************************
+
+    struct RobotStatechartContextProperties : ComponentPropertyDefinitions
+    {
+        RobotStatechartContextProperties(std::string prefix):
+            ComponentPropertyDefinitions(prefix)
+        {
+            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");
+
+        }
+    };
+
+    class ARMARXCOMPONENT_IMPORT_EXPORT RobotStatechartContext :
+        virtual public StatechartContext
+    {
+    public:
+        // inherited from Component
+        virtual std::string getDefaultName() { return "RobotStatechartContext"; };
+        virtual void onInitComponent();
+        virtual void onConnectComponent();
+
+        // todo:read access should only be allowed via const getters ?!
+        //const VirtualRobot::RobotPtr getRobot();
+    //private:
+
+
+        /**
+         * @see PropertyUser::createPropertyDefinitions()
+         */
+        virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+        std::string getKinematicUnitObserverName() { return kinematicUnitObserverName; }
+
+		RobotStateComponentInterfacePrx robotStateComponent;
+		KinematicUnitInterfacePrx kinematicUnitPrx;
+        KinematicUnitObserverInterfacePrx kinematicUnitObserverPrx;
+
+        //SystemObserverInterfacePrx	systemObserver; // already defined in StatechartContext
+        VirtualRobot::RobotPtr remoteRobot;
+    private:
+        std::string kinematicUnitObserverName;
+    };
+}
+
+#endif

motioncontrol/CMakeLists.txt

+armarx_set_target("HumanoidRobotAPI MotionControl Library: MotionControl")
+
+
+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(${VirtualRobot_INCLUDE_DIRS})
+
+    set(LIB_NAME       MotionControl)
+    set(LIB_VERSION    0.1.0)
+    set(LIB_SOVERSION  0)
+
+    set(LIBS HumanoidRobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers)
+
+    set(LIB_FILES MotionControl.cpp)
+    set(LIB_HEADERS MotionControl.h)
+
+    include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../..)
+    use_ice()
+
+    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()
+

motioncontrol/MotionControl.cpp

+#include "MotionControl.h"
+#include <VirtualRobot/IK/GenericIKSolver.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/MathTools.h>
+#include <Core/robotstate/remote/RemoteRobot.h>
+#include <Core/observers/variant/ChannelRef.h>
+#include <Core/observers/ConditionCheck.h>
+
+#include <Core/core/system/ArmarXDataPath.h>
+
+#include <Eigen/src/Geometry/Quaternion.h>
+
+using namespace armarx;
+using namespace armarx::MoveHelpers;
+
+MotionControl::MotionControl()
+{
+}
+
+void MotionControl::onInitRemoteStateHandler()
+{
+    addState<MoveJoints>("MoveJoints");
+    addState<MotionControlTestState>("MotionControlTestState");
+}
+
+void MotionControl::onConnectRemoteStateHandler()
+{
+}
+
+MoveJoints::MoveJoints()
+{
+
+}
+
+void MoveJoints::defineParameters()
+{
+
+    context = getContext<RobotStatechartContext>();
+
+    std::string configFilepathAbsolute;
+    ArmarXDataPath::getAbsolutePath("HumanoidRobotAPIConfigs/stateconfigs/MotionControl.xml", configFilepathAbsolute);
+    setConfigFile(configFilepathAbsolute);
+
+    addListToInput("jointNames", VariantType::String, false);
+    addListToInput("targetJointValues", VariantType::Float, false);
+
+    // TODO: add when we have decided how to do it
+    addListToInput("jointMaxSpeeds", VariantType::Float, true);
+    addToInput("jointMaxSpeed", VariantType::Float, true);
+
+    addToInput("targetTolerance", VariantType::Float, false);
+
+    addToInput("timeoutInMs", VariantType::Int, false);
+
+
+    addListToOutput("jointDistancesToTargets", VariantType::Float, true);
+
+
+
+}
+
+void MoveJoints::onEnter()
+{
+
+    NameValueMap targetJointAngles;
+    SingleTypeVariantListPtr jointNames = getInputList("jointNames");
+    SingleTypeVariantListPtr targetJointValues = getInputList("targetJointValues");
+
+    if (jointNames->getSize()!=targetJointValues->getSize())
+    {
+        throw LocalException("Sizes of joint name list and joint value list do not match!");
+    }
+
+    for(int i=0; i<jointNames->getSize(); i++ )
+    {
+        targetJointAngles[jointNames->getElement(i)->getString()] = targetJointValues->getElement(i)->getFloat();
+        ARMARX_VERBOSE << "setting joint angle for joint '" << jointNames->getElement(i)->getString() << "' to " << targetJointValues->getElement(i)->getFloat() << std::endl;
+    }
+    // TODO: Set Max Velocities
+
+    // now install the condition
+
+    ConditionPtr cond = Condition::create();
+    float tolerance = getInput<float>("targetTolerance");
+    for(int i=0; i<jointNames->getSize(); i++ )
+    {
+        ParameterList inrangeCheck;
+        inrangeCheck.push_back(new Variant(targetJointValues->getElement(i)->getFloat()-tolerance));
+        inrangeCheck.push_back(new Variant(targetJointValues->getElement(i)->getFloat()+tolerance));
+
+        cond->add(DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointangles", jointNames->getElement(i)->getString()), "inrange", inrangeCheck);
+    }
+
+    targetReachedCondition = installCondition<EvJointTargetReached>(cond);
+    ARMARX_INFO << "condition installed" << std::endl;
+    timeoutEvent = setTimeoutEvent(getInput<int>("timeoutInMs"), createEvent<EvTimeout>());
+    ARMARX_INFO << "timeout installed" << std::endl;
+    context->kinematicUnitPrx->setJointAngles(targetJointAngles);
+}
+
+void MoveJoints::onBreak()
+{
+    removeCondition(targetReachedCondition);
+    removeTimeoutEvent(timeoutEvent);
+}
+
+void MoveJoints::onExit()
+{
+
+    SingleTypeVariantListPtr jointNames = getInputList("jointNames");
+    SingleTypeVariantListPtr targetJointValues = getInputList("targetJointValues");
+    SingleTypeVariantList resultList(VariantType::Float);
+
+
+
+    for(int i=0; i<jointNames->getSize(); i++ )
+    {
+        DataFieldIdentifierPtr datafield = new DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointangles", jointNames->getElement(i)->getString());
+        float jointAngleActual = context->kinematicUnitObserverPrx->getDataField(datafield)->getFloat();
+        float jointTargetValue = targetJointValues->getElement(i)->getFloat();
+        resultList.addElement(new Variant(jointAngleActual - jointTargetValue));
+    }
+    setOutput("jointDistancesToTargets", resultList);
+
+    ARMARX_INFO << "output: " << StateUtilFunctions::getDictionaryString(getOutputParameters()) << std::endl;
+
+    removeCondition(targetReachedCondition);
+    removeTimeoutEvent(timeoutEvent);
+}
+
+void MoveTCPPoseIK::defineParameters()
+{
+
+    context = getContext<RobotStatechartContext>();
+
+    std::string configFilepathAbsolute;
+    ArmarXDataPath::getAbsolutePath("HumanoidRobotAPIConfigs/stateconfigs/MotionControl.xml", configFilepathAbsolute);
+    setConfigFile(configFilepathAbsolute);
+
+    addToInput("KinematicChainName", VariantType::String, false);
+    addToInput("targetTCPPosition", VariantType::LinkedPosition, false);
+    addToInput("targetTCPOrientation", VariantType::LinkedOrientation, false);
+
+    addToInput("tcpMaxSpeed", VariantType::Float, true);
+
+    addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
+    addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+    addToInput("timeoutInMs", VariantType::Int, false);
+
+
+    addToOutput("TCPDistanceToTarget", VariantType::Float, true);
+    addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
+
+
+
+}
+
+void MoveTCPPoseIK::onEnter()
+{
+    timeoutEvent = setTimeoutEvent(getInput<int>("timeoutInMs"), createEvent<EvTimeout>());
+}
+
+void MoveTCPPoseIK::onExit()
+{
+    removeTimeoutEvent(timeoutEvent);
+}
+
+void MoveTCPPoseIK::defineSubstates()
+{
+    StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
+    StatePtr calculateJointAngleConfiguration = addState<CalculateJointAngleConfiguration>("CalculateJointAngleConfiguration");
+    StatePtr validateJointAngleConfiguration = addState<ValidateJointAngleConfiguration>("ValidateJointAngleConfiguration");
+    StatePtr movingDone = addState<SuccessState>("movingDone") ;
+    StatePtr movingFailed = addState<FailureState>("movingFailed") ;
+    setInitState(calculateJointAngleConfiguration,
+                 PM::createMapping()->mapFromParent("*","*"));
+    addTransition<EvCalculationDone>(calculateJointAngleConfiguration, moveJoints, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
+    addTransition<EvJointTargetReached>(moveJoints, validateJointAngleConfiguration, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
+    addTransition<EvSuccess>(validateJointAngleConfiguration, movingDone);
+    addTransitionFromAllStates<EvTimeout>(movingFailed);
+    addTransitionFromAllStates<EvFailure>(movingFailed);
+
+
+}
+
+void CalculateJointAngleConfiguration::defineParameters()
+{
+    addToInput("KinematicChainName", VariantType::String, false);
+    addToInput("targetTCPPosition", VariantType::LinkedPosition, false);
+    addToInput("targetTCPOrientation", VariantType::LinkedOrientation, false);
+
+    addListToOutput("jointNames", VariantType::String, true);
+    addListToOutput("targetJointValues", VariantType::Float, true);
+}
+
+void CalculateJointAngleConfiguration::run()
+{
+    RobotStatechartContext* context = getContext<RobotStatechartContext>();
+    VirtualRobot::RobotPtr robot = (RemoteRobot(context->robotStateComponent->getRobotSnapshot("CalculateTCPPoseTime"))).clone("IKCopy");
+
+    VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(getInput<std::string>("KinematicChainName")) ));
+
+    VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot,
+                                                                              getInput<LinkedPosition>("targetTCPPosition"),
+                                                                              getInput<LinkedOrientation>("targetTCPOrientation"));
+
+    if(!ikSolver->solve(target.getInFrame(robot->getRootNode()), VirtualRobot::IKSolver::All, 50))
+        sendEvent<EvFailure>();
+    else
+    {
+
+        SingleTypeVariantList jointNames = SingleTypeVariantList(VariantType::String);
+        SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
+        VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(getInput<std::string>("KinematicChainName") );
+        for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
+        {
+            jointNames.addElement(new Variant(kinematicChain->getNode(i)->getName()));
+            targetJointValues.addElement(new Variant(kinematicChain->getNode(i)->getJointValue()));
+        }
+        setOutput("targetJointValues",targetJointValues);
+        setOutput("jointNames",jointNames);
+        sendEvent<EvCalculationDone>();
+    }
+}
+
+void ValidateJointAngleConfiguration::defineParameters()
+{
+    addToInput("targetTCPPosition", VariantType::LinkedPosition, false);
+    addToInput("targetTCPOrientation", VariantType::LinkedOrientation, false);
+    addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
+    addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+    addToInput("KinematicChainName", VariantType::String, false);
+
+    addToOutput("TCPDistanceToTarget", VariantType::Float, true);
+    addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
+}
+
+void ValidateJointAngleConfiguration::onEnter()
+{
+    RobotStatechartContext* context = getContext<RobotStatechartContext>();
+    VirtualRobot::RobotPtr robot = (RemoteRobot(context->robotStateComponent->getRobotSnapshot("ValidateTCPPoseTime"))).clone("IKCopy");
+using namespace Eigen;
+    VirtualRobot::LinkedCoordinate actualPose(robot);
+    actualPose.set(getInput<std::string>("KinematicChainName"), Eigen::Vector3f(0,0,0));
+    actualPose.changeFrame(robot->getRootNode());
+    Vector3f actualPosition = actualPose.getPosition();
+    Quaternionf actualOrientation(actualPose.getPose().block<3,3>(0,0));
+
+
+    VirtualRobot::LinkedCoordinate targetPose = ArmarPose::createLinkedCoordinate(robot, getInput<LinkedPosition>("targetTCPPosition"), getInput<LinkedOrientation>("targetTCPOrientation"));
+    targetPose.changeFrame(robot->getRootNode());
+    Eigen::Vector3f targetPosition = targetPose.getPosition();
+    Eigen::Quaternionf targetOrientation(targetPose.getPose().block<3,3>(0,0));
+
+    float cartesianDistance = sqrtf(((targetPosition-actualPosition).dot(targetPosition-actualPosition)));
+    float orientationDistance = actualOrientation.angularDistance(targetOrientation);
+
+    setOutput("TCPDistanceToTarget", cartesianDistance);
+    setOutput("TCPOrientationDistanceToTarget", orientationDistance);
+    if (cartesianDistance <= getInput<float>("targetPositionDistanceTolerance") && orientationDistance <= getInput<float>("targetOrientationDistanceTolerance"))
+        sendEvent<EvSuccess>();
+    else
+        sendEvent<EvFailure>();
+}
+
+void MotionControlTestState::defineParameters()
+{
+    std::string configFilepathAbsolute;
+    ArmarXDataPath::getAbsolutePath("HumanoidRobotAPIConfigs/stateconfigs/MotionControl.xml", configFilepathAbsolute);
+    setConfigFile(configFilepathAbsolute);
+
+    addListToInput("jointNames", VariantType::String, false);
+    addListToInput("targetJointValues", VariantType::Float, false);
+
+    addToInput("jointMaxSpeed", VariantType::Float, true);
+
+    addToInput("targetTolerance", VariantType::Float, false);
+
+    addToInput("timeoutInMs", VariantType::Int, false);
+
+    addListToOutput("jointDistancesToTargets", VariantType::Float, true);
+
+}
+
+
+void MotionControlTestState::defineSubstates()
+{
+    StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
+    StatePtr movingDone = addState<SuccessState>("movingDone") ;
+    StatePtr movingFailed = addState<FailureState>("movingFailed") ;
+
+    setInitState(moveJoints,
+                 PM::createMapping()->mapFromParent("*","*"));
+
+    addTransition<EvJointTargetReached>(moveJoints, movingDone);
+    addTransition<EvTimeout>(moveJoints, movingFailed);
+
+
+}
+
+void MoveHelpers::StopRobot::onEnter()
+{
+    RobotStatechartContext* context = getContext<RobotStatechartContext>();
+
+    VirtualRobot::RobotPtr robot = (RemoteRobot(context->robotStateComponent->getRobotSnapshot("StopRobotTime"))).clone("StopRobotCopy");
+    NameValueMap jointVelocities;
+
+    std::vector<VirtualRobot::RobotNodePtr> nodesToCheck;
+    nodesToCheck.push_back(robot->getRootNode());
+    while(!nodesToCheck.empty())
+    {
+        VirtualRobot::RobotNodePtr current = *nodesToCheck.end();
+        nodesToCheck.pop_back();
+        nodesToCheck.insert(nodesToCheck.end(), current->getChildren().begin(), current->getChildren().end());
+        jointVelocities[current->getName()] = 0.f;
+    }
+
+    context->kinematicUnitPrx->begin_setJointVelocities(jointVelocities);
+    sendEvent<EvSuccess>();
+}
+
+

motioncontrol/MotionControl.h

+#ifndef MOTIONCONTROL_H
+#define MOTIONCONTROL_H
+
+#include <Core/statechart/Statechart.h>
+#include <core/RobotStatechartContext.h>
+
+namespace armarx
+{
+
+namespace MoveHelpers
+{
+    class MotionControl : public RemoteStateHandler<RobotStatechartContext>
+    {
+    public:
+        MotionControl();
+        void onInitRemoteStateHandler();
+        void onConnectRemoteStateHandler();
+        std::string getDefaultName() const { return "MotionControl"; }
+    };
+
+    DEFINEEVENT(EvJointTargetReached)
+    DEFINEEVENT(EvCalculationDone)
+    DEFINEEVENT(EvTimeout)
+
+    struct MotionControlTestState : StateTemplate<MotionControlTestState>
+    {
+        void defineParameters();
+        void defineSubstates();
+    };
+
+    struct MoveJoints : StateTemplate<MoveJoints>
+    {
+        RobotStatechartContext* context;
+        ConditionIdentifier targetReachedCondition;
+        ActionEventIdentifier timeoutEvent;
+        MoveJoints();
+        void defineParameters();
+        void onEnter();
+        void onBreak();
+        void onExit();
+    };
+
+
+
+    struct MoveTCPPoseIK : StateTemplate<MoveTCPPoseIK>
+    {
+        RobotStatechartContext* context;
+        ConditionIdentifier targetReachedCondition;
+        ActionEventIdentifier timeoutEvent;
+        void defineParameters();
+        void defineSubstates();
+        void onEnter();
+        void onExit();
+    };
+
+    struct CalculateJointAngleConfiguration : StateTemplate<CalculateJointAngleConfiguration>
+    {
+
+        ActionEventIdentifier timeoutEvent;
+        void defineParameters();
+        void run();
+
+    };
+
+    struct ValidateJointAngleConfiguration : StateTemplate<ValidateJointAngleConfiguration>
+    {
+        RobotStatechartContext* context;
+        ActionEventIdentifier timeoutEvent;
+        void defineParameters();
+        void onEnter();
+    };
+
+    /////////////////////////////////////////////////////////////////////
+    //// Stopping robot
+    /////////////////////////////////////////////////////////////////////
+    struct StopRobot : StateTemplate<StopRobot>
+    {
+        RobotStatechartContext* context;
+
+        void onEnter();
+    };
+}
+
+}
+#endif // MOTIONCONTROL_H