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