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

model3d.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 "model3d.h"
00023 #include "defs.h"
00024 
00025 #include <LEDA/REDEFINE_NAMES.h>
00026 
00027 // *********************************************************************
00028 // *********************************************************************
00029 // CLASS:     Model3D
00030 // 
00031 // *********************************************************************
00032 // *********************************************************************
00033 
00034 
00035 Model3D::Model3D(string path = ""):Model(path) {
00036 }
00037 
00038 
00039 
00040 // *********************************************************************
00041 // *********************************************************************
00042 // CLASS:     Model3DRigid
00043 // 
00044 // *********************************************************************
00045 // *********************************************************************
00046 
00047 
00048 // Constructor
00049 Model3DRigid::Model3DRigid(string path = ""):Model3D(path) {
00050   vector u;
00051 
00052   StateDim = 6;
00053   InputDim = 6;
00054 
00055   if (is_file(FilePath+"LowerState"))
00056     ReadLowerState();
00057   else {
00058     LowerState = vector(StateDim);
00059   }
00060 
00061   if (is_file(FilePath+"UpperState"))
00062     ReadUpperState();
00063   else {
00064     UpperState = vector(StateDim);
00065   }
00066 
00067   // Make inputs
00068   u = vector(6);
00069   u[0] = 1.0; u[1] = 0.0; u[2] = 0.0; u[3] = 0.0; u[4] = 0.0; u[5] = 0.0;
00070   Inputs.push(u);
00071 
00072   u = vector(6);
00073   u[0] = -1.0; u[1] = 0.0; u[2] = 0.0; u[3] = 0.0; u[4] = 0.0; u[5] = 0.0;
00074   Inputs.push(u);
00075 
00076   u = vector(6);
00077   u[0] = 0.0; u[1] = 1.0; u[2] = 0.0; u[3] = 0.0; u[4] = 0.0; u[5] = 0.0;
00078   Inputs.push(u);
00079 
00080   u = vector(6);
00081   u[0] = 0.0; u[1] = -1.0; u[2] = 0.0; u[3] = 0.0; u[4] = 0.0; u[5] = 0.0;
00082   Inputs.push(u);
00083 
00084   u = vector(6);
00085   u[0] = 0.0; u[1] = 0.0; u[2] = 1.0; u[3] = 0.0; u[4] = 0.0; u[5] = 0.0;
00086   Inputs.push(u);
00087 
00088   u = vector(6);
00089   u[0] = 0.0; u[1] = 0.0; u[2] = -1.0; u[3] = 0.15; u[4] = 0.0; u[5] = 0.0;
00090   Inputs.push(u);
00091 
00092   u = vector(6);
00093   u[0] = 0.0; u[1] = 0.0; u[2] = -1.0; u[3] = -0.15; u[4] = 0.0; u[5] = 0.0;
00094   Inputs.push(u);
00095 
00096   u = vector(6);
00097   u[0] = 0.0; u[1] = 0.0; u[2] = 0.0; u[3] = 0.0; u[4] = 0.15; u[5] = 0.0;
00098   Inputs.push(u);
00099 
00100   u = vector(6);
00101   u[0] = 0.0; u[1] = 0.0; u[2] = 0.0; u[3] = 0.0; u[4] = -0.15; u[5] = 0.0;
00102   Inputs.push(u);
00103 
00104   u = vector(6);
00105   u[0] = 0.0; u[1] = 0.0; u[2] = 0.0; u[3] = 0.0; u[4] = 0.0; u[5] = 0.15;
00106   Inputs.push(u);
00107 
00108   u = vector(6);
00109   u[0] = 0.0; u[1] = 0.0; u[2] = 0.0; u[3] = 0.0; u[4] = 0.0; u[5] = -0.15;
00110   Inputs.push(u);
00111 }
00112 
00113 
00114 vector Model3DRigid::StateTransitionEquation(const vector &x, const vector &u) {
00115 
00116   vector dx;
00117 
00118   dx = u;
00119 
00120   return dx;
00121 }
00122 
00123 
00124 
00125 double Model3DRigid::Metric(const vector &x1, const vector &x2) {
00126 
00127   double fd;
00128 
00129   fd = fabs(x1[3]-x2[3]);
00130   double dtheta1 = min(fd,2.0*PI - fd);
00131   fd = fabs(x1[4]-x2[4]);
00132   double dtheta2 = min(fd,2.0*PI - fd);
00133   fd = fabs(x1[5]-x2[5]);
00134   double dtheta3 = min(fd,2.0*PI - fd);
00135 
00136   return sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + sqr(x1[2] - x2[2]) +
00137               sqr(50.0/PI*dtheta1) + sqr(50.0/PI*dtheta2) +
00138               sqr(50.0/PI*dtheta3));
00139   //  return sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + sqr(x1[2] - x2[2]) +
00140   //          sqr(dtheta1) + sqr(dtheta2) +
00141   //          sqr(dtheta3));
00142 }
00143 
00144 
00145 vector Model3DRigid::Integrate(const vector &x, const vector &u, const double &h)
00146 {
00147   return EulerIntegrate(x,u,h);
00148 }
00149 
00150 
00151 vector Model3DRigid::LinearInterpolate(const vector &x1, const vector &x2, const double &a){
00152   vector v;
00153 
00154   v = (1.0-a)*x1 + a*x2;
00155 
00156   if (fabs(x2[3] - x1[3]) > PI) {
00157     if (x1[3] > x2[3])
00158       v[3] = (1.0-a)*x1[3] + a*(x2[3]+2.0*PI);
00159     else
00160       v[3] = (1.0-a)*(x1[3]+2.0*PI) + a*x2[3];
00161   }
00162 
00163   if (v[3] > PI)
00164     v[3] -= 2.0*PI;
00165 
00166   if (fabs(x2[4] - x1[4]) > PI) {
00167     if (x1[4] > x2[4])
00168       v[4] = (1.0-a)*x1[4] + a*(x2[4]+2.0*PI);
00169     else
00170       v[4] = (1.0-a)*(x1[4]+2.0*PI) + a*x2[4];
00171   }
00172 
00173   if (v[4] > PI)
00174     v[4] -= 2.0*PI;
00175 
00176   if (fabs(x2[5] - x1[5]) > PI) {
00177     if (x1[5] > x2[5])
00178       v[5] = (1.0-a)*x1[5] + a*(x2[5]+2.0*PI);
00179     else
00180       v[5] = (1.0-a)*(x1[5]+2.0*PI) + a*x2[5];
00181   }
00182 
00183   if (v[5] > PI)
00184     v[5] -= 2.0*PI;
00185 
00186 
00187   return v;
00188 
00189 }
00190 
00191 
00192 
00193 
00194 
00195 // *********************************************************************
00196 // *********************************************************************
00197 // CLASS:     Model3DRigidMulti
00198 // 
00199 // *********************************************************************
00200 // *********************************************************************
00201 
00202 
00203 // Constructor
00204 Model3DRigidMulti::Model3DRigidMulti(string path = ""):Model3DRigid(path) {
00205   vector u;
00206   int i,j;
00207 
00208   file_istream fin(FilePath + "NumBodies");
00209   fin >> NumBodies;
00210 
00211   StateDim = 6*NumBodies;
00212   InputDim = 6*NumBodies;
00213 
00214   if (is_file(FilePath+"LowerState"))
00215     ReadLowerState();
00216   else {
00217     LowerState = vector(StateDim);
00218   }
00219 
00220   if (is_file(FilePath+"UpperState"))
00221     ReadUpperState();
00222   else {
00223     UpperState = vector(StateDim);
00224   }
00225 
00226   u = vector(StateDim);
00227   Inputs.clear();
00228   for (i = 0; i < StateDim; i++) {
00229     for (j = 0; j < StateDim; j++)
00230       u[j] = (i==j) ? 1.0 : 0.0; 
00231     Inputs.push(u);
00232     for (j = 0; j < StateDim; j++)
00233       u[j] = (i==j) ? -1.0 : 0.0; 
00234     Inputs.push(u);
00235   }
00236 }
00237 
00238 
00239 
00240 
00241 double Model3DRigidMulti::Metric(const vector &x1, const vector &x2) {
00242 
00243   double d,fd,dtheta1,dtheta2,dtheta3;
00244   int i;
00245 
00246   d = 0.0;
00247 
00248   for (i = 0; i < NumBodies; i++) {
00249     fd = fabs(x1[6*i+3]-x2[6*i+3]);
00250     dtheta1 = min(fd,2.0*PI - fd);
00251     fd = fabs(x1[6*i+4]-x2[6*i+4]);
00252     dtheta2 = min(fd,2.0*PI - fd);
00253     fd = fabs(x1[6*i+5]-x2[6*i+5]);
00254     dtheta3 = min(fd,2.0*PI - fd);
00255     d += sqr(x1[6*i] - x2[6*i]);
00256     d += sqr(x1[6*i+1] - x2[6*i+1]);
00257     d += sqr(x1[6*i+2] - x2[6*i+2]);
00258     d += sqr(dtheta1);
00259     d += sqr(dtheta2);
00260     d += sqr(dtheta3);
00261   }
00262 
00263   return sqrt(d);
00264 }
00265 
00266 
00267 
00268 vector Model3DRigidMulti::LinearInterpolate(const vector &x1, const vector &x2, const double &a){
00269   vector v;
00270   int i;
00271 
00272   v = (1.0-a)*x1 + a*x2;
00273 
00274   for (i = 0; i < NumBodies; i++) {
00275     if (fabs(x2[6*i+3] - x1[6*i+3]) > PI) {
00276       if (x1[6*i+3] > x2[6*i+3])
00277         v[6*i+3] = (1.0-a)*x1[6*i+3] + a*(x2[6*i+3]+2.0*PI);
00278       else
00279         v[6*i+3] = (1.0-a)*(x1[6*i+3]+2.0*PI) + a*x2[6*i+3];
00280     }
00281     
00282     if (v[6*i+3] > PI)
00283       v[6*i+3] -= 2.0*PI;
00284     
00285     if (fabs(x2[6*i+4] - x1[6*i+4]) > PI) {
00286       if (x1[6*i+4] > x2[6*i+4])
00287         v[6*i+4] = (1.0-a)*x1[6*i+4] + a*(x2[6*i+4]+2.0*PI);
00288       else
00289         v[6*i+4] = (1.0-a)*(x1[6*i+4]+2.0*PI) + a*x2[6*i+4];
00290     }
00291     
00292     if (v[6*i+4] > PI)
00293       v[6*i+4] -= 2.0*PI;
00294     
00295     if (fabs(x2[6*i+5] - x1[6*i+5]) > PI) {
00296       if (x1[6*i+5] > x2[6*i+5])
00297         v[6*i+5] = (1.0-a)*x1[6*i+5] + a*(x2[6*i+5]+2.0*PI);
00298       else
00299         v[6*i+5] = (1.0-a)*(x1[6*i+5]+2.0*PI) + a*x2[6*i+5];
00300     }
00301     
00302     if (v[6*i+5] > PI)
00303       v[6*i+5] -= 2.0*PI;
00304   }
00305 
00306   return v;
00307 
00308 }
00309 
00310 
00311 
00312 // *********************************************************************
00313 // *********************************************************************
00314 // CLASS:     Model3DRigidChain
00315 // A 3D kinematic chain of bodies
00316 //  
00317 // *********************************************************************
00318 // *********************************************************************
00319 
00320 Model3DRigidChain::Model3DRigidChain(string path = ""):Model3DRigid(path) {
00321 
00322   int i;
00323   vector u;
00324 
00325   file_istream fin(FilePath + "NumBodies");
00326   fin >> NumBodies;
00327   
00328   file_istream fin1(FilePath + "StateDim");
00329   fin1 >> StateDim;
00330 
00331   InputDim = StateDim; 
00332 
00333   file_istream fin2(FilePath + "DH");
00334   fin2 >> DH;
00335 
00336   
00337   StateIndices = array<int>(StateDim);
00338   file_istream fin3(FilePath + "StateIndices");
00339   fin3 >> StateIndices;
00340 
00341   LowerState = vector(StateDim);
00342   UpperState = vector(StateDim);
00343 
00344   if (is_file(FilePath+"LowerState"))
00345     ReadLowerState();
00346   else {
00347     for (i = 0; i < StateDim; i++) {
00348       LowerState[i] = -1.0;
00349     }
00350   }
00351 
00352   if (is_file(FilePath+"UpperState"))
00353     ReadUpperState();
00354   else {
00355     for (i = 0; i < StateDim; i++) {
00356       UpperState[i] = 1.0;
00357     }
00358   }
00359 
00360   Inputs.clear();  // Otherwise its parent constructor will make some inputs
00361   if (is_file(FilePath+"Inputs"))
00362     ReadInputs();
00363   else {
00364     for (i = 0; i < StateDim; i++) {
00365       u = vector(StateDim);
00366       u[i] = 0.1;
00367       Inputs.push(u);
00368       u = vector(StateDim);
00369       u[i] = -0.1;
00370       Inputs.push(u);
00371     }    
00372   } 
00373 
00374 }
00375 
00376 vector Model3DRigidChain::StateToConfiguration(const vector &x) {
00377   vector q;
00378   vector A, Alpha, D, Theta;
00379   int i;
00380   matrix r(4,4), rn(4,4), ro(4,4);
00381 
00382   q = vector(6*NumBodies);
00383   A = vector(NumBodies);
00384   D = vector(NumBodies);
00385   Alpha = vector(NumBodies);
00386   Theta = vector(NumBodies);
00387 
00388   for (i = 0; i < StateDim; i++ ) {
00389     if (StateIndices[i] != 0) {
00390       int y = StateIndices[i];
00391       DH[y-1] = x[i];
00392     }
00393   } 
00394  
00395   for (i = 0; i < NumBodies; i++) {
00396        Alpha[i] = DH[i];
00397        Theta[i] = DH[NumBodies*1+i];
00398        A[i] = DH[NumBodies*2+i];
00399        D[i] = DH[NumBodies*3+i];
00400   }
00401 
00402   for (i = 0; i < 4 ; i ++ ) {
00403     for (int j = 0 ; j < 4 ; j ++ ) {
00404       if (i==j) {r[i][j] = 1.0;}
00405       else {r[i][j] = 0.0;}
00406     }
00407   }
00408   
00409   for (i = 0; i < NumBodies; i++) {
00410     ro=r;
00411     rn[0][0] = cos(Theta[i]);
00412     rn[0][1] = -sin(Theta[i]);
00413     rn[0][2] = 0.0;
00414     rn[0][3] = A[i];
00415     rn[1][2] = -sin(Alpha[i]);
00416     rn[2][2] = cos(Alpha[i]);
00417     rn[3][2] = 0.0;
00418     rn[1][3] = rn[1][2]*D[i];
00419     rn[2][3] = rn[2][2]*D[i];
00420     rn[3][3] = 1.0;
00421     rn[1][0] = -rn[0][1]*rn[2][2];
00422     rn[2][0] = -rn[0][1]*(-rn[1][2]);
00423     rn[3][0] = 0.0;
00424     rn[1][1] = rn[0][0]*rn[2][2];
00425     rn[2][1] = rn[0][0]*(-rn[1][2]);
00426     rn[3][1] = 0.0;
00427   
00428     //R = R*Rnew
00429     for (int k=0; k<4; k++){
00430       for (int l=0; l<4; l++) {
00431         r[k][l]=0.0 ;
00432         for (int m=0; m<4; m++){
00433           r[k][l]+=ro[k][m]*rn[m][l];
00434         }
00435       }
00436     }
00437 
00438     q[6*i] = r[0][3];
00439     q[6*i+1] = r[1][3];
00440     q[6*i+2] = r[2][3];
00441 
00442     q[6*i+4] = atan2(  -r[2][0], sqrt(sqr(r[0][0])+sqr(r[1][0]))  );
00443     if ( (q[6*i+4]>0.0) && (cos(q[6*i+4])==0.0) ) {
00444       q[6*i+5] = 0.0;
00445       q[6*i+3] = atan2(r[0][1], r[1][1]);
00446     }  
00447     else {
00448       if ( (q[6*i+4]<0.0) && (cos(q[6*i+4])==0.0) ) {
00449         q[6*i+5] = 0.0;
00450         q[6*i+3] = -atan2(r[0][1], r[1][1]);
00451       }
00452       else {
00453         q[6*i+5] = atan2(  r[1][0]/cos(q[6*i+4]), r[0][0]/cos(q[6*i+4])  );
00454         q[6*i+3] = atan2(  r[2][1]/cos(q[6*i+4]), r[2][2]/cos(q[6*i+4])  );
00455       }
00456     }
00457     if (q[6*i+3] < 0.0)
00458       q[6*i+3] += 2.0*PI; // Force the orientation into [0,2pi)
00459     if (q[6*i+4] < 0.0)
00460       q[6*i+4] += 2.0*PI; // Force the orientation into [0,2pi)
00461     if (q[6*i+5] < 0.0)
00462       q[6*i+5] += 2.0*PI; // Force the orientation into [0,2pi)
00463   }
00464 
00465   return q;
00466 }
00467 
00468 
00469 
00470 vector Model3DRigidChain::LinearInterpolate(const vector &x1, 
00471                                             const vector &x2, 
00472                                             const double &a) {
00473   return (1.0-a)*x1 + a*x2;
00474 }
00475 
00476 
00477 vector Model3DRigidChain::StateTransitionEquation(const vector &x, const vector &u) {
00478 
00479   vector dx(StateDim);
00480 
00481   dx = u;
00482   return dx;
00483 }
00484 
00485 
00486 
00487 double Model3DRigidChain::Metric(const vector &x1, const vector &x2) {
00488 
00489   double rho;
00490   vector dtheta(StateDim);
00491   int i;
00492   
00493   rho = 0.0;
00494   for ( i= 0 ; i < StateDim; i++){
00495     if (StateIndices[i] > 2*NumBodies) { 
00496       rho += sqr(x1[i] - x2[i]);
00497     }
00498     else { 
00499       dtheta[i] = min(fabs(x1[i]-x2[i]),2.0*PI - fabs(x1[i]-x2[i]));
00500       rho += sqr(10.0/PI*dtheta[i]);
00501     }
00502   }
00503   rho = sqrt(rho);
00504 
00505   return rho;
00506 }
00507 
00508 
00509 // Make sure the joint position and limits are respected
00510 bool Model3DRigidChain::Satisfied(const vector &x) 
00511 {
00512   int i;
00513 
00514   for (i = 0; i < StateDim; i++)
00515     if ((x[i] > UpperState[i]) || (x[i] < LowerState[i]))
00516       return false;
00517 
00518   return true;
00519 }
00520 
00521 
00522 // *********************************************************************
00523 // *********************************************************************
00524 // CLASS:     Model3DRigidChainTree
00525 // A 3D kinematic tree of bodies
00526 //  
00527 // *********************************************************************
00528 // *********************************************************************
00529 
00530 Model3DRigidTree::Model3DRigidTree(string path = ""):Model3DRigid(path) {
00531 
00532   int i;
00533   vector u;
00534 
00535   file_istream fin(FilePath + "NumBodies");
00536   fin >> NumBodies;
00537   
00538   file_istream fin1(FilePath + "StateDim");
00539   fin1 >> StateDim;
00540 
00541   InputDim = StateDim; 
00542 
00543   file_istream fin2(FilePath + "DH");
00544   fin2 >> DH;
00545 
00546   StateIndices = array<int>(StateDim);
00547   file_istream fin3(FilePath + "StateIndices");
00548   fin3 >> StateIndices;
00549 
00550   Parents = array<int>(NumBodies);
00551   file_istream fin4(FilePath + "Parents");
00552   fin4 >> Parents;
00553 
00554   for (i = 1; i < NumBodies; i++) {
00555     if (Parents[i] >= i) { 
00556       cout << "There is a mistake in Parents\n";
00557       exit(-1);
00558     }
00559   }
00560 
00561   LowerState = vector(StateDim);
00562   UpperState = vector(StateDim);
00563 
00564   if (is_file(FilePath+"LowerState"))
00565     ReadLowerState();
00566   else {
00567     for (i = 0; i < StateDim; i++) {
00568       LowerState[i] = -1.0;
00569     }
00570   }
00571 
00572   if (is_file(FilePath+"UpperState"))
00573     ReadUpperState();
00574   else {
00575     for (i = 0; i < StateDim; i++) {
00576       UpperState[i] = 1.0;
00577     }
00578   }
00579 
00580   Inputs.clear();  // Otherwise its parent constructor will make some inputs
00581   if (is_file(FilePath+"Inputs"))
00582     ReadInputs();
00583   else {
00584     for (i = 0; i < StateDim; i++) {
00585       u = vector(StateDim);
00586       u[i] = 0.1;
00587       Inputs.push(u);
00588       u = vector(StateDim);
00589       u[i] = -0.1;
00590       Inputs.push(u);
00591     }    
00592   }
00593 
00594 }
00595 
00596 
00597 vector Model3DRigidTree::StateToConfiguration(const vector &x) {
00598   vector q;
00599   vector A, Alpha, D, Theta;
00600   int i;
00601   matrix r(4,4), rn(4,4), ro(4,4);
00602 
00603   matrix Tr[NumBodies];
00604 
00605   q = vector(6*NumBodies);
00606   A = vector(NumBodies);
00607   D = vector(NumBodies);
00608   Alpha = vector(NumBodies);
00609   Theta = vector(NumBodies);
00610 
00611   for (i = 0; i < StateDim; i++ ) {
00612     if (StateIndices[i] != 0) {
00613       int y = StateIndices[i];
00614       DH[y-1] = x[i];
00615     }
00616   } 
00617  
00618   for (i = 0; i < NumBodies; i++){
00619        Alpha[i] = DH[i];
00620        Theta[i] = DH[NumBodies*1+i];
00621        A[i] = DH[NumBodies*2+i];
00622        D[i] = DH[NumBodies*3+i];
00623   }
00624 
00625   for (i = 0; i < 4 ; i ++ ) {
00626     for (int j = 0 ; j < 4 ; j ++ ) {
00627       if (i==j) {
00628         r[i][j] = 1.0;
00629       }
00630       else {
00631         r[i][j] = 0.0;
00632       }
00633     }
00634   }
00635   ro = r;
00636 
00637   for (i = 0; i < NumBodies; i++ ) {
00638 
00639     int ParInd = Parents[i];
00640 
00641     rn[0][0] = cos(Theta[i]);
00642     rn[0][1] = -sin(Theta[i]);
00643     rn[0][2] = 0.0;
00644     rn[0][3] = A[i];
00645     rn[1][2] = -sin(Alpha[i]);
00646     rn[2][2] = cos(Alpha[i]);
00647     rn[3][2] = 0.0;
00648     rn[1][3] = rn[1][2]*D[i];
00649     rn[2][3] = rn[2][2]*D[i];
00650     rn[3][3] = 1.0;
00651     rn[1][0] = -rn[0][1]*rn[2][2];
00652     rn[2][0] = -rn[0][1]*(-rn[1][2]);
00653     rn[3][0] = 0.0;
00654     rn[1][1] = rn[0][0]*rn[2][2];
00655     rn[2][1] = rn[0][0]*(-rn[1][2]);
00656     rn[3][1] = 0.0;
00657   
00658     //R = R*Rnew
00659 
00660     if (i != 0) { ro = Tr[ParInd];}
00661 
00662     for (int k=0; k<4; k++){
00663       for (int l=0; l<4; l++) {
00664         r[k][l]=0.0 ;
00665         for (int m=0; m<4; m++){
00666             r[k][l]+=ro[k][m]*rn[m][l];
00667 
00668         }
00669       }
00670     }
00671 
00672     Tr[i] = r;
00673 
00674     //Finding State coordinates
00675     
00676     q[6*i] = r[0][3];
00677     q[6*i+1] = r[1][3];
00678     q[6*i+2] = r[2][3];
00679 
00680     q[6*i+4] = atan2(  -r[2][0], sqrt(sqr(r[0][0])+sqr(r[1][0]))  );
00681     if ( (q[6*i+4]>0.0) && (cos(q[6*i+4])==0.0) ) {
00682       q[6*i+5] = 0.0;
00683       q[6*i+3] = atan2(r[0][1], r[1][1]);
00684     }  
00685     else{
00686       if ( (q[6*i+4]<0.0) && (cos(q[6*i+4])==0.0) ) {
00687         q[6*i+5] = 0.0;
00688         q[6*i+3] = -atan2(r[0][1], r[1][1]);
00689       }
00690       else {
00691         q[6*i+5] = atan2(  r[1][0]/cos(q[6*i+4]), r[0][0]/cos(q[6*i+4])  );
00692         q[6*i+3] = atan2(  r[2][1]/cos(q[6*i+4]), r[2][2]/cos(q[6*i+4])  );
00693       }
00694     }
00695     if (q[6*i+3] < 0.0)
00696       q[6*i+3] += 2.0*PI; // Force the orientation into [0,2pi)
00697     if (q[6*i+4] < 0.0)
00698       q[6*i+4] += 2.0*PI; // Force the orientation into [0,2pi)
00699     if (q[6*i+5] < 0.0)
00700       q[6*i+5] += 2.0*PI; // Force the orientation into [0,2pi)
00701   }
00702   return q;
00703 }
00704 
00705 
00706 
00707 vector Model3DRigidTree::LinearInterpolate(const vector &x1, 
00708                                             const vector &x2, 
00709                                             const double &a) {
00710   return (1.0-a)*x1 + a*x2;
00711 }
00712 
00713 
00714 vector Model3DRigidTree::StateTransitionEquation(const vector &x, const vector &u) {
00715 
00716   vector dx(StateDim);
00717 
00718   dx = u;
00719   return dx;
00720 }
00721 
00722 
00723 
00724 double Model3DRigidTree::Metric(const vector &x1, const vector &x2) {
00725 
00726   double rho;
00727   vector dtheta(StateDim);
00728   int i;
00729   
00730   rho = 0.0;
00731   for (i = 0 ; i < StateDim; i++){
00732     if (StateIndices[i] > 2*NumBodies) {
00733       rho += sqr(x1[i] - x2[i]);
00734     }
00735     else { 
00736       dtheta[i] = min(fabs(x1[i]-x2[i]),2.0*PI - fabs(x1[i]-x2[i]));
00737       rho += sqr(10.0/PI*dtheta[i]);
00738     }
00739   }
00740   rho = sqrt(rho);
00741 
00742   return rho;
00743 }
00744 
00745 
00746 // Make sure the joint position and limits are respected
00747 bool Model3DRigidTree::Satisfied(const vector &x) 
00748 {
00749   int i;
00750 
00751   for (i = 0; i < StateDim; i++)
00752     if ((x[i] > UpperState[i]) || (x[i] < LowerState[i]))
00753       return false;
00754 
00755   return true;
00756 }
00757 
00758 
00759 #include <LEDA/UNDEFINE_NAMES.h>
00760 
00761 
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.