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