pid controller: add optional filter to differential part

(cherry picked from commit 8dda6b86)

source/RobotAPI/libraries/core/PIDController.cpp

@@ -26,6 +26,7 @@
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <RobotAPI/libraries/core/math/MathUtils.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
 
 using namespace armarx;
 
@@ -131,6 +132,10 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
         if (deltaSec > 0.0)
         {
             derivative = (error - previousError) / deltaSec;
+            if (differentialFilter)
+            {
+                derivative = differentialFilter->update(deltaSec, derivative);
+            }
         }
     }
 

source/RobotAPI/libraries/core/PIDController.h

@@ -25,6 +25,7 @@
 #pragma once
 
 #include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
 #include <Eigen/Core>
 #include "MultiDimPIDController.h"
 
@@ -65,6 +66,7 @@ namespace armarx
         bool firstRun;
         bool limitless;
         bool threadSafe = true;
+        rtfilters::RTFilterBasePtr differentialFilter;
     private:
         ScopedRecursiveLockPtr getLock() const;
         mutable RecursiveMutex mutex;