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