diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index 4b0e26b6c2a55346b9619b28ca6b0bf27c11a0b0..82d37a662f3ea1001a07c7076b594f50d69c1835 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -162,6 +162,10 @@ namespace armarx
         getWriterControlStruct().targetPose = targetPose;
 
         writeControlStruct();
+
+        NJointTSDMPControllerInterfaceData initInterfaceData;
+        initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
+        interfaceData.reinitAllBuffers(initInterfaceData);
     }
 
 
@@ -187,6 +191,8 @@ namespace armarx
         controllerSensorData.getWriteBuffer().currentTime += deltaT;
         controllerSensorData.commitWrite();
 
+        interfaceData.getWriteBuffer().currentTcpPose = currentPose;
+        interfaceData.commitWrite();
 
         Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
         Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
@@ -358,7 +364,14 @@ namespace armarx
 
     void NJointTSDMPController::runDMP(const Ice::DoubleSeq&  goals, double tau, const Ice::Current&)
     {
-        Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
+        while (!interfaceData.updateReadBuffer())
+        {
+            usleep(100);
+        }
+
+        Eigen::Matrix4f pose = interfaceData.getReadBuffer().currentTcpPose;
+
+        //        Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
         taskSpaceDMPController->prepareExecution(taskSpaceDMPController->eigen4f2vec(pose), goals);
         finished = false;
 
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index b92059f2585205f5c752316afe4e0b8d2f25eb53..1b08572f60e2a45988c1d58e295c834e0ca4f6c9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -127,6 +127,12 @@ namespace armarx
         };
         TripleBuffer<NJointTSDMPControllerSensorData> controllerSensorData;
 
+        struct NJointTSDMPControllerInterfaceData
+        {
+            Eigen::Matrix4f currentTcpPose;
+        };
+
+        TripleBuffer<NJointTSDMPControllerInterfaceData> interfaceData;
 
         std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
         std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;