RRT.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; tab-width: 4; -*-
2 #include "Roadmap.h"
3 #include "RoadmapNode.h"
4 #include "ConfigurationSpace.h"
5 #include "RRT.h"
6 
7 using namespace PathEngine;
8 
9 static bool debug=false;
10 //static bool debug=true;
11 
12 RRT::RRT(PathPlanner* plan) : Algorithm(plan), extraConnectionCheckFunc_(NULL)
13 {
14  // set default properties
15  properties_["max-trials"] = "10000";
16  properties_["eps"] = "0.1";
17 
18  Tstart_ = Ta_ = roadmap_;
21 
22  extendFromStart_ = true;
23  extendFromGoal_ = false;
24 
26  ignoreCollisionAtGoal_ = false;
27 }
28 
30 {
31 }
32 
33 int RRT::extend(RoadmapPtr tree, Configuration& qRand, bool reverse) {
34  if (debug) std::cout << "RRT::extend("<< qRand << ", " << reverse << ")"
35  << std::endl;
36  TlastExtended_ = tree;
37 
38  RoadmapNodePtr minNode;
39  double min;
40  tree->findNearestNode(qRand, minNode, min);
41  if (debug) std::cout << "nearest : pos = (" << minNode->position()
42  << "), d = " << min << std::endl;
43 
44  if (minNode != NULL) {
45  Mobility* mobility = planner_->getMobility();
46 
47  if (min > eps_){
48  Configuration qRandOrg = qRand;
49  qRand = mobility->interpolate(minNode->position(), qRand, eps_/min);
50  if (debug) std::cout << "qRand = (" << qRand << ")" << std::endl;
51  if (mobility->distance(minNode->position(), qRand) > min){
52  std::cout << "distance didn't decrease" << std::endl;
53  std::cout << "qRandOrg : (" << qRandOrg << "), d = " << min
54  << std::endl;
55  std::cout << "qRand : (" << qRand << "), d = "
56  << mobility->distance(minNode->position(), qRand)
57  << std::endl;
58  getchar();
59  }
60  }
61 
62  if (planner_->checkCollision(qRand)) return Trapped;
63 
64  if (reverse){
65  if (mobility->isReachable(qRand, minNode->position())){
66  RoadmapNodePtr newNode = RoadmapNodePtr(new RoadmapNode(qRand));
67  tree->addNode(newNode);
68  tree->addEdge(newNode, minNode);
69  if (min <= eps_) {
70  if (debug) std::cout << "reached(" << qRand << ")"<< std::endl;
71  return Reached;
72  }
73  else {
74  if (debug) std::cout << "advanced(" << qRand << ")" << std::endl;
75  return Advanced;
76  }
77  } else {
78  if (debug) std::cout << "trapped" << std::endl;
79  return Trapped;
80  }
81  }else{
82  if (mobility->isReachable(minNode->position(), qRand)){
83  RoadmapNodePtr newNode = RoadmapNodePtr(new RoadmapNode(qRand));
84  tree->addNode(newNode);
85  tree->addEdge(minNode, newNode);
86  if (min <= eps_) {
87  if (debug) std::cout << "reached(" << qRand << ")"<< std::endl;
88  return Reached;
89  }
90  else {
91  if (debug) std::cout << "advanced(" << qRand << ")" << std::endl;
92  return Advanced;
93  }
94  } else {
95  if (debug) std::cout << "trapped" << std::endl;
96  return Trapped;
97  }
98  }
99  }
100 
101  return Trapped;
102 }
103 
104 int RRT::connect(RoadmapPtr tree,const Configuration &qNew, bool reverse) {
105  if (debug) std::cout << "RRT::connect(" << qNew << ")" << std::endl;
106 
107  int ret = Reached;
108  Configuration q = qNew;
109 
110  do {
111  ret = extend(tree, q, reverse);
112  q = qNew;
113  } while (ret == Advanced);
114  return ret;
115 }
116 
119 }
120 
121 void RRT::extractPath(std::vector<Configuration>& o_path) {
122  //std::cout << "RRT::path" << std::endl;
123  RoadmapNodePtr startMidNode = Tstart_->lastAddedNode();
124  RoadmapNodePtr goalMidNode = Tgoal_ ->lastAddedNode();
125 
126  o_path.clear();
127  if (!startMidNode || !goalMidNode) return;
128 
129  RoadmapNodePtr node;
130 
131  if (extendFromStart_){
132  node = startMidNode;
133  do {
134  o_path.insert(o_path.begin(), node->position());
135  node = node->parent(0);
136  } while (node != NULL);
137  }
138 
139  if (extendFromGoal_){
140  // note: If trees are extend from both of start and goal,
141  // goalMidNode == startMidNode.
142  node = extendFromStart_ ? goalMidNode->child(0) : goalMidNode;
143  do {
144  o_path.push_back(node->position());
145  node = node->child(0);
146  } while (node != NULL);
147  }
148 #if 0
149  startMidNode->children_.push_back(goalMidNode);
150  goalMidNode->parent_ = startMidNode;
151 #endif
152 }
153 
155 {
158 
159  if (extend(Ta_, qNew, Tb_ == Tstart_) != Trapped) {
160  if (connect(Tb_, qNew, Ta_ == Tstart_) == Reached) {
162  return extraConnectionCheckFunc_(Tstart_->lastAddedNode()->position());
163  }else{
164  return true;
165  }
166  }
167  }
168  swapTrees();
169  }else if (extendFromStart_ && !extendFromGoal_){
170  if (extend(Tstart_, qNew) != Trapped) {
171  Configuration p = goal_;
172  int ret;
173  do {
174  ret = extend(Tstart_, p);
175  p = goal_;
176  }while (ret == Advanced);
177  if (ret == Reached) return true;
178  }
179  }else if (!extendFromStart_ && extendFromGoal_){
180  std::cout << "this case is not implemented" << std::endl;
181  return false;
182  }
183  return false;
184 }
185 
187 {
188  if (verbose_) std::cout << "RRT::calcPath" << std::endl;
189 
190  // 回数
191  times_ = atoi(properties_["max-trials"].c_str());
192 
193  // eps
194  eps_ = atof(properties_["eps"].c_str());
195 
196  if (verbose_){
197  std::cout << "times:" << times_ << std::endl;
198  std::cout << "eps:" << eps_ << std::endl;
199  }
200 
203 
204  Tstart_->addNode(startNode);
205  Tgoal_ ->addNode(goalNode);
206 
207  bool isSucceed = false;
208 
209  for (int i=0; i<times_; i++) {
210  //if (!isRunning_) break;
211  if (verbose_){
212  printf("%5d/%5dtrials : %5d/%5dnodes\r", i+1, times_, Tstart_->nNodes(),Tgoal_->nNodes());
213  fflush(stdout);
214  }
215  if ((isSucceed = extendOneStep())) break;
216  }
217 
218  extractPath();
219  Tgoal_->integrate(Tstart_);
220 
221  if (verbose_) {
222  std::cout << std::endl << "fin.(calcPath), retval = " << isSucceed << std::endl;
223  }
224  return isSucceed;
225 }
226 
228 {
229  RoadmapPtr tmp = Ta_;
230  Ta_ = Tb_;
231  Tb_ = tmp;
232 }
233 
235  Tstart_ = Ta_ = tree;
236 }
237 
239  Tgoal_ = Tb_ = tree;
240 }
241 
243 {
244  extraConnectionCheckFunc_ = i_func;
245 }
int times_
Definition: RRT.h:84
~RRT()
デストラクタ
Definition: RRT.cpp:29
static bool debug
Definition: RRT.cpp:9
virtual double distance(const Configuration &from, const Configuration &to) const =0
Configuration random()
generate random position
干渉のため、近づくことができなかった
Definition: RRT.h:32
ロードマップのノード
Definition: RoadmapNode.h:17
RoadmapPtr roadmap_
ロードマップ
Definition: Algorithm.h:71
RoadmapPtr Tb_
Definition: RRT.h:54
ConfigurationSpace * getConfigurationSpace()
コンフィギュレーション空間設定を取得する
Configuration goal_
終了位置
Definition: Algorithm.h:37
static int min(int a, int b)
bool checkCollision()
現在の状態で干渉検出を行う
bool extendFromGoal_
Definition: RRT.h:94
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
RoadmapPtr Tstart_
RRT-connect において、初期位置からのツリー
Definition: RRT.h:39
png_uint_32 i
Definition: png.h:2735
RoadmapPtr Tgoal_
RRT-connect において、終了位置からのツリー
Definition: RRT.h:44
移動アルゴリズム実装用抽象クラス
Definition: Mobility.h:20
extraConnectionCheckFunc extraConnectionCheckFunc_
Definition: RRT.h:99
bool verbose_
デバッグ出力の制御
Definition: Algorithm.h:76
std::map< std::string, std::string > properties_
プロパティ
Definition: Algorithm.h:44
int connect(RoadmapPtr tree, const Configuration &qNew, bool reverse=false)
RRT-connect の connect 関数。伸ばせなくなるまで extend する
Definition: RRT.cpp:104
void extractPath()
計画した経路を抽出し、path_にセットする
Definition: RRT.cpp:117
Mobility * getMobility()
移動能力を取得する
int extend(RoadmapPtr tree, Configuration &qRand, bool reverse=false)
ランダムな点に向かってツリーを伸ばす。
Definition: RRT.cpp:33
boost::shared_ptr< RoadmapNode > RoadmapNodePtr
Definition: RoadmapNode.h:11
void setBackwardTree(RoadmapPtr tree)
ゴールからのツリーを設定する
Definition: RRT.cpp:238
RoadmapPtr TlastExtended_
a tree extened in the last phase
Definition: RRT.h:49
ロードマップ
Definition: Roadmap.h:19
RRT(PathPlanner *planner)
コンストラクタ
Definition: RRT.cpp:12
std::vector< Configuration > path_
計画された経路
Definition: Algorithm.h:58
bool ignoreCollisionAtStart_
Definition: Algorithm.h:81
指定の位置に近づいた
Definition: RRT.h:31
boost::function1< bool, const Configuration & > extraConnectionCheckFunc
Definition: RRT.h:24
bool extendFromStart_
Definition: RRT.h:89
void swapTrees()
Definition: RRT.cpp:227
経路計画アルゴリズム基底クラス
Definition: Algorithm.h:23
RoadmapPtr Ta_
ツリーの交換のために用いる変数。どちらか一方がTstart_をもう一方がTgoal_を指す
Definition: RRT.h:54
bool isReachable(Configuration &from, Configuration &to, bool checkCollision=true) const
fromからtoへ干渉なしに移動可能であるかどうか
Definition: Mobility.cpp:8
double eps_
Definition: RRT.h:79
PathPlanner * planner_
計画経路エンジン
Definition: Algorithm.h:66
void setExtraConnectionCheckFunc(extraConnectionCheckFunc i_func)
スタートからのツリーとゴールからのツリーが接続できたかの追加チェックを行うユーザ関数を設定する ...
Definition: RRT.cpp:242
指定の位置に到達した
Definition: RRT.h:33
boost::shared_ptr< Roadmap > RoadmapPtr
Definition: Roadmap.h:13
void setForwardTree(RoadmapPtr tree)
スタートからのツリーを設定する
Definition: RRT.cpp:234
bool calcPath()
親クラスのドキュメントを参照
Definition: RRT.cpp:186
Configuration start_
開始位置
Definition: Algorithm.h:30
bool extendOneStep()
ツリーを伸ばす処理を1回だけ行う
Definition: RRT.cpp:154
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 Thu Sep 8 2022 02:24:05