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