refactoring in PIDController

source/RobotAPI/libraries/core/PIDController.cpp

@@ -29,7 +29,7 @@
 using namespace armarx;
 
 
-PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxAcceleration) :
+PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation) :
     Kp(Kp),
     Ki(Ki),
     Kd(Kd),
@@ -40,7 +40,8 @@ PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValu
     target(0),
     controlValue(0),
     maxControlValue(maxControlValue),
-    maxAcceleration(maxAcceleration)
+    maxDerivation(maxDerivation),
+    controlValueDerivation(0)
 {
     reset();
 }
@@ -113,10 +114,11 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
     controlValue = Kp * error + Ki * integral + Kd * derivative;
     if (deltaSec > 0.0)
     {
-        double acc = (controlValue - oldControlValue) / deltaSec;
-        if (fabs(acc) > maxAcceleration)
+        double deriv = (controlValue - oldControlValue) / deltaSec;
+        if (fabs(deriv) > maxDerivation)
         {
-            controlValue = oldControlValue + maxAcceleration * deltaSec * math::MathUtils::Sign(acc);
+            controlValueDerivation = maxDerivation * deltaSec * math::MathUtils::Sign(deriv);
+            controlValue = oldControlValue + controlValueDerivation;
         }
     }
     controlValue = std::min(fabs(controlValue), maxControlValue) * math::MathUtils::Sign(controlValue);
@@ -134,7 +136,7 @@ double PIDController::getControlValue() const
 }
 
 
-MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxAcceleration) :
+MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation) :
     Kp(Kp),
     Ki(Ki),
     Kd(Kd),
@@ -142,7 +144,7 @@ MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, doubl
     derivative(0),
     previousError(0),
     maxControlValue(maxControlValue),
-    maxAcceleration(maxAcceleration)
+    maxDerivation(maxDerivation)
 {
     reset();
 }
@@ -195,10 +197,10 @@ void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf&
         float maxNewJointAcc = accVec.maxCoeff();
         float minNewJointAcc = accVec.minCoeff();
         maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
-        if (maxNewJointAcc > maxAcceleration)
+        if (maxNewJointAcc > maxDerivation)
         {
-            auto newValue = oldControlValue + accVec * maxAcceleration / maxNewJointAcc * deltaSec;
-            ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxAcceleration) << VAROUT(maxNewJointAcc) << VAROUT(controlValue)  << VAROUT(oldControlValue) << VAROUT(newValue);
+            auto newValue = oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
+            ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue)  << VAROUT(oldControlValue) << VAROUT(newValue);
             controlValue = newValue;
         }
     }

source/RobotAPI/libraries/core/PIDController.h

@@ -35,7 +35,7 @@ namespace armarx
         public Logging
     {
     public:
-        PIDController(float Kp, float Ki, float Kd, double maxControlValue = std::numeric_limits<double>::max(), double maxAcceleration = std::numeric_limits<double>::max());
+        PIDController(float Kp, float Ki, float Kd, double maxControlValue = std::numeric_limits<double>::max(), double maxDerivation = std::numeric_limits<double>::max());
         void update(double deltaSec, double measuredValue, double targetValue);
         void update(double measuredValue, double targetValue);
         void update(double measuredValue);
@@ -53,8 +53,9 @@ namespace armarx
         double target;
         IceUtil::Time lastUpdateTime;
         double controlValue;
+        double controlValueDerivation;
         double maxControlValue;
-        double maxAcceleration;
+        double maxDerivation;
         bool firstRun;
         mutable RecursiveMutex mutex;
     };
@@ -64,7 +65,7 @@ namespace armarx
         public Logging
     {
     public:
-        MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue = std::numeric_limits<double>::max(), double maxAcceleration = std::numeric_limits<double>::max());
+        MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue = std::numeric_limits<double>::max(), double maxDerivation = std::numeric_limits<double>::max());
         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;
@@ -81,7 +82,7 @@ namespace armarx
         IceUtil::Time lastUpdateTime;
         Eigen::VectorXf controlValue;
         double maxControlValue;
-        double maxAcceleration;
+        double maxDerivation;
         bool firstRun;
         mutable  RecursiveMutex mutex;
     };