00001 //---------------------------------------------------------------------- 00002 // The Motion Strategy Library (MSL) 00003 //---------------------------------------------------------------------- 00004 // 00005 // Copyright (c) 1998-2000 Iowa State University and Steve LaValle. 00006 // All Rights Reserved. 00007 // 00008 // Permission to use, copy, and distribute this software and its 00009 // documentation is hereby granted free of charge, provided that 00010 // (1) it is not a component of a commercial product, and 00011 // (2) this notice appears in all copies of the software and 00012 // related documentation. 00013 // 00014 // Iowa State University and the author make no representations 00015 // about the suitability or fitness of this software for any purpose. 00016 // It is provided "as is" without express or implied warranty. 00017 //---------------------------------------------------------------------- 00018 00019 #ifndef MSL_RRT_H 00020 #define MSL_RRT_H 00021 00022 #include <LEDA/file.h> 00023 #include <LEDA/graph.h> 00024 #include <LEDA/list.h> 00025 #include <LEDA/polygon.h> 00026 #include <LEDA/vector.h> 00027 00028 #include "planner.h" 00029 00030 #ifdef USE_ANN 00031 #include <ANN/ANN.h> // ANN declarations 00032 #include "nn.h" 00033 #include "multiann.h" 00034 #endif 00035 00036 #include <LEDA/REDEFINE_NAMES.h> 00037 00045 00046 00047 class RRT: public Planner { 00048 protected: 00050 virtual vector SelectBestInput(const vector &x1, const vector &x2, 00051 vector &nx_best, bool &success, bool forward); 00052 00054 virtual vector SelectRandomInput(const vector &x1, const vector &x2, 00055 vector &nx_best, bool &success, 00056 bool forward); 00057 00059 virtual node NearestNeighbor(const vector &x, const GRAPH<vector,vector> &g, 00060 bool forward); 00061 00063 virtual bool Extend(const vector &x, GRAPH<vector,vector> &g, node &nn, 00064 bool forward); 00065 00067 virtual bool Connect(const vector &x, GRAPH<vector,vector> &g, node &nn, 00068 bool forward); 00069 00071 virtual bool Project(const vector &x, GRAPH<vector,vector> &g, node &nn, 00072 bool forward); 00073 00075 virtual vector ChooseState(); 00076 00078 list<node> PathToLeaf(const node &n, const GRAPH<vector,vector> &g); 00079 00080 // Keep track of timings 00081 node_map<double> GEdgeTime; 00082 node_map<double> G2EdgeTime; 00083 00084 public: 00085 00088 bool UseANN; 00089 00091 double GoalDist; 00092 00094 vector BestState; 00095 00097 double ConnectTimeLimit; 00098 00099 #ifdef USE_ANN 00100 MultiANN MAG; 00101 MultiANN MAG2; 00102 #endif 00103 00105 RRT(Problem *problem); 00106 00108 virtual ~RRT() {}; 00109 00111 int SatisfiedCount; 00112 00114 virtual void Reset(); 00115 00117 virtual void Construct(); 00118 00120 virtual bool Plan(); 00121 00122 }; 00123 00124 00130 00131 class RRTGoalBias: public RRT { 00132 protected: 00133 virtual vector ChooseState(); 00134 public: 00135 double GoalProb; 00136 RRTGoalBias(Problem *p); 00137 virtual ~RRTGoalBias() {}; 00138 }; 00139 00140 00145 00146 00147 class RRTGoalPull: public RRT { 00148 public: 00149 RRTGoalPull(Problem *p); 00150 virtual ~RRTGoalPull() {}; 00151 virtual void Construct(); 00152 }; 00153 00154 00163 00164 00165 class RRTCon: public RRTGoalBias { 00166 public: 00167 RRTCon(Problem *p); 00168 00170 00171 virtual ~RRTCon() {}; 00172 00175 00176 virtual void Construct(); 00177 00179 00180 virtual bool Plan(); 00181 }; 00182 00183 00184 00192 00193 00194 class RRTDual: public RRT { 00195 protected: 00196 void RecoverSolution(const node &n1, const node &n2); 00197 public: 00198 RRTDual(Problem *p); 00199 virtual ~RRTDual() {}; 00200 00201 00202 00206 00207 virtual bool Plan(); 00208 }; 00209 00210 00211 00224 00225 class RRTExtExt: public RRTDual { 00226 public: 00227 RRTExtExt(Problem *p); 00228 virtual ~RRTExtExt() {}; 00229 virtual bool Plan(); 00230 }; 00231 00232 00237 00238 class RRTGoalZoom: public RRT { 00239 protected: 00240 virtual vector ChooseState(); 00241 public: 00242 double GoalProb,ZoomProb,ZoomFactor; 00243 RRTGoalZoom(Problem *p); 00244 virtual ~RRTGoalZoom() {}; 00245 }; 00246 00247 00251 00252 class RRTPolar: public RRT { 00253 protected: 00254 virtual vector ChooseState(); 00255 virtual vector SelectBestInput(const vector &x1, const vector &x2, 00256 vector &nx_best, bool &success); 00257 public: 00258 double RadiusExp; 00259 RRTPolar(Problem *p); 00260 virtual ~RRTPolar() {}; 00261 }; 00262 00266 00267 class RRTHull: public RRT { 00268 protected: 00269 virtual vector ChooseState(); 00270 public: 00271 double Radius; 00272 RRTHull(Problem *p); 00273 virtual ~RRTHull() {}; 00274 }; 00275 00276 00293 00294 class RRTExtCon: public RRTDual { 00295 public: 00296 RRTExtCon(Problem *p); 00297 virtual ~RRTExtCon() {}; 00298 00300 virtual bool Plan(); 00301 }; 00302 00303 00304 00321 00322 00323 class RRTConCon: public RRTDual { 00324 public: 00325 RRTConCon(Problem *p); 00326 virtual ~RRTConCon() {}; 00327 00330 virtual bool Plan(); 00331 }; 00332 00333 00334 00338 00339 class RRTStar: public RRT { 00340 protected: 00341 virtual node NearestNeighbor(const vector &x, const GRAPH<vector,vector> &g, 00342 bool forward); 00343 virtual bool Extend(const vector &x, GRAPH<vector,vector> &g, node &nn, 00344 bool forward); 00345 node_map<int> NodeDepth; 00346 public: 00347 double OptimalityFactor; // Should in (0,1); negative numbers are interesting too 00348 RRTStar(Problem *p); 00349 virtual ~RRTStar() {}; 00350 }; 00351 00352 00353 00360 00361 class PlannerHsu: public RRT { 00362 protected: 00363 virtual bool Extend(const vector &x, GRAPH<vector,vector> &g, node &nn, 00364 bool forward); 00365 bool Connect(const vector &x1, const vector &x2); 00366 node SelectNode(const GRAPH<vector,vector> &g); 00367 vector ChooseNearbyState(const vector &x); 00368 void UpdateWeights(const node &nn, const GRAPH<vector,vector> &g); 00369 node_map<double> InverseWeights; 00370 double WeightScale; 00371 public: 00372 double NeighborFactor; // For sampling 00373 double NeighborRadius; // For weighting 00374 double StepSize; 00375 PlannerHsu(Problem *p); 00376 virtual ~PlannerHsu() {}; 00377 }; 00378 00379 00384 00385 class RandomTree: public RRT { 00386 protected: 00387 virtual node NearestNeighbor(const vector &x, const GRAPH<vector,vector> &g, 00388 bool forward); 00389 virtual vector SelectBestInput(const vector &x1, const vector &x2, 00390 vector &nx_best, bool &success, bool forward); 00391 public: 00392 RandomTree(Problem *p); 00393 virtual ~RandomTree() {}; 00394 }; 00395 00396 00397 #include <LEDA/UNDEFINE_NAMES.h> 00398 00399 #endif 00400 00401 00402 00403 00404 00405 00406 00407 00408 00409