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 "model2d.h" 00023 00024 #include "defs.h" 00025 00026 #define NUM_INPUTS_2DPOINT 50 00027 #define NUM_INPUTS_2DRIGID 8 00028 00029 00030 // ********************************************************************* 00031 // ********************************************************************* 00032 // CLASS: Model2D 00033 // 00034 // ********************************************************************* 00035 // ********************************************************************* 00036 00037 00038 00039 Model2D::Model2D(string path = ""):Model(path) { 00040 } 00041 00042 00043 MSLVector Model2D::StateToConfiguration(const MSLVector &x) 00044 { 00045 MSLVector q(3); 00046 00047 q[0] = x[0]; q[1] = x[1]; q[2] = 0.0; 00048 00049 return q; 00050 } 00051 00052 00053 00054 // ********************************************************************* 00055 // ********************************************************************* 00056 // CLASS: Model2DPoint 00057 // 00058 // ********************************************************************* 00059 // ********************************************************************* 00060 00061 00062 // Constructor 00063 Model2DPoint::Model2DPoint(string path = ""):Model2D(path) { 00064 double theta; 00065 00066 StateDim = 2; 00067 InputDim = 2; 00068 00069 READ_PARAMETER_OR_DEFAULT(LowerState,MSLVector(0.0,0.0)); 00070 READ_PARAMETER_OR_DEFAULT(UpperState,MSLVector(100.0,100.0)); 00071 00072 // Make the list of Inputs 00073 Inputs.clear(); 00074 for (theta = 0.0; theta < 2.0*PI; theta += 2.0*PI/NUM_INPUTS_2DPOINT) { 00075 Inputs.push_back(MSLVector(cos(theta),sin(theta))); 00076 } 00077 00078 READ_OPTIONAL_PARAMETER(Inputs); 00079 00080 } 00081 00082 00083 00084 00085 MSLVector Model2DPoint::StateTransitionEquation(const MSLVector &x, const MSLVector &u) { 00086 00087 MSLVector dx(2); 00088 00089 dx = u; 00090 return dx; 00091 } 00092 00093 00094 00095 double Model2DPoint::Metric(const MSLVector &x1, const MSLVector &x2) { 00096 00097 double rho; 00098 00099 rho = (x1 - x2).length(); 00100 00101 return rho; 00102 } 00103 00104 00105 00106 00107 00108 MSLVector Model2DPoint::Integrate(const MSLVector &x, const MSLVector &u, const double &h) 00109 { 00110 return EulerIntegrate(x,u,h); 00111 } 00112 00113 00114 00115 // ********************************************************************* 00116 // ********************************************************************* 00117 // CLASS: Model::Model2DPointCar 00118 // 00119 // ********************************************************************* 00120 // ********************************************************************* 00121 00122 00123 // Constructor 00124 Model2DPointCar::Model2DPointCar(string path = ""):Model2DPoint(path) { 00125 double alpha; 00126 00127 StateDim = 3; 00128 InputDim = 2; 00129 00130 READ_PARAMETER_OR_DEFAULT(LowerState,MSLVector(0.0,0.0,0.0)); 00131 READ_PARAMETER_OR_DEFAULT(UpperState,MSLVector(100.0,100.0,2.0*PI)); 00132 00133 MaxSteeringAngle = PI/6.0; 00134 CarLength = 4.0; 00135 00136 // Make the list of Inputs 00137 Inputs.clear(); // Otherwise its parent constructor will make some inputs 00138 for (alpha = -MaxSteeringAngle; alpha <= MaxSteeringAngle; 00139 alpha += 2.0*MaxSteeringAngle/6.0) { 00140 Inputs.push_back(MSLVector(1.0,alpha)); 00141 Inputs.push_back(MSLVector(-1.0,alpha)); 00142 } 00143 00144 READ_OPTIONAL_PARAMETER(Inputs); 00145 00146 } 00147 00148 00149 00150 MSLVector Model2DPointCar::StateTransitionEquation(const MSLVector &x, const MSLVector &u) { 00151 00152 MSLVector dx(3); 00153 00154 dx[0] = u[0]*cos(x[2]); 00155 dx[1] = u[0]*sin(x[2]); 00156 dx[2] = u[0]*tan(u[1])/CarLength; 00157 return dx; 00158 } 00159 00160 00161 00162 double Model2DPointCar::Metric(const MSLVector &x1, const MSLVector &x2) { 00163 00164 double rho,dtheta; 00165 00166 dtheta = min(fabs(x1[2]-x2[2]),2.0*PI - fabs(x1[2]-x2[2])); 00167 00168 rho = sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + 50.0/PI*sqr(dtheta)); 00169 00170 return rho; 00171 } 00172 00173 00174 00175 MSLVector Model2DPointCar::Integrate(const MSLVector &x, const MSLVector &u, 00176 const double &h) { 00177 MSLVector nx(3); 00178 00179 nx = RungeKuttaIntegrate(x,u,h); 00180 00181 // Make sure the S^1 topology is preserved for 2D rotation 00182 if (nx[2] > 2.0*PI) 00183 nx[2] -= 2.0*PI; 00184 if (nx[2] < 0.0) 00185 nx[2] += 2.0*PI; 00186 00187 return nx; 00188 } 00189 00190 00191 00192 // ********************************************************************* 00193 // ********************************************************************* 00194 // CLASS: Model2DRigid 00195 // 00196 // ********************************************************************* 00197 // ********************************************************************* 00198 00199 00200 00201 // Constructor 00202 Model2DRigid::Model2DRigid(string path = ""):Model2D(path) { 00203 double theta; 00204 00205 StateDim = 3; 00206 InputDim = 3; 00207 00208 READ_PARAMETER_OR_DEFAULT(LowerState,MSLVector(0.0,0.0,0.0)); 00209 READ_PARAMETER_OR_DEFAULT(UpperState,MSLVector(100.0,100.0,2.0*PI)); 00210 00211 // Make the list of Inputs 00212 Inputs.clear(); 00213 for (theta = 0.0; theta < 2.0*PI; theta += 2.0*PI/NUM_INPUTS_2DRIGID) { 00214 Inputs.push_back(MSLVector(cos(theta),sin(theta),0.0)); 00215 } 00216 Inputs.push_back(MSLVector(0.0,0.0,-0.1)); 00217 Inputs.push_back(MSLVector(0.0,0.0,0.1)); 00218 00219 READ_OPTIONAL_PARAMETER(Inputs); 00220 00221 } 00222 00223 00224 00225 MSLVector Model2DRigid::StateTransitionEquation(const MSLVector &x, const MSLVector &u) { 00226 00227 MSLVector dx(3); 00228 00229 dx = u; 00230 return dx; 00231 } 00232 00233 00234 00235 double Model2DRigid::Metric(const MSLVector &x1, const MSLVector &x2) { 00236 00237 double fd = fabs(x1[2]-x2[2]); 00238 double dtheta = min(fd,2.0*PI - fd); 00239 00240 return sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + sqr(50.0/PI*dtheta)); 00241 00242 } 00243 00244 00245 00246 // Handle S^1 topology properly (for rotation) 00247 MSLVector Model2DRigid::LinearInterpolate(const MSLVector &x1, const MSLVector &x2, 00248 const double &a) { 00249 00250 MSLVector v; 00251 00252 v = (1.0-a)*x1 + a*x2; 00253 00254 if (fabs(x2[2] - x1[2]) > PI) { 00255 if (x1[2] > x2[2]) 00256 v[2] = (1.0-a)*x1[2] + a*(x2[2]+2.0*PI); 00257 else 00258 v[2] = (1.0-a)*(x1[2]+2.0*PI) + a*x2[2]; 00259 } 00260 00261 if (v[2] > 2.0*PI) 00262 v[2] -= 2.0*PI; 00263 00264 return v; 00265 } 00266 00267 00268 00269 MSLVector Model2DRigid::Integrate(const MSLVector &x, const MSLVector &u, const double &h) 00270 { 00271 MSLVector nx(3); 00272 00273 nx = RungeKuttaIntegrate(x,u,h); 00274 00275 // Make sure the S^1 topology is preserved for 2D rotation 00276 if (nx[2] > 2.0*PI) 00277 nx[2] -= 2.0*PI; 00278 if (nx[2] < 0.0) 00279 nx[2] += 2.0*PI; 00280 00281 return nx; 00282 } 00283 00284 00285 MSLVector Model2DRigid::StateToConfiguration(const MSLVector &x) 00286 { 00287 MSLVector q(3); 00288 00289 q[0] = x[0]; q[1] = x[1]; q[2] = x[2]; 00290 00291 return q; 00292 } 00293 00294 00295 // ********************************************************************* 00296 // ********************************************************************* 00297 // CLASS: Model2DRigidCar 00298 // 00299 // ********************************************************************* 00300 // ********************************************************************* 00301 00302 00303 // Constructor 00304 Model2DRigidCar::Model2DRigidCar(string path = ""):Model2DRigid(path) { 00305 double alpha; 00306 00307 StateDim = 3; 00308 InputDim = 2; 00309 00310 MaxSteeringAngle = PI/12.0; 00311 CarLength = 2.0; 00312 00313 // Make the list of Inputs 00314 Inputs.clear(); // Otherwise its parent constructor will make some inputs 00315 for (alpha = -MaxSteeringAngle; alpha <= MaxSteeringAngle; 00316 alpha += 2.0*MaxSteeringAngle/6.0) { 00317 Inputs.push_back(MSLVector(1.0,alpha)); 00318 Inputs.push_back(MSLVector(-1.0,alpha)); 00319 } 00320 00321 READ_OPTIONAL_PARAMETER(Inputs); 00322 00323 } 00324 00325 00326 MSLVector Model2DRigidCar::StateTransitionEquation(const MSLVector &x, const MSLVector &u) { 00327 00328 MSLVector dx(3); 00329 dx[0] = u[0]*cos(x[2]); 00330 dx[1] = u[0]*sin(x[2]); 00331 dx[2] = u[0]*tan(u[1])/CarLength; 00332 return dx; 00333 } 00334 00335 00336 // ********************************************************************* 00337 // ********************************************************************* 00338 // CLASS: Model2DRigidCarForward 00339 // 00340 // ********************************************************************* 00341 // ********************************************************************* 00342 00343 Model2DRigidCarForward::Model2DRigidCarForward(string path = ""):Model2DRigidCar(path) { 00344 double alpha; 00345 00346 StateDim = 3; 00347 InputDim = 2; 00348 00349 Inputs.clear(); // Otherwise its parent constructor will make some inputs 00350 for (alpha = -MaxSteeringAngle; alpha <= MaxSteeringAngle; 00351 alpha += 2.0*MaxSteeringAngle/6.0) { 00352 Inputs.push_back(MSLVector(1.0,alpha)); 00353 } 00354 00355 READ_OPTIONAL_PARAMETER(Inputs); 00356 00357 } 00358 00359 00360 // ********************************************************************* 00361 // ********************************************************************* 00362 // CLASS: Model2DRigidCarSmooth 00363 // Smooth steering 00364 // ********************************************************************* 00365 // ********************************************************************* 00366 00367 00368 Model2DRigidCarSmooth::Model2DRigidCarSmooth(string path = ""):Model2DRigidCar(path) { 00369 00370 StateDim = 4; 00371 InputDim = 2; 00372 00373 LowerState = MSLVector(4); 00374 UpperState = MSLVector(4); 00375 00376 READ_PARAMETER_OR_DEFAULT(SteeringSpeed,0.05); 00377 00378 00379 LowerState[0] = 0.0; LowerState[1] = 0.0; LowerState[2] = 0.0; 00380 LowerState[3] = -MaxSteeringAngle; 00381 READ_OPTIONAL_PARAMETER(LowerState); 00382 00383 UpperState[0] = 100.0; UpperState[1] = 100.0; UpperState[2] = 2.0*PI; 00384 UpperState[3] = MaxSteeringAngle; 00385 READ_OPTIONAL_PARAMETER(LowerState); 00386 00387 // Make the list of Inputs 00388 Inputs.clear(); // Otherwise its parent constructor will make some inputs 00389 Inputs.push_back(MSLVector(1.0,0.0)); // Keep the steering angle fixed 00390 Inputs.push_back(MSLVector(-1.0,0.0)); // Keep the steering angle fixed 00391 Inputs.push_back(MSLVector(1.0,SteeringSpeed)); 00392 Inputs.push_back(MSLVector(-1.0,SteeringSpeed)); 00393 Inputs.push_back(MSLVector(1.0,-SteeringSpeed)); 00394 Inputs.push_back(MSLVector(-1.0,-SteeringSpeed)); 00395 00396 READ_OPTIONAL_PARAMETER(Inputs); 00397 00398 } 00399 00400 00401 00402 MSLVector Model2DRigidCarSmooth::StateTransitionEquation(const MSLVector &x, const MSLVector &u) { 00403 00404 MSLVector dx(4); 00405 00406 dx[0] = u[0]*cos(x[2]); 00407 dx[1] = u[0]*sin(x[2]); 00408 dx[2] = u[0]*tan(x[3])/CarLength; 00409 dx[3] = u[1]; 00410 00411 //cout << "DX: " << dx << "\n"; 00412 00413 return dx; 00414 } 00415 00416 00417 00418 00419 MSLVector Model2DRigidCarSmooth::Integrate(const MSLVector &x, const MSLVector &u, const double &h) 00420 { 00421 return RungeKuttaIntegrate(x,u,h); 00422 } 00423 00424 00425 double Model2DRigidCarSmooth::Metric(const MSLVector &x1, const MSLVector &x2) { 00426 00427 double rho,dphi,dtheta; 00428 00429 dphi = min(fabs(x1[3]-x2[3]),2.0*PI - fabs(x1[3]-x2[3])); 00430 dtheta = min(fabs(x1[2]-x2[2]),2.0*PI - fabs(x1[2]-x2[2])); 00431 00432 rho = sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + 00433 sqr(2.0/PI*dphi) + 00434 sqr(50.0/PI*dtheta)); 00435 00436 return rho; 00437 } 00438 00439 00440 MSLVector Model2DRigidCarSmooth::StateToConfiguration(const MSLVector &x) 00441 { 00442 MSLVector q(3); 00443 00444 q[0] = x[0]; q[1] = x[1]; q[2] = x[2]; 00445 00446 return q; 00447 } 00448 00449 00450 bool Model2DRigidCarSmooth::Satisfied(const MSLVector &x) 00451 { 00452 return ((x[3] < UpperState[3])&& // Steering is too sharp! 00453 (x[3] > LowerState[3])); 00454 } 00455 00456 00457 // ********************************************************************* 00458 // ********************************************************************* 00459 // CLASS: Model2DRigidCarSmoothTrailer 00460 // Smooth steering 00461 // ********************************************************************* 00462 // ********************************************************************* 00463 00464 00465 Model2DRigidCarSmoothTrailer::Model2DRigidCarSmoothTrailer(string path = ""):Model2DRigidCarSmooth(path) { 00466 00467 StateDim = 5; 00468 InputDim = 2; 00469 00470 LowerState = MSLVector(5); 00471 UpperState = MSLVector(5); 00472 00473 READ_PARAMETER_OR_DEFAULT(HitchLength,10.0); 00474 READ_PARAMETER_OR_DEFAULT(HitchMaxAngle,PI/2.0); // From 0 to PI 00475 00476 00477 LowerState[0] = 0.0; LowerState[1] = 0.0; LowerState[2] = 0.0; 00478 LowerState[3] = -MaxSteeringAngle; LowerState[4] = 0.0; 00479 READ_OPTIONAL_PARAMETER(LowerState); 00480 00481 UpperState[0] = 100.0; UpperState[1] = 100.0; UpperState[2] = 2.0*PI; 00482 UpperState[3] = MaxSteeringAngle; UpperState[4] = 2.0*PI; 00483 READ_OPTIONAL_PARAMETER(UpperState); 00484 00485 } 00486 00487 00488 00489 MSLVector Model2DRigidCarSmoothTrailer::StateTransitionEquation(const MSLVector &x, const MSLVector &u) { 00490 00491 MSLVector dx(5); 00492 00493 dx[0] = u[0]*cos(x[2]); 00494 dx[1] = u[0]*sin(x[2]); 00495 dx[2] = u[0]*tan(x[3])/CarLength; 00496 dx[3] = u[1]; 00497 dx[4] = u[0]*sin(x[2] - x[4])/HitchLength; 00498 return dx; 00499 } 00500 00501 00502 00503 00504 double Model2DRigidCarSmoothTrailer::Metric(const MSLVector &x1, const MSLVector &x2) { 00505 00506 double rho,dphi,dtheta,dtheta1; 00507 00508 dphi = min(fabs(x1[3]-x2[3]),2.0*PI - fabs(x1[3]-x2[3])); 00509 dtheta = min(fabs(x1[2]-x2[2]),2.0*PI - fabs(x1[2]-x2[2])); 00510 dtheta1 = min(fabs(x1[4]-x2[4]),2.0*PI - fabs(x1[4]-x2[4])); 00511 00512 rho = sqrt(sqr(x1[0] - x2[0]) + 00513 sqr(x1[1] - x2[1]) + 00514 sqr(2.0/PI*dphi) + 00515 sqr(5.0/PI*dtheta) + 00516 sqr(5.0/PI*dtheta1)); 00517 00518 return rho; 00519 } 00520 00521 00522 MSLVector Model2DRigidCarSmoothTrailer::StateToConfiguration(const MSLVector &x) 00523 { 00524 MSLVector q(6); // Two bodies 00525 00526 // The car config 00527 q[0] = x[0]; q[1] = x[1]; q[2] = x[2]; 00528 00529 // The trailer config 00530 q[3] = -cos(x[4])*HitchLength+x[0]; 00531 q[4] = -sin(x[4])*HitchLength+x[1]; 00532 q[5] = x[4]; 00533 00534 return q; 00535 } 00536 00537 00538 bool Model2DRigidCarSmoothTrailer::Satisfied(const MSLVector &x) 00539 { 00540 return ((x[3] < UpperState[3])&& // Steering is too sharp! 00541 (x[3] > LowerState[3])&& 00542 (cos(x[2]-x[4]) >= cos(HitchMaxAngle))); 00543 } 00544 00545 00546 // ********************************************************************* 00547 // ********************************************************************* 00548 // CLASS: Model2DRigidCarSmooth2Trailers 00549 // Smooth steering 00550 // ********************************************************************* 00551 // ********************************************************************* 00552 00553 00554 Model2DRigidCarSmooth2Trailers::Model2DRigidCarSmooth2Trailers(string path = ""):Model2DRigidCarSmoothTrailer(path) { 00555 00556 StateDim = 6; 00557 InputDim = 2; 00558 00559 LowerState = MSLVector(6); 00560 UpperState = MSLVector(6); 00561 00562 READ_PARAMETER_OR_DEFAULT(Hitch2Length,10.0); 00563 READ_PARAMETER_OR_DEFAULT(Hitch2MaxAngle,PI/2.0); // From 0 to PI 00564 00565 LowerState[0] = 0.0; LowerState[1] = 0.0; LowerState[2] = 0.0; 00566 LowerState[3] = -MaxSteeringAngle; LowerState[4] = 0.0; 00567 LowerState[5] = 0.0; 00568 READ_OPTIONAL_PARAMETER(LowerState); 00569 00570 UpperState[0] = 100.0; UpperState[1] = 100.0; UpperState[2] = 2.0*PI; 00571 UpperState[3] = MaxSteeringAngle; UpperState[4] = 2.0*PI; 00572 UpperState[5] = 2.0*PI; 00573 READ_OPTIONAL_PARAMETER(UpperState); 00574 00575 } 00576 00577 00578 00579 MSLVector Model2DRigidCarSmooth2Trailers::StateTransitionEquation(const MSLVector &x, const MSLVector &u) { 00580 00581 MSLVector dx(6); 00582 00583 dx[0] = u[0]*cos(x[2]); 00584 dx[1] = u[0]*sin(x[2]); 00585 dx[2] = u[0]*tan(x[3])/CarLength; 00586 dx[3] = u[1]; 00587 dx[4] = u[0]*sin(x[2] - x[4])/HitchLength; 00588 dx[5] = u[0]*cos(x[2] - x[4])*sin(x[4] - x[5])/Hitch2Length; 00589 return dx; 00590 } 00591 00592 00593 00594 double Model2DRigidCarSmooth2Trailers::Metric(const MSLVector &x1, const MSLVector &x2) { 00595 00596 double rho,dphi,dtheta,dtheta1,dtheta2; 00597 00598 dphi = min(fabs(x1[3]-x2[3]),2.0*PI - fabs(x1[3]-x2[3])); 00599 dtheta = min(fabs(x1[2]-x2[2]),2.0*PI - fabs(x1[2]-x2[2])); 00600 dtheta1 = min(fabs(x1[4]-x2[4]),2.0*PI - fabs(x1[4]-x2[4])); 00601 dtheta2 = min(fabs(x1[5]-x2[5]),2.0*PI - fabs(x1[5]-x2[5])); 00602 00603 rho = sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + 00604 sqr(2.0/PI*dphi) + 00605 sqr(5.0/PI*dtheta) + 00606 sqr(5.0/PI*dtheta1) + 00607 sqr(5.0/PI*dtheta2)); 00608 00609 return rho; 00610 } 00611 00612 00613 MSLVector Model2DRigidCarSmooth2Trailers::StateToConfiguration(const MSLVector &x) 00614 { 00615 MSLVector q(9); // Three bodies 00616 00617 // The car config 00618 q[0] = x[0]; q[1] = x[1]; q[2] = x[2]; 00619 00620 // The 1st trailer config 00621 q[3] = -cos(x[4])*HitchLength+x[0]; 00622 q[4] = -sin(x[4])*HitchLength+x[1]; 00623 q[5] = x[4]; 00624 00625 // The 2nd trailer config 00626 q[6] = -cos(x[5])*Hitch2Length+q[3]; 00627 q[7] = -sin(x[5])*Hitch2Length+q[4]; 00628 q[8] = x[5]; 00629 00630 return q; 00631 } 00632 00633 00634 bool Model2DRigidCarSmooth2Trailers::Satisfied(const MSLVector &x) 00635 { 00636 return ((x[3] < UpperState[3])&& // Steering is too sharp! 00637 (x[3] > LowerState[3])&& 00638 (cos(x[2]-x[4]) >= cos(HitchMaxAngle))&& 00639 (cos(x[4]-x[5]) >= cos(Hitch2MaxAngle))); 00640 } 00641 00642 00643 // ********************************************************************* 00644 // ********************************************************************* 00645 // CLASS: Model2DRigidCarSmooth3Trailers 00646 // Smooth steering 00647 // ********************************************************************* 00648 // ********************************************************************* 00649 00650 00651 Model2DRigidCarSmooth3Trailers::Model2DRigidCarSmooth3Trailers(string path = ""):Model2DRigidCarSmooth2Trailers(path) { 00652 00653 StateDim = 7; 00654 InputDim = 2; 00655 00656 LowerState = MSLVector(7); 00657 UpperState = MSLVector(7); 00658 00659 READ_PARAMETER_OR_DEFAULT(Hitch3Length,10.0); 00660 READ_PARAMETER_OR_DEFAULT(Hitch3MaxAngle,PI/2.0); // From 0 to PI 00661 00662 LowerState[0] = 0.0; LowerState[1] = 0.0; LowerState[2] = 0.0; 00663 LowerState[3] = -MaxSteeringAngle; LowerState[4] = 0.0; 00664 LowerState[5] = 0.0; LowerState[6] = 0.0; 00665 READ_OPTIONAL_PARAMETER(LowerState); 00666 00667 UpperState[0] = 100.0; UpperState[1] = 100.0; UpperState[2] = 2.0*PI; 00668 UpperState[3] = MaxSteeringAngle; UpperState[4] = 2.0*PI; 00669 UpperState[5] = 2.0*PI; UpperState[6] = 2.0*PI; 00670 READ_OPTIONAL_PARAMETER(UpperState); 00671 00672 } 00673 00674 00675 00676 MSLVector Model2DRigidCarSmooth3Trailers::StateTransitionEquation(const MSLVector &x, const MSLVector &u) { 00677 00678 MSLVector dx(7); 00679 00680 dx[0] = u[0]*cos(x[2]); 00681 dx[1] = u[0]*sin(x[2]); 00682 dx[2] = u[0]*tan(x[3])/CarLength; 00683 dx[3] = u[1]; 00684 dx[4] = u[0]*sin(x[2] - x[4])/HitchLength; 00685 dx[5] = u[0]*cos(x[2] - x[4])*sin(x[4] - x[5])/Hitch2Length; 00686 dx[6] = u[0]*cos(x[2] - x[4])*cos(x[4] - x[5])*sin(x[5]-x[6])/Hitch3Length; 00687 return dx; 00688 } 00689 00690 00691 00692 00693 double Model2DRigidCarSmooth3Trailers::Metric(const MSLVector &x1, const MSLVector &x2) { 00694 00695 double rho,dphi,dtheta,dtheta1,dtheta2,dtheta3; 00696 00697 dphi = min(fabs(x1[3]-x2[3]),2.0*PI - fabs(x1[3]-x2[3])); 00698 dtheta = min(fabs(x1[2]-x2[2]),2.0*PI - fabs(x1[2]-x2[2])); 00699 dtheta1 = min(fabs(x1[4]-x2[4]),2.0*PI - fabs(x1[4]-x2[4])); 00700 dtheta2 = min(fabs(x1[5]-x2[5]),2.0*PI - fabs(x1[5]-x2[5])); 00701 dtheta3 = min(fabs(x1[6]-x2[6]),2.0*PI - fabs(x1[6]-x2[6])); 00702 00703 rho = sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + 00704 sqr(2.0/PI*dphi) + 00705 sqr(5.0/PI*dtheta) + 00706 sqr(5.0/PI*dtheta1) + 00707 sqr(5.0/PI*dtheta2) + 00708 sqr(5.0/PI*dtheta3)); 00709 00710 return rho; 00711 } 00712 00713 00714 00715 MSLVector Model2DRigidCarSmooth3Trailers::StateToConfiguration(const MSLVector &x) 00716 { 00717 MSLVector q(12); // Four bodies 00718 00719 // The car config 00720 q[0] = x[0]; q[1] = x[1]; q[2] = x[2]; 00721 00722 // The 1st trailer config 00723 q[3] = -cos(x[4])*HitchLength+x[0]; 00724 q[4] = -sin(x[4])*HitchLength+x[1]; 00725 q[5] = x[4]; 00726 00727 // The 2nd trailer config 00728 q[6] = -cos(x[5])*Hitch2Length+q[3]; 00729 q[7] = -sin(x[5])*Hitch2Length+q[4]; 00730 q[8] = x[5]; 00731 00732 // The 3rd trailer config 00733 q[9] = -cos(x[6])*Hitch3Length+q[6]; 00734 q[10] = -sin(x[6])*Hitch3Length+q[7]; 00735 q[11] = x[6]; 00736 00737 return q; 00738 } 00739 00740 00741 bool Model2DRigidCarSmooth3Trailers::Satisfied(const MSLVector &x) 00742 { 00743 return ((x[3] < UpperState[3])&& // Steering is too sharp! 00744 (x[3] > LowerState[3])&& 00745 (cos(x[2]-x[4]) >= cos(HitchMaxAngle))&& 00746 (cos(x[4]-x[5]) >= cos(Hitch2MaxAngle))&& 00747 (cos(x[5]-x[6]) >= cos(Hitch3MaxAngle))); 00748 } 00749 00750 00751 // ********************************************************************* 00752 // ********************************************************************* 00753 // CLASS: Model2DRigidDyncar 00754 // 00755 // ********************************************************************* 00756 // ********************************************************************* 00757 00758 // Constructor 00759 Model2DRigidDyncar::Model2DRigidDyncar(string path = ""):Model2DRigid(path) { 00760 double alpha; 00761 MSLVector v; 00762 00763 StateDim = 5; 00764 InputDim = 1; 00765 Mass = 100.0; 00766 CAF = 17000.0; 00767 CAR = 20000.0; 00768 Adist = 4.0; 00769 Bdist = 5.0; 00770 Izz = 1600.0; 00771 00772 WorldScale = 0.1; 00773 Speed = 88.0; // Feet per sec 00774 00775 LowerState = MSLVector(5); 00776 UpperState = MSLVector(5); 00777 00778 LowerState[0] = -50.0; LowerState[1] = -5.0; LowerState[2] = 0.0; 00779 LowerState[3] = -1000.0; LowerState[4] = 0.0; 00780 READ_OPTIONAL_PARAMETER(LowerState); 00781 00782 UpperState[0] = 50.0; UpperState[1] = 5.0; UpperState[2] = 1000.0; 00783 UpperState[3] = 0.0; UpperState[4] = 2.0*PI; 00784 READ_OPTIONAL_PARAMETER(UpperState); 00785 00786 MaxSteeringAngle = 0.6; 00787 00788 // Make the list of 1D Inputs 00789 Inputs.clear(); // Otherwise its parent constructor will make some inputs 00790 for (alpha = -MaxSteeringAngle; alpha <= MaxSteeringAngle; 00791 alpha += 2.0*MaxSteeringAngle/24.0) { 00792 v = MSLVector(1); v[0] = alpha; 00793 Inputs.push_back(v); 00794 } 00795 00796 READ_OPTIONAL_PARAMETER(Inputs); 00797 00798 } 00799 00800 00801 // This is xdot = f(x,u) for a 5dof state-space, and 1dof input 00802 // Model taken from Jim Bernard... 00803 // alphaf = (v + a*r)/u - deltaf 00804 // alphar = (v-b*r)/u 00805 // FYF = -CAF*alphaf 00806 // FYR = -CAR*alphar 00807 // vdot = -u*r + (FYF + FYR)/m 00808 // rdot = (FYF*a - FYR*b)/Izz 00809 // Xdot = u*cos(psi) - v*sin(psi) 00810 // Ydot = u*sin(spi) + v*cos(psi) 00811 // psidot = r 00812 // 00813 // m = mass of car, say about 100 slugs 00814 // CAF = front cornering stiffness in pounds per radiaof the tires, say about 00815 // 17000 00816 // CAR = rear cornering stiffness, say about 20000 00817 // a is dist from mass center to front tires, say 4 feet 00818 // b is dist from mass center to rear tires, say 5 feet 00819 // Izz is yaw moment of intertia, say about 1600 slug ft**2 00820 // u is forward speed which is assumed constant, input is in feet/sec 00821 // delta is your input, it is the steer angle of the tires in radians. 00822 00823 MSLVector Model2DRigidDyncar::StateTransitionEquation(const MSLVector &x, const MSLVector &u) 00824 { 00825 double alphaf,alphar,fyf,fyr,v,r,psi; 00826 MSLVector dx(5); 00827 00828 v = x[0]; r = x[1]; psi = x[4]; 00829 00830 alphaf = (v + Adist * r) / Speed - u[0]; 00831 alphar = (v - Bdist * r) / Speed; 00832 fyf = -CAF * alphaf; 00833 fyr = -CAR * alphar; 00834 00835 /* Transfer the velocity */ 00836 dx[0] = -Speed * r + (fyf + fyr) / Mass; 00837 dx[1] = (fyf * Adist - fyr * Bdist) / Izz; 00838 dx[2] = Speed * cos(psi) - v * sin(psi); 00839 dx[3] = Speed * sin(psi) + v * cos(psi); 00840 dx[4] = r; 00841 00842 //cout << "x: " << x << " Dx: " << dx << " u: " << u[0] << "\n"; 00843 00844 return dx; 00845 } 00846 00847 00848 00849 double Model2DRigidDyncar::Metric(const MSLVector &x1, const MSLVector &x2) { 00850 double d; 00851 00852 // Position difference 00853 d = sqr((x1[2] - x2[2]) / (UpperState[2] - LowerState[2])); 00854 d += sqr((x1[3] - x2[3]) / (UpperState[3] - LowerState[3])); 00855 00856 // Orientation difference 00857 d += sqr(min(fabs(x1[4]-x2[4]),2.0*PI - fabs(x1[4]-x2[4]))/2.0/PI); 00858 00859 // Velocities 00860 d += sqr((x1[0] - x2[0]) / (UpperState[0] - LowerState[0])); 00861 d += sqr((x1[1] - x2[1]) / (UpperState[1] - LowerState[1])); 00862 00863 00864 return sqrt(d); 00865 } 00866 00867 00868 00869 MSLVector Model2DRigidDyncar::Integrate(const MSLVector &x, const MSLVector &u, const double &h) 00870 { 00871 return RungeKuttaIntegrate(x,u,h); 00872 } 00873 00874 00875 // NOTE: This neglects the S^1 effect of orientation!! 00876 MSLVector Model2DRigidDyncar::StateToConfiguration(const MSLVector &x) { 00877 return MSLVector(x[2]*WorldScale,-x[3]*WorldScale,2.0*PI-x[4]); 00878 } 00879 00880 00881 MSLVector Model2DRigidDyncar::LinearInterpolate(const MSLVector &x1, 00882 const MSLVector &x2, 00883 const double &a) { 00884 00885 MSLVector v; 00886 00887 v = (1.0-a)*x1 + a*x2; 00888 00889 if (fabs(x2[4] - x1[4]) > PI) { 00890 if (x1[4] > x2[4]) 00891 v[4] = (1.0-a)*x1[4] + a*(x2[4]+2.0*PI); 00892 else 00893 v[4] = (1.0-a)*(x1[4]+2.0*PI) + a*x2[4]; 00894 } 00895 00896 if (v[4] > 2.0*PI) 00897 v[4] -= 2.0*PI; 00898 00899 return v; 00900 } 00901 00902 00903 00904 // ********************************************************************* 00905 // ********************************************************************* 00906 // CLASS: Model2DRigidDyncarNtire 00907 // 00908 // ********************************************************************* 00909 // ********************************************************************* 00910 00911 // Constructor 00912 Model2DRigidDyncarNtire::Model2DRigidDyncarNtire(string path = ""):Model2DRigidDyncar(path) { 00913 double alpha; 00914 MSLVector v; 00915 00916 StateDim = 5; 00917 InputDim = 2; 00918 00919 // These are the exact parameters from Ric's model 00920 Mass = 3518.0/32.2; 00921 Adist = 0.45*100.5/12.0; 00922 Bdist = 0.55*100.5/12.0; 00923 Izz = 0.25*Mass*(Adist+Bdist)*(Adist+Bdist); 00924 00925 // Constants for the nonlinear tire model 00926 Mu = 0.85; 00927 Nf = Mass*32.2*0.55; 00928 Nr = Mass*32.2*0.45; 00929 00930 // Make the list of 2D Inputs 00931 // Apparently, this models allows rear-wheel steering 00932 // This option is set to zero for now...hence v[1]=0.0 00933 Inputs.clear(); // Otherwise its parent constructor will make some inputs 00934 for (alpha = -MaxSteeringAngle; alpha <= MaxSteeringAngle; 00935 alpha += 2.0*MaxSteeringAngle/24.0) { 00936 v = MSLVector(2); v[0] = alpha; v[1] = 0.0; 00937 Inputs.push_back(v); 00938 } 00939 00940 READ_OPTIONAL_PARAMETER(Inputs); 00941 00942 } 00943 00944 00945 // The is the model from Jim Bernard and Ric Menendez 00946 MSLVector Model2DRigidDyncarNtire::StateTransitionEquation(const MSLVector &x, const MSLVector &u) 00947 { 00948 double alphaf,alphar,fyf,fyr,v,r,psi; 00949 double talff,talfr,xiblf,xiblr; 00950 MSLVector dx(5); 00951 00952 00953 v = x[0]; r = x[1]; psi = x[4]; 00954 00955 alphaf = (v + Adist * r) / Speed - u[0]; 00956 alphar = (v - Bdist * r) / Speed - u[1]; 00957 00958 // Here is where the nonlinear tire model is used 00959 // The result is new values for fyf and fyr 00960 talff = tan(fabs(alphaf)); 00961 talfr = tan(fabs(alphar)); 00962 xiblf = (CAF*talff == 0) ? 00963 INFINITY : 00964 Mu*Nf/(2.0*CAF*talff); 00965 xiblr = (CAR*talfr == 0) ? 00966 INFINITY : 00967 Mu*Nr/(2.0*CAR*talfr); 00968 fyf = (xiblf >= 1.0) ? 00969 CAF*talff : 00970 Mu*Nf*(1.0-0.5*xiblf); 00971 fyr = (xiblr >= 1.0) ? 00972 CAR*talfr : 00973 Mu*Nr*(1.0-0.5*xiblr); 00974 fyf = (alphaf > 0) ? -1.0*fabs(fyf) : fabs(fyf); 00975 fyr = (alphar > 0) ? -1.0*fabs(fyr) : fabs(fyr); 00976 00977 //cout << "talff: " << talff << " xiblf: " << xiblf << " fyf: " << fyf << "\n"; 00978 00979 /* Transfer the velocity */ 00980 dx[0] = -Speed * r + (fyf + fyr) / Mass; 00981 dx[1] = (fyf * Adist - fyr * Bdist) / Izz; 00982 dx[2] = Speed * cos(psi) - v * sin(psi); 00983 dx[3] = Speed * sin(psi) + v * cos(psi); 00984 dx[4] = r; 00985 00986 //cout << "x: " << x << " Dx: " << dx << " u: " << u[0] << "\n"; 00987 00988 return dx; 00989 } 00990 00991 00992 00993 00994 00995 00996 00997 // ********************************************************************* 00998 // ********************************************************************* 00999 // CLASS: Model2DRigidLander 01000 // 01001 // ********************************************************************* 01002 // ********************************************************************* 01003 01004 // Constructor 01005 Model2DRigidLander::Model2DRigidLander(string path = ""):Model2DRigid(path) { 01006 MSLVector v; 01007 01008 StateDim = 4; 01009 InputDim = 1; 01010 Mass = 1.0; 01011 G = 1.568; // Accel of gravity on moon (use 9.8 for earth) 01012 Fs = 10.0; 01013 Fu = 20.0; 01014 01015 LowerState = MSLVector(4); 01016 UpperState = MSLVector(4); 01017 01018 LowerState[0] = 0.0; LowerState[1] = 0.0; LowerState[2] = -10.0; 01019 LowerState[3] = -10.0; 01020 READ_OPTIONAL_PARAMETER(LowerState); 01021 01022 UpperState[0] = 100.0; UpperState[1] = 100.0; UpperState[2] = 10.0; 01023 UpperState[3] = 10.0; 01024 READ_OPTIONAL_PARAMETER(UpperState); 01025 01026 // Make the list of 1D Inputs 01027 Inputs.clear(); // Otherwise its parent constructor will make some inputs 01028 v = MSLVector(1); v[0] = 0; 01029 Inputs.push_back(v); 01030 v = MSLVector(1); v[0] = 1; 01031 Inputs.push_back(v); 01032 v = MSLVector(1); v[0] = 2; 01033 Inputs.push_back(v); 01034 v = MSLVector(1); v[0] = 3; 01035 Inputs.push_back(v); 01036 01037 READ_OPTIONAL_PARAMETER(Inputs); 01038 } 01039 01040 01041 01042 MSLVector Model2DRigidLander::Integrate(const MSLVector &x, const MSLVector &u, const double &h) 01043 { 01044 return RungeKuttaIntegrate(x,u,h); 01045 } 01046 01047 01048 01049 MSLVector Model2DRigidLander::StateToConfiguration(const MSLVector &x) { 01050 return MSLVector(x[0],x[1],0.0); // Yield a zero rotation every time 01051 } 01052 01053 01054 MSLVector Model2DRigidLander::StateTransitionEquation(const MSLVector &x, const MSLVector &u) 01055 { 01056 MSLVector dx(4); 01057 01058 /* Transfer the velocity */ 01059 dx[0] = x[2]; 01060 dx[1] = x[3]; 01061 dx[2] = 0.0; 01062 if (u[0] == 1) 01063 dx[2] = Fs; 01064 if (u[0] == 3) 01065 dx[2] = -Fs; 01066 dx[3] = -Mass*G; 01067 if (u[0] == 2) 01068 dx[3] += Fu; 01069 01070 //cout << "x: " << x << " Dx: " << dx << " u: " << u[0] << "\n"; 01071 01072 return dx; 01073 } 01074 01075 01076 01077 01078 01079 double Model2DRigidLander::Metric(const MSLVector &x1, const MSLVector &x2) { 01080 double d = 0.0; 01081 int i; 01082 01083 for (i = 0; i < 4; i++) { 01084 d += sqrt(sqr(x1[i] - x2[i]) / (UpperState[i] - LowerState[i])); 01085 } 01086 01087 //cout << "x1: " << x1 << " x2: " << x2 << " Metric: " << d << "\n"; 01088 01089 return d; 01090 } 01091 01092 01093 01094 01095 // ********************************************************************* 01096 // ********************************************************************* 01097 // CLASS: Model2DRigidMulti 01098 // 01099 // ********************************************************************* 01100 // ********************************************************************* 01101 01102 01103 // Constructor 01104 Model2DRigidMulti::Model2DRigidMulti(string path = ""):Model2DRigid(path) { 01105 MSLVector u; 01106 int i,j; 01107 01108 READ_PARAMETER_OR_ERROR(NumBodies); 01109 01110 StateDim = 3*NumBodies; 01111 InputDim = 3*NumBodies; 01112 01113 READ_PARAMETER_OR_DEFAULT(LowerState,MSLVector(StateDim)); 01114 READ_PARAMETER_OR_DEFAULT(UpperState,MSLVector(StateDim)); 01115 01116 u = MSLVector(StateDim); 01117 Inputs.clear(); 01118 for (i = 0; i < StateDim; i++) { 01119 for (j = 0; j < StateDim; j++) 01120 u[j] = (i==j) ? 1.0 : 0.0; 01121 Inputs.push_back(u); 01122 for (j = 0; j < StateDim; j++) 01123 u[j] = (i==j) ? -1.0 : 0.0; 01124 Inputs.push_back(u); 01125 } 01126 } 01127 01128 01129 01130 01131 double Model2DRigidMulti::Metric(const MSLVector &x1, const MSLVector &x2) { 01132 01133 double d,fd,dtheta; 01134 int i; 01135 01136 d = 0.0; 01137 01138 for (i = 0; i < NumBodies; i++) { 01139 fd = fabs(x1[3*i+2]-x2[3*i+2]); 01140 dtheta = min(fd,2.0*PI - fd); 01141 d += sqr(x1[3*i] - x2[3*i]); 01142 d += sqr(x1[3*i+1] - x2[3*i+1]); 01143 d += sqr(dtheta); 01144 } 01145 01146 return sqrt(d); 01147 } 01148 01149 01150 01151 MSLVector Model2DRigidMulti::LinearInterpolate(const MSLVector &x1, const MSLVector &x2, const double &a){ 01152 MSLVector v; 01153 int i; 01154 01155 v = (1.0-a)*x1 + a*x2; 01156 01157 for (i = 0; i < NumBodies; i++) { 01158 01159 if (fabs(x2[3*i+2] - x1[3*i+2]) > PI) { 01160 if (x1[3*i+2] > x2[3*i+2]) 01161 v[3*i+2] = (1.0-a)*x1[3*i+2] + a*(x2[3*i+2]+2.0*PI); 01162 else 01163 v[3*i+2] = (1.0-a)*(x1[3*i+2]+2.0*PI) + a*x2[3*i+2]; 01164 } 01165 01166 if (v[3*i+2] > PI) 01167 v[3*i+2] -= 2.0*PI; 01168 01169 } 01170 01171 return v; 01172 01173 } 01174 01175 01176 01177 MSLVector Model2DRigidMulti::StateToConfiguration(const MSLVector &x) 01178 { 01179 return x; 01180 } 01181 01182 01183 01184 01185 // ********************************************************************* 01186 // ********************************************************************* 01187 // CLASS: Model2DRigidChain 01188 // 01189 // ********************************************************************* 01190 // ********************************************************************* 01191 01192 Model2DRigidChain::Model2DRigidChain(string path = ""):Model2DRigid(path) { 01193 01194 int i; 01195 01196 READ_PARAMETER_OR_ERROR(NumBodies); 01197 01198 StopAngle = PI/1.5; 01199 01200 StateDim = NumBodies+2; 01201 InputDim = NumBodies+2; 01202 01203 READ_PARAMETER_OR_ERROR(A); 01204 01205 LowerState = MSLVector(NumBodies+2); 01206 UpperState = MSLVector(NumBodies+2); 01207 01208 LowerState[0] = 0.0; 01209 LowerState[1] = 0.0; 01210 for (i = 0; i < NumBodies; i++) { 01211 LowerState[i+2] = -StopAngle; 01212 } 01213 READ_OPTIONAL_PARAMETER(LowerState); 01214 01215 UpperState[0] = 100.0; 01216 UpperState[1] = 100.0; 01217 for (i = 0; i < NumBodies; i++) { 01218 UpperState[i+2] = StopAngle; 01219 } 01220 READ_OPTIONAL_PARAMETER(LowerState); 01221 01222 01223 Inputs.clear(); // Otherwise its parent constructor will make some inputs 01224 // NEED A DEFAULT HERE!!!! 01225 01226 READ_OPTIONAL_PARAMETER(Inputs); 01227 01228 } 01229 01230 01231 01232 MSLVector Model2DRigidChain::LinearInterpolate(const MSLVector &x1, 01233 const MSLVector &x2, 01234 const double &a) { 01235 return (1.0-a)*x1 + a*x2; 01236 } 01237 01238 01239 01240 MSLVector Model2DRigidChain::StateToConfiguration(const MSLVector &x) { 01241 MSLVector q; 01242 int i; 01243 double lx,ly,ltheta; 01244 01245 q = MSLVector(3*NumBodies); 01246 q[0] = x[0]; 01247 q[1] = x[1]; 01248 q[2] = x[2]; 01249 01250 for (i = 1; i < NumBodies; i++) { 01251 lx = q[3*(i-1)]; 01252 ly = q[3*(i-1)+1]; 01253 ltheta = q[3*(i-1)+2]; 01254 01255 q[3*i] = cos(ltheta)*A[i-1]+lx; 01256 q[3*i+1] = sin(ltheta)*A[i-1]+ly; 01257 q[3*i+2] = atan2(cos(ltheta)*sin(x[2+i])+sin(ltheta)*cos(x[2+i]), 01258 cos(ltheta)*cos(x[2+i])-sin(ltheta)*sin(x[2+i])); 01259 if (q[3*i+2] < 0.0) 01260 q[3*i+2] += 2.0*PI; // Force the orientation into [0,2pi) 01261 } 01262 01263 return q; 01264 } 01265 01266 01267 01268 01269 MSLVector Model2DRigidChain::StateTransitionEquation(const MSLVector &x, const MSLVector &u) { 01270 01271 MSLVector dx(StateDim); 01272 01273 dx = u; 01274 return dx; 01275 } 01276 01277 01278 01279 double Model2DRigidChain::Metric(const MSLVector &x1, const MSLVector &x2) { 01280 01281 double rho; 01282 MSLVector dtheta(StateDim); 01283 int i; 01284 01285 rho = sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]); 01286 01287 for (i = 2; i < StateDim; i++) { 01288 dtheta[i] = min(fabs(x1[i]-x2[i]),2.0*PI - fabs(x1[i]-x2[i])); 01289 rho += sqr(50.0/PI*dtheta[i]); 01290 } 01291 01292 rho = sqrt(rho); 01293 01294 return rho; 01295 } 01296 01297 01298 // Make sure the joint position and limits are respected 01299 bool Model2DRigidChain::Satisfied(const MSLVector &x) 01300 { 01301 int i; 01302 01303 for (i = 0; i < StateDim; i++) 01304 if ((x[i] > UpperState[i]) || (x[i] < LowerState[i])) 01305 return false; 01306 01307 return true; 01308 } 01309 01310 01311