wip

MotionPlanning/CSpace/CSpace.cpp

@@ -629,11 +629,13 @@ namespace Saba
         else
         {
             // borderless mode
-
+            // we map 'start' and 'end' to [0,2pi] (temporarily)
             float start = q1[dim] - robotJoints[dim]->getJointLimitLo();
             float end = q2[dim] - robotJoints[dim]->getJointLimitLo();
             float res = interpolateRotational(start, end , step);
+            // using fmod is okay here, because 'start' and 'end' where mapped to [0,2pi] and therefore 'res' is also in [0,2pi]
             res = (float)fmod((double)res, 2.0 * M_PI);
+            // map back to original interval
             res = res + robotJoints[dim]->getJointLimitLo();
             return res;
         }

MotionPlanning/CSpace/CSpace.h

@@ -289,7 +289,7 @@ namespace Saba
         /*!
             Interpolates between two values in dimension dim.
             Checks weather the corresponding joint moves translational or a rotational and performs the interpolation accordingly.
-            When joint boundaries of a rotational joint are >= 2PI (and checkForBorderlessDimensions was not disabled), the correct direction for interpolating is determined automatically.
+            When a rotational joint is limitless (and checkForBorderlessDimensions was not disabled), the correct direction for interpolating is determined automatically.
         */
         float interpolate(const Eigen::VectorXf& q1, const Eigen::VectorXf& q2, int dim, float step);
         Eigen::VectorXf interpolate(const Eigen::VectorXf& q1, const Eigen::VectorXf& q2, float step);
@@ -323,7 +323,8 @@ namespace Saba
         //! interpolates linear between a and b using step as step size
         float interpolateLinear(float a, float b, float step);
 
-        //! interpolates rotational between a and b using step as step size
+        //! interpolates rotational between a and b using step as step size ('a' and 'b' are expected to be in [0,2pi]).
+        //! The interpolation takes the direction of the shorter path between 'a' and 'b'.
         float interpolateRotational(float a, float b, float step);
 
         unsigned int dimension;                                     //!< dimension of this c-space

MotionPlanning/Planner/Rrt.cpp

@@ -263,7 +263,8 @@ namespace Saba
         {
             float factor = extendStepSize / totalLength;
             // go a specific length in the direction of randomly found configuration
-            tmpConfig = nn->configuration + ((c - nn->configuration) * factor);
+            tmpConfig = cspace->interpolate(nn->configuration, c, factor);
+            //tmpConfig = nn->configuration + ((c - nn->configuration) * factor);
             //for (unsigned int i = 0; i < m_nDimension; i++)
             //  m_pExtendRrtNewValue[i] = m_nnNode->configuration[i] + ((extGoal[i] - m_nnNode->configuration[i]) * factor);
         }