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