diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.cpp
index 2cf2bf26753f1bec6b91908071f7e25e70bfe6f6..4437bed7df9c07ad67230d00ff1f16682dd87562 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.cpp
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.cpp
@@ -28,26 +28,24 @@ void JointPWMTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
 {
     if (target.isValid())
     {
-        const float velocity = dataPtr->getVelocity();
+        const float velocity = dataPtr->getRawVelocity(); // dataPtr->getVelocity();
         const float targetTorque = target.torque;
 
-        const int maxIter = 30;
+        const int maxIter = 50;
         int counter = 0;
 
-        float pwm_approx_min = -1000.f;
-        float pwm_approx_max = 1000.f;
-        float pwm_approx_estimate = 0.f;
+        float pwm_approx_min = -3000.f;
+        float pwm_approx_max = 3000.f;
+        float pwm_approx_estimate = (pwm_approx_min + pwm_approx_max) * 0.5;
 
         auto f = [ = ](float pwm)
         {
             int32_t pwm_integer = (int32_t) pwm;
-            //return torqueEstimation->linearModel_dcx22(velocity, pwm_integer) - targetTorque;
             torqueEstimation->estimateTorque(velocity, pwm_integer);
             return torqueEstimation->getTorqueWithInertia() - targetTorque;
-            //return torqueEstimation->linearModel_dcx22(velocity, pwm_integer) - targetTorque;
         };
 
-        while (std::fabs(f(pwm_approx_estimate)) > .001f && counter < maxIter)
+        while (std::fabs(f(pwm_approx_estimate)) > .005f && counter < maxIter)
         {
             if (f(pwm_approx_min) * f(pwm_approx_max) < 0)
             {
@@ -70,34 +68,47 @@ void JointPWMTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
         }
 
         /*if (f(pwm_approx1) * f(pwm_approx2) >= 0)
-        {
-            //ARMARX_INFO << deactivateSpam(1) << "ASSERT";
-            dataPtr->setTargetPWM(0);
-            return;
-        }
+                {
+                    //ARMARX_INFO << deactivateSpam(1) << "ASSERT";
+                    dataPtr->setTargetPWM(0);
+                    return;
+                }
 
-        float pwm_approx3 = pwm_approx1;
+                float pwm_approx3 = pwm_approx1;
 
-        for (; counter < maxIter; counter++)
-        {
-            pwm_approx3 = (pwm_approx1 * f(pwm_approx2) - pwm_approx2 * f(pwm_approx1)) / (f(pwm_approx2) - f(pwm_approx1));
-            if (abs(f(pwm_approx3)) <= .01)
-            {
-                break;
-            }
-            else if (f(pwm_approx3) * f(pwm_approx1) < 0)
-            {
-                pwm_approx2 = pwm_approx3;
-            }
-            else
-            {
-                pwm_approx1 = pwm_approx3;
-            }
-        }*/
+                for (; counter < maxIter; counter++)
+                {
+                    pwm_approx3 = (pwm_approx1 * f(pwm_approx2) - pwm_approx2 * f(pwm_approx1)) / (f(pwm_approx2) - f(pwm_approx1));
+                    if (abs(f(pwm_approx3)) <= .01)
+                    {
+                        break;
+                    }
+                    else if (f(pwm_approx3) * f(pwm_approx1) < 0)
+                    {
+                        pwm_approx2 = pwm_approx3;
+                    }
+                    else
+                    {
+                        pwm_approx1 = pwm_approx3;
+                    }
+                }*/
 
-        ARMARX_INFO << deactivateSpam(0.1) << VAROUT(targetTorque) << VAROUT(pwm_approx_estimate) << VAROUT(f(pwm_approx_estimate)) << VAROUT(counter);
+
+
+
+        ARMARX_INFO << deactivateSpam(0.01) << VAROUT(targetTorque) << VAROUT(pwm_approx_estimate) << VAROUT(f(pwm_approx_estimate)) << VAROUT(counter);
         dataPtr->setTargetPWM(int(pwm_approx_estimate));
         //dataPtr->setTargetPWM(0);
+
+        //        torqueEstimation->estimateTorque(dataPtr->getVelocity(), dataPtr->getTargetPWM());
+        //        float torque_error = targetTorque - torqueEstimation->getTorqueWithInertia();
+        //        float P = 150.0f;
+        //        float I = 20.0f;
+        //        error_sum += torque_error;
+        //        float pwm =  P * torque_error + I * error_sum;
+        //        pwm = (1 - filter_coeff) * last_pwm + filter_coeff * pwm;
+        //        dataPtr->setTargetPWM(int(pwm));
+        //        last_pwm = pwm;
     }
 }
 
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.h
index c6e7f294f1daedbcc883b8a0915de7548536ef10..e8eef21fb96c75eb6976161b1b69443a061cf03b 100644
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.h
+++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMTorqueController.h
@@ -30,6 +30,9 @@ namespace armarx
 
         KITGripperBasisBoardPtr board;
         const std::string deviceName;
+        float last_pwm = 0.0f;
+        float filter_coeff = 0.8f;
+        float error_sum = 0.0f;
         MotorTorqueEstimatorPtr torqueEstimation;
 
         size_t actorIndex = 0;