ShortcutOptimizer.cpp
Go to the documentation of this file.
00001 #include "Mobility.h"
00002 #include "PathPlanner.h"
00003 #include "ShortcutOptimizer.h"
00004 
00005 using namespace PathEngine;
00006 
00007 std::vector<Configuration> ShortcutOptimizer::optimize(const std::vector<Configuration> &path)
00008 {
00009   std::vector<Configuration> optimized = path;
00010   if (path.size() < 3) return optimized;
00011 
00012   Mobility *mobility = planner_->getMobility(); 
00013   unsigned int index = 1;
00014   while(index != optimized.size() -1 ){
00015     if (mobility->isReachable(optimized[index-1], optimized[index+1])){
00016       optimized.erase(optimized.begin()+index); 
00017     }else{
00018       index++;
00019     }
00020   }
00021   return optimized;
00022 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19