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