diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSPositionDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSPositionDMPController.cpp
index 238b6dfef665c80e080d9c8a3145b9f16e37b512..3ebb3209316c38afe0d0e7dcda3b5e77425203f3 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSPositionDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSPositionDMPController.cpp
@@ -69,7 +69,7 @@ namespace armarx
         initSensorData.currentState.resize(cfg->jointNames.size());
         controllerSensorData.reinitAllBuffers(initSensorData);
 
-//        deltaT = 0;
+        //        deltaT = 0;
 
         qpos.resize(dimNames.size());
         qvel.resize(dimNames.size());
@@ -283,7 +283,7 @@ namespace armarx
             currentState[i] = currentPos;
             currentDMPState[i] = currentPos;
 
-            targetState.push_back(currentPos.pos);
+            targetState[i] = currentPos.pos;
 
             if (rtGetRobot()->getRobotNode(dimNames[i])->isLimitless())
             {