diff --git a/source/HumanoidRobotAPI/core/RobotStatechartContext.cpp b/source/HumanoidRobotAPI/core/RobotStatechartContext.cpp
index 55e685d536adc3bb1f670bd1efb4f48e76c2385f..98a186ff330cafb814ea1effd732d654262182e4 100644
--- a/source/HumanoidRobotAPI/core/RobotStatechartContext.cpp
+++ b/source/HumanoidRobotAPI/core/RobotStatechartContext.cpp
@@ -41,7 +41,7 @@ namespace armarx
     void RobotStatechartContext::onInitStatechartContext()
     {
 //        StatechartContext::onInitStatechart();
-        ARMARX_LOG << eINFO << "Init Armar4Context" << flush;
+        ARMARX_LOG << eINFO << "Init RobotStatechartContext" << flush;
 
         RobotStateObjectFactories::addFactories(getIceManager()->getCommunicator());
         kinematicUnitObserverName = getProperty<std::string>("KinematicUnitObserverName").getValue();
@@ -55,7 +55,7 @@ namespace armarx
     void RobotStatechartContext::onConnectStatechartContext()
     {
 //        StatechartContext::onConnectStatechart();
-        ARMARX_LOG << eINFO << "Starting Armar4Context" << flush;
+        ARMARX_LOG << eINFO << "Starting RobotStatechartContext" << flush;
 
         // retrieve proxies
         robotStateComponent = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
diff --git a/source/HumanoidRobotAPI/motioncontrol/MotionControl.cpp b/source/HumanoidRobotAPI/motioncontrol/MotionControl.cpp
index b2d17627699cf65a5dba240d00c173fb484e2325..8ce944e05bdbea6c7cd0d6d31c3107404598311b 100644
--- a/source/HumanoidRobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/HumanoidRobotAPI/motioncontrol/MotionControl.cpp
@@ -3,6 +3,7 @@
 #include <VirtualRobot/IK/DifferentialIK.h>
 #include <VirtualRobot/Nodes/RobotNode.h>
 #include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/XML/RobotIO.h>
 #include <Core/robotstate/remote/RemoteRobot.h>
 #include <Core/observers/variant/ChannelRef.h>
 #include <Core/observers/ConditionCheck.h>
@@ -22,8 +23,9 @@ MotionControl::MotionControl()
 
 void MotionControl::onInitRemoteStateHandler()
 {
-    addState<MoveJoints>("MoveJoints");
-    addState<MotionControlTestState>("MotionControlTestState");
+    //addState<MoveJoints>("MoveJoints");
+    //addState<MotionControlTestState>("MotionControlTestState");
+    addState<MotionControlTestStateIK>("MotionControlTestStateIK");
 }
 
 
@@ -34,12 +36,6 @@ void MotionControl::onConnectRemoteStateHandler()
 
 
 
-MoveJoints::MoveJoints()
-{
-}
-
-
-
 void MoveJoints::defineParameters()
 {
     context = getContext<RobotStatechartContext>();
@@ -99,7 +95,7 @@ void MoveJoints::onEnter()
     }
 
     targetReachedCondition = installCondition<EvJointTargetReached>(cond);
-    ARMARX_INFO << "condition installed" << std::endl;
+    ARMARX_INFO << "condition installed: EvJointTargetReached" << std::endl;
     timeoutEvent = setTimeoutEvent(getInput<int>("timeoutInMs"), createEvent<EvTimeout>());
     ARMARX_INFO << "timeout installed" << std::endl;
     context->kinematicUnitPrx->setJointAngles(targetJointAngles);
@@ -166,7 +162,7 @@ void MoveJointsVelocityControl::defineParameters()
 
 void MoveJointsVelocityControl::onEnter()
 {
-    float accelerationTime = getInput<float>("accelerationTime");
+    //float accelerationTime = getInput<float>("accelerationTime");
     NameValueMap targetJointVelocities;
     SingleTypeVariantListPtr jointNames = getInputList("jointNames");
     SingleTypeVariantListPtr targetVelocitiesValues = getInputList("targetJointVelocities");
@@ -436,16 +432,30 @@ void CalculateJointAngleConfiguration::defineParameters()
 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))
+    VirtualRobot::RobotPtr robotPtr(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("CalculateTCPPoseTime")));
+    VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot("/org/share/home/staff/schieben/armarx/data/ArmarIV/RobotModel/ArmarIV.xml");
+    //VirtualRobot::RobotConfigPtr configPtr(new VirtualRobot::RobotConfig(robotPtr, "blub"));
+    //robotPtr->getRobotNodeSet("Root")->getJointValues(configPtr);
+    //robot->setJointValues(robotPtr->getRobotNodeSet("Root"), robotPtr->getRobotNodeSet("Root")->getJointValues());
+
+    VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(getInput<std::string>("KinematicChainName"))));
+    ikSolver->setVerbose(true);
+
+    VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot, getInput<LinkedPosition>("targetTCPPosition"), getInput<LinkedOrientation>("targetTCPOrientation"));
+    Eigen::Matrix4f goalInRoot = target.getInFrame(robot->getRootNode());
+
+    // test
+    VirtualRobot::RobotNodePtr rn = robot->getRobotNode("LeftTCP");
+    Eigen::Matrix4f goalTmpTCP = rn->getGlobalPose();
+    Eigen::Matrix4f goalGlobal = robot->getRootNode()->toGlobalCoordinateSystem(goalInRoot);
+    ARMARX_LOG << eINFO << "GOAL local :" << goalInRoot << endl;
+    ARMARX_LOG << eINFO << "GOAL global :" << goalGlobal << endl;
+    ARMARX_LOG << eINFO << "GOAL TCP:" << goalTmpTCP << endl;
+
+    ARMARX_LOG << eINFO << "Calculating IK" << flush;
+    if (!ikSolver->solve(goalInRoot, VirtualRobot::IKSolver::Position, 1))
     {
+        ARMARX_WARNING << "IKSolver found no solution! " << flush;
         sendEvent<EvFailure>();
     }
     else
