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 <fstream.h> 00020 #include <math.h> 00021 00022 #include "modelmisc.h" 00023 #include "defs.h" 00024 00025 #include <LEDA/REDEFINE_NAMES.h> 00026 00027 00028 // ********************************************************************* 00029 // ********************************************************************* 00030 // CLASS: Model1D 00031 // 00032 // ********************************************************************* 00033 // ********************************************************************* 00034 00035 Model1D::Model1D(string path = ""):Model(path) { 00036 00037 vector v; 00038 00039 Force = 1.0; 00040 00041 StateDim = 1; 00042 InputDim = 1; 00043 00044 00045 if (is_file(FilePath+"LowerState")) 00046 ReadLowerState(); 00047 else { 00048 LowerState = vector(-50.0,-10.0); 00049 } 00050 00051 if (is_file(FilePath+"UpperState")) 00052 ReadUpperState(); 00053 else { 00054 UpperState = vector(50.0,10.0); 00055 } 00056 00057 // Make the list of 1D Inputs 00058 if (is_file(FilePath+"Inputs")) 00059 ReadInputs(); 00060 else { 00061 Inputs.clear(); // Just to be safe 00062 v = vector(1); v[0] = 0; // Float 00063 Inputs.push_back(v); 00064 v = vector(1); v[0] = 1; // Accel in +x 00065 Inputs.push_back(v); 00066 v = vector(1); v[0] = -1; // Accel in -x 00067 Inputs.push_back(v); 00068 } 00069 00070 } 00071 00072 00073 00074 vector Model1D::Integrate(const vector &x, const vector &u, const double &h) 00075 { 00076 return RungeKuttaIntegrate(x,u,h); 00077 } 00078 00079 00080 vector Model1D::StateTransitionEquation(const vector &x, const vector &u) 00081 { 00082 vector dx(2); 00083 00084 dx[0] = x[1]; 00085 dx[1] = u[0]*Force; 00086 00087 return dx; 00088 } 00089 00090 00091 point Model1D::StateToLedaPoint(const vector &x) { 00092 return point(x[0]+50.0,x[1]*(100.0/(UpperState[1]-LowerState[1]))+50.0); 00093 } 00094 00095 00096 vector Model1D::StateToConfiguration(const vector &x) { 00097 return vector(x[0]+50,x[1]*(100.0/(UpperState[1]-LowerState[1]))+50.0,0.0); // Yield a zero rotation every time 00098 } 00099 00100 double Model1D::Metric(const vector &x1, const vector &x2) { 00101 double d = 0.0; 00102 int i; 00103 00104 for (i = 0; i < 2; i++) { 00105 d += sqr(x1[i] - x2[i]) / (UpperState[i] - LowerState[i]); 00106 } 00107 00108 //cout << "x1: " << x1 << " x2: " << x2 << " Metric: " << d << "\n"; 00109 00110 return d; 00111 } 00112 00113 00114 00115 // ********************************************************************* 00116 // ********************************************************************* 00117 // CLASS: ModelLinear 00118 // 00119 // ********************************************************************* 00120 // ********************************************************************* 00121 00122 ModelLinear::ModelLinear(string path = ""):Model(path) { 00123 00124 vector v; 00125 int i; 00126 00127 file_istream fin(FilePath + "A"); 00128 fin >> A; 00129 file_istream fin2(FilePath + "B"); 00130 fin2 >> B; 00131 00132 StateDim = A.dim1(); 00133 InputDim = B.dim2(); 00134 00135 if (is_file(FilePath+"LowerState")) 00136 ReadLowerState(); 00137 else { 00138 LowerState = vector(StateDim); 00139 for (i = 0; i < StateDim; i++) { 00140 LowerState[i] -= 10.0; 00141 } 00142 } 00143 00144 if (is_file(FilePath+"UpperState")) 00145 ReadUpperState(); 00146 else { 00147 UpperState = vector(StateDim); 00148 for (i = 0; i < StateDim; i++) { 00149 UpperState[i] += 10.0; 00150 } 00151 } 00152 00153 if (is_file(FilePath+"Inputs")) 00154 ReadInputs(); 00155 else { 00156 Inputs.clear(); // Just to be safe 00157 v = vector(InputDim); // Initializes to the zero vector 00158 Inputs.push(v); 00159 for (i = 0; i < StateDim; i++) { 00160 v = vector(InputDim); 00161 v[i] = 1.0; 00162 Inputs.push(v); 00163 v[i] = -1.0; 00164 Inputs.push(v); 00165 } 00166 } 00167 00168 } 00169 00170 00171 point ModelLinear::StateToLedaPoint(const vector &x) { 00172 return point(5.0*(x[0]+10.0),5.0*(x[1]+10.0)); 00173 } 00174 00175 vector ModelLinear::StateToConfiguration(const vector &x) { 00176 return vector(5.0*(x[0]+10.0),5.0*(x[1]+10.0),0.0); 00177 } 00178 00179 00180 vector ModelLinear::Integrate(const vector &x, const vector &u, const double &h) 00181 { 00182 return RungeKuttaIntegrate(x,u,h); 00183 } 00184 00185 00186 vector ModelLinear::StateTransitionEquation(const vector &x, const vector &u) 00187 { 00188 vector dx(StateDim); 00189 00190 dx = A * x + B * u; // This looks too easy! 00191 00192 return dx; 00193 } 00194 00195 00196 00197 // ********************************************************************* 00198 // ********************************************************************* 00199 // CLASS: ModelND 00200 // 00201 // ********************************************************************* 00202 // ********************************************************************* 00203 00204 ModelND::ModelND(string path = ""):Model(path) { 00205 00206 vector v; 00207 int i; 00208 00209 if ((path.length() > 0)&&(path[path.length()-1] != '/')) 00210 path += "/"; 00211 00212 FilePath = path; 00213 00214 if (is_file(FilePath+"Dimension")) { 00215 file_istream fin(FilePath + "Dimension"); 00216 fin >> StateDim; } 00217 else 00218 StateDim = 2; 00219 00220 if (is_file(FilePath+"CorridorWidth")) { 00221 file_istream fin(FilePath + "CorridorWidth"); 00222 fin >> CorridorWidth; } 00223 else 00224 CorridorWidth = 6.0*StateDim; 00225 00226 InputDim = StateDim; 00227 00228 if (is_file(FilePath+"LowerState")) 00229 ReadLowerState(); 00230 else { 00231 LowerState = vector(StateDim); 00232 for (i = 0; i < StateDim; i++) { 00233 LowerState[i] = 0.0; 00234 } 00235 } 00236 00237 if (is_file(FilePath+"UpperState")) 00238 ReadUpperState(); 00239 else { 00240 UpperState = vector(StateDim); 00241 for (i = 0; i < StateDim; i++) { 00242 UpperState[i] = 100.0; 00243 } 00244 } 00245 00246 if (is_file(FilePath+"Inputs")) 00247 ReadInputs(); 00248 else { 00249 Inputs.clear(); // Just to be safe 00250 v = vector(InputDim); // Initializes to the zero vector 00251 Inputs.push(v); 00252 for (i = 0; i < StateDim; i++) { 00253 v = vector(InputDim); 00254 v[i] = 1.0; 00255 Inputs.push(v); 00256 v[i] = -1.0; 00257 Inputs.push(v); 00258 } 00259 } 00260 00261 } 00262 00263 00264 point ModelND::StateToLedaPoint(const vector &x) { 00265 return point(x[0],x[1]); 00266 } 00267 00268 00269 vector ModelND::StateToConfiguration(const vector &x) { 00270 return vector(x[0],x[1],0.0); 00271 } 00272 00273 00274 vector ModelND::StateTransitionEquation(const vector &x, const vector &u) 00275 { 00276 vector dx(StateDim); 00277 00278 dx = u; 00279 00280 return dx; 00281 } 00282 00283 00284 vector ModelND::Integrate(const vector &x, const vector &u, const double &h) 00285 { 00286 return EulerIntegrate(x,u,h); 00287 } 00288 00289 00290 // This makes a narrow circular corridor 00291 /* 00292 bool ModelND::Satisfied(const vector &x) { 00293 bool sat; 00294 vector sx,cx; // cx is the center 00295 int i; 00296 00297 sx = vector(x.dim()-1); // a vector of the last n-1 components 00298 cx = vector(x.dim()-1); 00299 for (i = 0; i < sx.dim(); i++) { 00300 sx[i] = x[i+1]; 00301 cx[i] = (UpperState[i+1] - LowerState[i+1])/2.0; 00302 } 00303 sat = ((x[0] < 30.0) || (x[0] > 70.0) || 00304 ((cx - sx).length() < CorridorWidth)); 00305 00306 return sat; 00307 } 00308 */ 00309 00310 // This makes a narrow square corridor 00311 /* 00312 bool ModelND::Satisfied(const vector &x) { 00313 int i; 00314 double maxdiff; 00315 00316 if ((x[0] < 30)||(x[0] > 70.0)) 00317 return true; 00318 00319 maxdiff = 0.0; 00320 for (i = 1; i < x.dim(); i++) { 00321 if (fabs(x[i]-50.0) > maxdiff) 00322 maxdiff = fabs(x[i]-50.0); 00323 } 00324 00325 return (maxdiff < CorridorWidth); 00326 } 00327 */ 00328 00329 /* 00330 // This makes a narrow square corridor with two bends 00331 bool ModelND::Satisfied(const vector &x) { 00332 int i; 00333 00334 if ((x[0] < 30)||(x[0] > 70.0)) 00335 return true; 00336 00337 // Check the proper slice for last n-2 coords 00338 if (x.dim() > 2) { 00339 for (i = 2; i < x.dim(); i++) { 00340 if (fabs(x[i]-50.0) > CorridorWidth) 00341 return false; 00342 } 00343 } 00344 00345 // Check upper left corridor piece 00346 if ((x[0] < 50) && (fabs(x[1]-70.0) < CorridorWidth)) 00347 return true; 00348 00349 // Check lower right corridor piece 00350 if ((x[0] >= 50)&&(fabs(x[1]-30.0) < CorridorWidth)) 00351 return true; 00352 00353 // Final check for center piece 00354 if ((x[0] < 50.0 + CorridorWidth)&& 00355 (x[0] > 50.0 - CorridorWidth)&& 00356 (x[1] < 70.0 + CorridorWidth)&& 00357 (x[1] > 30.0 - CorridorWidth)) 00358 return true; 00359 00360 // Finally, the point is in collision 00361 return false; 00362 } 00363 00364 */ 00365 00366 // ********************************************************************* 00367 // ********************************************************************* 00368 // CLASS: ModelNintegrator 00369 // 00370 // ********************************************************************* 00371 // ********************************************************************* 00372 00373 00374 ModelNintegrator::ModelNintegrator(string path = ""):Model(path) { 00375 00376 vector v; 00377 int i; 00378 double alpha,beta; 00379 00380 UBound = 0.5; 00381 VBound = 0.5; 00382 00383 StateDim = 3; 00384 InputDim = 2; 00385 00386 if (is_file(FilePath+"LowerState")) 00387 ReadLowerState(); 00388 else { 00389 LowerState = vector(StateDim); 00390 for (i = 0; i < StateDim; i++) { 00391 LowerState[i] -= 20.0; 00392 } 00393 } 00394 00395 if (is_file(FilePath+"UpperState")) 00396 ReadUpperState(); 00397 else { 00398 UpperState = vector(StateDim); 00399 for (i = 0; i < StateDim; i++) { 00400 UpperState[i] += 20.0; 00401 } 00402 } 00403 00404 if (is_file(FilePath+"Inputs")) 00405 ReadInputs(); 00406 else { 00407 Inputs.clear(); // Just to be safe 00408 for (alpha = -UBound; alpha <= UBound; 00409 alpha += 2.0*UBound/4.0) { 00410 for (beta = -VBound; beta <= VBound; 00411 beta += 2.0*VBound/4.0) { 00412 Inputs.push_back(vector(alpha,beta)); 00413 Inputs.push_back(vector(alpha,beta)); 00414 } 00415 } 00416 00417 } 00418 } 00419 00420 00421 00422 point ModelNintegrator::StateToLedaPoint(const vector &x) { 00423 return point(2.5*(x[0]+20.0),2.5*(x[1]+20.0)); 00424 } 00425 00426 vector ModelNintegrator::StateToConfiguration(const vector &x) { 00427 return vector(2.5*(x[0]+20.0),2.5*(x[1]+20.0),0.0); 00428 } 00429 00430 00431 vector ModelNintegrator::Integrate(const vector &x, const vector &u, const double &h) 00432 { 00433 return RungeKuttaIntegrate(x,u,h); 00434 } 00435 00436 00437 vector ModelNintegrator::StateTransitionEquation(const vector &x, const vector &u) 00438 { 00439 vector dx(StateDim); 00440 00441 dx[0] = u[0]; 00442 dx[1] = u[1]; 00443 dx[2] = x[0]*u[1] - x[1]*u[0]; 00444 00445 return dx; 00446 } 00447 00448