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

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