diff --git a/source/RobotAPI/core/RobotStatechartContext.h b/source/RobotAPI/core/RobotStatechartContext.h
index 1992440daa299f9766d9d6aec9cab0efd2ab8452..87497c10371e1d8a4be36eca2026bb396232a657 100644
--- a/source/RobotAPI/core/RobotStatechartContext.h
+++ b/source/RobotAPI/core/RobotStatechartContext.h
@@ -31,6 +31,8 @@
 #include <Core/robotstate/remote/RemoteRobot.h>
 #include <Core/interface/units/KinematicUnitInterface.h>
 
+#include <RobotAPI/interface/units/TCPControlUnit.h>
+
 #include <Core/units/KinematicUnitObserver.h>
 //#include <VirtualRobot/VirtualRobot.h>
 #include <IceUtil/Time.h>
@@ -82,7 +84,7 @@ namespace armarx
 		RobotStateComponentInterfacePrx robotStateComponent;
 		KinematicUnitInterfacePrx kinematicUnitPrx;
         KinematicUnitObserverInterfacePrx kinematicUnitObserverPrx;
-
+        TCPControlUnitInterfacePrx tcpControlPrx;
         //SystemObserverInterfacePrx	systemObserver; // already defined in StatechartContext
         VirtualRobot::RobotPtr remoteRobot;
     private:
diff --git a/source/RobotAPI/motioncontrol/CMakeLists.txt b/source/RobotAPI/motioncontrol/CMakeLists.txt
index a5d3bff31bf9c967e75a8a4be208d47533bab2bc..88550f6aeecb56eee26a4f9833fd3260892f0ed0 100755
--- a/source/RobotAPI/motioncontrol/CMakeLists.txt
+++ b/source/RobotAPI/motioncontrol/CMakeLists.txt
@@ -15,7 +15,7 @@ if (ARMARX_BUILD)
     set(LIB_VERSION    0.1.0)
     set(LIB_SOVERSION  0)
 
-    set(LIBS RobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers)
+    set(LIBS RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers)
 
     set(LIB_FILES MotionControl.cpp
          ZeroForceControl.cpp)
diff --git a/source/RobotAPI/motioncontrol/ZeroForceControl.cpp b/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
index 9188a476db50d3e2ad048988852a90712933e7d9..16f32e51b576b3e66a9f289ebf4fb6cf4cde8f00 100644
--- a/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
+++ b/source/RobotAPI/motioncontrol/ZeroForceControl.cpp
@@ -24,6 +24,9 @@
 #include "ZeroForceControl.h"
 
 #include <Core/robotstate/remote/RobotStateObjectFactories.h>
