diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index c005b862ab526e3d3647a03b5bab116e96c8c26c..3ea01e7168761901edcebf9bc6639490f27fd8df 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -472,7 +472,9 @@ namespace armarx
         const float posIfBrakingNow = currentPosition + brakingDistance;
         const float posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
         const bool brakingNeeded = std::abs(brakingDistance) > std::abs(positionError);
-        const float usedDeceleration = brakingNeeded ? std::abs(currentV * currentV / 2.f / positionError) : deceleration ;
+        const float usedDeceleration = (brakingNeeded && positionError != 0.0f) ?
+                                       std::abs(currentV * currentV / 2.f / positionError) :
+                                       deceleration;
         const bool decelerate =
             std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
             brakingNeeded ||
@@ -480,7 +482,9 @@ namespace armarx
         const float usedacc = decelerate ?  -usedDeceleration : acceleration;
         const float deltaVel = signV * usedacc * useddt;
         float newTargetVel = boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
-        bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVel) || std::abs(newTargetVelPController) < pControlVelLimit;
+        bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVel)
+                      || std::abs(newTargetVelPController) < pControlVelLimit
+                      || std::abs(positionError) < pControlPosErrorLimit;
         //        buffer.push_back({currentPosition, newTargetVelPController, newTargetVel, currentV, positionError});
 
         //        if (PIDModeActive != usePID)
@@ -491,7 +495,7 @@ namespace armarx
         //            }
         //            ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVel) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
         //        }
-        //        ARMARX_IMPORTANT << deactivateSpam(1) << VAROUT(newTargetVelPController) << VAROUT(newTargetVel) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
+        //        ARMARX_INFO << deactivateSpam(1) << VAROUT(newTargetVelPController) << VAROUT(newTargetVel) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV) << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
         //        PIDModeActive = usePID;
         //        ARMARX_INFO << deactivateSpam(0.2) << VAROUT(usePID) << VAROUT(dt) << VAROUT(decelerate) << VAROUT(usedacc) << VAROUT(maxDeltaV) << VAROUT(deltaVel) << VAROUT(newTargetVel) << VAROUT(newTargetVelPController)
         //                    << VAROUT(currentPosition) << VAROUT(targetPosition);
@@ -706,6 +710,7 @@ namespace armarx
         //run should be static, but we need to adapt the parabs for the subcontroler -> have the subcontroler as value;
         PositionThroughVelocityControllerWithAccelerationBounds sub = *this; // slice parent class from this
         sub.currentPosition = periodicClamp(currentPosition, positionPeriodLo, positionPeriodHi);
+        //        ARMARX_INFO << deactivateSpam(1) << VAROUT(currentPosition) << VAROUT(sub.currentPosition) << VAROUT(positionPeriodLo) << VAROUT(positionPeriodHi);
         sub.targetPosition  = periodicClamp(targetPosition , positionPeriodLo, positionPeriodHi);
 
         const float brakingDist = brakingDistance(currentV, deceleration);
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
index 485de8941726672a85b8ba39d1f3c866057f442d..c4235c90edfb958b0fda86d3bf76757bbaa47b8e 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
@@ -27,7 +27,7 @@
 #include <type_traits>
 #include <ArmarXCore/core/util/algorithm.h>
 #include <RobotAPI/libraries/core/math/MathUtils.h>
-//#include <boost/circular_buffer.hpp>
+#include <boost/circular_buffer.hpp>
 
 namespace armarx
 {
@@ -224,7 +224,7 @@ namespace armarx
         float deceleration;
         float currentPosition;
         float targetPosition;
-        //        float pControlPosErrorLimit;
+        float pControlPosErrorLimit = 0.01;
         float pControlVelLimit = 0.001; // if target is below this threshold, PID controller will be used
         float p;
         PositionThroughVelocityControllerWithAccelerationBounds();
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 740a6c7c3869f7505f38cc4a984fd956154aae3c..d209d686d5c8aae8e7f1b00f107b9e5684f77817 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -2474,7 +2474,7 @@ namespace armarx
         {
             bool collision = selfCollisionAvoidanceData.robot->getCollisionChecker()->checkCollision(selfCollisionAvoidanceData.collisionModels[it->first], selfCollisionAvoidanceData.collisionModels[it->second]);
             anyCollision |= collision;
-            distancesString += "---- " + it->first + " <-> " + it->second + " in Collsition: " + (collision ? "True" : "False") +  "\n";
+            distancesString += "---- " + it->first + " <-> " + it->second + " in Collision: " + (collision ? "True" : "False") +  "\n";
             //            distances.push_back(distance);
             if (collision)
             {