@@ -538,8 +548,8 @@ void MotionControlTestState::defineParameters()
 void MotionControlTestState::defineSubstates()
 {
     StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
-    StatePtr movingDone = addState<SuccessState>("movingDone") ;
-    StatePtr movingFailed = addState<FailureState>("movingFailed") ;
+    StatePtr movingDone = addState<SuccessState>("movingDone");
+    StatePtr movingFailed = addState<FailureState>("movingFailed");
 
     setInitState(moveJoints, PM::createMapping()->mapFromParent("*","*"));
 
@@ -549,6 +559,44 @@ void MotionControlTestState::defineSubstates()
 
 
 
+void MotionControlTestStateIK::defineParameters()
+{
+    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 MotionControlTestStateIK::defineSubstates()
+{
+    StatePtr moveTCPPoseIK = addState<MoveTCPPoseIK>("MoveTCPPoseIK");
+    StatePtr movingDone = addState<SuccessState>("movingDone");
+    StatePtr movingFailed = addState<FailureState>("movingFailed");
+
+    setInitState(moveTCPPoseIK, PM::createMapping()->mapFromParent("*","*"));
+
+    addTransition<EvSuccess>(moveTCPPoseIK, movingDone);
+    addTransition<EvTimeout>(moveTCPPoseIK, movingFailed);
+    addTransition<EvFailure>(moveTCPPoseIK, movingFailed);
+}
+
+
+
 void MoveHelpers::StopRobot::onEnter()
 {
     RobotStatechartContext* context = getContext<RobotStatechartContext>();
diff --git a/source/HumanoidRobotAPI/motioncontrol/MotionControl.h b/source/HumanoidRobotAPI/motioncontrol/MotionControl.h
index 9df74276bffea5c28af4fbd48b6b5679ff45abf6..eec7dda1c37ab27611d74f2281de12a788465599 100644
--- a/source/HumanoidRobotAPI/motioncontrol/MotionControl.h
+++ b/source/HumanoidRobotAPI/motioncontrol/MotionControl.h
@@ -37,6 +37,14 @@ namespace MoveHelpers
     };
 
 
+    struct MotionControlTestStateIK : StateTemplate<MotionControlTestStateIK>
+    {
+        void defineParameters();
+        void defineSubstates();
+    };
+
+
+
     /**
     *   Move the joints of a kinematic chain to the desired values.
     *   Parameters?
@@ -46,7 +54,6 @@ namespace MoveHelpers
         RobotStatechartContext* context;
         ConditionIdentifier targetReachedCondition;
         ActionEventIdentifier timeoutEvent;
-        MoveJoints();
         void defineParameters();
         void onEnter();
         void onBreak();