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