RandomShortcutOptimizer.cpp
Go to the documentation of this file.
00001 #include "Mobility.h"
00002 #include "PathPlanner.h"
00003 #include "RandomShortcutOptimizer.h"
00004 
00005 #ifdef WIN32
00006 #define random() rand()
00007 #endif
00008 
00009 using namespace PathEngine;
00010 
00011 std::vector<Configuration> RandomShortcutOptimizer::optimize(const std::vector<Configuration> &path)
00012 {
00013     if (path.size() < 3) return path;
00014 
00015     Mobility *mobility = planner_->getMobility(); 
00016     int nSegment = path.size()-1;
00017     int index1 = ((float)random())/RAND_MAX*nSegment; 
00018     int index2;
00019     do {
00020         index2 = ((float)random())/RAND_MAX*nSegment; 
00021     }while(index1 == index2);
00022     if (index2 < index1) std::swap(index1, index2);
00023 
00024     double ratio1 = ((double)random())/RAND_MAX;
00025     double ratio2 = ((double)random())/RAND_MAX;
00026     Configuration cfg1 = mobility->interpolate(path[index1], path[index1+1],
00027                                                ratio1);
00028     Configuration cfg2 = mobility->interpolate(path[index2], path[index2+1],
00029                                                ratio2);
00030     if (mobility->isReachable(cfg1, cfg2)){
00031         std::vector<Configuration> optimized;
00032         for (int i=0; i<=index1; i++) optimized.push_back(path[i]);
00033         optimized.push_back(cfg1);
00034         optimized.push_back(cfg2);
00035         for (unsigned int i=index2+1; i<path.size(); i++) optimized.push_back(path[i]);
00036         return optimized;
00037     }else{
00038         return path;
00039     }
00040 }


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