diff --git a/VirtualRobot/IK/constraints/TSRConstraint.cpp b/VirtualRobot/IK/constraints/TSRConstraint.cpp
index 4af9f9fee9ee9dd41a6e721f24dc6e1a64e3c26c..399139aa84c3f6b682128dd02d1af2ad962d6147 100644
--- a/VirtualRobot/IK/constraints/TSRConstraint.cpp
+++ b/VirtualRobot/IK/constraints/TSRConstraint.cpp
@@ -40,7 +40,7 @@ Eigen::MatrixXf TSRConstraint::getJacobianMatrix()
     {
         if(error(i) == 0)
         {
-            J.row(i).setZero();
+            //J.row(i).setZero();
         }
     }
 
@@ -90,13 +90,15 @@ Eigen::VectorXf TSRConstraint::getError(float stepSize)
     float e_global[6];
     MathTools::eigen4f2rpy(eef_global * eefOffset, e_global);
 
+    resolveRPYAmbiguities(e_global, target_global);
+
     Eigen::VectorXf dx(6);
     dx << (target_global[0] - e_global[0]),
           (target_global[1] - e_global[1]),
           (target_global[2] - e_global[2]),
-          (target_global[3] - e_global[3]),
-          (target_global[4] - e_global[4]),
-          (target_global[5] - e_global[5]);
+          getShortestDistanceForRPYComponent(e_global[3], target_global[3]),
+          getShortestDistanceForRPYComponent(e_global[4], target_global[4]),
+          getShortestDistanceForRPYComponent(e_global[5], target_global[5]);
 
     return dx * stepSize;
 }
@@ -122,4 +124,55 @@ const Eigen::Matrix<float, 6, 2> &TSRConstraint::getBounds()
     return bounds;
 }
 
+void TSRConstraint::resolveRPYAmbiguities(float *pose, const float *reference)
+{
+    Eigen::Vector3f ref;
+    ref << reference[3], reference[4], reference[5];
+
+    Eigen::Vector3f tmp;
+
+    Eigen::Vector3f best;
+    best << pose[3], pose[4], pose[5];
+
+    for(int i = -1; i <= 1; i += 2)
+    {
+        for(int j = -1; j <= 1; j += 2)
+        {
+            for(int k = -1; k <= 1; k += 2)
+            {
+                tmp << reference[3] + i * M_PI,
+                       reference[4] + j * M_PI,
+                       reference[5] + k * M_PI;
+
+                if((tmp - ref).norm() < (best - ref).norm())
+                {
+                    best = tmp;
+                }
+            }
+        }
+    }
+
+    pose[3] = best(0);
+    pose[4] = best(1);
+    pose[5] = best(2);
+}
+
+float TSRConstraint::getShortestDistanceForRPYComponent(float from, float to)
+{
+    float direct = to - from;
+
+    if(direct > M_PI)
+    {
+        return -2*M_PI + direct;
+    }
+    else if(direct < -M_PI)
+    {
+        return 2*M_PI - direct;
+    }
+    else
+    {
+        return direct;
+    }
+}
+
 
diff --git a/VirtualRobot/IK/constraints/TSRConstraint.h b/VirtualRobot/IK/constraints/TSRConstraint.h
index ee3c7e1266b1bfcfd2fd2ab471a29405a1f9d704..4ba90533f768dd9936b6636fb9aef2eb39a96577 100644
--- a/VirtualRobot/IK/constraints/TSRConstraint.h
+++ b/VirtualRobot/IK/constraints/TSRConstraint.h
@@ -48,6 +48,9 @@ namespace VirtualRobot
             const Eigen::Matrix<float, 6, 2> &getBounds();
 
         protected:
+            void resolveRPYAmbiguities(float *pose, const float *reference);
+            float getShortestDistanceForRPYComponent(float from, float to);
+
             RobotPtr robot;
             RobotNodeSetPtr nodeSet;
             RobotNodePtr eef;