multidim pid controller

source/RobotAPI/libraries/core/PIDController.cpp

@@ -87,8 +87,72 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
 
 }
 
-double PIDController::getControlValue()
+double PIDController::getControlValue() const
 {
     ScopedRecursiveLock lock(mutex);
     return controlValue;
 }
+
+
+MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd) :
+    Kp(Kp),
+    Ki(Ki),
+    Kd(Kd),
+    integral(0),
+    derivative(0),
+    previousError(0)
+{
+    reset();
+}
+
+void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue)
+{
+    ScopedRecursiveLock lock(mutex);
+    processValue = measuredValue;
+    target = targetValue;
+
+    double error = (target - processValue).norm();
+    //double dt = (now - lastUpdateTime).toSecondsDouble();
+//    ARMARX_INFO << deactivateSpam() << VAROUT(dt);
+    if(!firstRun)
+    {
+        integral += error * deltaSec;
+        derivative = (error - previousError) / deltaSec;
+    }
+    firstRun = false;
+    Eigen::VectorXf direction = (target - processValue).normalized();
+    controlValue = direction * (Kp * error + Ki * integral + Kd * derivative);
+    if(error > 0.04)
+        ARMARX_INFO << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) <<  " i: " << (Ki*integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
+
+    previousError = error;
+    lastUpdateTime += IceUtil::Time::seconds(deltaSec);
+
+}
+
+void MultiDimPIDController::update(const Eigen::VectorXf &measuredValue, const Eigen::VectorXf &targetValue)
+{
+    ScopedRecursiveLock lock(mutex);
+    IceUtil::Time now = IceUtil::Time::now();
+    if(firstRun)
+    {
+        lastUpdateTime = IceUtil::Time::now();
+    }
+    double dt = (now - lastUpdateTime).toSecondsDouble();
+    update(dt,measuredValue,targetValue);
+    lastUpdateTime = now;
+}
+
+const Eigen::VectorXf& MultiDimPIDController::getControlValue() const
+{
+    return controlValue;
+}
+
+void MultiDimPIDController::reset()
+{
+    ScopedRecursiveLock lock(mutex);
+    firstRun = true;
+    previousError = 0;
+    integral = 0;
+    lastUpdateTime = IceUtil::Time::now();
+}

source/RobotAPI/libraries/core/PIDController.h

@@ -25,6 +25,7 @@
 #define _ARMARX_COMPONENT_RobotAPI_PIDController_H
 
 #include <Core/core/logging/Logging.h>
+#include <Eigen/Core>
 
 namespace armarx
 {
@@ -36,7 +37,7 @@ namespace armarx
         PIDController(float Kp, float Ki, float Kd);
         void update(double deltaSec, double measuredValue, double targetValue);
         void update(double measuredValue, double targetValue);
-        double getControlValue();
+        double getControlValue() const;
 
         void reset();
 //    protected:
@@ -49,7 +50,31 @@ namespace armarx
         IceUtil::Time lastUpdateTime;
         double controlValue;
         bool firstRun;
-        RecursiveMutex mutex;
+        mutable RecursiveMutex mutex;
+    };
+    typedef boost::shared_ptr<PIDController> PIDControllerPtr;
+
+    class MultiDimPIDController :
+            public Logging
+    {
+    public:
+        MultiDimPIDController(float Kp, float Ki, float Kd);
+        void update(const double deltaSec, const Eigen::VectorXf &measuredValue, const Eigen::VectorXf &targetValue);
+        void update(const Eigen::VectorXf &measuredValue, const Eigen::VectorXf &targetValue);
+        const Eigen::VectorXf &getControlValue() const;
+
+        void reset();
+//    protected:
+        float Kp, Ki, Kd;
+        double integral;
+        double derivative;
+        double previousError;
+        Eigen::VectorXf processValue;
+        Eigen::VectorXf target;
+        IceUtil::Time lastUpdateTime;
+        Eigen::VectorXf controlValue;
+        bool firstRun;
+        mutable  RecursiveMutex mutex;
     };
     typedef boost::shared_ptr<PIDController> PIDControllerPtr;
 }