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
00060
00061
00062
00063
00064 #include "robot.h"
00065 #include <sstream>
00066
00067 #ifdef use_namespace
00068 namespace ROBOOP {
00069 using namespace NEWMAT;
00070 #endif
00071
00072 static const char rcsid[] __UNUSED__ = "$Id: invkine.cpp,v 1.25 2005/07/26 03:22:09 ejt Exp $";
00073
00074 #define NITMAX 1000
00075 #ifdef USING_FLOAT //from newmat's include.h
00076 # define ITOL 1e-4f
00077 #else
00078 # define ITOL 1e-6
00079 #endif
00080
00081 ReturnMatrix Robot_basic::inv_kin(const Matrix & Tobj, const int mj)
00082
00083 {
00084 bool converge = false;
00085 return inv_kin(Tobj, mj, dof, converge);
00086 }
00087
00088
00089 ReturnMatrix Robot_basic::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101 {
00102 ColumnVector qout, dq, q_tmp;
00103 UpperTriangularMatrix U;
00104
00105 qout = get_available_q();
00106 if(qout.nrows()==0) {
00107 converge=true;
00108 return qout;
00109 }
00110 q_tmp = qout;
00111
00112 converge = false;
00113 if(mj == 0) {
00114 Matrix Ipd, A, B(6,1), M;
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),B,M;
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,endlink).SubMatrix(1,3,1,4).AsColumn();
00151 k++;
00152 }
00153 if(A.ncols()==0) {
00154 converge=true;
00155 break;
00156 }
00157 QRZ(A,U);
00158 QRZ(A,B,M);
00159 dq = U.i()*M;
00160
00161 while(dq.MaximumAbsoluteValue() > 1)
00162 dq /= 10;
00163
00164 for(k = 1; k<=adof; k++)
00165 qout(k)+=dq(k);
00166 set_q(qout);
00167 if (dq.MaximumAbsoluteValue() < ITOL)
00168 {
00169 converge = true;
00170 break;
00171 }
00172 }
00173 }
00174
00175 if(converge)
00176 {
00177
00178 int j=1;
00179 for(int i = 1; i <= dof; i++)
00180 {
00181 if(links[i].immobile)
00182 continue;
00183 if(links[i].get_joint_type() == 0) {
00184 while(qout(j) >= M_PI)
00185 qout(j) -= 2*M_PI;
00186 while(qout(j) <= -M_PI)
00187 qout(j) += 2*M_PI;
00188 }
00189 j++;
00190 }
00191 set_q(qout);
00192 qout.Release();
00193 return qout;
00194 }
00195 else
00196 {
00197 set_q(q_tmp);
00198 q_tmp.Release();
00199 return q_tmp;
00200 }
00201 }
00202
00203
00204
00205
00206 #define DEBUG_ET ;
00207
00208
00209
00210 ReturnMatrix Robot_basic::inv_kin_pos(const ColumnVector & Pobj, const int mj, const int endlink, const ColumnVector& , bool & converge)
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223 {
00224 ColumnVector qout, dq, q_tmp;
00225 UpperTriangularMatrix U;
00226
00227 DEBUG_ET;
00228
00229 qout = get_available_q();
00230 if(qout.nrows()==0) {
00231 converge=true;
00232 return qout;
00233 }
00234 q_tmp = qout;
00235
00236 ColumnVector PHobj(4);
00237 if(Pobj.nrows()!=4) {
00238 PHobj.SubMatrix(1,Pobj.nrows(),1,1)=Pobj;
00239 PHobj.SubMatrix(Pobj.nrows()+1,4,1,1)=1;
00240 } else {
00241 PHobj=Pobj;
00242 }
00243
00244 DEBUG_ET;
00245 converge = false;
00246 if(mj == 0) {
00247 DEBUG_ET;
00248 Matrix Ipd, A, B(3,1),M;
00249 for(int j = 1; j <= NITMAX; j++) {
00250 Ipd = (kine(endlink)).i()*PHobj;
00251 B(1,1) = Ipd(1,1);
00252 B(2,1) = Ipd(2,1);
00253 B(3,1) = Ipd(3,1);
00254 A = jacobian(endlink,endlink);
00255 A = A.SubMatrix(1,3,1,A.ncols());
00256
00257
00258 int removeCount=0;
00259 for(int k=1; k<= A.ncols(); k++)
00260 if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()<ITOL)
00261 removeCount++;
00262 Matrix A2(3,A.ncols()-removeCount);
00263 if(removeCount==0)
00264 A2=A;
00265 else
00266 for(int k=1,m=1; k<= A.ncols(); k++) {
00267 if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()<ITOL)
00268 continue;
00269 A2.SubMatrix(1,3,m,m)=A.SubMatrix(1,3,k,k);
00270 m++;
00271 }
00272
00273 if(A2.ncols()==0) {
00274 converge=true;
00275 break;
00276 }
00277 {
00278 stringstream ss;
00279 ss << "A2-pre:\n";
00280 for(int r=1; r<=A2.nrows(); r++) {
00281 for(int c=1; c<=A2.ncols(); c++) {
00282 ss << A2(r,c) << ' ';
00283 }
00284 ss << '\n';
00285 }
00286 QRZ(A2,U);
00287
00288
00289
00290
00291
00292
00293
00294 QRZ(A2,B,M);
00295
00296 }
00297 DEBUG_ET;
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320 DEBUG_ET;
00321 dq = U.i()*M;
00322
00323 DEBUG_ET;
00324 while(dq.MaximumAbsoluteValue() > 1)
00325 dq /= 10;
00326
00327 for(int k = 1,m=1; m<= dq.nrows(); k++)
00328 if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()>=ITOL)
00329 qout(k)+=dq(m++);
00330 set_q(qout);
00331
00332 if (dq.MaximumAbsoluteValue() < ITOL)
00333 {
00334 converge = true;
00335 break;
00336 }
00337 }
00338 } else {
00339 int adof=get_available_dof(endlink);
00340 Matrix A(3,adof),Rcur,B,M;
00341 ColumnVector pcur;
00342 bool used[adof];
00343 for(int j = 1; j <= NITMAX; j++) {
00344 kine(Rcur,pcur,endlink);
00345 B = (PHobj.SubMatrix(1,3,1,1)-pcur);
00346 int k=1,m=1;
00347 for(int i = 1; m<=adof; i++) {
00348 if(links[i].immobile)
00349 continue;
00350 Matrix Atmp=dTdqi(i,endlink).SubMatrix(1,3,4,4);
00351 used[m]=(Atmp.MaximumAbsoluteValue()>=ITOL);
00352 if(!used[m++])
00353 continue;
00354 A.SubMatrix(1,3,k,k) = Atmp;
00355 k++;
00356 }
00357 Matrix A2=A.SubMatrix(1,3,1,k-1);
00358 if(A2.ncols()==0) {
00359 converge=true;
00360 break;
00361 }
00362 QRZ(A2,U);
00363 QRZ(A2,B,M);
00364 dq = U.i()*M;
00365
00366 while(dq.MaximumAbsoluteValue() > 1)
00367 dq /= 10;
00368
00369 for(k = m = 1; k<=adof; k++)
00370 if(used[k])
00371 qout(k)+=dq(m++);
00372 set_q(qout);
00373
00374 if (dq.MaximumAbsoluteValue() < ITOL)
00375 {
00376 converge = true;
00377 break;
00378 }
00379 }
00380 }
00381 DEBUG_ET;
00382
00383 if(converge)
00384 {
00385 DEBUG_ET;
00386
00387 int j=1;
00388 for(int i = 1; i <= dof; i++)
00389 {
00390 if(links[i].immobile)
00391 continue;
00392 unsigned int * test=(unsigned int*)&qout(j);
00393 if(((*test)&(255U<<23))==(255U<<23)) {
00394
00395 set_q(q_tmp);
00396 q_tmp.Release();
00397 return q_tmp;
00398 }
00399
00400
00401
00402
00403
00404
00405
00406
00407 j++;
00408 }
00409 DEBUG_ET;
00410 set_q(qout);
00411 qout.Release();
00412 DEBUG_ET;
00413 return qout;
00414 }
00415 else
00416 {
00417 DEBUG_ET;
00418 set_q(q_tmp);
00419 q_tmp.Release();
00420 DEBUG_ET;
00421 return q_tmp;
00422 }
00423 }
00424
00425 ReturnMatrix Robot_basic::inv_kin_orientation(const Matrix & Robj, const int mj, const int endlink, bool & converge)
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437 {
00438 ColumnVector qout, dq, q_tmp;
00439 UpperTriangularMatrix U;
00440
00441 qout = get_available_q();
00442 if(qout.nrows()==0) {
00443 converge=true;
00444 return qout;
00445 }
00446 q_tmp = qout;
00447
00448 Matrix RHobj(4,3);
00449 RHobj.SubMatrix(1,3,1,3)=Robj;
00450 RHobj.SubMatrix(4,4,1,3)=0;
00451
00452 converge = false;
00453 if(mj == 0) {
00454 Matrix Ipd, A, B(3,1),M;
00455 for(int j = 1; j <= NITMAX; j++) {
00456 Ipd = kine(endlink).i()*RHobj;
00457 B(1,1) = Ipd(3,2);
00458 B(2,1) = Ipd(1,3);
00459 B(3,1) = Ipd(2,1);
00460 A = jacobian(endlink,endlink);
00461 A = A.SubMatrix(4,6,1,A.ncols());
00462
00463
00464 int removeCount=0;
00465 for(int k=1; k<= A.ncols(); k++)
00466 if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()<ITOL)
00467 removeCount++;
00468 Matrix A2(3,A.ncols()-removeCount);
00469 if(removeCount==0)
00470 A2=A;
00471 else
00472 for(int k=1,m=1; k<= A.ncols(); k++) {
00473 if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()<ITOL)
00474 continue;
00475 A2.SubMatrix(1,3,m,m)=A.SubMatrix(1,3,k,k);
00476 m++;
00477 }
00478
00479 if(A2.ncols()==0) {
00480 converge=true;
00481 break;
00482 }
00483 QRZ(A2,U);
00484 QRZ(A2,B,M);
00485 dq = U.i()*M;
00486
00487 while(dq.MaximumAbsoluteValue() > 1)
00488 dq /= 10;
00489
00490 for(int k = 1,m=1; m<= dq.nrows(); k++)
00491 if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()>=ITOL)
00492 qout(k)+=dq(m++);
00493 set_q(qout);
00494
00495 if (dq.MaximumAbsoluteValue() < ITOL)
00496 {
00497 converge = true;
00498 break;
00499 }
00500 }
00501 } else {
00502 int adof=get_available_dof(endlink);
00503 Matrix A(9,adof),Rcur,B,M;
00504 ColumnVector pcur;
00505 bool used[adof];
00506 for(int j = 1; j <= NITMAX; j++) {
00507 kine(Rcur,pcur,endlink);
00508 B = (Robj-Rcur).AsColumn();
00509 int k=1,m=1;
00510 for(int i = 1; m<=adof; i++) {
00511 if(links[i].immobile)
00512 continue;
00513 Matrix Atmp=dTdqi(i,endlink).SubMatrix(1,3,1,3).AsColumn();
00514 used[m]=(Atmp.MaximumAbsoluteValue()>=ITOL);
00515 if(!used[m++])
00516 continue;
00517 A.SubMatrix(1,9,k,k) = Atmp;
00518 k++;
00519 }
00520 Matrix A2=A.SubMatrix(1,9,1,k-1);
00521 if(A2.ncols()==0) {
00522 converge=true;
00523 break;
00524 }
00525 QRZ(A2,U);
00526 QRZ(A2,B,M);
00527 dq = U.i()*M;
00528
00529 while(dq.MaximumAbsoluteValue() > 1)
00530 dq /= 10;
00531
00532 for(k = m = 1; k<=adof; k++)
00533 if(used[k])
00534 qout(k)+=dq(m++);
00535 set_q(qout);
00536
00537 if (dq.MaximumAbsoluteValue() < ITOL)
00538 {
00539 converge = true;
00540 break;
00541 }
00542 }
00543 }
00544
00545 if(converge)
00546 {
00547
00548 int j=1;
00549 for(int i = 1; i <= dof; i++)
00550 {
00551 if(links[i].immobile)
00552 continue;
00553 if(links[i].get_joint_type() == 0) {
00554 while(qout(j) >= M_PI)
00555 qout(j) -= 2*M_PI;
00556 while(qout(j) <= -M_PI)
00557 qout(j) += 2*M_PI;
00558 }
00559 j++;
00560 }
00561 set_q(qout);
00562 qout.Release();
00563 return qout;
00564 }
00565 else
00566 {
00567 set_q(q_tmp);
00568 q_tmp.Release();
00569 return q_tmp;
00570 }
00571 }
00572
00573
00574
00575 ReturnMatrix Robot::inv_kin(const Matrix & Tobj, const int mj)
00576
00577 {
00578 bool converge = false;
00579 return inv_kin(Tobj, mj, dof, converge);
00580 }
00581
00582
00583 ReturnMatrix Robot::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
00584
00585
00586
00587
00588
00589
00590
00591 {
00592 switch (robotType) {
00593 case RHINO:
00594 return inv_kin_rhino(Tobj, converge);
00595 case PUMA:
00596 return inv_kin_puma(Tobj, converge);
00597 case ERS_LEG:
00598 case ERS2XX_HEAD:
00599 case ERS7_HEAD:
00600
00601 default:
00602 return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
00603 }
00604 }
00605
00606 ReturnMatrix Robot::inv_kin_pos(const ColumnVector & Pobj, const int mj, const int endlink, const ColumnVector& Plink, bool & converge)
00607
00608
00609
00610
00611
00612
00613
00614 {
00615 switch (robotType) {
00616 case ERS_LEG:
00617 case ERS2XX_HEAD:
00618 case ERS7_HEAD:
00619 return inv_kin_ers_pos(Pobj, endlink, Plink, converge);
00620 case RHINO:
00621 case PUMA:
00622
00623 default:
00624 return Robot_basic::inv_kin_pos(Pobj, mj, endlink, Plink, converge);
00625 }
00626 }
00627
00628 ReturnMatrix Robot::inv_kin_orientation(const Matrix & Robj, const int mj, const int endlink, bool & converge)
00629
00630
00631
00632
00633
00634
00635
00636 {
00637 switch (robotType) {
00638 case ERS_LEG:
00639 case ERS2XX_HEAD:
00640 case ERS7_HEAD:
00641 case RHINO:
00642 case PUMA:
00643
00644 default:
00645 return Robot_basic::inv_kin_orientation(Robj, mj, endlink, converge);
00646 }
00647 }
00648
00649
00650
00651 ReturnMatrix Robot::inv_kin_rhino(const Matrix & Tobj, bool & converge)
00652
00653
00654
00655
00656
00657 {
00658 ColumnVector qout(5), q_actual;
00659 q_actual = get_q();
00660
00661 try
00662 {
00663 Real theta[6] , diff1, diff2, tmp,
00664 angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , Gl=0.0 ;
00665
00666
00667 theta[0] = atan2(Tobj(2,4),
00668 Tobj(1,4));
00669
00670 theta[1] = atan2(-Tobj(2,4),
00671 -Tobj(1,4)) ;
00672
00673 diff1 = fabs(q_actual(1)-theta[0]) ;
00674 if (diff1 > M_PI)
00675 diff1 = 2*M_PI - diff1;
00676
00677 diff2 = fabs(q_actual(1)-theta[1]);
00678 if (diff2 > M_PI)
00679 diff1 = 2*M_PI - diff2 ;
00680
00681
00682 if (diff1 < diff2)
00683 theta[1] = theta[0] ;
00684
00685 theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1),
00686 sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
00687
00688
00689 angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
00690 -1*Tobj(3,3));
00691
00692 L = cos(theta[1])*Tobj(1,4) +
00693 sin(theta[1])*Tobj(2,4) +
00694 links[5].d*sin(angle) -
00695 links[4].a*cos(angle);
00696 M = links[1].d -
00697 Tobj(3,4) -
00698 links[5].d*cos(angle) -
00699 links[4].a*sin(angle);
00700 K = (L*L + M*M - links[3].a*links[3].a -
00701 links[2].a*links[2].a) /
00702 (2 * links[3].a * links[2].a);
00703
00704 tmp = 1-K*K;
00705 if (tmp < 0)
00706 throw std::out_of_range("sqrt of negative number not allowed.");
00707
00708 theta[0] = atan2( sqrt(tmp) , K );
00709 theta[3] = atan2( -sqrt(tmp) , K );
00710
00711 diff1 = fabs(q_actual(3)-theta[0]) ;
00712 if (diff1 > M_PI)
00713 diff1 = 2*M_PI - diff1 ;
00714
00715 diff2 = fabs(q_actual(3)-theta[3]);
00716 if (diff2 > M_PI)
00717 diff1 = 2*M_PI - diff2 ;
00718
00719 if (diff1 < diff2)
00720 theta[3] = theta[0] ;
00721
00722 H = cos(theta[3]) * links[3].a + links[2].a;
00723 Gl = sin(theta[3]) * links[3].a;
00724
00725 theta[2] = atan2( M , L ) - atan2( Gl , H );
00726 theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) ,
00727 -1*Tobj(3,3)) - theta[2] - theta[3] ;
00728
00729 qout(1) = theta[1];
00730 qout(2) = theta[2];
00731 qout(3) = theta[3];
00732 qout(4) = theta[4];
00733 qout(5) = theta[5];
00734 set_q(qout);
00735
00736 converge = true;
00737 }
00738 catch(std::out_of_range & e)
00739 {
00740 converge = false;
00741 set_q(q_actual);
00742 qout = q_actual;
00743 }
00744
00745 qout.Release();
00746 return qout;
00747 }
00748
00749
00750 ReturnMatrix Robot::inv_kin_puma(const Matrix & Tobj, bool & converge)
00751
00752
00753
00754
00755
00756 {
00757 ColumnVector qout(6), q_actual;
00758 q_actual = get_q();
00759
00760 try
00761 {
00762 Real theta[7] , diff1, diff2, tmp,
00763 A = 0.0 , B = 0.0 , Cl = 0.0 , D =0.0, Ro = 0.0,
00764 H = 0.0 , L = 0.0 , M = 0.0;
00765
00766
00767
00768 if (links[6].d)
00769 {
00770 ColumnVector tmpd6(3);
00771 tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
00772 tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
00773 Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
00774 }
00775
00776 tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
00777 if (tmp < 0)
00778 throw std::out_of_range("sqrt of negative number not allowed.");
00779
00780 Ro = sqrt(tmp);
00781 D = (links[2].d+links[3].d) / Ro;
00782
00783 tmp = 1-D*D;
00784 if (tmp < 0)
00785 throw std::out_of_range("sqrt of negative number not allowed.");
00786
00787
00788 theta[0] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));
00789
00790 theta[1] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
00791
00792 diff1 = fabs(q_actual(1)-theta[0]);
00793 if (diff1 > M_PI)
00794 diff1 = 2*M_PI - diff1;
00795
00796 diff2 = fabs(q_actual(1)-theta[1]);
00797 if (diff2 > M_PI)
00798 diff1 = 2*M_PI - diff2;
00799
00800
00801 if (diff1 < diff2)
00802 theta[1] = theta[0];
00803
00804 tmp = links[3].a*links[3].a + links[4].d*links[4].d;
00805 if (tmp < 0)
00806 throw std::out_of_range("sqrt of negative number not allowed.");
00807
00808 Ro = sqrt(tmp);
00809 B = atan2(links[4].d,links[3].a);
00810 Cl = Tobj(1,4)*Tobj(1,4) +
00811 Tobj(2,4)*Tobj(2,4) +
00812 Tobj(3,4)*Tobj(3,4) -
00813 (links[2].d + links[3].d)*(links[2].d + links[3].d) -
00814 links[2].a*links[2].a -
00815 links[3].a*links[3].a -
00816 links[4].d*links[4].d;
00817 A = Cl / (2*links[2].a);
00818
00819 tmp = 1-A/Ro*A/Ro;
00820 if (tmp < 0)
00821 throw std::out_of_range("sqrt of negative number not allowed.");
00822
00823 theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
00824 theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
00825
00826 diff1 = fabs(q_actual(3)-theta[0]);
00827 if (diff1 > M_PI)
00828 diff1 = 2*M_PI - diff1 ;
00829
00830 diff2 = fabs(q_actual(3)-theta[3]);
00831 if (diff2 > M_PI)
00832 diff1 = 2*M_PI - diff2;
00833
00834
00835 if (diff1 < diff2)
00836 theta[3] = theta[0];
00837
00838 H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
00839 L = sin(theta[3])*links[4].d + cos(theta[3])*links[3].a + links[2].a;
00840 M = cos(theta[3])*links[4].d - sin(theta[3])*links[3].a;
00841
00842 theta[2] = atan2( M , L ) - atan2(Tobj(3,4) , H );
00843
00844 theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) ,
00845 cos(theta[2] + theta[3]) *
00846 (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
00847 - (sin(theta[2]+theta[3])*Tobj(3,3)) );
00848
00849 theta[4] = atan2(-1*(-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 diff1 = fabs(q_actual(4)-theta[0]);
00855 if (diff1 > M_PI)
00856 diff1 = 2*M_PI - diff1;
00857
00858 diff2 = fabs(q_actual(4)-theta[4]);
00859 if (diff2 > M_PI)
00860 diff1 = 2*M_PI - diff2;
00861
00862
00863 if (diff1 < diff2)
00864 theta[4] = theta[0];
00865
00866 theta[5] = atan2( cos(theta[4]) *
00867 ( cos(theta[2] + theta[3]) *
00868 (cos(theta[1]) * Tobj(1,3)
00869 + sin(theta[1])*Tobj(2,3))
00870 - (sin(theta[2]+theta[3])*Tobj(3,3)) ) +
00871 sin(theta[4])*(-sin(theta[1])*Tobj(1,3)
00872 + cos(theta[1])*Tobj(2,3)) ,
00873 sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3)
00874 + sin(theta[1])*Tobj(2,3) )
00875 + (cos(theta[2]+theta[3])*Tobj(3,3)) );
00876
00877 theta[6] = atan2( -sin(theta[4])
00878 * ( cos(theta[2] + theta[3]) *
00879 (cos(theta[1]) * Tobj(1,1)
00880 + sin(theta[1])*Tobj(2,1))
00881 - (sin(theta[2]+theta[3])*Tobj(3,1))) +
00882 cos(theta[4])*(-sin(theta[1])*Tobj(1,1)
00883 + cos(theta[1])*Tobj(2,1)),
00884 -sin(theta[4]) * ( cos(theta[2] + theta[3]) *
00885 (cos(theta[1]) * Tobj(1,2)
00886 + sin(theta[1])*Tobj(2,2))
00887 - (sin(theta[2]+theta[3])*Tobj(3,2))) +
00888 cos(theta[4])*(-sin(theta[1])*Tobj(1,2)
00889 + cos(theta[1])*Tobj(2,2)) );
00890
00891 qout(1) = theta[1];
00892 qout(2) = theta[2];
00893 qout(3) = theta[3];
00894 qout(4) = theta[4];
00895 qout(5) = theta[5];
00896 qout(6) = theta[6];
00897 set_q(qout);
00898
00899 converge = true;
00900 }
00901 catch(std::out_of_range & e)
00902 {
00903 converge = false;
00904 set_q(q_actual);
00905 qout = q_actual;
00906 }
00907
00908 qout.Release();
00909 return qout;
00910 }
00911
00912 ReturnMatrix Robot::inv_kin_ers_pos(const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool & converge) {
00913 bool converges[3];
00914
00915 bool third_invert=(robotType==ERS7_HEAD || robotType==ERS2XX_HEAD);
00916 bool second_invert=false;
00917
00918 if(endlink>=2) {
00919 if(endlink>=3) {
00920 if(endlink>=4)
00921 set_q(computeThirdERSLink(4,Pobj,endlink,Plink,third_invert,links[4].get_theta_min(),links[4].get_theta_max(),converges[2]),4);
00922 set_q(computeSecondERSLink(3,Pobj,endlink,Plink,second_invert,links[3].get_theta_min(),links[3].get_theta_max(),converges[1]),3);
00923 }
00924 set_q(computeFirstERSLink(2,Pobj,endlink,Plink,links[2].get_theta_min(),links[2].get_theta_max(),converges[0]),2);
00925 }
00926
00927
00928 if(!converges[0]) {
00929
00930 if(endlink>=3) {
00931 if(endlink>=4)
00932 set_q(computeSecondERSLink(4,Pobj,endlink,Plink,second_invert,links[4].get_theta_min(),links[4].get_theta_max(),converges[2]),4);
00933 set_q(computeFirstERSLink(3,Pobj,endlink,Plink,links[3].get_theta_min(),links[3].get_theta_max(),converges[1]),3);
00934 }
00935 }
00936
00937
00938 if(!converges[1] && endlink>=4) {
00939
00940 set_q(computeFirstERSLink(4,Pobj,endlink,Plink,links[4].get_theta_min(),links[4].get_theta_max(),converges[2]),4);
00941 }
00942
00943 converge=converges[0] && converges[1] && converges[2];
00944 return get_q();
00945 }
00946
00947 Real Robot::computeFirstERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, Real min, Real max, bool & converge) {
00948
00949 ColumnVector po=convertFrame(0,curlink)*Pobj;
00950 ColumnVector pl=convertLink(endlink,curlink)*Plink;
00951 if(fabs(pl(1))<ITOL && fabs(pl(2))<ITOL) {
00952
00953 converge=false;
00954 return links[curlink].get_q();
00955 }
00956 Real to=atan2(po(2),po(1));
00957 Real tl=atan2(pl(2),pl(1));
00958 Real qtgt=normalize_angle(to-tl);
00959 Real qans=limit_angle(qtgt,min,max);
00960 converge=(qtgt==qans);
00961 return qans;
00962 }
00963
00964 Real Robot::computeSecondERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool invert, Real min, Real max, bool & converge) {
00965 if(Plink(4)==0)
00966 return computeFirstERSLink(curlink,Pobj,endlink,Plink,min,max,converge);
00967
00968 ColumnVector po=convertFrame(0,curlink)*Pobj;
00969 ColumnVector pl=convertLink(endlink,curlink)*Plink;
00970 if(fabs(pl(1))<ITOL && fabs(pl(2))<ITOL) {
00971
00972 converge=false;
00973 return links[curlink].get_q();
00974 }
00975 Matrix Rp;
00976 ColumnVector pp;
00977 convertFrame(Rp,pp,curlink-1,curlink);
00978 ColumnVector zp=Rp.SubMatrix(1,3,3,3);
00979 Real dot_zppo=zp(1)*po(1)+zp(2)*po(2)+zp(3)*po(3);
00980 Real ao=M_PI_2-acos(dot_zppo/sqrt(zp.SumSquare()*po.SumSquare()));
00981 Real r=(pl(1)*pl(1)+pl(2)*pl(2))/(pl(4)*pl(4));
00982 Real tao=tan(ao);
00983 tao*=tao;
00984 Real tors=(r-pl(3)*pl(3)*tao)/(r+r*tao);
00985 Real sign;
00986 if(dot_zppo>0)
00987 sign=(DotProduct(zp,pp)<0)?1:-1;
00988 else
00989 sign=(DotProduct(zp,pp)<0)?-1:1;
00990 if(tors<0) {
00991
00992 converge=false;
00993 return limit_angle(sign*M_PI_2,min,max);
00994 } else {
00995 Real to=sign*acos(sqrt(tors));
00996 if(invert)
00997 to=M_PI-to;
00998 Real tl=atan2(pl(2),pl(1));
00999 Real qtgt=normalize_angle(to-tl);
01000 Real qans=limit_angle(qtgt,min,max);
01001 converge=(qtgt==qans);
01002 return qans;
01003 }
01004 }
01005
01006 Real Robot::computeThirdERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool invert, Real min, Real max, bool & converge) {
01007
01008
01009
01010
01011
01012 Matrix Rp;
01013 ColumnVector pp;
01014 convertFrame(Rp,pp,curlink-1,curlink);
01015
01016
01017 Real previous_to_cur_len=sqrt(pp(1)*pp(1)+pp(2)*pp(2));
01018
01019 ColumnVector pl=convertLink(endlink,curlink)*Plink;
01020
01021 if(fabs(pl(1))<ITOL && fabs(pl(2))<ITOL) {
01022
01023 converge=true;
01024 return links[curlink].get_q();
01025 }
01026 Real tl=atan2(pl(2),pl(1));
01027 Real tp=atan2(pp(2),pp(1));
01028
01029 if(Plink(4)==0) {
01030
01031
01032 Real qtgt=normalize_angle(tp-tl);
01033 Real qans=limit_angle(qtgt,min,max);
01034 converge=(qtgt==qans);
01035 return qans;
01036 } else {
01037
01038 Real plz=pl(3)/pl(4);
01039 pl(3)=0;
01040
01041
01042 Real cur_to_plink_len=homog_norm(pl);
01043
01044 ColumnVector prev_to_pobj=convertFrame(0,curlink-1)*Pobj;
01045
01046 Real prev_to_pobj_xyz_len=homog_norm(prev_to_pobj);
01047
01048 prev_to_pobj(3)=0;
01049
01050 if(plz>prev_to_pobj_xyz_len) {
01051
01052 converge=false;
01053 return limit_angle(normalize_angle(tp-tl),min,max);
01054 }
01055 Real tgt_len=sqrt(prev_to_pobj_xyz_len*prev_to_pobj_xyz_len-plz*plz);
01056
01057 Real aor_d=(2*cur_to_plink_len*previous_to_cur_len);
01058
01059
01060 if(fabs(aor_d)<=ITOL) {
01061
01062
01063 cout << "ASSERTION FAILED: " << __FILE__ << ':' << __LINE__ << endl;
01064 converge=false;
01065 return links[curlink].get_q();
01066 } else {
01067 Real aor=(cur_to_plink_len*cur_to_plink_len+previous_to_cur_len*previous_to_cur_len-tgt_len*tgt_len)/aor_d;
01068
01069 if(aor<-1) {
01070
01071 converge=false;
01072 return limit_angle(normalize_angle(M_PI+tp-tl),min,max);
01073 } else if(aor>1) {
01074
01075 converge=false;
01076 return limit_angle(normalize_angle(tp-tl),min,max);
01077 } else {
01078 Real ao=-acos(aor);
01079
01080 if(invert)
01081 ao=-ao;
01082
01083 Real qtgt=normalize_angle(ao+tp-tl);
01084
01085 Real qans=limit_angle(qtgt,min,max);
01086
01087 converge=(qtgt==qans);
01088 return qans;
01089 }
01090 }
01091 }
01092 }
01093
01094
01095
01096
01097
01098
01099 ReturnMatrix mRobot::inv_kin(const Matrix & Tobj, const int mj)
01100
01101 {
01102 bool converge = false;
01103 return inv_kin(Tobj, mj, dof, converge);
01104 }
01105
01106
01107 ReturnMatrix mRobot::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
01108
01109
01110
01111
01112
01113
01114
01115 {
01116 switch (robotType) {
01117 case RHINO:
01118 return inv_kin_rhino(Tobj, converge);
01119 break;
01120 case PUMA:
01121 return inv_kin_puma(Tobj, converge);
01122 break;
01123 default:
01124 return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
01125 }
01126 }
01127
01128
01129 ReturnMatrix mRobot::inv_kin_rhino(const Matrix & Tobj, bool & converge)
01130
01131
01132
01133
01134
01135 {
01136 ColumnVector qout(5), q_actual;
01137 q_actual = get_q();
01138
01139 try
01140 {
01141 Real theta[6] , diff1, diff2, tmp,
01142 angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , Gl=0.0 ;
01143
01144 if (links[6].d > 0)
01145 {
01146 ColumnVector tmpd6(3);
01147 tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
01148 tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
01149 Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
01150 }
01151
01152
01153 theta[0] = atan2(Tobj(2,4),
01154 Tobj(1,4));
01155
01156 theta[1] = atan2(-Tobj(2,4),
01157 -Tobj(1,4)) ;
01158
01159 diff1 = fabs(q_actual(1)-theta[0]) ;
01160 if (diff1 > M_PI)
01161 diff1 = 2*M_PI - diff1;
01162
01163 diff2 = fabs(q_actual(1)-theta[1]);
01164 if (diff2 > M_PI)
01165 diff1 = 2*M_PI - diff2 ;
01166
01167
01168 if (diff1 < diff2)
01169 theta[1] = theta[0] ;
01170
01171 theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1),
01172 sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
01173
01174
01175 angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
01176 -1*Tobj(3,3));
01177
01178 L = cos(theta[1])*Tobj(1,4) +
01179 sin(theta[1])*Tobj(2,4) +
01180 links[5].d*sin(angle) -
01181 links[5].a*cos(angle);
01182 M = links[1].d -
01183 Tobj(3,4) -
01184 links[5].d*cos(angle) -
01185 links[5].a*sin(angle);
01186 K = (L*L + M*M - links[4].a*links[4].a -
01187 links[3].a*links[3].a) /
01188 (2 * links[4].a * links[4].a);
01189
01190 tmp = 1-K*K;
01191 if (tmp < 0)
01192 throw std::out_of_range("sqrt of negative number not allowed.");
01193
01194 theta[0] = atan2( sqrt(tmp) , K );
01195 theta[3] = atan2( -sqrt(tmp) , K );
01196
01197 diff1 = fabs(q_actual(3)-theta[0]) ;
01198 if (diff1 > M_PI)
01199 diff1 = 2*M_PI - diff1 ;
01200
01201 diff2 = fabs(q_actual(3)-theta[3]);
01202 if (diff2 > M_PI)
01203 diff1 = 2*M_PI - diff2 ;
01204
01205 if (diff1 < diff2)
01206 theta[3] = theta[0] ;
01207
01208 H = cos(theta[3]) * links[4].a + links[3].a;
01209 Gl = sin(theta[3]) * links[4].a;
01210
01211 theta[2] = atan2( M , L ) - atan2( Gl , H );
01212 theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) ,
01213 -1*Tobj(3,3)) - theta[2] - theta[3] ;
01214
01215 qout(1) = theta[1];
01216 qout(2) = theta[2];
01217 qout(3) = theta[3];
01218 qout(4) = theta[4];
01219 qout(5) = theta[5];
01220 set_q(qout);
01221
01222 converge = true;
01223 }
01224 catch(std::out_of_range & e)
01225 {
01226 converge = false;
01227 set_q(q_actual);
01228 qout = q_actual;
01229 }
01230
01231 qout.Release();
01232 return qout;
01233 }
01234
01235
01236 ReturnMatrix mRobot::inv_kin_puma(const Matrix & Tobj, bool & converge)
01237
01238
01239
01240
01241
01242 {
01243 ColumnVector qout(6), q_actual;
01244 q_actual = get_q();
01245
01246 try
01247 {
01248 Real theta[7] , diff1, diff2, tmp,
01249 A = 0.0 , B = 0.0 , Cl = 0.0 , D =0.0, Ro = 0.0,
01250 H = 0.0 , L = 0.0 , M = 0.0;
01251
01252
01253
01254 if (links[6].d)
01255 {
01256 ColumnVector tmpd6(3);
01257 tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
01258 tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
01259 Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
01260 }
01261
01262 tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
01263 if (tmp < 0)
01264 throw std::out_of_range("sqrt of negative number not allowed.");
01265
01266 Ro = sqrt(tmp);
01267 D = (links[2].d+links[3].d) / Ro;
01268
01269 tmp = 1-D*D;
01270 if (tmp < 0)
01271 throw std::out_of_range("sqrt of negative number not allowed.");
01272
01273
01274 theta[0] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));
01275
01276 theta[1] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
01277
01278 diff1 = fabs(q_actual(1)-theta[0]);
01279 if (diff1 > M_PI)
01280 diff1 = 2*M_PI - diff1;
01281
01282 diff2 = fabs(q_actual(1)-theta[1]);
01283 if (diff2 > M_PI)
01284 diff1 = 2*M_PI - diff2;
01285
01286
01287 if (diff1 < diff2)
01288 theta[1] = theta[0];
01289
01290 tmp = links[4].a*links[4].a + links[4].d*links[4].d;
01291 if (tmp < 0)
01292 throw std::out_of_range("sqrt of negative number not allowed.");
01293
01294 Ro = sqrt(tmp);
01295 B = atan2(links[4].d,links[4].a);
01296 Cl = Tobj(1,4)*Tobj(1,4) +
01297 Tobj(2,4)*Tobj(2,4) +
01298 Tobj(3,4)*Tobj(3,4) -
01299 (links[2].d + links[3].d)*(links[2].d + links[3].d) -
01300 links[3].a*links[3].a -
01301 links[4].a*links[4].a -
01302 links[4].d*links[4].d;
01303 A = Cl / (2*links[3].a);
01304
01305 tmp = 1-A/Ro*A/Ro;
01306 if (tmp < 0)
01307 throw std::out_of_range("sqrt of negative number not allowed.");
01308
01309 theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
01310 theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
01311
01312 diff1 = fabs(q_actual(3)-theta[0]);
01313 if (diff1 > M_PI)
01314 diff1 = 2*M_PI - diff1 ;
01315
01316 diff2 = fabs(q_actual(3)-theta[3]);
01317 if (diff2 > M_PI)
01318 diff1 = 2*M_PI - diff2;
01319
01320
01321 if (diff1 < diff2)
01322 theta[3] = theta[0];
01323
01324 H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
01325 L = sin(theta[3])*links[4].d + cos(theta[3])*links[4].a + links[3].a;
01326 M = cos(theta[3])*links[4].d - sin(theta[3])*links[4].a;
01327
01328 theta[2] = atan2( M , L ) - atan2(Tobj(3,4) , H );
01329
01330 theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) ,
01331 cos(theta[2] + theta[3]) *
01332 (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
01333 - (sin(theta[2]+theta[3])*Tobj(3,3)) );
01334
01335 theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)),
01336 -cos(theta[2] + theta[3]) *
01337 (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
01338 + (sin(theta[2]+theta[3])*Tobj(3,3)) );
01339
01340 diff1 = fabs(q_actual(4)-theta[0]);
01341 if (diff1 > M_PI)
01342 diff1 = 2*M_PI - diff1;
01343
01344 diff2 = fabs(q_actual(4)-theta[4]);
01345 if (diff2 > M_PI)
01346 diff1 = 2*M_PI - diff2;
01347
01348
01349 if (diff1 < diff2)
01350 theta[4] = theta[0];
01351
01352 theta[5] = atan2( cos(theta[4]) *
01353 ( cos(theta[2] + theta[3]) *
01354 (cos(theta[1]) * Tobj(1,3)
01355 + sin(theta[1])*Tobj(2,3))
01356 - (sin(theta[2]+theta[3])*Tobj(3,3)) ) +
01357 sin(theta[4])*(-sin(theta[1])*Tobj(1,3)
01358 + cos(theta[1])*Tobj(2,3)) ,
01359 sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3)
01360 + sin(theta[1])*Tobj(2,3) )
01361 + (cos(theta[2]+theta[3])*Tobj(3,3)) );
01362
01363 theta[6] = atan2( -sin(theta[4])
01364 * ( cos(theta[2] + theta[3]) *
01365 (cos(theta[1]) * Tobj(1,1)
01366 + sin(theta[1])*Tobj(2,1))
01367 - (sin(theta[2]+theta[3])*Tobj(3,1))) +
01368 cos(theta[4])*(-sin(theta[1])*Tobj(1,1)
01369 + cos(theta[1])*Tobj(2,1)),
01370 -sin(theta[4]) * ( cos(theta[2] + theta[3]) *
01371 (cos(theta[1]) * Tobj(1,2)
01372 + sin(theta[1])*Tobj(2,2))
01373 - (sin(theta[2]+theta[3])*Tobj(3,2))) +
01374 cos(theta[4])*(-sin(theta[1])*Tobj(1,2)
01375 + cos(theta[1])*Tobj(2,2)) );
01376
01377 qout(1) = theta[1];
01378 qout(2) = theta[2];
01379 qout(3) = theta[3];
01380 qout(4) = theta[4];
01381 qout(5) = theta[5];
01382 qout(6) = theta[6];
01383 set_q(qout);
01384
01385 converge = true;
01386 }
01387 catch(std::out_of_range & e)
01388 {
01389 converge = false;
01390 set_q(q_actual);
01391 qout = q_actual;
01392 }
01393
01394 qout.Release();
01395 return qout;
01396 }
01397
01398
01399 ReturnMatrix mRobot_min_para::inv_kin(const Matrix & Tobj, const int mj)
01400
01401 {
01402 bool converge = false;
01403 return inv_kin(Tobj, mj, dof, converge);
01404 }
01405
01406
01407 ReturnMatrix mRobot_min_para::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
01408
01409
01410
01411
01412
01413
01414
01415 {
01416 switch (robotType) {
01417 case RHINO:
01418 return inv_kin_rhino(Tobj, converge);
01419 break;
01420 case PUMA:
01421 return inv_kin_puma(Tobj, converge);
01422 break;
01423 default:
01424 return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
01425 }
01426 }
01427
01428
01429 ReturnMatrix mRobot_min_para::inv_kin_rhino(const Matrix & Tobj, bool & converge)
01430
01431
01432
01433
01434
01435 {
01436 ColumnVector qout(5), q_actual;
01437 q_actual = get_q();
01438
01439 try
01440 {
01441 Real theta[6] , diff1, diff2, tmp,
01442 angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , Gl=0.0 ;
01443
01444
01445 theta[0] = atan2(Tobj(2,4),
01446 Tobj(1,4));
01447
01448 theta[1] = atan2(-Tobj(2,4),
01449 -Tobj(1,4)) ;
01450
01451 diff1 = fabs(q_actual(1)-theta[0]) ;
01452 if (diff1 > M_PI)
01453 diff1 = 2*M_PI - diff1;
01454
01455 diff2 = fabs(q_actual(1)-theta[1]);
01456 if (diff2 > M_PI)
01457 diff1 = 2*M_PI - diff2 ;
01458
01459
01460 if (diff1 < diff2)
01461 theta[1] = theta[0] ;
01462
01463 theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1),
01464 sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
01465
01466
01467 angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
01468 -1*Tobj(3,3));
01469
01470 L = cos(theta[1])*Tobj(1,4) +
01471 sin(theta[1])*Tobj(2,4) +
01472 links[5].d*sin(angle) -
01473 links[5].a*cos(angle);
01474 M = links[1].d -
01475 Tobj(3,4) -
01476 links[5].d*cos(angle) -
01477 links[5].a*sin(angle);
01478 K = (L*L + M*M - links[4].a*links[4].a -
01479 links[3].a*links[3].a) /
01480 (2 * links[4].a * links[4].a);
01481
01482 tmp = 1-K*K;
01483 if (tmp < 0)
01484 throw std::out_of_range("sqrt of negative number not allowed.");
01485
01486 theta[0] = atan2( sqrt(tmp) , K );
01487 theta[3] = atan2( -sqrt(tmp) , K );
01488
01489 diff1 = fabs(q_actual(3)-theta[0]) ;
01490 if (diff1 > M_PI)
01491 diff1 = 2*M_PI - diff1 ;
01492
01493 diff2 = fabs(q_actual(3)-theta[3]);
01494 if (diff2 > M_PI)
01495 diff1 = 2*M_PI - diff2 ;
01496
01497 if (diff1 < diff2)
01498 theta[3] = theta[0] ;
01499
01500 H = cos(theta[3]) * links[4].a + links[3].a;
01501 Gl = sin(theta[3]) * links[4].a;
01502
01503 theta[2] = atan2( M , L ) - atan2( Gl , H );
01504 theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) ,
01505 -1*Tobj(3,3)) - theta[2] - theta[3] ;
01506
01507 qout(1) = theta[1];
01508 qout(2) = theta[2];
01509 qout(3) = theta[3];
01510 qout(4) = theta[4];
01511 qout(5) = theta[5];
01512 set_q(qout);
01513
01514 converge = true;
01515 }
01516 catch(std::out_of_range & e)
01517 {
01518 converge = false;
01519 set_q(q_actual);
01520 qout = q_actual;
01521 }
01522
01523 qout.Release();
01524 return qout;
01525 }
01526
01527
01528 ReturnMatrix mRobot_min_para::inv_kin_puma(const Matrix & Tobj, bool & converge)
01529
01530
01531
01532
01533
01534 {
01535 ColumnVector qout(6), q_actual;
01536 q_actual = get_q();
01537
01538 try
01539 {
01540 Real theta[7] , diff1, diff2, tmp,
01541 A = 0.0 , B = 0.0 , Cl = 0.0 , D =0.0, Ro = 0.0,
01542 H = 0.0 , L = 0.0 , M = 0.0;
01543
01544
01545
01546 if (links[6].d > 0)
01547 {
01548 ColumnVector tmpd6(3);
01549 tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
01550 tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
01551 Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
01552 }
01553
01554 tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
01555 if (tmp < 0)
01556 throw std::out_of_range("sqrt of negative number not allowed.");
01557
01558 Ro = sqrt(tmp);
01559 D = (links[2].d+links[3].d) / Ro;
01560
01561 tmp = 1-D*D;
01562 if (tmp < 0)
01563 throw std::out_of_range("sqrt of negative number not allowed.");
01564
01565
01566 theta[0] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));
01567
01568 theta[1] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
01569
01570 diff1 = fabs(q_actual(1)-theta[0]);
01571 if (diff1 > M_PI)
01572 diff1 = 2*M_PI - diff1;
01573
01574 diff2 = fabs(q_actual(1)-theta[1]);
01575 if (diff2 > M_PI)
01576 diff1 = 2*M_PI - diff2;
01577
01578
01579 if (diff1 < diff2)
01580 theta[1] = theta[0];
01581
01582 tmp = links[4].a*links[4].a + links[4].d*links[4].d;
01583 if (tmp < 0)
01584 throw std::out_of_range("sqrt of negative number not allowed.");
01585
01586 Ro = sqrt(tmp);
01587 B = atan2(links[4].d,links[4].a);
01588 Cl = Tobj(1,4)*Tobj(1,4) +
01589 Tobj(2,4)*Tobj(2,4) +
01590 Tobj(3,4)*Tobj(3,4) -
01591 (links[2].d + links[3].d)*(links[2].d + links[3].d) -
01592 links[3].a*links[3].a -
01593 links[4].a*links[4].a -
01594 links[4].d*links[4].d;
01595 A = Cl / (2*links[3].a);
01596
01597 tmp = 1-A/Ro*A/Ro;
01598 if (tmp < 0)
01599 throw std::out_of_range("sqrt of negative number not allowed.");
01600
01601 theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
01602 theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
01603
01604 diff1 = fabs(q_actual(3)-theta[0]);
01605 if (diff1 > M_PI)
01606 diff1 = 2*M_PI - diff1 ;
01607
01608 diff2 = fabs(q_actual(3)-theta[3]);
01609 if (diff2 > M_PI)
01610 diff1 = 2*M_PI - diff2;
01611
01612
01613 if (diff1 < diff2)
01614 theta[3] = theta[0];
01615
01616 H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
01617 L = sin(theta[3])*links[4].d + cos(theta[3])*links[4].a + links[3].a;
01618 M = cos(theta[3])*links[4].d - sin(theta[3])*links[4].a;
01619
01620 theta[2] = atan2( M , L ) - atan2(Tobj(3,4) , H );
01621
01622 theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) ,
01623 cos(theta[2] + theta[3]) *
01624 (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
01625 - (sin(theta[2]+theta[3])*Tobj(3,3)) );
01626
01627 theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)),
01628 -cos(theta[2] + theta[3]) *
01629 (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3))
01630 + (sin(theta[2]+theta[3])*Tobj(3,3)) );
01631
01632 diff1 = fabs(q_actual(4)-theta[0]);
01633 if (diff1 > M_PI)
01634 diff1 = 2*M_PI - diff1;
01635
01636 diff2 = fabs(q_actual(4)-theta[4]);
01637 if (diff2 > M_PI)
01638 diff1 = 2*M_PI - diff2;
01639
01640
01641 if (diff1 < diff2)
01642 theta[4] = theta[0];
01643
01644 theta[5] = atan2( cos(theta[4]) *
01645 ( cos(theta[2] + theta[3]) *
01646 (cos(theta[1]) * Tobj(1,3)
01647 + sin(theta[1])*Tobj(2,3))
01648 - (sin(theta[2]+theta[3])*Tobj(3,3)) ) +
01649 sin(theta[4])*(-sin(theta[1])*Tobj(1,3)
01650 + cos(theta[1])*Tobj(2,3)) ,
01651 sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3)
01652 + sin(theta[1])*Tobj(2,3) )
01653 + (cos(theta[2]+theta[3])*Tobj(3,3)) );
01654
01655 theta[6] = atan2( -sin(theta[4])
01656 * ( cos(theta[2] + theta[3]) *
01657 (cos(theta[1]) * Tobj(1,1)
01658 + sin(theta[1])*Tobj(2,1))
01659 - (sin(theta[2]+theta[3])*Tobj(3,1))) +
01660 cos(theta[4])*(-sin(theta[1])*Tobj(1,1)
01661 + cos(theta[1])*Tobj(2,1)),
01662 -sin(theta[4]) * ( cos(theta[2] + theta[3]) *
01663 (cos(theta[1]) * Tobj(1,2)
01664 + sin(theta[1])*Tobj(2,2))
01665 - (sin(theta[2]+theta[3])*Tobj(3,2))) +
01666 cos(theta[4])*(-sin(theta[1])*Tobj(1,2)
01667 + cos(theta[1])*Tobj(2,2)) );
01668
01669 qout(1) = theta[1];
01670 qout(2) = theta[2];
01671 qout(3) = theta[3];
01672 qout(4) = theta[4];
01673 qout(5) = theta[5];
01674 qout(6) = theta[6];
01675 set_q(qout);
01676
01677 converge = true;
01678 }
01679 catch(std::out_of_range & e)
01680 {
01681 converge = false;
01682 set_q(q_actual);
01683 qout = q_actual;
01684 }
01685
01686 qout.Release();
01687 return qout;
01688 }
01689
01690 #ifdef use_namespace
01691 }
01692 #endif