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 "prm.h" 00023 #include "defs.h" 00024 00025 00026 // These are used for quasi-random sampling 00027 static int Primes[] = {2,3,5,7,11,13,17,19,23,29,31,37,41,43,47,53,59, 00028 61,67,71,73,79,83,89,97,101,103,107,109,113,127, 00029 131,137,139,149,151,157,163,167,173}; 00030 00031 00032 // ********************************************************************* 00033 // ********************************************************************* 00034 // CLASS: PRM base class 00035 // 00036 // ********************************************************************* 00037 // ********************************************************************* 00038 00039 PRM::PRM(Problem *problem): RoadmapPlanner(problem) { 00040 MaxEdgesPerVertex = 20; 00041 MaxNeighbors = 20; 00042 00043 READ_PARAMETER_OR_DEFAULT(Radius,20.0); 00044 00045 SatisfiedCount = 0; 00046 QuasiRandom = is_file(P->FilePath + "QuasiRandom"); 00047 00049 QuasiRandomHammersley = true; 00050 00051 if (!Holonomic) 00052 cout << "WARNING: Differential constraints will be ignored.\n"; 00053 } 00054 00055 00056 00057 00058 list<MSLVertex*> PRM::NeighboringVertices(const MSLVector &x) { 00059 double d; 00060 MSLVertex n; 00061 list<MSLVertex*> best_list,all_vertices; 00062 list<MSLVertex*>::iterator vi; 00063 int k; 00064 00065 all_vertices = Roadmap->Vertices(); 00066 00067 best_list.clear(); 00068 k = 0; 00069 forall(vi,all_vertices) { 00070 d = P->Metric((*vi)->State(),x); 00071 if ((d < Radius)&&(k < MaxNeighbors)) { 00072 best_list.push_back(*vi); 00073 k++; // Count the number of attempted connections 00074 } 00075 } 00076 00077 //cout << "Number of Neighbors: " << best_list.size() << "\n"; 00078 00079 return best_list; 00080 } 00081 00082 00083 00084 00085 // This function essentially iterates Extend until it has to stop 00086 00087 bool PRM::Connect(const MSLVector &x1, const MSLVector &x2, MSLVector &u_best) { 00088 double a; 00089 int k; 00090 MSLVector x; 00091 00092 k = (int) (P->Metric(x1,x2) / StepSize); 00093 00094 //cout << "metric: " << P->Metric(x1,x2) << 00095 // " stepsize: " << StepSize << "\n"; 00096 00097 if (k == 0) 00098 return true; 00099 00100 for (a = 1.0/(k+1); a < 1.0; a += 1.0/(k+1) ) { 00101 x = P->InterpolateState(x1,x2,a); 00102 SatisfiedCount++; 00103 if (!P->Satisfied(x)) 00104 return false; 00105 } 00106 00107 return true; 00108 } 00109 00110 00111 00112 void PRM::Construct() 00113 { 00114 int i,k; 00115 MSLVector u_best,nx; 00116 MSLVertex *nn; 00117 list<MSLVertex*> nhbrs; 00118 list<MSLVertex*>::iterator ni; 00119 00120 float t = used_time(); 00121 00122 if (!Roadmap) 00123 Roadmap = new MSLGraph(); 00124 00125 // Set the step size 00126 StepSize = P->Metric(P->InitialState,P->Integrate(P->InitialState, 00127 P->GetInputs(P->InitialState).front(),PlannerDeltaT)); 00128 00129 i = 0; 00130 while (i < NumNodes) { 00131 nx = ChooseState(i,NumNodes,P->InitialState.dim()); 00132 SatisfiedCount++; 00133 i++; 00134 while (!P->Satisfied(nx)) { 00135 nx = ChooseState(i,NumNodes,P->InitialState.dim()); 00136 SatisfiedCount++; 00137 i++; 00138 // Keep trying until a good sample is found 00139 } 00140 nhbrs = NeighboringVertices(nx); 00141 nn = Roadmap->AddVertex(nx); 00142 00143 k = 0; 00144 forall(ni,nhbrs) { 00145 if (Connect((*ni)->State(),nx,u_best)) { 00146 Roadmap->AddEdge(nn,*ni,u_best,1.0); 00147 Roadmap->AddEdge(*ni,nn,-1.0*u_best,1.0); 00148 k++; 00149 } 00150 if (k > MaxEdgesPerVertex) 00151 break; 00152 } 00153 00154 if (Roadmap->NumVertices() % 1000 == 0) 00155 cout << Roadmap->NumVertices() << " vertices in the PRM.\n"; 00156 } 00157 00158 //MSLVertex_array<int> labels(G); 00159 //c = COMPONENTS(G,labels); // Compute connected components 00160 00161 /* Show component sizes 00162 int count; 00163 for (i = 0; i < c; i++) { 00164 count = 0; 00165 forall_MSLVertexs(n,G) { 00166 if (labels[n] == i) 00167 count++; 00168 } 00169 cout << "Component " << i << " has " << count << " MSLVertexs.\n"; 00170 } 00171 */ 00172 00173 cout << "PRM Vertices: " << Roadmap->NumVertices() << 00174 " Edges: " << Roadmap->NumEdges() << 00175 " ColDet: " << SatisfiedCount; 00176 //cout << " Components: " << c; 00177 cout << "\n"; 00178 00179 CumulativeConstructTime += ((double)used_time(t)); 00180 cout << "Construct Time: " << CumulativeConstructTime << "s\n"; 00181 00182 } 00183 00184 00185 00186 bool PRM::Plan() 00187 { 00188 list<MSLVertex*> nlist; 00189 list<MSLEdge*> elist; 00190 MSLVertex *n,*ni,*ng,*nn,*n_best; 00191 MSLVector u_best; 00192 bool success; 00193 list<MSLVertex*> vpath; 00194 list<MSLVertex*>::iterator vi; 00195 list<MSLEdge*>::iterator ei; 00196 priority_queue<MSLVertex*,vector<MSLVertex*>,MSLVertexGreater> Q; 00197 double cost,mincost,time; 00198 00199 float t = used_time(); 00200 00201 if (!Roadmap) { 00202 cout << "Empty roadmap. Run Construct before Plan.\n"; 00203 return false; 00204 } 00205 00206 // Set the step size 00207 StepSize = P->Metric(P->InitialState,P->Integrate(P->InitialState, 00208 P->GetInputs(P->InitialState).front(),PlannerDeltaT)); 00209 00210 // Connect to the initial state 00211 nlist = NeighboringVertices(P->InitialState); 00212 00213 if (nlist.size() == 0) { 00214 cout << "No neighboring vertices to the Initial State\n"; 00215 cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 00216 return false; 00217 } 00218 00219 vi = nlist.begin(); 00220 while ((!success) && (vi != nlist.end())) { 00221 success = Connect(P->InitialState,(*vi)->State(),u_best); 00222 vi++; 00223 } 00224 00225 if (!success) { 00226 cout << "Failure to connect to Initial State\n"; 00227 cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 00228 return false; 00229 } 00230 00231 // Make ni a new vertex in the PRM 00232 n = *vi; 00233 ni = Roadmap->AddVertex(P->InitialState); 00234 Roadmap->AddEdge(n,ni,u_best,1.0); 00235 Roadmap->AddEdge(ni,n,-1.0*u_best,1.0); 00236 00237 // Connect to the goal state 00238 nlist = NeighboringVertices(P->GoalState); 00239 00240 if (nlist.size() == 0) { 00241 cout << "No neighboring vertices to the Goal State\n"; 00242 cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 00243 return false; 00244 } 00245 00246 vi = nlist.begin(); 00247 while ((!success) && (vi != nlist.end())) { 00248 success = Connect((*vi)->State(),P->GoalState,u_best); 00249 vi++; 00250 } 00251 00252 if (!success) { 00253 cout << "Failure to connect to Goal State\n"; 00254 cout << "Planning Time: " << ((double)used_time(t)) << "s\n"; 00255 return false; 00256 } 00257 00258 // Make ng a new MSLVertex in the PRM 00259 n = *vi; 00260 ng = Roadmap->AddVertex(P->GoalState); 00261 Roadmap->AddEdge(n,ng,u_best,1.0); 00262 Roadmap->AddEdge(ng,n,-1.0*u_best,1.0); 00263 00264 // Initialize for DP search (the original PRM used A^*) 00265 ni->SetCost(0.0); 00266 Q.push(ni); 00267 nlist = Roadmap->Vertices(); 00268 forall(vi,nlist) 00269 (*vi)->Unmark(); 00270 ni->Mark(); 00271 00272 // Loop until Q is empty or goal is found 00273 success = false; 00274 while ((!success)&&(!Q.empty())) { 00275 // Remove smallest element 00276 n = Q.top(); 00277 cost = n->Cost(); 00278 Q.pop(); 00279 00280 // Expand its unexplored neighbors 00281 elist = n->Edges(); 00282 forall(ei,elist) { 00283 nn = (*ei)->Target(); 00284 if (!nn->IsMarked()) { // If not yet visited 00285 nn->Mark(); 00286 nn->SetCost(cost+(*ei)->Cost()); 00287 Q.push(nn); 00288 } 00289 } 00290 } 00291 00292 CumulativePlanningTime += ((double)used_time(t)); 00293 cout << "Planning Time: " << CumulativePlanningTime << "s\n"; 00294 00295 if (ng->IsMarked()) { 00296 // Get the path 00297 n = ng; 00298 while (n != ni) { 00299 mincost = INFINITY; 00300 vpath.push_front(n); 00301 // Pick neighboring vertex with lowest cost 00302 elist = n->Edges(); 00303 forall(ei,elist) { 00304 nn = (*ei)->Target(); 00305 if (nn->Cost() < mincost) { 00306 n_best = nn; 00307 mincost = nn->Cost(); 00308 } 00309 } 00310 n = n_best; 00311 } 00312 vpath.push_front(ni); 00313 } 00314 else { 00315 cout << " Failure to find a path in the graph.\n"; 00316 return false; 00317 } 00318 00319 // Make the solution 00320 Path.clear(); 00321 TimeList.clear(); 00322 time = 0.0; 00323 forall(vi,vpath) { 00324 Path.push_back((*vi)->State()); 00325 TimeList.push_back(time); 00326 time += 1.0; 00327 } 00328 00329 cout << " Success\n"; 00330 00331 return true; 00332 } 00333 00334 00335 00336 MSLVector PRM::ChooseState(int i, int maxnum, int dim) { 00337 if (QuasiRandom) { 00338 if (QuasiRandomHammersley) 00339 return QuasiRandomStateHammersley(i,maxnum,dim); 00340 else 00341 return QuasiRandomStateHalton(i,dim); 00342 } 00343 else 00344 return RandomState(); 00345 } 00346 00347 00348 00349 00350 MSLVector PRM::QuasiRandomStateHammersley(int i, int maxnum, int dim) { 00351 int j,k,r,ppow; 00352 MSLVector qrx; 00353 00354 if (dim > 30) { 00355 cout << "ERROR: Dimension too high for quasi-random samples\n"; 00356 exit(-1); 00357 } 00358 00359 qrx = MSLVector(dim); 00360 00361 k = i; 00362 00363 qrx[0] = ((double) k / maxnum) * (P->UpperState[0] - P->LowerState[0]); 00364 qrx[0] += P->LowerState[0]; 00365 for (j = 1; j < dim; j++) { 00366 qrx[j] = 0.0; 00367 ppow = Primes[j-1]; 00368 k = i; 00369 while (k != 0) { 00370 r = k % Primes[j-1]; 00371 k = (int) (k / Primes[j-1]); 00372 qrx[j] += (double) r / ppow; 00373 ppow *= Primes[j-1]; 00374 } 00375 qrx[j] *= (P->UpperState[j] - P->LowerState[j]); 00376 qrx[j] += P->LowerState[j]; 00377 } 00378 00379 return qrx; 00380 } 00381 00382 00383 MSLVector PRM::QuasiRandomStateHalton(int i, int dim) { 00384 int j,k,r,ppow; 00385 MSLVector qrx; 00386 00387 if (dim > 30) { 00388 cout << "ERROR: Dimension too high for quasi-random samples\n"; 00389 exit(-1); 00390 } 00391 00392 qrx = MSLVector(dim); 00393 k = i; 00394 00395 for (j = 0; j < dim; j++) { 00396 qrx[j] = 0.0; 00397 ppow = Primes[j]; 00398 k = i; 00399 while (k != 0) { 00400 r = k % Primes[j]; 00401 k = (int) (k / Primes[j]); 00402 qrx[j] += (double) r / ppow; 00403 ppow *= Primes[j]; 00404 } 00405 qrx[j] *= (P->UpperState[j] - P->LowerState[j]); 00406 qrx[j] += P->LowerState[j]; 00407 } 00408 00409 return qrx; 00410 } 00411