diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h
index 0d0cce9ec7904331f09394c0bb65f61fba62a09b..8c97be3dd22267f25fc34171c961e0a74bb07907 100644
--- a/source/RobotAPI/libraries/core/MultiDimPIDController.h
+++ b/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;
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.cpp b/source/RobotAPI/libraries/core/TrajectoryController.cpp
index 77207aa85cc7f30480fa3e23cee49b676d0f7cbd..ef74dd1fc47b1f67dbc44bb97f87f5dc5346a39c 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.cpp
+++ b/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();
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.h b/source/RobotAPI/libraries/core/TrajectoryController.h
index 3fdcd37c9b81553af1030d9770e223d9011810f6..2b4c44983357322de378f71ee753711a349b02f6 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.h
+++ b/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);