Motion Strategy Library

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