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 <LEDA/graph_alg.h> 00023 00024 #include "prm.h" 00025 #include "defs.h" 00026 00027 #include <LEDA/REDEFINE_NAMES.h> 00028 00029 00030 // These are used for quasi-random sampling 00031 static int Primes[] = {2,3,5,7,11,13,17,19,23,29,31,37,41,43,47,53,59, 00032 61,67,71,73,79,83,89,97,101,103,107,109,113,127, 00033 131,137,139,149,151,157,163,167,173}; 00034 00035 00036 // ********************************************************************* 00037 // ********************************************************************* 00038 // CLASS: PRM base class 00039 // 00040 // ********************************************************************* 00041 // ********************************************************************* 00042 00043 PRM::PRM(Problem *problem): Planner(problem) { 00044 MaxEdgesPerNode = 20; 00045 MaxNeighbors = 20; 00046 00047 if (is_file(P->FilePath+"Radius")) { 00048 file_istream fin(P->FilePath + "Radius"); 00049 fin >> Radius; } 00050 else 00051 Radius = 20.0; 00052 00053 SatisfiedCount = 0; 00054 QuasiRandom = is_file(P->FilePath + "QuasiRandom"); 00055 00057 QuasiRandomHammersley = true; 00058 00059 if (!Holonomic) 00060 cout << "WARNING: Differential constraints will be ignored.\n"; 00061 } 00062 00063 00064 00065 00066 list<node> PRM::NeighboringNodes(const vector &x) { 00067 double d; 00068 node n; 00069 list<node> best_list; 00070 int k; 00071 00072 best_list.clear(); 00073 k = 0; 00074 forall_rev_nodes(n,G) { // Very important to use reverse here! 00075 d = P->Metric(G[n],x); 00076 if ((d < Radius)&&(k < MaxNeighbors)) { 00077 best_list.push(n); 00078 k++; // Count the number of attempted connections 00079 } 00080 } 00081 00082 //cout << "Number of Neighbors: " << best_list.length() << "\n"; 00083 00084 return best_list; 00085 } 00086 00087 00088 00089 00090 // This function essentially iterates Extend until it has to stop 00091 00092 bool PRM::Connect(const vector &x1, const vector &x2, vector &u_best) { 00093 double a; 00094 int k; 00095 vector x; 00096 00097 k = (int) (P->Metric(x1,x2) / StepSize); 00098 00099 //cout << "metric: " << P->Metric(x1,x2) << 00100 // " stepsize: " << StepSize << "\n"; 00101 00102 if (k == 0) 00103 return true; 00104 00105 for (a = 1.0/(k+1); a < 1.0; a += 1.0/(k+1) ) { 00106 x = P->InterpolateState(x1,x2,a); 00107 SatisfiedCount++; 00108 if (!P->Satisfied(x)) 00109 return false; 00110 } 00111 00112 return true; 00113 } 00114 00115 00116 00117 void PRM::Construct() 00118 { 00119 int i,c,k; 00120 vector u_best,nx; 00121 node n,nn; 00122 list<node> nhbrs; 00123 00124 // Set the step size 00125 StepSize = P->Metric(P->InitialState,P->Integrate(P->InitialState, 00126 P->GetInputs(P->InitialState).head(),PlannerDeltaT)); 00127 00128 i = 0; 00129 while (i < NumNodes) { 00130 nx = ChooseState(i,NumNodes,P->InitialState.dim()); 00131 SatisfiedCount++; 00132 i++; 00133 while (!P->Satisfied(nx)) { 00134 nx = ChooseState(i,NumNodes,P->InitialState.dim()); 00135 SatisfiedCount++; 00136 i++; 00137 // Keep trying until a good sample is found 00138 } 00139 nhbrs = NeighboringNodes(nx); 00140 nn = G.new_node(nx); 00141 00142 k = 0; 00143 while ((k < MaxEdgesPerNode)&&(nhbrs.length() > 0)) { 00144 n = nhbrs.pop(); 00145 if (Connect(G.inf(n),nx,u_best)) { 00146 G.new_edge(nn,n,u_best); 00147 G.new_edge(n,nn,u_best); 00148 k++; 00149 } 00150 } 00151 00152 if (G.number_of_nodes() % 1000 == 0) 00153 cout << G.number_of_nodes() << " nodes in the PRM.\n"; 00154 } 00155 00156 node_array<int> labels(G); 00157 c = COMPONENTS(G,labels); // Compute connected components 00158 00159 /* Show component sizes 00160 int count; 00161 for (i = 0; i < c; i++) { 00162 count = 0; 00163 forall_nodes(n,G) { 00164 if (labels[n] == i) 00165 count++; 00166 } 00167 cout << "Component " << i << " has " << count << " nodes.\n"; 00168 } 00169 */ 00170 00171 cout << "PRM Nodes: " << G.number_of_nodes() << 00172 " Edges: " << G.number_of_edges() << 00173 " ColDet: " << SatisfiedCount << 00174 " Components: " << c << "\n"; 00175 00176 } 00177 00178 00179 00180 bool PRM::Plan() 00181 { 00182 list<node> nhbrs; 00183 node n,ni,ng; 00184 vector u_best; 00185 bool success; 00186 list<node> path; 00187 00188 // Set the step size 00189 StepSize = P->Metric(P->InitialState,P->Integrate(P->InitialState, 00190 P->GetInputs(P->InitialState).head(),PlannerDeltaT)); 00191 00192 n = ni = ng = G.choose_node(); 00193 00194 // Connect to the initial state 00195 nhbrs = NeighboringNodes(P->InitialState); 00196 if (nhbrs.length() > 0) 00197 n = nhbrs.pop(); 00198 else { 00199 cout << "No neighboring nodes to the Initial State\n"; 00200 //cout << " FAilure\n"; 00201 return false; 00202 } 00203 00204 success = Connect(P->InitialState,G.inf(n),u_best); 00205 while((nhbrs.length() > 0)&& 00206 (!success)) { 00207 n = nhbrs.pop(); 00208 success = Connect(P->InitialState,G.inf(n),u_best); 00209 } 00210 00211 if (!success) { 00212 cout << "Failure to connect to Initial State\n"; 00213 //cout << " FaIlure\n"; 00214 return false; 00215 } 00216 //else 00217 // cout << "Connection to Initial State\n"; 00218 00219 ni = n; 00220 00221 // Connect to the goal state 00222 nhbrs = NeighboringNodes(P->GoalState); 00223 if (nhbrs.length() > 0) 00224 n = nhbrs.pop(); 00225 else { 00226 cout << "No neighboring nodes to the Goal State\n"; 00227 //cout << " FaiLure\n"; 00228 return false; 00229 } 00230 00231 success = Connect(P->GoalState,G.inf(n),u_best); 00232 while((nhbrs.length() > 0)&& 00233 (!success)) { 00234 n = nhbrs.pop(); 00235 success = Connect(P->GoalState,G.inf(n),u_best); 00236 } 00237 00238 if (!success) { 00239 cout << "Failure to connect to Goal State\n"; 00240 //cout << " FailUre\n"; 00241 return false; 00242 } 00243 //else 00244 // cout << "Connection to Goal State\n"; 00245 00246 ng = n; 00247 00248 // Search G for a path from ni to ng 00249 if (ni == ng) { 00250 cout << "Initial and Goal are in the same neighborhood.\n"; 00251 path.append(ni); 00252 path.append(ng); 00253 //cout << " Success\n"; 00254 RecordSolution(path); 00255 return true; 00256 } 00257 00258 // Perform the search 00259 00260 EdgeCost = edge_array<double>(G,1.0); 00261 CostToGo = node_array<double>(G); 00262 NextEdge = node_array<edge>(G); 00263 SHORTEST_PATH_T(G,ng,EdgeCost,CostToGo,NextEdge); 00264 00265 if ((CostToGo[ni] > 0)&&(ni != ng)) { 00266 //cout << "Cost to goal node: " << CostToGo[ni] << "\n"; 00267 00268 // Get the path 00269 n = ni; 00270 while (n != ng) { 00271 path.append(n); 00272 n = G.opposite(n,NextEdge[n]); 00273 } 00274 path.append(ng); 00275 } 00276 else { 00277 cout << " FailuRe\n"; 00278 return false; 00279 } 00280 00281 RecordSolution(path); 00282 cout << " Success\n"; 00283 00284 return true; 00285 } 00286 00287 00288 00289 vector PRM::ChooseState(int i, int maxnum, int dim) { 00290 if (QuasiRandom) { 00291 if (QuasiRandomHammersley) 00292 return QuasiRandomStateHammersley(i,maxnum,dim); 00293 else 00294 return QuasiRandomStateHalton(i,dim); 00295 } 00296 else 00297 return RandomState(); 00298 } 00299 00300 00301 00302 00303 vector PRM::QuasiRandomStateHammersley(int i, int maxnum, int dim) { 00304 int j,k,r,ppow; 00305 vector qrx; 00306 00307 if (dim > 30) { 00308 cout << "ERROR: Dimension too high for quasi-random samples\n"; 00309 exit(-1); 00310 } 00311 00312 qrx = vector(dim); 00313 00314 k = i; 00315 00316 qrx[0] = ((double) k / maxnum) * (P->UpperState[0] - P->LowerState[0]); 00317 qrx[0] += P->LowerState[0]; 00318 for (j = 1; j < dim; j++) { 00319 qrx[j] = 0.0; 00320 ppow = Primes[j-1]; 00321 k = i; 00322 while (k != 0) { 00323 r = k % Primes[j-1]; 00324 k = (int) (k / Primes[j-1]); 00325 qrx[j] += (double) r / ppow; 00326 ppow *= Primes[j-1]; 00327 } 00328 qrx[j] *= (P->UpperState[j] - P->LowerState[j]); 00329 qrx[j] += P->LowerState[j]; 00330 } 00331 00332 return qrx; 00333 } 00334 00335 00336 vector PRM::QuasiRandomStateHalton(int i, int dim) { 00337 int j,k,r,ppow; 00338 vector qrx; 00339 00340 if (dim > 30) { 00341 cout << "ERROR: Dimension too high for quasi-random samples\n"; 00342 exit(-1); 00343 } 00344 00345 qrx = vector(dim); 00346 k = i; 00347 00348 for (j = 0; j < dim; j++) { 00349 qrx[j] = 0.0; 00350 ppow = Primes[j]; 00351 k = i; 00352 while (k != 0) { 00353 r = k % Primes[j]; 00354 k = (int) (k / Primes[j]); 00355 qrx[j] += (double) r / ppow; 00356 ppow *= Primes[j]; 00357 } 00358 qrx[j] *= (P->UpperState[j] - P->LowerState[j]); 00359 qrx[j] += P->LowerState[j]; 00360 } 00361 00362 return qrx; 00363 } 00364