PIDController: added conditional integral based on error value

source/RobotAPI/libraries/core/PIDController.cpp

@@ -127,8 +127,11 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
     //    ARMARX_INFO << deactivateSpam() << VAROUT(dt);
     if (!firstRun)
     {
-        integral += error * deltaSec;
-        integral = math::MathUtils::LimitTo(integral, maxIntegral);
+        if (error < conditionalIntegralErrorTreshold)
+        {
+            integral += error * deltaSec;
+            integral = math::MathUtils::LimitTo(integral, maxIntegral);
+        }
         if (deltaSec > 0.0)
         {
             derivative = (error - previousError) / deltaSec;

source/RobotAPI/libraries/core/PIDController.h

@@ -54,6 +54,7 @@ namespace armarx
         float Kp, Ki, Kd;
         double integral;
         double maxIntegral = std::numeric_limits<double>::max();
+        double conditionalIntegralErrorTreshold = std::numeric_limits<double>::max(); // anti-windup
         double derivative;
         double previousError;
         double processValue;