+#include <Core/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/core/RobotStatechartContext.h>
 
 namespace armarx {
 
@@ -49,6 +52,7 @@ namespace armarx {
 
     void ZeroForceControl::defineParameters()
     {
+        addToInput("robotNodeSetName",VariantType::String, false);
         addToInput("tcpName",VariantType::String, false);
         addToInput("sensitivity",VariantType::Float, false);
         addToInput("maxAcc",VariantType::Float, false);
@@ -61,14 +65,20 @@ namespace armarx {
 
     void ZeroForceControl::onEnter()
     {
+        RobotStatechartContext* c = getContext<RobotStatechartContext>();
+        c->tcpControlPrx->request();
+        Eigen::Vector3f null(3);
+        null.setZero();
         setLocal("currentSensitivity", 0.0f);
-        setLocal("currentVelocity", 0.0f);
-        setLocal("currentAcc", 0.0f);
+        setLocal("currentVelocity", new FramedVector3(null, getInput<std::string>("tcpName")));
+        setLocal("currentAcc", new FramedVector3(null, getInput<std::string>("tcpName")));
         setLocal("timestamp", (float)IceUtil::Time::now().toMilliSecondsDouble());
     }
 
     void ZeroForceControl::onExit()
     {
+        RobotStatechartContext* c = getContext<RobotStatechartContext>();
+        c->tcpControlPrx->release();
     }
 
 
@@ -83,7 +93,7 @@ namespace armarx {
 
     void ZeroForceControlForceToAcc::defineParameters()
     {
-        addToInput("tcpName",VariantType::String, false);
+        addToInput("robotNodeSetName",VariantType::String, false);
         addToInput("sensitivity",VariantType::Float, false);
         addToInput("maxAcc",VariantType::Float, false);
 
@@ -107,27 +117,33 @@ namespace armarx {
         installCondition<EvSensorUpdate>(update);
         IceUtil::Time duration = IceUtil::Time::now() - IceUtil::Time::milliSecondsDouble(getInput<float>("timestamp"));
 
-        std::string tcpName = getInput<std::string>("tcpName");
+        std::string robotNodeSetName = getInput<std::string>("robotNodeSetName");
 //        FramedVector3Ptr vel = getInput<FramedVector3>("currentVelocity");
         FramedVector3Ptr vel = getInput<FramedVector3>("currentVelocity");
         FramedVector3Ptr curAcc = getInput<FramedVector3>("currentAcc");
         FramedVector3Ptr curForce = getInput<FramedVector3>("currentForce");
-
+        ARMARX_CHECK_EXPRESSION(vel->frame == curAcc->frame);
+        ARMARX_CHECK_EXPRESSION(vel->frame == curForce->frame);
+        float forceThreshold = 2.0f;
+        float maxSensitivity = 2.0f;
 
         Eigen::Vector3f newVel(3);
         Eigen::Vector3f newAcc(3);
-        if(curForce->toEigen().norm() > 3){
-            newAcc = 20*curForce->toEigen().normalized();
+        if(curForce->toEigen().norm() > forceThreshold){
+            newAcc = 2 *  curForce->toEigen() * 5;
         }
         else
         {
-            newAcc = -10*curForce->toEigen().normalized();
+            newAcc = -10*vel->toEigen().normalized();
         }
 
         newVel = vel->toEigen() + newAcc * duration.toMilliSecondsDouble()*0.001;
 
 
         setOutput("currentAcc", Variant(new FramedVector3(newAcc, curAcc->frame)));
+        RobotStatechartContext* c = getContext<RobotStatechartContext>();
+
+        c->tcpControlPrx->setTCPVelocity(robotNodeSetName, "", new FramedVector3(newVel, vel->frame), NULL);
     }
 
     void ZeroForceControlForceToAcc::onExit()
diff --git a/source/RobotAPI/units/ForceTorqueObserver.cpp b/source/RobotAPI/units/ForceTorqueObserver.cpp
index dd43ebb29a7ad5e331d6aed852f6e672f196bbbc..23a978f70844733883f952a2af392b88014c476c 100644
--- a/source/RobotAPI/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/units/ForceTorqueObserver.cpp
@@ -23,9 +23,17 @@ ForceTorqueObserver::ForceTorqueObserver()
 {
 }
 
+void ForceTorqueObserver::setTopicName(std::string topicName)
+{
+    this->topicName = topicName;
+}
+
 void ForceTorqueObserver::onInitObserver()
 {
-    usingTopic(getProperty<std::string>("ForceTorqueTopicName").getValue());
+    if(topicName.empty())
+        usingTopic(getProperty<std::string>("ForceTorqueTopicName").getValue());
+    else
+        usingTopic(topicName);
 
     // register all checks
     offerConditionCheck("updated", new ConditionCheckUpdated());
diff --git a/source/RobotAPI/units/ForceTorqueObserver.h b/source/RobotAPI/units/ForceTorqueObserver.h
index 8b9c34dc2276e7f88c74577ed65fbbaebd62e450..50c5ea1d7d333ef95a373e17172e440973549790 100644
--- a/source/RobotAPI/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/units/ForceTorqueObserver.h
@@ -25,6 +25,8 @@ namespace armarx
     public:
         ForceTorqueObserver();
 
+        void setTopicName(std::string topicName);
+
         // framework hooks
         virtual std::string getDefaultName() const { return "ForceTorqueUnitObserver"; }
         void onInitObserver();
@@ -41,6 +43,7 @@ namespace armarx
         virtual PropertyDefinitionsPtr createPropertyDefinitions();
     private:
         armarx::Mutex dataMutex;
+        std::string topicName;
 
     };
 }