diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 54e21c277517f5fcaa0ef2e69aba78f7410a92c4..08ce64c58adf069652a363f90f2e4210b37231dc 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -365,7 +365,8 @@ module armarx
         float reactThreshold;
         float dragForceDeadZone;
         float adaptForceCoeff;
-
+        float changePositionTolerance;
+        float changeTimerThreshold;
     };
 
 
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index 6211c78b090caa2b29dfc1b20c3a0d13bd77d7c4..a5f4129a9e23f48a7d392c3512ee6bec9d02ac2b 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -91,9 +91,7 @@ namespace armarx
 
         forceOffset.setZero();
         filteredForce.setZero();
-        reactForce.setZero();
         filteredForceInRoot.setZero();
-        forceIntegral.setZero();
 
 
         UserToRTData initUserData;
@@ -120,6 +118,9 @@ namespace armarx
         ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
         ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
 
+        adaptK = kpos;
+        lastDiff = 0;
+        changeTimer = 0;
     }
 
     void NJointPeriodicTSDMPCompliantController::onInitNJointController()
@@ -259,6 +260,7 @@ namespace armarx
         targetVel.setZero();
         if (firstRun || !dmpRunning)
         {
+            lastPosition = currentPose.block<2, 1>(0, 3);
             targetPose = currentPose;
             firstRun = false;
             filteredForce.setZero();
@@ -351,48 +353,30 @@ namespace armarx
         }
 
         bool isPhaseStop = false;
-        Eigen::Vector3f adaptK = kpos;
 
         float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
         if (diff > cfg->phaseDist0)
         {
             isPhaseStop = true;
-            //            float reduceTimes = (float)exp(-(diff - cfg->phaseDist0) * cfg->adaptCoeff) ;
-            //            if (reduceTimes < 0.2)
-            //            {
-            //                reduceTimes = 0.2;
-            //            }
-            //            adaptK = reduceTimes * adaptK;
-            //            adaptK(2) = kpos(2);
         }
 
         float dTf = (float)deltaT;
 
-
-
         bool isTooFar = false;
+
         if (filteredForceInRoot.block<2, 1>(0, 0).norm() > cfg->dragForceDeadZone)
         {
-            forceIntegral += filteredForceInRoot * dTf;
-            float reduceTimes = (float)exp(-(forceIntegral.block<2, 1>(0, 0).norm()) * cfg->adaptForceCoeff) ;
-            adaptK = reduceTimes * adaptK;
-            adaptK(2) = kpos(2);
+            Eigen::Vector2f dragForce = filteredForceInRoot.block<2, 1>(0, 0) - cfg->dragForceDeadZone * filteredForceInRoot.block<2, 1>(0, 0) / filteredForceInRoot.block<2, 1>(0, 0).norm();
+            adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
+            adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
+            lastDiff = diff;
         }
         else
         {
-            forceIntegral << 0, 0, 0;
-
-            float reduceTimes = (diff - cfg->phaseDist1) / (cfg->phaseDist0 - cfg->phaseDist1) ;
-            if (diff > cfg->phaseDist1)
-            {
-                isTooFar = true;
-            }
-            float ckp = cfg->maxLinearVel / cfg->phaseDist1;
-            Eigen::Vector3f constKp(ckp, ckp, ckp);
-            adaptK = (diff - cfg->phaseDist1) / (cfg->phaseDist0 - cfg->phaseDist1) * (kpos - constKp) + constKp;
-            adaptK(2) = kpos(2);
+            adaptK(0) = fmin(adaptK(0) + fabs(dTf * cfg->adaptCoeff * lastDiff), kpos(0));
+            adaptK(1) = fmin(adaptK(1) + fabs(dTf * cfg->adaptCoeff * lastDiff), kpos(1));
         }
-
+        adaptK(2) = kpos(2);
 
         // adaptive control with distance
 
@@ -412,7 +396,6 @@ namespace armarx
         debugRT.getWriteBuffer().targetPose = targetPose;
         debugRT.getWriteBuffer().currentPose = currentPose;
         debugRT.getWriteBuffer().filteredForce = filteredForceInRoot;
-        debugRT.getWriteBuffer().reactForce = reactForce;
         debugRT.getWriteBuffer().targetVel = targetVel;
         debugRT.getWriteBuffer().adaptK = adaptK;
 
@@ -420,19 +403,23 @@ namespace armarx
 
         if (isPhaseStop)
         {
-            reactForce += filteredForceInRoot * dTf;
-            float xyforce = reactForce.block<2, 1>(0, 0).norm();
-            if (xyforce > cfg->reactThreshold)
+            Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
+            if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
+            {
+                changeTimer += deltaT;
+            }
+            if (changeTimer > cfg->changeTimerThreshold * 1000)
             {
                 targetPose(0, 3) = currentPose(0, 3);
                 targetPose(1, 3) = currentPose(1, 3);
-                isPhaseStop = false;
-                reactForce << 0, 0, 0;
+                changeTimer = 0;
+                lastPosition = currentPose.block<2, 1>(0, 3);
             }
         }
         else
         {
-            reactForce << 0, 0, 0;
+            lastPosition = currentPose.block<2, 1>(0, 3);
+            changeTimer = 0;
         }
 
         rt2CtrlData.getWriteBuffer().currentPose = currentPose;
@@ -463,7 +450,10 @@ namespace armarx
             Eigen::Vector3f linearVel = adaptK.cwiseProduct(desiredPosition - currentTCPPosition);
             if (isTooFar)
             {
-                linearVel(0) = cfg->maxLinearVel;
+                if (linearVel.block<2, 1>(0, 0).norm() > cfg->maxLinearVel)
+                {
+                    linearVel.block<2, 1>(0, 0) = cfg->maxLinearVel * linearVel.block<2, 1>(0, 0) / linearVel.block<2, 1>(0, 0).norm();
+                }
             }
             //            if (isPhaseStop)
             //            {
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
index aeaf1b1d1dd306b30a0e448d6dd11f0d88410680..abdf25cc4f40cec64ec3e13239be30a3c5f6ee9c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -173,16 +173,17 @@ namespace armarx
 
         Eigen::Vector3f filteredForce;
         Eigen::Vector3f forceOffset;
-        Eigen::Vector3f reactForce;
         Eigen::Vector3f filteredForceInRoot;
 
         Eigen::Matrix3f toolTransform;
         Eigen::Vector3f oriToolDir;
         Eigen::Matrix3f origHandOri;
-
-        Eigen::Vector3f forceIntegral;
         Eigen::VectorXf qvel_filtered;
 
+        Eigen::Vector3f adaptK;
+        float lastDiff;
+        Eigen::Vector2f lastPosition;
+        double changeTimer;
 
     };