diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index ee185d0bdcfc48ed2901b05dc31a9b2ee64517df..3c303ee6d03d4d4905096a44fba89af59859b2ef 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -356,6 +356,12 @@ module armarx
 
         float maxLinearVel;
         float maxAngularVel;
+
+        Ice::FloatSeq ws_x;
+        Ice::FloatSeq ws_y;
+        Ice::FloatSeq ws_z;
+
+
     };
 
 
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index ba0d95adb5448180c68979578b81b7231592acd9..e7948a9fc4a65aff35c517e5b004d51026560632 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -100,6 +100,22 @@ namespace armarx
 
         qvel_filtered.setZero(targets.size());
 
+        ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
+        ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
+        ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
+        // only for ARMAR-6 (safe-guard)
+        ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
+        ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
+        ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
+
+        ARMARX_CHECK_LESS(cfg->ws_y[0],  cfg->ws_y[1]);
+        ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
+        ARMARX_CHECK_LESS(0,  cfg->ws_y[1]);
+
+        ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
+        ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
+        ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
+
     }
 
     void NJointPeriodicTSDMPCompliantController::onInitNJointController()
@@ -292,8 +308,6 @@ namespace armarx
                 targetVel(i) = 0;
             }
 
-
-
             // rotation changes
 
             //            if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
@@ -356,6 +370,17 @@ namespace armarx
 
         float dTf = (float)deltaT;
         targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
+        targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
+        targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
+
+        targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
+        targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
+
+        targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
+        targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
+
+
+
         //            Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
         //            targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
 
@@ -384,7 +409,7 @@ namespace armarx
             {
                 linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
             }
-            Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+            Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(- currentTCPLinearVelocity);
 
             Eigen::Vector3f currentTCPAngularVelocity;
             currentTCPAngularVelocity << currentTwist(3),   currentTwist(4),  currentTwist(5);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
index 91aa9f460b104ea7eda5d1bc102bd13fa9f14709..5ca91b2bcef1f98428fc3f1c11601ac44bcfd5d9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -174,6 +174,8 @@ namespace armarx
         Eigen::Matrix3f origHandOri;
 
         Eigen::VectorXf qvel_filtered;
+
+
     };
 
 } // namespace armarx