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

model2d.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 "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 
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.