diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 5be7b7967c044268043390727cdc95b7ac434590..a1ba8214cc56505b4a22af4d136d11e4cff5e7ac 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -298,7 +298,6 @@ module armarx
         string dmpType = "Discrete";
         double timeDuration;
         string nodeSetName;
-        string forceSensorName;
 
         // phaseStop technique
         double phaseL = 10;
@@ -321,8 +320,13 @@ module armarx
 
         float torqueLimit;
 
-        float waitTimeForCalibration;
 
+        string forceSensorName;
+        float waitTimeForCalibration;
+        float forceFilter;
+        float forceDeadZone;
+        Eigen::Vector3f forceThreshold;
+        string forceFrameName = "ArmR8_Wri2";
     };
 
     interface NJointTaskSpaceImpedanceDMPControllerInterface extends NJointControllerInterface
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 1c02bc7a4c1849e44e62ef2b88dc5163efc35cfa..5fc97fcf5e21a6368abf52956457a7ec9f642193 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -40,7 +40,7 @@ namespace armarx
             positionSensors.push_back(positionSensor);
         };
 
-        const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
+        const SensorValueBase* svlf = robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
         forceSensor = svlf->asA<SensorValueForceTorque>();
 
         forceOffset.setZero();
@@ -121,6 +121,7 @@ namespace armarx
         controllerSensorData.reinitAllBuffers(initControllerSensorData);
 
         firstRun = true;
+        useForceStop = false;
 
         ARMARX_INFO << "Finished controller constructor ";
     }
@@ -239,25 +240,74 @@ namespace armarx
     void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
             const IceUtil::Time& timeSinceLastIteration)
     {
+
+        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+
         double deltaT = timeSinceLastIteration.toSecondsDouble();
         Eigen::Matrix4f targetPose;
         Eigen::VectorXf targetVel;
         Eigen::VectorXf desiredNullSpaceJointValues;
-        if (firstRun || !started)
+        if (firstRun)
         {
             firstRun = false;
             targetPose = currentPose;
+            stopPose = currentPose;
             targetVel.setZero(6);
-            desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+            desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
         }
         else
         {
-            targetPose = rtGetControlStruct().targetPose;
-            targetVel = rtGetControlStruct().targetVel;
-            desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
+            if (!started)
+            {
+                targetPose = stopPose;
+                targetVel.setZero(6);
+                desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
+                forceOffset = (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
+                timeForCalibration = timeForCalibration + timeSinceLastIteration.toSecondsDouble();
+            }
+            else
+            {
+                targetPose = rtGetControlStruct().targetPose;
+                targetVel = rtGetControlStruct().targetVel;
+                desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
+
+                if (useForceStop)
+                {
+                    /* handle force stop */
+                    filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
+
+                    for (size_t i = 0; i < 3; ++i)
+                    {
+                        if (fabs(filteredForce(i)) > cfg->forceDeadZone)
+                        {
+                            filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
+                        }
+                        else
+                        {
+                            filteredForce(i) = 0;
+                        }
+                    }
+                    Eigen::Matrix4f forceFrameInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
+                    filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
+
+                    for (size_t i = 0; i < 3; ++i)
+                    {
+                        if (fabs(filteredForceInRoot[i]) > cfg->forceThreshold[i])
+                        {
+                            stopPose = currentPose;
+                            targetVel.setZero(6);
+                            desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
+                            started = false;
+                            break;
+                        }
+                    }
+                }
+
+            }
         }
 
 
+
         Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
 
         Eigen::VectorXf qpos(positionSensors.size());
@@ -268,7 +318,6 @@ namespace armarx
             qvel(i) = velocitySensors[i]->velocity;
         }
 
-        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
         Eigen::VectorXf currentTwist = jacobi * qvel;
 
         controllerSensorData.getWriteBuffer().currentPose = currentPose;
@@ -318,9 +367,6 @@ namespace armarx
             jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
 
 
-
-
-
         // torque limit
         ARMARX_CHECK_EXPRESSION(!targets.empty());
         ARMARX_CHECK_LESS(targets.size(), 1000);
@@ -500,7 +546,7 @@ namespace armarx
     {
         firstRun = true;
         timeForCalibration = 0;
-        while (firstRun || timeForCalibration < cfg.waitTimeForCalibration)
+        while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
         {
             usleep(100);
         }
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 9c15961eb71a8977e1934ffbc2a0a89de94fa1c3..c0f8fea9525c5f261141a0658bdca7852a9eb263 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -6,6 +6,7 @@
 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
 #include <VirtualRobot/IK/DifferentialIK.h>
 #include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
 #include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
@@ -59,7 +60,7 @@ namespace armarx
         void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
 
         void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&) override;
-        void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&) override;
+        void runDMP(const Ice::DoubleSeq& goals, const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
         void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override;
 
         Ice::Double getVirtualTime(const Ice::Current&) override
@@ -242,12 +243,15 @@ namespace armarx
         bool started = false;
         bool stopped = false;
 
+        Eigen::Matrix4f stopPose;
+
         Eigen::Vector3f filteredForce;
         Eigen::Vector3f forceOffset;
         Eigen::Vector3f filteredForceInRoot;
         std::atomic<float> forceThreshold;
         std::atomic<bool> useForceStop;
         std::atomic<float> timeForCalibration;
+        const SensorValueForceTorque* forceSensor;
 
         Eigen::Matrix4f oldPose;
         // NJointController interface