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

planner.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 "planner.h"
00023 #include "defs.h"
00024 
00025 
00026 // *********************************************************************
00027 // *********************************************************************
00028 // CLASS:     Planner base class
00029 // 
00030 // *********************************************************************
00031 // *********************************************************************
00032 
00033 Planner::Planner(Problem *problem):Solver(problem) {
00034   T = NULL;
00035   T2 = NULL;
00036   Roadmap = NULL;
00037   Reset();
00038 }
00039 
00040 
00041 Planner::~Planner() {
00042   Reset();
00043 }
00044 
00045 void Planner::Reset() {
00046   int i;
00047 
00048   NumNodes = 1000;
00049   std::ifstream fin;
00050 
00051   READ_PARAMETER_OR_DEFAULT(PlannerDeltaT,1.0);
00052 
00053   GapError = MSLVector(P->StateDim);
00054   for (i = 0; i < P->StateDim; i++)
00055     GapError[i] = 1.0;
00056   READ_OPTIONAL_PARAMETER(GapError);
00057 
00058   Path.clear();
00059   Policy.clear();
00060 
00061   fin.open((FilePath+"Holonomic").c_str());
00062   Holonomic = fin ? true : false; // Nonholonomic by default
00063   fin.close();
00064 
00065   CumulativePlanningTime = 0.0;
00066   CumulativeConstructTime = 0.0;
00067 
00068   if (T)
00069     delete T;
00070   if (T2)
00071     delete T2;
00072   T = NULL;
00073   T2 = NULL;
00074 
00075   if (Roadmap)
00076     delete Roadmap;
00077   Roadmap = NULL;
00078 }
00079 
00080 
00081 MSLVector Planner::RandomState() {
00082   int i;
00083   double r;
00084   MSLVector rx;
00085 
00086   rx = P->LowerState;
00087   for (i = 0; i < P->StateDim; i++) {
00088       R >> r; 
00089       rx[i] += r * (P->UpperState[i] - P->LowerState[i]);
00090     }
00091 
00092   return rx;
00093 }
00094 
00095 
00096 
00097 MSLVector Planner::NormalState(MSLVector mean, double sd = 0.5) {
00098   int i,j;
00099   double r;
00100   MSLVector rx;
00101   bool success = false;
00102 
00103   rx = mean;
00104   for (i = 0; i < P->StateDim; i++) {
00105     success = false;
00106     while (!success) {
00107       rx[i] = 0.0;
00108       for (j = 0; j < 12; j++) {  // Increase 12 here and below for more accuracy
00109         R >> r; rx[i] += r;
00110       }
00111       rx[i] = (rx[i] - 12/2)*sd*(P->UpperState[i]-P->LowerState[i])+mean[i];
00112       if ((rx[i] <= P->UpperState[i])&&(rx[i] >= P->LowerState[i]))
00113         success = true;
00114     }
00115   }
00116 
00117   return rx;
00118 }
00119 
00120 
00121 
00122 bool Planner::GapSatisfied(const MSLVector &x1, const MSLVector &x2) {
00123   MSLVector x;
00124   int i;
00125 
00126   x = P->StateDifference(x1,x2);
00127   for (i = 0; i < P->StateDim; i++) {
00128     if (fabs(x[i]) > GapError[i])
00129       return false;
00130   }
00131 
00132   return true;
00133 }
00134 
00135 
00136 
00137 // *********************************************************************
00138 // *********************************************************************
00139 // CLASS:     IncrementalPlanner base class
00140 // 
00141 // *********************************************************************
00142 // *********************************************************************
00143 
00144 IncrementalPlanner::IncrementalPlanner(Problem *problem):Planner(problem) {
00145 }
00146 
00147 void IncrementalPlanner::Construct() {
00148   cout << "  Incremental planners do not use Construct.\n";
00149   cout << "  Try Plan.\n";
00150 }
00151 
00152 
00153 void IncrementalPlanner::RecordSolution(const list<MSLNode*> &glist, 
00154                                         const list<MSLNode*> &g2list)
00155 {
00156   list<MSLNode*>::const_iterator n,nfirst,nlast;
00157   double ptime;
00158 
00159   Path.clear();
00160   Policy.clear();
00161 
00162   ptime = 0.0; TimeList.clear();
00163   nfirst = glist.begin();
00164 
00165   forall(n,glist) {
00166     Path.push_back((*n)->State());
00167     if (n != nfirst) {
00168       Policy.push_back((*n)->Input());
00169     }
00170     ptime += (*n)->Time();
00171     TimeList.push_back(ptime);
00172   }
00173 
00174   // The GapState is always comes from last node in glist
00175   GapState = Path.back();
00176 
00177   // Make a dummy input to get from GapState to the next state
00178   Policy.push_back(P->GetInputs().front());
00179 
00180   if (g2list.size() == 0) {
00181     // Push the goal state onto the end (jumps the gap)
00182     Path.push_back(P->GoalState);
00183   }
00184   else { // Using two graphs
00185     ptime += PlannerDeltaT; // Add a time step for the gap
00186     nlast = g2list.end();
00187     forall(n,g2list) {
00188       Path.push_back((*n)->State());
00189       if (n != nlast) {
00190         Policy.push_back((*n)->Input());
00191       }
00192     TimeList.push_back(ptime);
00193     ptime += (*n)->Time();
00194     }
00195   }
00196 
00197   //cout << "Path: " << Path << "\n";
00198   //cout << "Policy: " << Policy << "\n";
00199   //cout << "TimeList: " << TimeList << "\n";
00200 }
00201 
00202 
00203 
00204 void IncrementalPlanner::RecordSolution(const list<MSLNode*> &glist)
00205 {
00206   list<MSLNode*> emptylist;
00207   emptylist.clear(); // Make sure it is clear
00208 
00209   RecordSolution(glist,emptylist);
00210 }
00211 
00212 
00213 
00214 void IncrementalPlanner::WriteGraphs(ofstream &fout)
00215 {
00216   if (T)
00217     fout << *T << "\n\n\n";
00218   if (T2)
00219     fout << *T2;
00220 }
00221 
00222 
00223 
00224 void IncrementalPlanner::ReadGraphs(ifstream &fin)
00225 {
00226   if (T)
00227     delete T;
00228   if (T2)
00229     delete T2;
00230 
00231   T = new MSLTree();
00232   T2 = new MSLTree();
00233 
00234   fin >> *T >> *T2;
00235 }
00236 
00237 
00238 
00239 
00240 // *********************************************************************
00241 // *********************************************************************
00242 // CLASS:     RoadmapPlanner base class
00243 // 
00244 // *********************************************************************
00245 // *********************************************************************
00246 
00247 RoadmapPlanner::RoadmapPlanner(Problem *problem):Planner(problem) {
00248 }
00249 
00250 
00251 
00252 
00253 void RoadmapPlanner::WriteGraphs(ofstream &fout)
00254 {
00255   fout << *Roadmap << "\n\n\n";
00256 }
00257 
00258 
00259 
00260 void RoadmapPlanner::ReadGraphs(ifstream &fin)
00261 {
00262   if (Roadmap)
00263     delete Roadmap;
00264 
00265   Roadmap = new MSLGraph();
00266   fin >> *Roadmap;
00267 }
00268 
00269 
00270 
00271 
00272 
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.