diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
index 744f174907473959f88a75167cb2cedcee7c26d9..687f352d40ef074ee68ab4088c17f549c8ba56dd 100644
--- a/source/RobotAPI/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -54,6 +54,9 @@ namespace armarx
 
     void TCPControlUnit::onConnectComponent()
     {
+        ScopedLock lock(dataMutex);
+        if(getProperty<float>("MaxJointVelocity").isSet())
+            maxJointVelocity = getProperty<float>("MaxJointVelocity").getValue();
         kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
         robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
 
@@ -61,7 +64,7 @@ namespace armarx
 
         // retrieve Jacobian pseudo inverse for TCP and chain
         remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
-        localRobot.reset(new RemoteRobot(remoteRobotPrx));
+//        localRobot.reset(new RemoteRobot(remoteRobotPrx));
 //        ARMARX_INFO << "hi limit : " << localRobot->getRobotNode("LeftArm_Joint1")->getJointLimitHi();
 //        localRobot = localRobot->clone("localRobot");
 //        ARMARX_INFO << "hi limit after: " << localRobot->getRobotNode("LeftArm_Joint1")->getJointLimitHi();
@@ -75,7 +78,6 @@ namespace armarx
         localRobot->setUpdateVisualization(false);
         localRobot->setThreadsafe(false);
 
-
         //        dIK = new VirtualRobot::DifferentialIK(robotNodeSet);
 
         FramedVector3Ptr  tcpVel = new FramedVector3();
@@ -104,9 +106,10 @@ namespace armarx
 
     void TCPControlUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current & c)
     {
-        ScopedLock lock(dataMutex);
+        ScopedLock lock(taskMutex);
         cycleTime = milliseconds;
-        execTask->changeInterval(cycleTime);
+        if(execTask)
+            execTask->changeInterval(cycleTime);
     }
 
     void TCPControlUnit::setTCPVelocity(const std::string &tcpNodeName, const FramedVector3BasePtr &translationVelocity, const FramedVector3BasePtr &orientationVelocityRPY, const Ice::Current &c)
@@ -152,6 +155,11 @@ namespace armarx
 
     void TCPControlUnit::request(const Ice::Current & c)
     {
+        ScopedLock lock(taskMutex);
+        int cycleTime;
+        {
+            cycleTime = this->cycleTime;
+        }
         ARMARX_IMPORTANT << "Requesting TCPControlUnit";
         execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, true, "TCPVelocityCalculator");
         execTask->start();
@@ -159,8 +167,10 @@ namespace armarx
 
     void TCPControlUnit::release(const Ice::Current & c)
     {
+        ScopedLock lock(taskMutex);
         ARMARX_IMPORTANT << "Releasing TCPControlUnit";
-        execTask->stop();
+        if(execTask)
+            execTask->stop();
         kinematicUnitPrx->stop();
     }
 
@@ -196,7 +206,6 @@ namespace armarx
         localRobot->setJointValues(jointValues);
         //        ARMARX_IMPORTANT << "LeftArm_Joint3: " << localRobot->getRobotNode("LeftArm_Joint3")->getJointValue();
 
-
         FramedVector3Map::const_iterator itTrans = targetTranslationVelocities.begin();
         for(; itTrans != targetTranslationVelocities.end(); itTrans++)
         {
@@ -258,9 +267,11 @@ namespace armarx
 
             }
 
-//            Eigen::Vector3f currentTCPPosition = robotNodeSet->getTCP()->getGlobalPose().block(0,3,3,1);
+            jointDelta = applyMaxJointVelocity(jointDelta,this->maxJointVelocity);
+
+            Eigen::Vector3f currentTCPPosition = robotNodeSet->getTCP()->getGlobalPose().block(0,3,3,1);
 //            ARMARX_INFO << "Current TCP Position: \n" << currentTCPPosition;
-//            ARMARX_INFO << "ActualTCPVelocity: " << (currentTCPPosition - lastTCPPose.block(0,3,3,1))/(0.001*cycleTime);
+            ARMARX_DEBUG  << deactivateSpam(2)<< "ActualTCPVelocity: " << (currentTCPPosition - lastTCPPose.block(0,3,3,1))/(0.001*cycleTime);
             lastTCPPose = robotNodeSet->getTCP()->getGlobalPose();
 
             // build name value map
@@ -363,7 +374,24 @@ namespace armarx
         return delta;
     }
 
+    Eigen::VectorXf TCPControlUnit::applyMaxJointVelocity(const Eigen::VectorXf& jointVelocity, float maxJointVelocity)
+    {
+        float currentMaxJointVel = std::numeric_limits<float>::min();
+        unsigned int rows = jointVelocity.rows();
+        for(unsigned int i=0; i < rows; i++)
+        {
+            currentMaxJointVel = std::max(jointVelocity(i), currentMaxJointVel);
+        }
 
+        if(currentMaxJointVel > maxJointVelocity)
+        {
+            ARMARX_WARNING << deactivateSpam(1) << "max joint velocity too high: " << currentMaxJointVel << " allowed: " << maxJointVelocity;
+            return jointVelocity * (maxJointVelocity/currentMaxJointVel);
+        }
+        else
+            return jointVelocity;
+
+    }
 
 
     PropertyDefinitionsPtr TCPControlUnit::createPropertyDefinitions()
diff --git a/source/RobotAPI/units/TCPControlUnit.h b/source/RobotAPI/units/TCPControlUnit.h
index 2802e554c9cfe66ee247e581035c525745483d04..3da7bb6728d7781b0a6adf7911c6ffe3534a737a 100644
--- a/source/RobotAPI/units/TCPControlUnit.h
+++ b/source/RobotAPI/units/TCPControlUnit.h
@@ -43,7 +43,7 @@ namespace armarx {
             ComponentPropertyDefinitions(prefix)
         {
             defineRequiredProperty<std::string>("KinematicUnitName","Name of the KinematicUnit Proxy");
-            defineOptionalProperty<std::string>("MaxJointVelocity","Maximal joint velocity in rad/sec");
+            defineOptionalProperty<float>("MaxJointVelocity",30.f/180*3.141, "Maximal joint velocity in rad/sec");
             defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
 
         }
@@ -86,6 +86,7 @@ namespace armarx {
     private:
         void periodicExec();
         Eigen::VectorXf calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, VirtualRobot::RobotNodePtr refFrame, VirtualRobot::IKSolver::CartesianSelection mode);
+        Eigen::VectorXf applyMaxJointVelocity(const Eigen::VectorXf &jointVelocity, float maxJointVelocity);
 
         float maxJointVelocity;
         int cycleTime;
@@ -104,6 +105,7 @@ namespace armarx {
 //        VirtualRobot::DifferentialIKPtr dIK;
 
         Mutex dataMutex;
+        Mutex taskMutex;
 
     };