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 <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 
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.