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