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 #include <LEDA/REDEFINE_NAMES.h> 00026 00027 00028 // ********************************************************************* 00029 // ********************************************************************* 00030 // CLASS: Planner base class 00031 // 00032 // ********************************************************************* 00033 // ********************************************************************* 00034 00035 Planner::Planner(Problem *problem):Solver(problem) { 00036 Reset(); 00037 } 00038 00039 00040 void Planner::Reset() { 00041 int i; 00042 00043 NumNodes = 1000; 00044 00045 if (is_file(P->FilePath+"PlannerDeltaT")) { 00046 file_istream fin(P->FilePath + "PlannerDeltaT"); 00047 fin >> PlannerDeltaT; 00048 fin.close(); 00049 } 00050 else { 00051 PlannerDeltaT = 1.0; 00052 } 00053 00054 if (is_file(P->FilePath+"GapError")) { 00055 file_istream fin(P->FilePath + "GapError"); 00056 fin >> GapError; 00057 fin.close(); 00058 } 00059 else { 00060 GapError = vector(P->StateDim); 00061 for (i = 0; i < P->StateDim; i++) 00062 GapError[i] = 1.0; 00063 } 00064 00065 Path.clear(); 00066 Policy.clear(); 00067 00068 Holonomic = is_file(P->FilePath+"Holonomic"); // Nonholonomic by default 00069 00070 G.clear(); 00071 G2.clear(); 00072 } 00073 00074 00075 vector Planner::RandomState() { 00076 int i; 00077 double r; 00078 vector rx; 00079 00080 rx = P->LowerState; 00081 for (i = 0; i < P->StateDim; i++) { 00082 R >> r; 00083 rx[i] += r * (P->UpperState[i] - P->LowerState[i]); 00084 } 00085 00086 return rx; 00087 } 00088 00089 00090 00091 vector Planner::NormalState(vector mean, double sd = 0.5) { 00092 int i,j; 00093 double r; 00094 vector rx; 00095 bool success = false; 00096 00097 rx = mean; 00098 for (i = 0; i < P->StateDim; i++) { 00099 success = false; 00100 while (!success) { 00101 rx[i] = 0.0; 00102 for (j = 0; j < 12; j++) { // Increase 12 here and below for more accuracy 00103 R >> r; rx[i] += r; 00104 } 00105 rx[i] = (rx[i] - 12/2)*sd*(P->UpperState[i]-P->LowerState[i])+mean[i]; 00106 if ((rx[i] <= P->UpperState[i])&&(rx[i] >= P->LowerState[i])) 00107 success = true; 00108 } 00109 } 00110 00111 return rx; 00112 } 00113 00114 00115 00116 00117 void Planner::WriteGraphs(const string &fname) 00118 { 00119 ofstream fout(fname); 00120 G.write(fout); 00121 G2.write(fout); 00122 fout.close(); 00123 } 00124 00125 00126 00127 void Planner::ReadGraphs(const string &fname) 00128 { 00129 ifstream fin(fname); 00130 G.read(fin); 00131 G2.read(fin); 00132 fin.close(); 00133 } 00134 00135 00136 00137 void Planner::RecordSolution(const list<node> &glist, 00138 const list<node> &g2list) 00139 { 00140 node n,nfirst,nlast; 00141 00142 Path.clear(); 00143 Policy.clear(); 00144 00145 nfirst = glist.head(); 00146 forall(n,glist) { 00147 Path.push_back(G.inf(n)); 00148 if (n != nfirst) { 00149 Policy.push_back(G.inf(G.first_in_edge(n))); 00150 } 00151 } 00152 00153 // The GapState is always comes from last node in glist 00154 GapState = Path.tail(); 00155 // Make a dummy input to get from GapState to the next state 00156 Policy.push_back(P->GetInputs().head()); 00157 00158 if (g2list.length() == 0) { 00159 // Push the goal state onto the end (jumps the gap) 00160 Path.push_back(P->GoalState); 00161 } 00162 else { // Using two graphs 00163 nlast = g2list.tail(); 00164 forall(n,g2list) { 00165 Path.push_back(G2.inf(n)); 00166 if (n != nlast) { 00167 Policy.push_back(G2.inf(G2.first_in_edge(n))); 00168 } 00169 } 00170 } 00171 00172 //cout << "Path: " << Path << "\n"; 00173 //cout << "Policy: " << Policy << "\n"; 00174 } 00175 00176 00177 00178 void Planner::RecordSolution(const list<node> &glist) 00179 { 00180 list<node> emptylist; 00181 emptylist.clear(); // Make sure it is clear 00182 00183 RecordSolution(glist,emptylist); 00184 } 00185 00186 00187 00188 bool Planner::GapSatisfied(const vector &x1, const vector &x2) { 00189 vector x; 00190 int i; 00191 00192 x = P->StateDifference(x1,x2); 00193 for (i = 0; i < P->StateDim; i++) { 00194 if (fabs(x[i]) > GapError[i]) 00195 return false; 00196 } 00197 00198 return true; 00199 } 00200 00201