diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
index 37cf71bfc6ab71a1eef4c148be3d16dbc60cffdb..eb8d9b4ddd0dd5d9fcd7ff502767f862516c3c53 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
@@ -10,6 +10,8 @@ namespace armarx
     NJointBimanualObjLevelController::NJointBimanualObjLevelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
     {
         ARMARX_INFO << "Preparing ... bimanual ";
+        ARMARX_INFO << "I am here";
+
         useSynchronizedRtRobot();
         cfg = NJointBimanualObjLevelControllerConfigPtr::dynamicCast(config);
         //        ARMARX_CHECK_EXPRESSION(prov);
@@ -391,8 +393,23 @@ namespace armarx
         deltaPoseForWrenchControl.setZero(12);
         for (size_t i = 0; i < 12; ++i)
         {
-            deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
-            //            deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
+            if (KpImpedance(i) == 0)
+            {
+                deltaPoseForWrenchControl(i) = 0;
+            }
+            else
+            {
+                deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
+                if (deltaPoseForWrenchControl(i) > 0.1)
+                {
+                    deltaPoseForWrenchControl(i) = 0.1;
+                }
+                else if (deltaPoseForWrenchControl(i) < -0.1)
+                {
+                    deltaPoseForWrenchControl(i) = -0.1;
+                }
+                //            deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
+            }
         }
 
         // ------------------------------------------- current tcp pose -------------------------------------------
@@ -787,6 +804,7 @@ namespace armarx
 
     void NJointBimanualObjLevelController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
     {
+
         Eigen::VectorXf setpoint;
         setpoint.setZero(value.size());
         ARMARX_CHECK_EQUAL(value.size(), 12);