fix PID controller max acc feature

source/RobotAPI/libraries/core/PIDController.cpp

@@ -113,7 +113,7 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
     double acc = (controlValue - oldControlValue) / deltaSec;
     if (fabs(acc) > maxAcceleration)
     {
-        controlValue = oldControlValue +  maxAcceleration * deltaSec * math::MathUtils::Sign(acc);
+        controlValue = oldControlValue + maxAcceleration * deltaSec * math::MathUtils::Sign(acc);
     }
     controlValue = std::min(fabs(controlValue), maxControlValue) * math::MathUtils::Sign(controlValue);
     ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) <<  " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
@@ -174,16 +174,26 @@ void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf&
     {
         direction.setZero();
     }
-    Eigen::VectorXf oldControlValue = controlValue;
+    Eigen::VectorXf oldControlValue;
+    if (controlValue.rows() > 0)
+    {
+        oldControlValue = controlValue;
+    }
+    else
+    {
+        oldControlValue = Eigen::VectorXf::Zero(measuredValue.rows());
+    }
     controlValue = direction * (Kp * error + Ki * integral + Kd * derivative);
 
     Eigen::VectorXf accVec = (controlValue - oldControlValue) / deltaSec;
-    float maxAcc = accVec.maxCoeff();
-    float minAcc = accVec.minCoeff();
-    maxAcc = std::max<float>(fabs(minAcc), fabs(maxAcc));
-    if (maxAcc > maxAcceleration)
+    float maxNewJointAcc = accVec.maxCoeff();
+    float minNewJointAcc = accVec.minCoeff();
+    maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
+    if (maxNewJointAcc > maxAcceleration)
     {
-        controlValue *= maxAcceleration / maxAcc;
+        auto newValue = oldControlValue + accVec * maxAcceleration / maxNewJointAcc * deltaSec;
+        ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxAcceleration) << VAROUT(maxNewJointAcc) << VAROUT(controlValue)  << VAROUT(oldControlValue) << VAROUT(newValue);
+        controlValue = newValue;
     }
 
 
@@ -196,7 +206,7 @@ void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf&
     if (max > maxControlValue)
     {
         auto newValue = controlValue  * maxControlValue / max;
-        ARMARX_IMPORTANT << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
+        ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
         controlValue = newValue;
     }
     ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) <<  " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;