bugfix

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@223 042f3d55-54a8-47e9-b7fb-15903f145c44

MotionPlanning/PostProcessing/ShortcutProcessor.cpp

@@ -74,7 +74,7 @@ int ShortcutProcessor::doShortcut( int startIndex, int endIndex )
 	optimizedPath->print();*/
 
 	Eigen::VectorXf s = optimizedPath->getPoint(startIndex);
-	Eigen::VectorXf e = optimizedPath->getPoint(endIndex);
+	Eigen::VectorXf e = optimizedPath->getPoint(startIndex+1);
 
 	// create intermediate path		
 	CSpacePathPtr intermediatePath = cspace->createPath(s, e);