added max integral value to PID controller

source/RobotAPI/libraries/core/MultiDimPIDController.h

@@ -96,7 +96,7 @@ namespace armarx
             if (!firstRun)
             {
                 integral += error * deltaSec;
-
+                integral = std::min(integral,maxIntegral);
                 if (deltaSec > 0.0)
                 {
                     derivative = (error - previousError) / deltaSec;
@@ -199,6 +199,7 @@ namespace armarx
         //    protected:
         float Kp, Ki, Kd;
         double integral;
+        double maxIntegral = std::numeric_limits<double>::max();
         double derivative;
         double previousError;
         PIDVectorX processValue;

source/RobotAPI/libraries/core/TrajectoryController.cpp

@@ -28,7 +28,7 @@
 namespace armarx
 {
 
-    TrajectoryController::TrajectoryController(const TrajectoryPtr& traj, float kp, float ki, float kd, bool threadSafe) :
+    TrajectoryController::TrajectoryController(const TrajectoryPtr& traj, float kp, float ki, float kd, bool threadSafe, float maxIntegral) :
         traj(traj)
     {
         std::vector<bool> limitless;
@@ -37,6 +37,7 @@ namespace armarx
             limitless.push_back(ls.enabled);
         }
         pid.reset(new MultiDimPIDController(kp, ki, kd, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), threadSafe, limitless));
+        pid->maxIntegral = maxIntegral;
         pid->preallocate(traj->dim());
         ARMARX_CHECK_EXPRESSION(traj);
         currentTimestamp = traj->begin()->getTimestamp();

source/RobotAPI/libraries/core/TrajectoryController.h

@@ -32,7 +32,7 @@ namespace armarx
     class TrajectoryController
     {
     public:
-        TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f, bool threadSafe = true);
+        TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f, bool threadSafe = true, float maxIntegral = std::numeric_limits<float>::max());
         const Eigen::VectorXf& update(double deltaT, const Eigen::VectorXf& currentPosition);
         //const MultiDimPIDControllerPtr& getPid() const;
         //void setPid(const MultiDimPIDControllerPtr& value);