diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp
index 749adef44ea4b094a18799de863d02de332197b2..cdea23cbdb844ee764b6267c80a77eba1f19f537 100644
--- a/VirtualRobot/IK/DifferentialIK.cpp
+++ b/VirtualRobot/IK/DifferentialIK.cpp
@@ -92,12 +92,12 @@ namespace VirtualRobot
         }
 
         // tcp not in list yet?
-        if (find(tcp_set.begin(), tcp_set.end(), tcp) == tcp_set.end())
+        /*if (find(tcp_set.begin(), tcp_set.end(), tcp) == tcp_set.end())
         {
             tcp_set.push_back(tcp);
-        }
+        }*/
 
-        this->initialize();
+        initialize();
     }
 
     MatrixXf DifferentialIK::getJacobianMatrix()
@@ -159,7 +159,7 @@ namespace VirtualRobot
 
     VectorXf DifferentialIK::getError(float stepSize)
     {
-        updateError(currentError);
+        updateError(currentError, stepSize);
         return currentError;
     }