diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 93b018801eb5e7f6b870e06b914798d511dc5780..878f0b28bf5318095f28006dfd6e219a4e68217f 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -298,6 +298,7 @@ module armarx
         string dmpType = "Discrete";
         double timeDuration;
         string nodeSetName;
+        string forceSensorName;
 
         // phaseStop technique
         double phaseL = 10;
@@ -306,6 +307,7 @@ module armarx
         double phaseDist1 = 10;
         double posToOriRatio = 100;
 
+
         Ice::FloatSeq Kpos;
         Ice::FloatSeq Dpos;
         Ice::FloatSeq Kori;
@@ -319,6 +321,8 @@ module armarx
 
         float torqueLimit;
 
+        float waitTimeForCalibration;
+
     };
 
     interface NJointTaskSpaceImpedanceDMPControllerInterface extends NJointControllerInterface
@@ -348,6 +352,9 @@ module armarx
         void setAngularVelocityKd(Ice::FloatSeq kd);
         void setAngularVelocityKp(Ice::FloatSeq kp);
 
+        void enableForceStop();
+        void disableForceStop();
+        void setForceThreshold(float forceThreshold);
     };
 
     class NJointTaskSpaceAdaptiveDMPControllerConfig extends NJointControllerConfig
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 65da4d862d7696bccaca2b68e5576777e722d54d..945b5d69585de2af97f7237d0107de03861fdf8c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -37,6 +37,13 @@ namespace armarx
             positionSensors.push_back(positionSensor);
         };
 
+        const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
+        forceSensor = svlf->asA<SensorValueForceTorque>();
+
+        forceOffset.setZero();
+        filteredForce.setZero();
+        filteredForceInRoot.setZero();
+
         tcp =  rns->getTCP();
         ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
         numOfJoints = targets.size();
@@ -135,8 +142,6 @@ namespace armarx
 
     void NJointTaskSpaceImpedanceDMPController::controllerRun()
     {
-
-
         if (!dmpCtrl)
         {
             return;
@@ -226,6 +231,22 @@ namespace armarx
     void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
     {
         double deltaT = timeSinceLastIteration.toSecondsDouble();
+        Eigen::Matrix4f targetPose;
+        Eigen::VectorXf targetVel;
+        Eigen::VectorXf desiredNullSpaceJointValues;
+        if (firstRun || !started)
+        {
+            firstRun = false;
+            targetPose = currentPose;
+            targetVel.setZero(6);
+            desiredNullSpaceJointValues = defaultNullSpaceJointValues;
+        }
+        else
+        {
+            targetPose = rtGetControlStruct().targetPose;
+            targetVel = rtGetControlStruct().targetVel;
+            desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
+        }
 
 
         Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
@@ -253,22 +274,6 @@ namespace armarx
         jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
 
 
-        Eigen::Matrix4f targetPose;
-        Eigen::VectorXf targetVel;
-        Eigen::VectorXf desiredNullSpaceJointValues;
-        if (firstRun)
-        {
-            firstRun = false;
-            targetPose = currentPose;
-            targetVel.setZero(6);
-            desiredNullSpaceJointValues = defaultNullSpaceJointValues;
-        }
-        else
-        {
-            targetPose = rtGetControlStruct().targetPose;
-            targetVel = rtGetControlStruct().targetVel;
-            desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
-        }
 
         Eigen::Vector3f kpos = ctrlParams.getUpToDateReadBuffer().kpos;
         Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
@@ -442,34 +447,17 @@ namespace armarx
 
     void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
     {
-        firstRun = true;
-        while (firstRun)
-        {
-            usleep(100);
-        }
-
-        Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
-        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-
         dmpCtrl->canVal = timeDuration;
         dmpCtrl->config.motionTimeDuration = timeDuration;
 
-        finished = false;
-
-        if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
-        {
-            ARMARX_INFO << "Using Null Space Joint DMP";
-        }
-
-        started = true;
-        stopped = false;
-        //        controllerTask->start();
+        runDMP(goals);
     }
 
     void NJointTaskSpaceImpedanceDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
     {
         firstRun = true;
-        while (firstRun)
+        timeForCalibration = 0;
+        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 1b49c99634870147df440d88709f828c0e074872..d68faf54980d663be30cad8d6cd6e1e6555e6c2a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -81,7 +81,18 @@ namespace armarx
         void setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)override;
         void setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)override;
 
-
+        void enableForceStop(const Ice::Current&) override
+        {
+            useForceStop = true;
+        }
+        void disableForceStop(const Ice::Current&) override
+        {
+            useForceStop = false;
+        }
+        void setForceThreshold(float forceThreshold, const Ice::Current&) override
+        {
+            this->forceThreshold = forceThreshold;
+        }
     protected:
         virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
@@ -152,6 +163,7 @@ namespace armarx
         TripleBuffer<CtrlParams> ctrlParams;
 
 
+
         DMP::Vec<DMP::DMPState> currentJointState;
         DMP::UMIDMPPtr nullSpaceJointDMPPtr;
 
@@ -202,6 +214,13 @@ namespace armarx
         bool started = false;
         bool stopped = false;
 
+        Eigen::Vector3f filteredForce;
+        Eigen::Vector3f forceOffset;
+        Eigen::Vector3f filteredForceInRoot;
+        std::atomic<float> forceThreshold;
+        std::atomic<bool> useForceStop;
+        std::atomic<float> timeForCalibration;
+
         Eigen::Matrix4f oldPose;
         // NJointController interface
     protected: