Mobility.cpp
Go to the documentation of this file.
1 #include "PathPlanner.h"
2 #include "Mobility.h"
3 
4 using namespace PathEngine;
5 
7 
9  bool checkCollision) const
10 {
11  std::vector<Configuration> path;
12  if (!getPath(from, to, path)) return false;
13 #if 0
14  std::cout << "isReachable(" << from << ", " << to << ")" << std::endl;
15  for (unsigned int i = 0; i<path.size(); i++){
16  std::cout << i << ":" << path[i] << std::endl;
17  }
18 #endif
19  if (checkCollision){
20  return !planner_->checkCollision(path);
21  }else{
22  return true;
23  }
24 }
25 
27  std::vector<Configuration>& o_path) const
28 {
29  o_path.push_back(from);
30 
31  unsigned int n = (unsigned int)(distance(from, to)/interpolationDistance())+1;
33  for (unsigned int i=1; i<n; i++){
34  pos = interpolate(from, to, ((double)i)/n);
35  o_path.push_back(pos);
36  }
37  o_path.push_back(to);
38 
39  return true;
40 }
41 
42 
43 
unsigned int size()
get the number of degrees of freedom
virtual double distance(const Configuration &from, const Configuration &to) const =0
static double interpolationDistance()
補間時の隣接する2点間の最大距離を取得する
Definition: Mobility.h:90
static double interpolationDistance_
補間時の隣接する2点間の最大距離
Definition: Mobility.h:95
ConfigurationSpace * getConfigurationSpace()
コンフィギュレーション空間設定を取得する
bool checkCollision()
現在の状態で干渉検出を行う
PathPlanner * planner_
計画経路エンジン
Definition: Mobility.h:28
png_uint_32 i
Definition: png.h:2735
virtual bool getPath(Configuration &from, Configuration &to, std::vector< Configuration > &o_path) const
開始位置から目標地点への移動を補間して生成された姿勢列を取得する。
Definition: Mobility.cpp:26
bool isReachable(Configuration &from, Configuration &to, bool checkCollision=true) const
fromからtoへ干渉なしに移動可能であるかどうか
Definition: Mobility.cpp:8
path
typedef int
Definition: png.h:1113
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 Apr 13 2019 02:14:24