9 std::vector<Configuration> optimized = path;
10 if (path.size() < 3)
return optimized;
13 unsigned int index = 1;
14 while(index != optimized.size() -1 ){
15 if (mobility->
isReachable(optimized[index-1], optimized[index+1])){
16 optimized.erase(optimized.begin()+index);