diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 56a17ef1928de4779241c59a432f7b6baaf50019..84614f0d5d98bb23fc63a14bc4c0a237189b2779 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -336,6 +336,7 @@ module armarx
         void setUseNullSpaceJointDMP(bool enable);
 
         bool isFinished();
+        bool isDMPRunning();
         void runDMP(Ice::DoubleSeq goals);
         void runDMPWithTime(Ice::DoubleSeq goals, double timeDuration);
 
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index 79a090adbae49e70b400ca3cd358b0599a11c9f1..e8a401f485b38a2c85f4a9a718f94d8a1d0a905a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -472,6 +472,7 @@ namespace armarx
         debugRT.getWriteBuffer().targetVel = targetVel;
         debugRT.getWriteBuffer().adaptK = adaptK;
         debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
+        debugRT.getWriteBuffer().currentTwist = currentTwist;
 
         rt2CtrlData.getWriteBuffer().currentPose = currentPose;
         rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
@@ -673,6 +674,11 @@ namespace armarx
         datafields["targetVel_y"] = new Variant(targetVel(1));
         datafields["targetVel_z"] = new Variant(targetVel(2));
 
+        Eigen::VectorXf currentVel = debugRT.getUpToDateReadBuffer().currentTwist;
+        datafields["currentVel_x"] = new Variant(currentVel(0));
+        datafields["currentVel_y"] = new Variant(currentVel(1));
+        datafields["currentVel_z"] = new Variant(currentVel(2));
+
         Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
         datafields["adaptK_x"] = new Variant(adaptK(0));
         datafields["adaptK_y"] = new Variant(adaptK(1));
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
index ebb3b05a5241c338cbdfc73ebd30ef3226d7219d..477feb6900efcdaca0981deb92534cc5dfb0c172 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -99,6 +99,7 @@ namespace armarx
             Eigen::Vector3f adaptK;
             Eigen::VectorXf targetVel;
             Eigen::Matrix4f currentPose;
+            Eigen::VectorXf currentTwist;
             bool isPhaseStop;
             float manidist;
         };
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index d550b478c148c97372098e8e40dd7927bafa144f..0a08c5396283856d5e5ccc5e8ebfdbb9cb59500c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -11,12 +11,14 @@ namespace armarx
             const armarx::NJointControllerConfigPtr& config,
             const VirtualRobot::RobotPtr&)
     {
+        ARMARX_TRACE;
         ARMARX_INFO << "creating impedance dmp controller";
         cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
+        ARMARX_CHECK_NOT_NULL(cfg);
         useSynchronizedRtRobot();
         rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
         ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-
+        ARMARX_INFO << "1";
         for (size_t i = 0; i < rns->getSize(); ++i)
         {
             std::string jointName = rns->getNode(i)->getName();
@@ -39,19 +41,20 @@ namespace armarx
             velocitySensors.push_back(velocitySensor);
             positionSensors.push_back(positionSensor);
         };
-
         const SensorValueBase* svlf = robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
         forceSensor = svlf->asA<SensorValueForceTorque>();
 
+        ARMARX_TRACE;
         forceOffset.setZero();
         filteredForce.setZero();
         filteredForceInRoot.setZero();
-        forceThreshold.getWriteBuffer() = cfg->forceThreshold;
-        forceThreshold.commitWrite();
+        ARMARX_INFO << cfg->forceThreshold;
+        forceThreshold.reinitAllBuffers(cfg->forceThreshold);
         tcp =  rns->getTCP();
         ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
         ik->setDampedSvdLambda(0.0001);
 
+        ARMARX_TRACE;
         numOfJoints = targets.size();
         // set DMP
         TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
@@ -88,6 +91,7 @@ namespace armarx
         }
         defaultNullSpaceJointValues.reinitAllBuffers(nullspaceValues);
 
+        ARMARX_TRACE;
         Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
         Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
         Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
@@ -136,6 +140,7 @@ namespace armarx
 
     void NJointTaskSpaceImpedanceDMPController::rtPreActivateController()
     {
+        ARMARX_TRACE;
         NJointTaskSpaceImpedanceDMPControllerControlData initData;
         initData.targetPose = tcp->getPoseInRootFrame();
         initData.targetVel.resize(6);
@@ -547,6 +552,8 @@ namespace armarx
     {
         firstRun = true;
         timeForCalibration = 0;
+        started = false;
+
         while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
         {
             usleep(100);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 3a6f685e412940d03b483a90dd92b9b346d9436f..58fd840fb1121b4a159c185abb2e72fd7470f082 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -56,6 +56,11 @@ namespace armarx
             return finished;
         }
 
+        bool isDMPRunning(const Ice::Current&) override
+        {
+            return started;
+        }
+
         void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
         void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;