ShortcutOptimizer.cpp
Go to the documentation of this file.
1 #include "Mobility.h"
2 #include "PathPlanner.h"
3 #include "ShortcutOptimizer.h"
4 
5 using namespace PathEngine;
6 
7 std::vector<Configuration> ShortcutOptimizer::optimize(const std::vector<Configuration> &path)
8 {
9  std::vector<Configuration> optimized = path;
10  if (path.size() < 3) return optimized;
11 
12  Mobility *mobility = planner_->getMobility();
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);
17  }else{
18  index++;
19  }
20  }
21  return optimized;
22 }
PathPlanner * planner_
Definition: Optimizer.h:37
移動アルゴリズム実装用抽象クラス
Definition: Mobility.h:20
Mobility * getMobility()
移動能力を取得する
list index
bool isReachable(Configuration &from, Configuration &to, bool checkCollision=true) const
fromからtoへ干渉なしに移動可能であるかどうか
Definition: Mobility.cpp:8
std::vector< Configuration > optimize(const std::vector< Configuration > &path)
親クラスのドキュメントを参照


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05