change interpolated solution of limitless joints to actual defined limits instead of 0-2PI

check limits in any case after creation of random config

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;
         }
 

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;
                 }