RandomShortcutOptimizer.cpp
Go to the documentation of this file.
1 #include "Mobility.h"
2 #include "PathPlanner.h"
4 
5 #ifdef WIN32
6 #define random() rand()
7 #endif
8 
9 using namespace PathEngine;
10 
11 std::vector<Configuration> RandomShortcutOptimizer::optimize(const std::vector<Configuration> &path)
12 {
13  if (path.size() < 3) return path;
14 
15  Mobility *mobility = planner_->getMobility();
16  int nSegment = path.size()-1;
17  int index1 = ((float)random())/RAND_MAX*nSegment;
18  int index2;
19  do {
20  index2 = ((float)random())/RAND_MAX*nSegment;
21  }while(index1 == index2);
22  if (index2 < index1) std::swap(index1, index2);
23 
24  double ratio1 = ((double)random())/RAND_MAX;
25  double ratio2 = ((double)random())/RAND_MAX;
26  Configuration cfg1 = mobility->interpolate(path[index1], path[index1+1],
27  ratio1);
28  Configuration cfg2 = mobility->interpolate(path[index2], path[index2+1],
29  ratio2);
30  if (mobility->isReachable(cfg1, cfg2)){
31  std::vector<Configuration> optimized;
32  for (int i=0; i<=index1; i++) optimized.push_back(path[i]);
33  optimized.push_back(cfg1);
34  optimized.push_back(cfg2);
35  for (unsigned int i=index2+1; i<path.size(); i++) optimized.push_back(path[i]);
36  return optimized;
37  }else{
38  return path;
39  }
40 }
PathPlanner * planner_
Definition: Optimizer.h:37
png_uint_32 i
Definition: png.h:2735
移動アルゴリズム実装用抽象クラス
Definition: Mobility.h:20
Mobility * getMobility()
移動能力を取得する
std::vector< Configuration > optimize(const std::vector< Configuration > &path)
親クラスのドキュメントを参照
bool isReachable(Configuration &from, Configuration &to, bool checkCollision=true) const
fromからtoへ干渉なしに移動可能であるかどうか
Definition: Mobility.cpp:8
virtual Configuration interpolate(const Configuration &from, const Configuration &to, double ratio) const =0


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:40