Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

prm.C

Go to the documentation of this file.
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 
Motion Strategy Library


Web page maintained by Steve LaValle
Partial support provided by NSF CAREER Award IRI-970228 (LaValle), Honda Research, and Iowa State University.
Contributors: Anna Atramentov, Peng Cheng, James Kuffner, Steve LaValle, and Libo Yang.