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 }