00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00065
00066 static const char rcsid[] = "$Id: invkine.cpp,v 1.8 2006/05/16 16:11:15 gourdeau Exp $";
00067
00068 #include <stdexcept>
00069 #include "robot.h"
00070
00071 #ifdef use_namespace
00072 namespace ROBOOP {
00073 using namespace NEWMAT;
00074 #endif
00075
00076 #define NITMAX 1000 //!< def maximum number of iterations in inv_kin
00077 #ifdef USING_FLOAT //from newmat's include.h
00078 # define ITOL 1e-4 //!< def tolerance for the end of iterations in inv_kin
00079 #else
00080 # define ITOL 1e-6 //!< def tolerance for the end of iterations in inv_kin
00081 #endif
00082
00083 ReturnMatrix Robot_basic::inv_kin(const Matrix & Tobj, const int mj)
00085 {
00086 bool converge = false;
00087 return inv_kin(Tobj, mj, dof, converge);
00088 }
00089
00090
00091 ReturnMatrix Robot_basic::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
00103 {
00104 ColumnVector qPrev, qout, dq, q_tmp;
00105 Matrix B, M;
00106 UpperTriangularMatrix U;
00107
00108 qPrev = get_available_q();
00109 qout = qPrev;
00110 q_tmp = qout;
00111
00112 converge = false;
00113 if(mj == 0) {
00114 Matrix Ipd, A, B(6,1);
00115 for(int j = 1; j <= NITMAX; j++) {
00116 Ipd = (kine(endlink)).i()*Tobj;
00117 B(1,1) = Ipd(1,4);
00118 B(2,1) = Ipd(2,4);
00119 B(3,1) = Ipd(3,4);
00120 B(4,1) = Ipd(3,2);
00121 B(5,1) = Ipd(1,3);
00122 B(6,1) = Ipd(2,1);
00123 A = jacobian(endlink,endlink);
00124 QRZ(A,U);
00125 QRZ(A,B,M);
00126 dq = U.i()*M;
00127
00128 while(dq.MaximumAbsoluteValue() > 1)
00129 dq /= 10;
00130
00131 for(int k = 1; k<= dq.nrows(); k++)
00132 qout(k)+=dq(k);
00133 set_q(qout);
00134
00135 if (dq.MaximumAbsoluteValue() < ITOL)
00136 {
00137 converge = true;
00138 break;
00139 }
00140 }
00141 } else {
00142 int adof=get_available_dof(endlink);
00143 Matrix A(12,adof);
00144 for(int j = 1; j <= NITMAX; j++) {
00145 B = (Tobj-kine(endlink)).SubMatrix(1,3,1,4).AsColumn();
00146 int k=1;
00147 for(int i = 1; i<=dof && k<=adof; i++) {
00148 if(links[i].immobile)
00149 continue;
00150 A.SubMatrix(1,12,k,k) = dTdqi(i).SubMatrix(1,3,1,4).AsColumn();
00151 k++;
00152 }
00153 QRZ(A,U);
00154 QRZ(A,B,M);
00155 dq = U.i()*M;
00156
00157 while(dq.MaximumAbsoluteValue() > 1)
00158 dq /= 10;
00159
00160 for(k = 1; k<=adof; k++)
00161 qout(k)+=dq(k);
00162 set_q(qout);
00163
00164 if (dq.MaximumAbsoluteValue() < ITOL)
00165 {
00166 converge = true;
00167 break;
00168 }
00169 }
00170 }
00171
00172 if(converge)
00173 {
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184 for(int i = 1; i <= dof; i++)
00185 {
00186 if(links[i].immobile)
00187 continue;
00188 if(links[i].get_joint_type() == 0) {
00189
00190 if(links[i].get_theta_min() > qout(i)) {
00191 converge = false;
00192 }
00193 if(links[i].get_theta_max() < qout(i)) {
00194 converge = false;
00195 }
00196 }
00197 }
00198
00199 set_q(qPrev);
00200 qout.Release();
00201 return qout;
00202
00203
00204
00205
00206
00207 }
00208 else
00209 {
00210 set_q(qPrev);
00211 q_tmp.Release();
00212 return q_tmp;
00213 }
00214 }
00215
00216
00217
00218 ReturnMatrix Robot::inv_kin(const Matrix & Tobj, const int mj)
00220 {
00221 bool converge = false;
00222 return inv_kin(Tobj, mj, dof, converge);
00223 }
00224
00225
00226 ReturnMatrix Robot::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
00234 {
00235 switch (robotType) {
00236 case RHINO:
00237 return inv_kin_rhino(Tobj, converge);
00238 break;
00239 case PUMA:
00240 return inv_kin_puma(Tobj, converge);
00241 break;
00242 case SCHILLING:
00243 return inv_kin_schilling(Tobj, converge);
00244 break;
00245 default:
00246 return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
00247 }
00248 }
00249
00250
00251 ReturnMatrix Robot::inv_kin_rhino(const Matrix & Tobj, bool & converge)
00257 {
00258 ColumnVector qout(5), q_actual;
00259 q_actual = get_q();
00260
00261 try
00262 {
00263 Real theta[6] , diff1, diff2, tmp,
00264 angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , G=0.0 ;
00265
00266
00267 theta[0] = atan2(Tobj(2,4),
00268 Tobj(1,4));
00269
00270 theta[1] = atan2(-Tobj(2,4),
00271 -Tobj(1,4)) ;
00272
00273 diff1 = fabs(q_actual(1)-theta[0]) ;
00274 if (diff1 > M_PI)
00275 diff1 = 2*M_PI - diff1;
00276
00277 diff2 = fabs(q_actual(1)-theta[1]);
00278 if (diff2 > M_PI)
00279 diff2 = 2*M_PI - diff2 ;
00280
00281
00282 if (diff1 < diff2)
00283 theta[1] = theta[0] ;
00284
00285 theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1),
00286 sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
00287
00288
00289 angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
00290 -1*Tobj(3,3));
00291
00292 L = cos(theta[1])*Tobj(1,4) +
00293 sin(theta[1])*Tobj(2,4) +
00294 links[5].d*sin(angle) -
00295 links[4].a*cos(angle);
00296 M = links[1].d -
00297 Tobj(3,4) -
00298 links[5].d*cos(angle) -
00299 links[4].a*sin(angle);
00300 K = (L*L + M*M - links[3].a*links[3].a -
00301 links[2].a*links[2].a) /
00302 (2 * links[3].a * links[2].a);
00303
00304 tmp = 1-K*K;
00305 if (tmp < 0)
00306 throw out_of_range("sqrt of negative number not allowed.");
00307
00308 theta[0] = atan2( sqrt(tmp) , K );
00309 theta[3] = atan2( -sqrt(tmp) , K );
00310
00311 diff1 = fabs(q_actual(3)-theta[0]) ;
00312 if (diff1 > M_PI)
00313 diff1 = 2*M_PI - diff1 ;
00314
00315 diff2 = fabs(q_actual(3)-theta[3]);
00316 if (diff2 > M_PI)
00317 diff2 = 2*M_PI - diff2 ;
00318
00319 if (diff1 < diff2)
00320 theta[3] = theta[0] ;
00321
00322 H = cos(theta[3]) * links[3].a + links[2].a;
00323 G = sin(theta[3]) * links[3].a;
00324
00325 theta[2] = atan2( M , L ) - atan2( G , H );
00326 theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) ,
00327 -1*Tobj(3,3)) - theta[2] - theta[3] ;
00328
00329 qout(1) = theta[1];
00330 qout(2) = theta[2];
00331 qout(3) = theta[3];
00332 qout(4) = theta[4];
00333 qout(5) = theta[5];
00334
00335 converge = true;
00336 }
00337 catch(std::out_of_range & e)
00338 {
00339 converge = false;
00340 qout = q_actual;
00341 }
00342
00343 qout.Release();
00344 return qout;
00345 }
00346
00347
00348 ReturnMatrix Robot::inv_kin_puma(const Matrix & Tobj, bool & converge)
00354 {
00355 ColumnVector qout(6), q_actual;
00356 q_actual = get_q();
00357
00358 try
00359 {
00360 Real theta[7] , diff1, diff2, tmp,
00361 A = 0.0 , B = 0.0 , C = 0.0 , D =0.0, Ro = 0.0,
00362 H = 0.0 , L = 0.0 , M = 0.0;
00363
00364
00365
00366 if (links[6].d)
00367 {
00368 ColumnVector tmpd6(3); Matrix tmp;
00369 tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
00370 tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
00371 Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
00372 }
00373
00374 tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
00375 if (tmp < 0)
00376 throw std::out_of_range("sqrt of negative number not allowed.");
00377
00378 Ro = sqrt(tmp);
00379 D = (links[2].d+links[3].d) / Ro;
00380
00381 tmp = 1-D*D;
00382 if (tmp < 0)
00383 throw std::out_of_range("sqrt of negative number not allowed.");
00384
00385
00386 theta[0] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));
00387
00388 theta[1] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
00389
00390 diff1 = fabs(q_actual(1)-theta[0]);
00391 if (diff1 > M_PI)
00392 diff1 = 2*M_PI - diff1;
00393
00394 diff2 = fabs(q_actual(1)-theta[1]);
00395 if (diff2 > M_PI)
00396 diff2 = 2*M_PI - diff2;
00397
00398
00399 if (diff1 < diff2)
00400 theta[1] = theta[0];
00401
00402 tmp = links[3].a*links[3].a + links[4].d*links[4].d;
00403 if (tmp < 0)
00404 throw std::out_of_range("sqrt of negative number not allowed.");
00405
00406 Ro = sqrt(tmp);
00407 B = atan2(links[4].d,links[3].a);
00408 C = Tobj(1,4)*Tobj(1,4) +
00409 Tobj(2,4)*Tobj(2,4) +
00410 (Tobj(3,4)-links[1].d)*(Tobj(3,4)-links[1].d) -
00411 (links[2].d + links[3].d)*(links[2].d + links[3].d) -
00412 links[2].a*links[2].a -
00413 links[3].a*links[3].a -
00414 links[4].d*links[4].d;
00415 A = C / (2*links[2].a);
00416
00417 tmp = 1-A/Ro*A/Ro;
00418 if (tmp < 0)
00419 throw std::out_of_range("sqrt of negative number not allowed.");
00420
00421 theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
00422 theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
00423
00424 diff1 = fabs(q_actual(3)-theta[0]);
00425 if (diff1 > M_PI)
00426 diff1 = 2*M_PI - diff1 ;
00427
00428 diff2 = fabs(q_actual(3)-theta[3]);
00429 if (diff2 > M_PI)
00430 diff1 = 2*M_PI - diff2;
00431
00432
00433 if (diff1 < diff2)
00434 theta[3] = theta[0];
00435
00436 H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
00437 L = sin(theta[3])*links[4].d + cos(theta[3])*links[3].a + links[2].a;
00438 M = cos(theta[3])*links[4].d - sin(theta[3])*links[3].a;
00439
00440 theta[2] = atan2( M , L ) - atan2(Tobj(3,4)-links[1].d , H );
00441
00442 theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) ,
00443 cos(theta[2] + theta[3]) *
00444 (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
00445 - (sin(theta[2]+theta[3])*Tobj(3,3)) );
00446
00447 theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)),
00448 -cos(theta[2] + theta[3]) *
00449 (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
00450 + (sin(theta[2]+theta[3])*Tobj(3,3)) );
00451
00452 diff1 = fabs(q_actual(4)-theta[0]);
00453 if (diff1 > M_PI)
00454 diff1 = 2*M_PI - diff1;
00455
00456 diff2 = fabs(q_actual(4)-theta[4]);
00457 if (diff2 > M_PI)
00458 diff2 = 2*M_PI - diff2;
00459
00460
00461 if (diff1 < diff2)
00462 theta[4] = theta[0];
00463
00464 theta[5] = atan2( cos(theta[4]) *
00465 ( cos(theta[2] + theta[3]) *
00466 (cos(theta[1]) * Tobj(1,3)
00467 + sin(theta[1])*Tobj(2,3))
00468 - (sin(theta[2]+theta[3])*Tobj(3,3)) ) +
00469 sin(theta[4])*(-sin(theta[1])*Tobj(1,3)
00470 + cos(theta[1])*Tobj(2,3)) ,
00471 sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3)
00472 + sin(theta[1])*Tobj(2,3) )
00473 + (cos(theta[2]+theta[3])*Tobj(3,3)) );
00474
00475 theta[6] = atan2( -sin(theta[4])
00476 * ( cos(theta[2] + theta[3]) *
00477 (cos(theta[1]) * Tobj(1,1)
00478 + sin(theta[1])*Tobj(2,1))
00479 - (sin(theta[2]+theta[3])*Tobj(3,1))) +
00480 cos(theta[4])*(-sin(theta[1])*Tobj(1,1)
00481 + cos(theta[1])*Tobj(2,1)),
00482 -sin(theta[4]) * ( cos(theta[2] + theta[3]) *
00483 (cos(theta[1]) * Tobj(1,2)
00484 + sin(theta[1])*Tobj(2,2))
00485 - (sin(theta[2]+theta[3])*Tobj(3,2))) +
00486 cos(theta[4])*(-sin(theta[1])*Tobj(1,2)
00487 + cos(theta[1])*Tobj(2,2)) );
00488
00489 qout(1) = theta[1];
00490 qout(2) = theta[2];
00491 qout(3) = theta[3];
00492 qout(4) = theta[4];
00493 qout(5) = theta[5];
00494 qout(6) = theta[6];
00495
00496 converge = true;
00497 }
00498 catch(std::out_of_range & e)
00499 {
00500 converge = false;
00501 qout = q_actual;
00502 }
00503
00504 qout.Release();
00505 return qout;
00506 }
00507
00508 ReturnMatrix Robot::inv_kin_schilling(const Matrix & Tobj, bool & converge)
00514 {
00515 ColumnVector qout(6), q_actual;
00516 q_actual = get_q();
00517
00518 try
00519 {
00520 Real theta[7], K=0.0, A=0.0, B=0.0, C=0.0, D=0.0, tmp=0.0, theta234 , diff1, diff2;
00521
00522 if (links[6].d)
00523 {
00524 ColumnVector tmpd6(3);
00525 Matrix tmp;
00526 tmpd6(1)=0;
00527 tmpd6(2)=0;
00528 tmpd6(3)=links[6].d;
00529 tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
00530 Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
00531 }
00532
00533 theta[1] = atan2(Tobj(2,4),Tobj(1,4));
00534
00535 theta[0] = atan2(-Tobj(2,4),-Tobj(1,4));
00536
00537 diff1 = fabs(q_actual(1)-theta[0]);
00538 if (diff1 > M_PI)
00539 diff1 = 2*M_PI - diff1;
00540
00541 diff2 = fabs(q_actual(1)-theta[1]);
00542 if (diff2 > M_PI)
00543 diff2 = 2*M_PI - diff2;
00544
00545
00546 if (diff1 < diff2)
00547 theta[1] = theta[0];
00548
00549
00550 theta234 = atan2( Tobj(3,3) , cos(theta[1])*Tobj(1,3) + sin(theta[1])*Tobj(2,3) );
00551
00552 theta[5] = atan2( (cos(theta234)*(cos(theta[1])*Tobj(1,3) +
00553 sin(theta[1])*Tobj(2,3)) + sin(theta234)*Tobj(3,3)),
00554 (sin(theta[1])*Tobj(1,3) - cos(theta[1])*Tobj(2,3)));
00555
00556 theta[6] = atan2( (-sin(theta234)*(cos(theta[1])*Tobj(1,1) +
00557 sin(theta[1])*Tobj(2,1)) + cos(theta234)*Tobj(3,1)),
00558 (-sin(theta234)*(cos(theta[1])*Tobj(1,2) + sin(theta[1])*Tobj(2,2)) +
00559 cos(theta234)*Tobj(3,2)));
00560
00561 A= cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4) - links[1].a -
00562 links[4].a*cos(theta234);
00563
00564 B= Tobj(3,4) - links[1].d - links[4].a*sin(theta234);
00565
00566
00567 K= ( A*A + B*B - (links[3].a*links[3].a) - (links[2].a*links[2].a) ) /
00568 ( 2*links[2].a*links[3].a );
00569
00570 tmp = 1-K*K;
00571 if (tmp < 0)
00572 throw std::out_of_range("sqrt of negative number not allowed.");
00573
00574 theta[3] = atan2(sqrt(tmp),K);
00575 theta[0] = atan2(-sqrt(tmp),K);
00576
00577 diff1 = fabs(q_actual(3)-theta[0]);
00578 if (diff1 > M_PI)
00579 diff1 = 2*M_PI - diff1;
00580
00581 diff2 = fabs(q_actual(3)-theta[3]);
00582 if (diff2 > M_PI)
00583 diff2 = 2*M_PI - diff2;
00584
00585
00586 if (diff1 < diff2)
00587 theta[3] = theta[0];
00588
00589 C = cos(theta[3])*links[3].a + links[2].a;
00590 D = sin(theta[3])*links[3].a;
00591
00592 theta[2] = atan2(B,A) - atan2(D,C);
00593
00594 theta[4] = theta234 - theta[2] - theta[3];
00595
00596 qout(1) = theta[1];
00597 qout(2) = theta[2];
00598 qout(3) = theta[3];
00599 qout(4) = theta[4];
00600 qout(5) = theta[5];
00601 qout(6) = theta[6];
00602 converge = true;
00603 }
00604 catch(std::out_of_range & e)
00605 {
00606 converge = false;
00607 qout = q_actual;
00608 }
00609
00610 qout.Release();
00611 return qout;
00612 }
00613
00614
00615
00616
00617 ReturnMatrix mRobot::inv_kin(const Matrix & Tobj, const int mj)
00619 {
00620 bool converge = false;
00621 return inv_kin(Tobj, mj, dof, converge);
00622 }
00623
00624
00625 ReturnMatrix mRobot::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
00633 {
00634 switch (robotType) {
00635 case RHINO:
00636 return inv_kin_rhino(Tobj, converge);
00637 break;
00638 case PUMA:
00639 return inv_kin_puma(Tobj, converge);
00640 break;
00641 case SCHILLING:
00642 return inv_kin_schilling(Tobj, converge);
00643 break;
00644 default:
00645 return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
00646 }
00647 }
00648
00649
00650 ReturnMatrix mRobot::inv_kin_rhino(const Matrix & Tobj, bool & converge)
00656 {
00657 ColumnVector qout(5), q_actual;
00658 q_actual = get_q();
00659
00660 try
00661 {
00662 Real theta[6] , diff1, diff2, tmp,
00663 angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , G=0.0;
00664
00665 if (links[6].d > 0)
00666 {
00667 ColumnVector tmpd6(3);
00668 tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
00669 tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
00670 Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
00671 }
00672
00673
00674 theta[0] = atan2(Tobj(2,4),
00675 Tobj(1,4));
00676
00677 theta[1] = atan2(-Tobj(2,4),
00678 -Tobj(1,4)) ;
00679
00680 diff1 = fabs(q_actual(1)-theta[0]) ;
00681 if (diff1 > M_PI)
00682 diff1 = 2*M_PI - diff1;
00683
00684 diff2 = fabs(q_actual(1)-theta[1]);
00685 if (diff2 > M_PI)
00686 diff2 = 2*M_PI - diff2 ;
00687
00688
00689 if (diff1 < diff2)
00690 theta[1] = theta[0] ;
00691
00692 theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1),
00693 sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
00694
00695
00696 angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
00697 -1*Tobj(3,3));
00698
00699 L = cos(theta[1])*Tobj(1,4) +
00700 sin(theta[1])*Tobj(2,4) +
00701 links[5].d*sin(angle) -
00702 links[5].a*cos(angle);
00703 M = links[1].d -
00704 Tobj(3,4) -
00705 links[5].d*cos(angle) -
00706 links[5].a*sin(angle);
00707 K = (L*L + M*M - links[4].a*links[4].a -
00708 links[3].a*links[3].a) /
00709 (2 * links[4].a * links[4].a);
00710
00711 tmp = 1-K*K;
00712 if (tmp < 0)
00713 throw std::out_of_range("sqrt of negative number not allowed.");
00714
00715 theta[0] = atan2( sqrt(tmp) , K );
00716 theta[3] = atan2( -sqrt(tmp) , K );
00717
00718 diff1 = fabs(q_actual(3)-theta[0]) ;
00719 if (diff1 > M_PI)
00720 diff1 = 2*M_PI - diff1 ;
00721
00722 diff2 = fabs(q_actual(3)-theta[3]);
00723 if (diff2 > M_PI)
00724 diff2 = 2*M_PI - diff2 ;
00725
00726 if (diff1 < diff2)
00727 theta[3] = theta[0] ;
00728
00729 H = cos(theta[3]) * links[4].a + links[3].a;
00730 G = sin(theta[3]) * links[4].a;
00731
00732 theta[2] = atan2( M , L ) - atan2( G , H );
00733 theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) ,
00734 -1*Tobj(3,3)) - theta[2] - theta[3] ;
00735
00736 qout(1) = theta[1];
00737 qout(2) = theta[2];
00738 qout(3) = theta[3];
00739 qout(4) = theta[4];
00740 qout(5) = theta[5];
00741
00742 converge = true;
00743 }
00744 catch(std::out_of_range & e)
00745 {
00746 converge = false;
00747 qout = q_actual;
00748 }
00749
00750 qout.Release();
00751 return qout;
00752 }
00753
00754
00755 ReturnMatrix mRobot::inv_kin_puma(const Matrix & Tobj, bool & converge)
00761 {
00762 ColumnVector qout(6), q_actual;
00763 q_actual = get_q();
00764
00765 try
00766 {
00767 Real theta[7] , diff1, diff2, tmp,
00768 A = 0.0 , B = 0.0 , C = 0.0 , D =0.0, Ro = 0.0,
00769 H = 0.0 , L = 0.0 , M = 0.0;
00770
00771
00772
00773 if (links[6].d)
00774 {
00775 ColumnVector tmpd6(3); Matrix tmp;
00776 tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
00777 tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
00778 Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
00779 }
00780
00781 tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
00782 if (tmp < 0)
00783 throw std::out_of_range("sqrt of negative number not allowed.");
00784
00785 Ro = sqrt(tmp);
00786 D = (links[2].d+links[3].d) / Ro;
00787
00788 tmp = 1-D*D;
00789 if (tmp < 0)
00790 throw std::out_of_range("sqrt of negative number not allowed.");
00791
00792
00793 theta[0] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));
00794
00795 theta[1] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
00796
00797 diff1 = fabs(q_actual(1)-theta[0]);
00798 if (diff1 > M_PI)
00799 diff1 = 2*M_PI - diff1;
00800
00801 diff2 = fabs(q_actual(1)-theta[1]);
00802 if (diff2 > M_PI)
00803 diff2 = 2*M_PI - diff2;
00804
00805
00806 if (diff1 < diff2)
00807 theta[1] = theta[0];
00808
00809 tmp = links[4].a*links[4].a + links[4].d*links[4].d;
00810 if (tmp < 0)
00811 throw std::out_of_range("sqrt of negative number not allowed.");
00812
00813 Ro = sqrt(tmp);
00814 B = atan2(links[4].d,links[4].a);
00815 C = Tobj(1,4)*Tobj(1,4) +
00816 Tobj(2,4)*Tobj(2,4) +
00817 (Tobj(3,4)-links[1].d)*(Tobj(3,4)-links[1].d) -
00818 (links[2].d + links[3].d)*(links[2].d + links[3].d) -
00819 links[3].a*links[3].a -
00820 links[4].a*links[4].a -
00821 links[4].d*links[4].d;
00822 A = C / (2*links[3].a);
00823
00824 tmp = 1-A/Ro*A/Ro;
00825 if (tmp < 0)
00826 throw std::out_of_range("sqrt of negative number not allowed.");
00827
00828 theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
00829 theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
00830
00831 diff1 = fabs(q_actual(3)-theta[0]);
00832 if (diff1 > M_PI)
00833 diff1 = 2*M_PI - diff1 ;
00834
00835 diff2 = fabs(q_actual(3)-theta[3]);
00836 if (diff2 > M_PI)
00837 diff2 = 2*M_PI - diff2;
00838
00839
00840 if (diff1 < diff2)
00841 theta[3] = theta[0];
00842
00843 H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
00844 L = sin(theta[3])*links[4].d + cos(theta[3])*links[4].a + links[3].a;
00845 M = cos(theta[3])*links[4].d - sin(theta[3])*links[4].a;
00846
00847 theta[2] = atan2( M , L ) - atan2(Tobj(3,4)-links[1].d , H );
00848
00849 theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) ,
00850 cos(theta[2] + theta[3]) *
00851 (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
00852 - (sin(theta[2]+theta[3])*Tobj(3,3)) );
00853
00854 theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)),
00855 -cos(theta[2] + theta[3]) *
00856 (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
00857 + (sin(theta[2]+theta[3])*Tobj(3,3)) );
00858
00859 diff1 = fabs(q_actual(4)-theta[0]);
00860 if (diff1 > M_PI)
00861 diff1 = 2*M_PI - diff1;
00862
00863 diff2 = fabs(q_actual(4)-theta[4]);
00864 if (diff2 > M_PI)
00865 diff2 = 2*M_PI - diff2;
00866
00867
00868 if (diff1 < diff2)
00869 theta[4] = theta[0];
00870
00871 theta[5] = atan2( cos(theta[4]) *
00872 ( cos(theta[2] + theta[3]) *
00873 (cos(theta[1]) * Tobj(1,3)
00874 + sin(theta[1])*Tobj(2,3))
00875 - (sin(theta[2]+theta[3])*Tobj(3,3)) ) +
00876 sin(theta[4])*(-sin(theta[1])*Tobj(1,3)
00877 + cos(theta[1])*Tobj(2,3)) ,
00878 sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3)
00879 + sin(theta[1])*Tobj(2,3) )
00880 + (cos(theta[2]+theta[3])*Tobj(3,3)) );
00881
00882 theta[6] = atan2( -sin(theta[4])
00883 * ( cos(theta[2] + theta[3]) *
00884 (cos(theta[1]) * Tobj(1,1)
00885 + sin(theta[1])*Tobj(2,1))
00886 - (sin(theta[2]+theta[3])*Tobj(3,1))) +
00887 cos(theta[4])*(-sin(theta[1])*Tobj(1,1)
00888 + cos(theta[1])*Tobj(2,1)),
00889 -sin(theta[4]) * ( cos(theta[2] + theta[3]) *
00890 (cos(theta[1]) * Tobj(1,2)
00891 + sin(theta[1])*Tobj(2,2))
00892 - (sin(theta[2]+theta[3])*Tobj(3,2))) +
00893 cos(theta[4])*(-sin(theta[1])*Tobj(1,2)
00894 + cos(theta[1])*Tobj(2,2)) );
00895
00896 qout(1) = theta[1];
00897 qout(2) = theta[2];
00898 qout(3) = theta[3];
00899 qout(4) = theta[4];
00900 qout(5) = theta[5];
00901 qout(6) = theta[6];
00902
00903 converge = true;
00904 }
00905 catch(std::out_of_range & e)
00906 {
00907 converge = false;
00908 qout = q_actual;
00909 }
00910
00911 qout.Release();
00912 return qout;
00913 }
00914
00915 ReturnMatrix mRobot::inv_kin_schilling(const Matrix & Tobj, bool & converge)
00921 {
00922 ColumnVector qout(6), q_actual;
00923 q_actual = get_q();
00924
00925 try
00926 {
00927 Real theta[7], K=0.0, A=0.0, B=0.0, C=0.0, D=0.0, tmp=0.0, theta234 , diff1, diff2;
00928
00929 if (links[6].d)
00930 {
00931 ColumnVector tmpd6(3);
00932 Matrix tmp;
00933 tmpd6(1)=0;
00934 tmpd6(2)=0;
00935 tmpd6(3)=links[6].d;
00936 tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
00937 Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
00938 }
00939
00940 theta[1] = atan2(Tobj(2,4),Tobj(1,4));
00941
00942 theta[0] = atan2(-Tobj(2,4),-Tobj(1,4));
00943
00944 diff1 = fabs(q_actual(1)-theta[0]);
00945 if (diff1 > M_PI)
00946 diff1 = 2*M_PI - diff1;
00947
00948 diff2 = fabs(q_actual(1)-theta[1]);
00949 if (diff2 > M_PI)
00950 diff2 = 2*M_PI - diff2;
00951
00952
00953 if (diff1 < diff2)
00954 theta[1] = theta[0];
00955
00956
00957 theta234 = atan2( Tobj(3,3) , cos(theta[1])*Tobj(1,3) + sin(theta[1])*Tobj(2,3) );
00958
00959 theta[5] = atan2( (cos(theta234)*(cos(theta[1])*Tobj(1,3) +
00960 sin(theta[1])*Tobj(2,3)) + sin(theta234)*Tobj(3,3)),
00961 (sin(theta[1])*Tobj(1,3) - cos(theta[1])*Tobj(2,3)));
00962
00963 theta[6] = atan2( (-sin(theta234)*(cos(theta[1])*Tobj(1,1) +
00964 sin(theta[1])*Tobj(2,1)) + cos(theta234)*Tobj(3,1)),
00965 (-sin(theta234)*(cos(theta[1])*Tobj(1,2) + sin(theta[1])*Tobj(2,2)) +
00966 cos(theta234)*Tobj(3,2)));
00967
00968 A= cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4) - links[2].a -
00969 links[5].a*cos(theta234);
00970
00971 B= Tobj(3,4) - links[1].d - links[5].a*sin(theta234);
00972
00973
00974 K= ( A*A + B*B - (links[4].a*links[4].a) - (links[3].a*links[3].a) ) /
00975 ( 2*links[3].a*links[4].a );
00976
00977 tmp = 1-K*K;
00978 if (tmp < 0)
00979 throw std::out_of_range("sqrt of negative number not allowed.");
00980
00981 theta[3] = atan2(sqrt(tmp),K);
00982 theta[0] = atan2(-sqrt(tmp),K);
00983
00984 diff1 = fabs(q_actual(3)-theta[0]);
00985 if (diff1 > M_PI)
00986 diff1 = 2*M_PI - diff1;
00987
00988 diff2 = fabs(q_actual(3)-theta[3]);
00989 if (diff2 > M_PI)
00990 diff2 = 2*M_PI - diff2;
00991
00992
00993 if (diff1 < diff2)
00994 theta[3] = theta[0];
00995
00996 C = cos(theta[3])*links[4].a + links[3].a;
00997 D = sin(theta[3])*links[4].a;
00998
00999 theta[2] = atan2(B,A) - atan2(D,C);
01000
01001 theta[4] = theta234 - theta[2] - theta[3];
01002
01003 qout(1) = theta[1];
01004 qout(2) = theta[2];
01005 qout(3) = theta[3];
01006 qout(4) = theta[4];
01007 qout(5) = theta[5];
01008 qout(6) = theta[6];
01009
01010 converge = true;
01011 }
01012 catch(std::out_of_range & e)
01013 {
01014 converge = false;
01015 qout = q_actual;
01016 }
01017
01018 qout.Release();
01019 return qout;
01020 }
01021
01022
01023 ReturnMatrix mRobot_min_para::inv_kin(const Matrix & Tobj, const int mj)
01025 {
01026 bool converge = false;
01027 return inv_kin(Tobj, mj, dof, converge);
01028 }
01029
01030
01031 ReturnMatrix mRobot_min_para::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
01039 {
01040 switch (robotType) {
01041 case RHINO:
01042 return inv_kin_rhino(Tobj, converge);
01043 break;
01044 case PUMA:
01045 return inv_kin_puma(Tobj, converge);
01046 break;
01047 default:
01048 return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
01049 }
01050 }
01051
01052
01053 ReturnMatrix mRobot_min_para::inv_kin_rhino(const Matrix & Tobj, bool & converge)
01059 {
01060 ColumnVector qout(5), q_actual;
01061 q_actual = get_q();
01062
01063 try
01064 {
01065 Real theta[6] , diff1, diff2, tmp,
01066 angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , G=0.0 ;
01067
01068
01069 theta[0] = atan2(Tobj(2,4),
01070 Tobj(1,4));
01071
01072 theta[1] = atan2(-Tobj(2,4),
01073 -Tobj(1,4)) ;
01074
01075 diff1 = fabs(q_actual(1)-theta[0]) ;
01076 if (diff1 > M_PI)
01077 diff1 = 2*M_PI - diff1;
01078
01079 diff2 = fabs(q_actual(1)-theta[1]);
01080 if (diff2 > M_PI)
01081 diff2 = 2*M_PI - diff2 ;
01082
01083
01084 if (diff1 < diff2)
01085 theta[1] = theta[0] ;
01086
01087 theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1),
01088 sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
01089
01090
01091 angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
01092 -1*Tobj(3,3));
01093
01094 L = cos(theta[1])*Tobj(1,4) +
01095 sin(theta[1])*Tobj(2,4) +
01096 links[5].d*sin(angle) -
01097 links[5].a*cos(angle);
01098 M = links[1].d -
01099 Tobj(3,4) -
01100 links[5].d*cos(angle) -
01101 links[5].a*sin(angle);
01102 K = (L*L + M*M - links[4].a*links[4].a -
01103 links[3].a*links[3].a) /
01104 (2 * links[4].a * links[4].a);
01105
01106 tmp = 1-K*K;
01107 if (tmp < 0)
01108 throw std::out_of_range("sqrt of negative number not allowed.");
01109
01110 theta[0] = atan2( sqrt(tmp) , K );
01111 theta[3] = atan2( -sqrt(tmp) , K );
01112
01113 diff1 = fabs(q_actual(3)-theta[0]) ;
01114 if (diff1 > M_PI)
01115 diff1 = 2*M_PI - diff1 ;
01116
01117 diff2 = fabs(q_actual(3)-theta[3]);
01118 if (diff2 > M_PI)
01119 diff2 = 2*M_PI - diff2 ;
01120
01121 if (diff1 < diff2)
01122 theta[3] = theta[0] ;
01123
01124 H = cos(theta[3]) * links[4].a + links[3].a;
01125 G = sin(theta[3]) * links[4].a;
01126
01127 theta[2] = atan2( M , L ) - atan2( G , H );
01128 theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) ,
01129 -1*Tobj(3,3)) - theta[2] - theta[3] ;
01130
01131 qout(1) = theta[1];
01132 qout(2) = theta[2];
01133 qout(3) = theta[3];
01134 qout(4) = theta[4];
01135 qout(5) = theta[5];
01136
01137 converge = true;
01138 }
01139 catch(std::out_of_range & e)
01140 {
01141 converge = false;
01142 qout = q_actual;
01143 }
01144
01145 qout.Release();
01146 return qout;
01147 }
01148
01149
01150 ReturnMatrix mRobot_min_para::inv_kin_puma(const Matrix & Tobj, bool & converge)
01156 {
01157 ColumnVector qout(6), q_actual;
01158 q_actual = get_q();
01159
01160 try
01161 {
01162 Real theta[7] , diff1, diff2, tmp,
01163 A = 0.0 , B = 0.0 , C = 0.0 , D =0.0, Ro = 0.0,
01164 H = 0.0 , L = 0.0 , M = 0.0;
01165
01166
01167
01168 if (links[6].d > 0)
01169 {
01170 ColumnVector tmpd6(3); Matrix tmp;
01171 tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
01172 tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
01173 Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
01174 }
01175
01176 tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
01177 if (tmp < 0)
01178 throw std::out_of_range("sqrt of negative number not allowed.");
01179
01180 Ro = sqrt(tmp);
01181 D = (links[2].d+links[3].d) / Ro;
01182
01183 tmp = 1-D*D;
01184 if (tmp < 0)
01185 throw std::out_of_range("sqrt of negative number not allowed.");
01186
01187
01188 theta[0] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));
01189
01190 theta[1] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
01191
01192 diff1 = fabs(q_actual(1)-theta[0]);
01193 if (diff1 > M_PI)
01194 diff1 = 2*M_PI - diff1;
01195
01196 diff2 = fabs(q_actual(1)-theta[1]);
01197 if (diff2 > M_PI)
01198 diff2 = 2*M_PI - diff2;
01199
01200
01201 if (diff1 < diff2)
01202 theta[1] = theta[0];
01203
01204 tmp = links[4].a*links[4].a + links[4].d*links[4].d;
01205 if (tmp < 0)
01206 throw std::out_of_range("sqrt of negative number not allowed.");
01207
01208 Ro = sqrt(tmp);
01209 B = atan2(links[4].d,links[4].a);
01210 C = Tobj(1,4)*Tobj(1,4) +
01211 Tobj(2,4)*Tobj(2,4) +
01212 (Tobj(3,4)-links[1].d)*(Tobj(3,4)-links[1].d) -
01213 (links[2].d + links[3].d)*(links[2].d + links[3].d) -
01214 links[3].a*links[3].a -
01215 links[4].a*links[4].a -
01216 links[4].d*links[4].d;
01217 A = C / (2*links[3].a);
01218
01219 tmp = 1-A/Ro*A/Ro;
01220 if (tmp < 0)
01221 throw std::out_of_range("sqrt of negative number not allowed.");
01222
01223 theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
01224 theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
01225
01226 diff1 = fabs(q_actual(3)-theta[0]);
01227 if (diff1 > M_PI)
01228 diff1 = 2*M_PI - diff1 ;
01229
01230 diff2 = fabs(q_actual(3)-theta[3]);
01231 if (diff2 > M_PI)
01232 diff2 = 2*M_PI - diff2;
01233
01234
01235 if (diff1 < diff2)
01236 theta[3] = theta[0];
01237
01238 H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
01239 L = sin(theta[3])*links[4].d + cos(theta[3])*links[4].a + links[3].a;
01240 M = cos(theta[3])*links[4].d - sin(theta[3])*links[4].a;
01241
01242 theta[2] = atan2( M , L ) - atan2(Tobj(3,4)-links[1].d , H );
01243
01244 theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) ,
01245 cos(theta[2] + theta[3]) *
01246 (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
01247 - (sin(theta[2]+theta[3])*Tobj(3,3)) );
01248
01249 theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)),
01250 -cos(theta[2] + theta[3]) *
01251 (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
01252 + (sin(theta[2]+theta[3])*Tobj(3,3)) );
01253
01254 diff1 = fabs(q_actual(4)-theta[0]);
01255 if (diff1 > M_PI)
01256 diff1 = 2*M_PI - diff1;
01257
01258 diff2 = fabs(q_actual(4)-theta[4]);
01259 if (diff2 > M_PI)
01260 diff2 = 2*M_PI - diff2;
01261
01262
01263 if (diff1 < diff2)
01264 theta[4] = theta[0];
01265
01266 theta[5] = atan2( cos(theta[4]) *
01267 ( cos(theta[2] + theta[3]) *
01268 (cos(theta[1]) * Tobj(1,3)
01269 + sin(theta[1])*Tobj(2,3))
01270 - (sin(theta[2]+theta[3])*Tobj(3,3)) ) +
01271 sin(theta[4])*(-sin(theta[1])*Tobj(1,3)
01272 + cos(theta[1])*Tobj(2,3)) ,
01273 sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3)
01274 + sin(theta[1])*Tobj(2,3) )
01275 + (cos(theta[2]+theta[3])*Tobj(3,3)) );
01276
01277 theta[6] = atan2( -sin(theta[4])
01278 * ( cos(theta[2] + theta[3]) *
01279 (cos(theta[1]) * Tobj(1,1)
01280 + sin(theta[1])*Tobj(2,1))
01281 - (sin(theta[2]+theta[3])*Tobj(3,1))) +
01282 cos(theta[4])*(-sin(theta[1])*Tobj(1,1)
01283 + cos(theta[1])*Tobj(2,1)),
01284 -sin(theta[4]) * ( cos(theta[2] + theta[3]) *
01285 (cos(theta[1]) * Tobj(1,2)
01286 + sin(theta[1])*Tobj(2,2))
01287 - (sin(theta[2]+theta[3])*Tobj(3,2))) +
01288 cos(theta[4])*(-sin(theta[1])*Tobj(1,2)
01289 + cos(theta[1])*Tobj(2,2)) );
01290
01291 qout(1) = theta[1];
01292 qout(2) = theta[2];
01293 qout(3) = theta[3];
01294 qout(4) = theta[4];
01295 qout(5) = theta[5];
01296 qout(6) = theta[6];
01297
01298 converge = true;
01299 }
01300 catch(std::out_of_range & e)
01301 {
01302 converge = false;
01303 qout = q_actual;
01304 }
01305
01306 qout.Release();
01307 return qout;
01308 }
01309
01310
01311 ReturnMatrix mRobot_min_para::inv_kin_schilling(const Matrix & Tobj, bool & converge)
01317 {
01318 ColumnVector qout(6), q_actual;
01319 q_actual = get_q();
01320
01321 try
01322 {
01323 Real theta[7], K=0.0, A=0.0, B=0.0, C=0.0, D=0.0, tmp=0.0, theta234 , diff1, diff2;
01324
01325 if (links[6].d)
01326 {
01327 ColumnVector tmpd6(3);
01328 Matrix tmp;
01329 tmpd6(1)=0;
01330 tmpd6(2)=0;
01331 tmpd6(3)=links[6].d;
01332 tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
01333 Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
01334 }
01335
01336 theta[1] = atan2(Tobj(2,4),Tobj(1,4));
01337
01338 theta[0] = atan2(-Tobj(2,4),-Tobj(1,4));
01339
01340 diff1 = fabs(q_actual(1)-theta[0]);
01341 if (diff1 > M_PI)
01342 diff1 = 2*M_PI - diff1;
01343
01344 diff2 = fabs(q_actual(1)-theta[1]);
01345 if (diff2 > M_PI)
01346 diff2 = 2*M_PI - diff2;
01347
01348
01349 if (diff1 < diff2)
01350 theta[1] = theta[0];
01351
01352
01353 theta234 = atan2( Tobj(3,3) , cos(theta[1])*Tobj(1,3) + sin(theta[1])*Tobj(2,3) );
01354
01355 theta[5] = atan2( (cos(theta234)*(cos(theta[1])*Tobj(1,3) +
01356 sin(theta[1])*Tobj(2,3)) + sin(theta234)*Tobj(3,3)),
01357 (sin(theta[1])*Tobj(1,3) - cos(theta[1])*Tobj(2,3)));
01358
01359 theta[6] = atan2( (-sin(theta234)*(cos(theta[1])*Tobj(1,1) +
01360 sin(theta[1])*Tobj(2,1)) + cos(theta234)*Tobj(3,1)),
01361 (-sin(theta234)*(cos(theta[1])*Tobj(1,2) + sin(theta[1])*Tobj(2,2)) +
01362 cos(theta234)*Tobj(3,2)));
01363
01364 A= cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4) - links[2].a -
01365 links[5].a*cos(theta234);
01366
01367 B= Tobj(3,4) - links[1].d - links[5].a*sin(theta234);
01368
01369
01370 K= ( A*A + B*B - (links[4].a*links[4].a) - (links[3].a*links[3].a) ) /
01371 ( 2*links[3].a*links[4].a );
01372
01373 tmp = 1-K*K;
01374 if (tmp < 0)
01375 throw std::out_of_range("sqrt of negative number not allowed.");
01376
01377 theta[3] = atan2(sqrt(tmp),K);
01378 theta[0] = atan2(-sqrt(tmp),K);
01379
01380 diff1 = fabs(q_actual(3)-theta[0]);
01381 if (diff1 > M_PI)
01382 diff1 = 2*M_PI - diff1;
01383
01384 diff2 = fabs(q_actual(3)-theta[3]);
01385 if (diff2 > M_PI)
01386 diff2 = 2*M_PI - diff2;
01387
01388
01389 if (diff1 < diff2)
01390 theta[3] = theta[0];
01391
01392 C = cos(theta[3])*links[4].a + links[3].a;
01393 D = sin(theta[3])*links[4].a;
01394
01395 theta[2] = atan2(B,A) - atan2(D,C);
01396
01397 theta[4] = theta234 - theta[2] - theta[3];
01398
01399 qout(1) = theta[1];
01400 qout(2) = theta[2];
01401 qout(3) = theta[3];
01402 qout(4) = theta[4];
01403 qout(5) = theta[5];
01404 qout(6) = theta[6];
01405
01406 converge = true;
01407 }
01408 catch(std::out_of_range & e)
01409 {
01410 converge = false;
01411 qout = q_actual;
01412 }
01413
01414 qout.Release();
01415 return qout;
01416 }
01417
01418
01419 #ifdef use_namespace
01420 }
01421 #endif