diff --git a/MotionPlanning/CSpace/CSpace.cpp b/MotionPlanning/CSpace/CSpace.cpp
index 65c913935cf469c0200b6dcfcf00bd3af6046841..839b6300216aa3fa961165cd2cc74c72ff6d220e 100644
--- a/MotionPlanning/CSpace/CSpace.cpp
+++ b/MotionPlanning/CSpace/CSpace.cpp
@@ -625,6 +625,8 @@ namespace Saba
             float start = q1[dim];
             float end = q2[dim];
             float res = interpolateRotational(start, end , step);
+            // change to actual defined borders
+            res = VirtualRobot::MathTools::angleModX(res, 0.5f*(boundaryMax[dim]+boundaryMin[dim]));
             return res;
         }
 
diff --git a/MotionPlanning/CSpace/CSpaceSampled.cpp b/MotionPlanning/CSpace/CSpaceSampled.cpp
index 2794ecb0c6a3c355b71196555677af0d52c898c6..91c7119d0584258be77cd389ea3410f660a7c96b 100644
--- a/MotionPlanning/CSpace/CSpaceSampled.cpp
+++ b/MotionPlanning/CSpace/CSpaceSampled.cpp
@@ -208,7 +208,7 @@ namespace Saba
                 generateNewConfig(goal, tmpConfig, tmpConfig, colCheckDist, dist);
                 LOCAL_DEBUG("--> " << endl << tmpConfig << endl);
 
-                if (!(isConfigValid(tmpConfig, false, true, true)))
+                if (!(isConfigValid(tmpConfig, true, true, true)))
                 {
                     return p;
                 }