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