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