diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
index 460e20be4a2c0e04069fe1f1522ba40cf60f064c..d13c9d954d32b5a7a3b78f98543cdc4595c4c352 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
@@ -262,6 +262,13 @@ namespace armarx
         debugOutputData.getWriteBuffer().dmpTargets["dmp_pitch"] = targetState[4];
         debugOutputData.getWriteBuffer().dmpTargets["dmp_yaw"] = targetState[5];
 
+        debugOutputData.getWriteBuffer().realTCP["real_x"] = currentPosition[0];
+        debugOutputData.getWriteBuffer().realTCP["real_y"] = currentPosition[1];
+        debugOutputData.getWriteBuffer().realTCP["real_z"] = currentPosition[2];
+        debugOutputData.getWriteBuffer().realTCP["real_roll"] = currentRPY[0];
+        debugOutputData.getWriteBuffer().realTCP["real_pitch"] = currentRPY[1];
+        debugOutputData.getWriteBuffer().realTCP["real_yaw"] = currentRPY[2];
+
         debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
         debugOutputData.getWriteBuffer().error = error;
         debugOutputData.getWriteBuffer().phaseStop = phaseStop;
@@ -548,6 +555,13 @@ namespace armarx
             datafields[pair.first] = new Variant(pair.second);
         }
 
+        auto realTCP = debugOutputData.getUpToDateReadBuffer().realTCP;
+        for (auto& pair : realTCP)
+        {
+            datafields[pair.first] = new Variant(pair.second);
+        }
+
+
         datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
         datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
         datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
index 2247c56390202d995283df7edac40918f38f6378..cde02bb4e350de36c37c022e9904597f6811e8c9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
@@ -93,6 +93,8 @@ namespace armarx
         {
             StringFloatDictionary latestTargetVelocities;
             StringFloatDictionary dmpTargets;
+            StringFloatDictionary realTCP;
+
             double mpcFactor;
             double error;
             double phaseStop;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index 92df61bbc9a916d76e403329d823c70f7ed3cd6a..a9d87b71878959e6dc716a1ccb99c748c5f30521 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -105,6 +105,9 @@ namespace armarx
         double posError = 0;
         double oriError = 0;
         double deltaT = 0;
+        std::vector<double> currentposi;
+        std::vector<double> currentori;
+
         if (canVal > 1e-8)
         {
             if (!controllerSensorData.updateReadBuffer())
@@ -115,7 +118,6 @@ namespace armarx
             currentState = controllerSensorData.getReadBuffer().currentState;
 
 
-            std::vector<double> currentposi;
             currentposi.resize(3);
             for (size_t i = 0; i < 3; ++i)
             {
@@ -123,7 +125,6 @@ namespace armarx
                 posError += pow(currentState[i].pos - targetState[i], 2);
             }
 
-            std::vector<double> currentori;
             currentori.resize(3);
             for (size_t i = 0; i < 3; ++i)
             {
@@ -204,6 +205,14 @@ namespace armarx
         debugOutputData.getWriteBuffer().dmpTargets["dmp_pitch"] = targetState[4];
         debugOutputData.getWriteBuffer().dmpTargets["dmp_yaw"] = targetState[5];
 
+        debugOutputData.getWriteBuffer().realTCP["real_x"] = currentposi[0];
+        debugOutputData.getWriteBuffer().realTCP["real_y"] = currentposi[1];
+        debugOutputData.getWriteBuffer().realTCP["real_z"] = currentposi[2];
+        debugOutputData.getWriteBuffer().realTCP["real_roll"] = currentori[0];
+        debugOutputData.getWriteBuffer().realTCP["real_pitch"] = currentori[1];
+        debugOutputData.getWriteBuffer().realTCP["real_yaw"] = currentori[2];
+
+
         debugOutputData.getWriteBuffer().currentCanVal = canVal;
         debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
         debugOutputData.getWriteBuffer().error = error;
@@ -483,6 +492,12 @@ namespace armarx
             datafields[pair.first] = new Variant(pair.second);
         }
 
+        auto realTCP = debugOutputData.getUpToDateReadBuffer().realTCP;
+        for (auto& pair : realTCP)
+        {
+            datafields[pair.first] = new Variant(pair.second);
+        }
+
         datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
         datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
         datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
@@ -497,7 +512,7 @@ namespace armarx
     void NJointTSDMPController::onInitComponent()
     {
         ARMARX_INFO << "init ...";
-        controllerTask = new PeriodicTask<NJointTSDMPController>(this, &NJointTSDMPController::controllerRun, 1);
+        controllerTask = new PeriodicTask<NJointTSDMPController>(this, &NJointTSDMPController::controllerRun, 0.3);
     }
 
     void NJointTSDMPController::onDisconnectComponent()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index e23451a67396f9e3303b760b25fb70aed9d2269f..d472aee7a1c6a8a62235891567560768fd05e41e 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -93,6 +93,7 @@ namespace armarx
         {
             StringFloatDictionary latestTargetVelocities;
             StringFloatDictionary dmpTargets;
+            StringFloatDictionary realTCP;
             double currentCanVal;
             double mpcFactor;
             double error;