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