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 #include <math.h> 00020 #include <stdio.h> 00021 00022 #include "fdp.h" 00023 #include "defs.h" 00024 00025 // This is needed because g++ does not support ANSI export keyword! 00026 #include "marray.C" 00027 00028 00029 // ********************************************************************* 00030 // ********************************************************************* 00031 // CLASS: FDP base class 00032 // 00033 // ********************************************************************* 00034 // ********************************************************************* 00035 00036 FDP::FDP(Problem *problem): IncrementalPlanner(problem) { 00037 00038 GridDefaultResolution = 50; 00039 Reset(); 00040 } 00041 00042 00043 00044 void FDP::Reset() { 00045 int i,dim; 00046 bool done; 00047 00048 IncrementalPlanner::Reset(); 00049 00050 SatisfiedCount = 0; 00051 NumNodes = 20000; 00052 00053 GridDimensions = vector<int>(P->StateDim); 00054 for (i = 0; i < P->StateDim; i++) 00055 GridDimensions[i] = GridDefaultResolution; 00056 if (is_file(P->FilePath + "GridDimensions")) { 00057 ifstream fin((P->FilePath + "GridDimensions").c_str()); 00058 for (i = 0; i < P->StateDim; i++) { 00059 fin >> dim; 00060 GridDimensions[i] = dim; 00061 } 00062 fin.close(); 00063 } 00064 00065 Grid = new MultiArray<int>(GridDimensions, 0); 00066 00067 Quantization = MSLVector(P->StateDim); 00068 00069 for (i = 0; i < P->StateDim; i++) { 00070 Quantization[i] = (P->UpperState[i] - P->LowerState[i])/ 00071 (GridDimensions[i]); 00072 } 00073 00074 if (!is_file(P->FilePath+"GapError")) { 00075 for (i = 0; i < P->StateDim; i++) 00076 GapError[i] = Quantization[i] / 2.0; 00077 } 00078 00079 // Mark the collision region 00080 vector<int> indices(P->StateDim); 00081 for (i = 0; i < P->StateDim; i++) 00082 indices[i] = 0; 00083 00084 // Loop through all of the indices and check each for collision 00085 cout << "Performing collision detection to initialize grid.\n"; 00086 done = false; 00087 while (!done) { 00088 (*Grid)[indices] = 00089 (P->Satisfied(IndicesToState(indices))) ? UNVISITED : COLLISION; 00090 done = Grid->Increment(indices); // This modifies indices 00091 } 00092 cout << "Finished.\n"; 00093 } 00094 00095 00096 00097 bool FDP::Plan() 00098 { 00099 int i; 00100 MSLNode *n,*nn; 00101 MSLVector nx,x; 00102 double ptime; 00103 list<MSLNode*> path; 00104 double cost; 00105 vector<int> indices; 00106 list<MSLVector>::iterator u; 00107 list<MSLNode*>::iterator ni; 00108 list<MSLVector> ulist; 00109 00110 // Make the root node of G 00111 if (!T) { 00112 T = new MSLTree(P->InitialState); 00113 T->Root()->SetCost(0.0); 00114 Q.push(T->Root()); // Add the root node to the queue 00115 } 00116 00117 i = 0; 00118 while ((i < NumNodes)&& 00119 (!Q.empty())) { 00120 00121 // Remove the element with smallest cost 00122 n = Q.top(); 00123 cost = n->Cost(); 00124 Q.pop(); 00125 x = n->State(); 00126 00127 // Try all inputs 00128 ulist = P->GetInputs(x); 00129 forall(u,ulist) { 00130 nx = P->Integrate(x,*u,PlannerDeltaT); 00131 indices = StateToIndices(nx); 00132 // If we are visiting an UNVISITED place... 00133 if ((*Grid)[indices] == UNVISITED) { 00134 (*Grid)[indices] = VISITED; 00135 // Make a new node and edge 00136 nn = T->Extend(n,nx,*u,PlannerDeltaT); // Make a new node with state nx 00137 nn->SetCost(SearchCost(cost,n,nn)); 00138 Q.push(nn); // Put it into the priority queue 00139 //cout << "New node: " << nn->State() << " " << nn->Cost() << "\n"; 00140 00141 // Check if goal reached 00142 if (GapSatisfied(nn->State(),P->GoalState)) { 00143 cout << "Successful Path Found\n"; 00144 path = T->PathToRoot(nn); path.reverse(); 00145 // Make the correct times 00146 ptime = 0.0; TimeList.clear(); 00147 forall(ni,path) { 00148 ptime += (*ni)->Time(); 00149 TimeList.push_back(ptime); 00150 } 00151 00152 RecordSolution(path); // Write to Path and Policy 00153 return true; 00154 } 00155 } 00156 } 00157 00158 i++; 00159 } 00160 00161 cout << "Failure to find a path\n"; 00162 return false; 00163 } 00164 00165 00166 double FDP::SearchCost(double initcost, MSLNode* &n, MSLNode* &nn) { 00167 return initcost + PlannerDeltaT; 00168 } 00169 00170 00171 00172 00173 vector<int> FDP::StateToIndices(const MSLVector &x) { 00174 int i; 00175 00176 vector<int> indices(P->StateDim); 00177 00178 for (i = 0; i < P->StateDim; i++) { 00179 indices[i] = (int) ((x[i] - P->LowerState[i])/Quantization[i]); 00180 // These are needed because the state is sometimes out of bounds. 00181 // The other way to fix this is to ensure that every instance of 00182 // Integrate in Model and subclasses stays within bounds. 00183 if (indices[i] < 0) 00184 indices[i] = 0; 00185 if (indices[i] >= GridDimensions[i]) 00186 indices[i] = GridDimensions[i] - 1; 00187 } 00188 00189 return indices; 00190 } 00191 00192 00193 MSLVector FDP::IndicesToState(const vector<int> &indices) { 00194 int i; 00195 00196 MSLVector x(P->StateDim); 00197 for (i = 0; i < P->StateDim; i++) { 00198 x[i] = Quantization[i]*(indices[i]+0.5) + P->LowerState[i]; 00199 } 00200 00201 return x; 00202 } 00203 00204 00205 // ********************************************************************* 00206 // ********************************************************************* 00207 // CLASS: FDPStar 00208 // 00209 // ********************************************************************* 00210 // ********************************************************************* 00211 00212 FDPStar::FDPStar(Problem *problem): FDP(problem) { 00213 } 00214 00215 00216 // The cost always represents the cost to come using the Metric 00217 // for each step, plus the cost to go from nn to the goal. 00218 double FDPStar::SearchCost(double initcost, MSLNode* &n, MSLNode* &nn) { 00219 return initcost - 00220 P->Metric(n->State(),P->GoalState) + 00221 P->Metric(n->State(),nn->State()) + 00222 P->Metric(nn->State(),P->GoalState); 00223 } 00224 00225 00226 00227 // ********************************************************************* 00228 // ********************************************************************* 00229 // CLASS: FDPBestFirst 00230 // 00231 // ********************************************************************* 00232 // ********************************************************************* 00233 00234 FDPBestFirst::FDPBestFirst(Problem *problem): FDP(problem) { 00235 } 00236 00237 00238 double FDPBestFirst::SearchCost(double initcost, MSLNode* &n, 00239 MSLNode* &nn) { 00240 return P->Metric(n->State(),P->GoalState); 00241 } 00242 00243 00244 00245 00246 00247 // ********************************************************************* 00248 // ********************************************************************* 00249 // CLASS: FDPBi 00250 // 00251 // ********************************************************************* 00252 // ********************************************************************* 00253 00254 FDPBi::FDPBi(Problem *problem): FDP(problem) { 00255 } 00256 00257 00258 00259 void FDPBi::Reset() { 00260 FDP::Reset(); 00261 } 00262 00263 00264 00265 bool FDPBi::Plan() 00266 { 00267 int i,k; 00268 MSLNode *n,*nn,*nn2; 00269 MSLVector nx,x; 00270 double ptime; 00271 list<MSLNode*> path,nlist; 00272 double cost; 00273 vector<int> indices,indices2; 00274 bool match; 00275 list<MSLVector>::iterator u; 00276 list<MSLNode*>::iterator ni; 00277 list<MSLVector> ulist; 00278 00279 // Make the root node of T 00280 if (!T) { 00281 T = new MSLTree(P->InitialState); 00282 T->Root()->SetCost(0.0); 00283 Q.push(T->Root()); // Add the root node to the queue 00284 } 00285 00286 // Make the root node of T2 00287 if (!T2) { 00288 T2 = new MSLTree(P->GoalState); 00289 T2->Root()->SetCost(0.0); 00290 Q2.push(T2->Root()); // Add the root node to the queue 00291 } 00292 00293 i = 0; 00294 while ((i < NumNodes)&& 00295 (!Q.empty())&& 00296 (!Q2.empty())) { 00297 00298 // ******** Handle the tree from the initial state ************* 00299 // Remove the element with smallest cost 00300 n = Q.top(); 00301 cost = n->Cost(); 00302 Q.pop(); 00303 x = n->State(); 00304 00305 // Try all inputs 00306 ulist = P->GetInputs(x); 00307 forall(u,ulist) { 00308 nx = P->Integrate(x,*u,PlannerDeltaT); 00309 indices = StateToIndices(nx); 00310 00311 // If we are visiting a place visited by T2... 00312 if ((*Grid)[indices] == VISITED2) { 00313 // Make a new node and edge 00314 nn = T->Extend(n,nx,*u,PlannerDeltaT); // Make a new node with state nx 00315 nn->SetCost(SearchCost(cost,n,nn)); 00316 00317 // Get the node in T2 that was visited 00318 nlist = T2->Nodes(); 00319 forall(ni,nlist) { 00320 indices2 = StateToIndices((*ni)->State()); 00321 match = true; 00322 for (k = 0; k < P->StateDim; k++) 00323 if (indices[k] != indices2[k]) 00324 match = false; 00325 if (match) 00326 nn2 = (*ni); 00327 } 00328 RecoverSolution(nn,nn2); 00329 cout << "Successful Path Found\n"; 00330 return true; 00331 } 00332 00333 // If we are visiting an UNVISITED place... 00334 if ((*Grid)[indices] == UNVISITED) { 00335 (*Grid)[indices] = VISITED; 00336 // Make a new node and edge 00337 nn = T->Extend(n,nx,*u,PlannerDeltaT); // Make a new node with state nx 00338 nn->SetCost(SearchCost(cost,n,nn)); 00339 Q.push(nn); // Put it into the priority queue 00340 00341 // Check if goal reached 00342 if (GapSatisfied(nn->State(),P->GoalState)) { 00343 cout << "Successful Path Found\n"; 00344 path = T->PathToRoot(nn); path.reverse(); 00345 // Make the correct times 00346 ptime = 0.0; TimeList.clear(); 00347 forall(ni,path) { 00348 ptime += (*ni)->Time(); 00349 TimeList.push_back(ptime); 00350 } 00351 00352 RecordSolution(path); // Write to Path and Policy 00353 return true; 00354 } 00355 } 00356 } 00357 00358 // ******** Handle the tree from the goal state ************* 00359 // Remove the element with smallest cost 00360 n = Q2.top(); 00361 cost = n->Cost(); 00362 Q2.pop(); 00363 x = n->State(); 00364 00365 // Try all inputs 00366 ulist = P->GetInputs(x); 00367 forall(u,ulist) { 00368 nx = P->Integrate(x,*u,-PlannerDeltaT); // Reverse time integration 00369 indices = StateToIndices(nx); 00370 00371 // If we are visiting a place visited by T... 00372 if ((*Grid)[indices] == VISITED) { 00373 // Make a new node and edge 00374 nn = T2->Extend(n,nx,*u,PlannerDeltaT); // Make new node with state nx 00375 nn->SetCost(SearchCost(cost,n,nn)); 00376 00377 // Get the node in T that was visited 00378 nlist = T->Nodes(); 00379 forall(ni,nlist) { 00380 indices2 = StateToIndices((*ni)->State()); 00381 match = true; 00382 for (k = 0; k < P->StateDim; k++) 00383 if (indices[k] != indices2[k]) 00384 match = false; 00385 if (match) 00386 nn2 = (*ni); 00387 } 00388 RecoverSolution(nn2,nn); 00389 cout << "Successful Path Found\n"; 00390 return true; 00391 } 00392 00393 // If we are visiting an UNVISITED place... 00394 if ((*Grid)[indices] == UNVISITED) { 00395 (*Grid)[indices] = VISITED2; 00396 // Make a new node and edge 00397 nn = T2->Extend(n,nx,*u,PlannerDeltaT); // Make new node with state nx 00398 nn->SetCost(SearchCost(cost,n,nn)); 00399 Q2.push(nn); // Put it into the priority queue 00400 00401 // Check if initial reached 00402 if (GapSatisfied(nn->State(),P->InitialState)) { 00403 cout << "Successful Path Found\n"; 00404 path = T2->PathToRoot(nn); path.reverse(); 00405 // Make the correct times 00406 ptime = 0.0; TimeList.clear(); 00407 forall(ni,path) { 00408 ptime += (*ni)->Time(); 00409 TimeList.push_back(ptime); 00410 } 00411 00412 RecordSolution(path); // Write to Path and Policy 00413 return true; 00414 } 00415 } 00416 } 00417 00418 i++; 00419 } 00420 00421 cout << "Failure to find a path\n"; 00422 return false; 00423 } 00424 00425 00426 // This is duplicated from RRTDual 00427 void FDPBi::RecoverSolution(MSLNode* &n1, MSLNode* &n2) { 00428 list<MSLNode*> path,path2; 00429 double ptime; 00430 list<MSLNode*>::iterator n; 00431 00432 path = T->PathToRoot(n1); path.reverse(); 00433 // Figure out forward timings 00434 ptime = 0.0; TimeList.clear(); 00435 forall(n,path) { 00436 ptime += (*n)->Time(); 00437 TimeList.push_back(ptime); 00438 } 00439 00440 path2 = T2->PathToRoot(n2); 00441 // Figure out backwards timings 00442 ptime += PlannerDeltaT; // Add a time step for the gap 00443 forall(n,path2) { // Compute total time in T2 part of path 00444 TimeList.push_back(ptime); 00445 ptime += (*n)->Time(); 00446 } 00447 00448 //cout << "Path: " << path; 00449 //cout << "Path2: " << path2; 00450 //cout << "\n TimeList: " << TimeList << " L: " << TimeList.length(); 00451 00452 RecordSolution(path,path2); // Write to Path and Policy 00453 } 00454 00455 00456 00457 00458 00459 00460 00461 00462 00463