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

modelmisc.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 <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 
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.