invkine.cpp

Go to the documentation of this file.
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  
00077 #ifdef USING_FLOAT //from newmat's include.h
00078 #  define ITOL   1e-4  
00079 #else
00080 #  define ITOL   1e-6  
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          if (dq.MaximumAbsoluteValue() < ITOL)
00164          {
00165             converge = true;
00166             break;
00167          }
00168       }
00169    }
00170 
00171    if(converge)
00172    {
00173       // Make sure that: -pi < qout <= pi for revolute joints
00174       for(int i = 1; i <= dof; i++)
00175       {
00176          if(links[i].immobile)
00177             continue;
00178          if(links[i].get_joint_type() == 0) {
00179              qout(i) = fmod(qout(i), 2*M_PI);
00180          }
00181       }
00182       set_q(qPrev);
00183       qout.Release();
00184       return qout;
00185    }
00186    else
00187    {
00188       set_q(qPrev);
00189       q_tmp.Release();
00190       return q_tmp;
00191    }
00192 }
00193 
00194 // ---------------------  R O B O T   DH   N O T A T I O N  --------------------------
00195 
00196 ReturnMatrix Robot::inv_kin(const Matrix & Tobj, const int mj)
00198 {
00199    bool converge = false;
00200    return inv_kin(Tobj, mj, dof, converge);
00201 }
00202 
00203 
00204 ReturnMatrix Robot::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
00212 {
00213     switch (robotType) {
00214         case RHINO:
00215             return inv_kin_rhino(Tobj, converge);
00216             break;
00217         case PUMA:
00218             return inv_kin_puma(Tobj, converge);
00219             break;
00220         case SCHILLING:
00221             return inv_kin_schilling(Tobj, converge);
00222             break;
00223         default:
00224             return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
00225     }
00226 }
00227 
00228 
00229 ReturnMatrix Robot::inv_kin_rhino(const Matrix & Tobj, bool & converge)
00235 {
00236     ColumnVector qout(5), q_actual;
00237     q_actual = get_q();
00238 
00239     try
00240     {
00241         Real theta[6] , diff1, diff2, tmp,
00242         angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , G=0.0 ;
00243 
00244         // Calcul les deux angles possibles
00245         theta[0] = atan2(Tobj(2,4),
00246                          Tobj(1,4));
00247 
00248         theta[1] = atan2(-Tobj(2,4),
00249                          -Tobj(1,4))  ;
00250 
00251         diff1 = fabs(q_actual(1)-theta[0]) ;            
00252         if (diff1 > M_PI)
00253             diff1 = 2*M_PI - diff1;
00254 
00255         diff2 = fabs(q_actual(1)-theta[1]);
00256         if (diff2 > M_PI)
00257             diff2 = 2*M_PI - diff2 ;
00258 
00259         // Prend l'angle le plus proche de sa position actuel
00260         if (diff1 < diff2)                 
00261             theta[1] = theta[0] ;
00262 
00263         theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1), 
00264                          sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
00265 
00266         // angle = theta1 +theta2 + theta3
00267         angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
00268                       -1*Tobj(3,3));
00269 
00270         L = cos(theta[1])*Tobj(1,4) + 
00271             sin(theta[1])*Tobj(2,4) + 
00272             links[5].d*sin(angle) - 
00273             links[4].a*cos(angle);
00274         M = links[1].d - 
00275             Tobj(3,4) - 
00276             links[5].d*cos(angle) - 
00277             links[4].a*sin(angle);
00278         K = (L*L + M*M - links[3].a*links[3].a - 
00279              links[2].a*links[2].a) / 
00280             (2 * links[3].a * links[2].a);
00281 
00282         tmp = 1-K*K;
00283         if (tmp < 0)
00284             throw out_of_range("sqrt of negative number not allowed.");
00285 
00286         theta[0] = atan2( sqrt(tmp) , K );
00287         theta[3] = atan2( -sqrt(tmp) , K );     
00288 
00289         diff1 = fabs(q_actual(3)-theta[0]) ;
00290         if (diff1 > M_PI)
00291             diff1 = 2*M_PI - diff1 ;
00292 
00293         diff2 = fabs(q_actual(3)-theta[3]);
00294         if (diff2 > M_PI)
00295             diff2 = 2*M_PI - diff2 ;
00296 
00297         if (diff1 < diff2)
00298             theta[3] = theta[0] ;
00299 
00300         H = cos(theta[3]) * links[3].a + links[2].a;
00301         G = sin(theta[3]) * links[3].a;
00302 
00303         theta[2] = atan2( M , L ) - atan2( G , H );
00304         theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) , 
00305                           -1*Tobj(3,3)) - theta[2] - theta[3] ;
00306 
00307         qout(1) = theta[1];
00308         qout(2) = theta[2];
00309         qout(3) = theta[3];
00310         qout(4) = theta[4];
00311         qout(5) = theta[5];
00312 
00313         converge = true;
00314     }
00315     catch(std::out_of_range & e)
00316     {
00317         converge = false; 
00318         qout = q_actual;
00319     }
00320 
00321     qout.Release();
00322     return qout;
00323 }
00324 
00325 
00326 ReturnMatrix Robot::inv_kin_puma(const Matrix & Tobj, bool & converge)
00332 {
00333     ColumnVector qout(6), q_actual;
00334     q_actual = get_q();
00335 
00336     try
00337     {  
00338         Real theta[7] , diff1, diff2, tmp,
00339         A = 0.0 , B = 0.0 , C = 0.0 , D =0.0, Ro = 0.0,
00340         H = 0.0 , L = 0.0 , M = 0.0;
00341 
00342         // Removed d6 component because in the Puma inverse kinematics solution 
00343         // d6 = 0. 
00344         if (links[6].d)
00345         {
00346             ColumnVector tmpd6(3); Matrix tmp;
00347             tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
00348             tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
00349             Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
00350         }
00351 
00352         tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
00353         if (tmp < 0)
00354             throw std::out_of_range("sqrt of negative number not allowed.");
00355 
00356         Ro = sqrt(tmp);
00357         D = (links[2].d+links[3].d) / Ro;
00358 
00359         tmp = 1-D*D;
00360         if (tmp < 0)
00361             throw std::out_of_range("sqrt of negative number not allowed.");
00362 
00363         //Calcul les deux angles possibles
00364         theta[0] =  atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));    
00365         //Calcul les deux angles possibles
00366         theta[1] =  atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
00367 
00368         diff1 = fabs(q_actual(1)-theta[0]);
00369         if (diff1 > M_PI)
00370             diff1 = 2*M_PI - diff1;
00371 
00372         diff2 = fabs(q_actual(1)-theta[1]);
00373         if (diff2 > M_PI)
00374             diff2 = 2*M_PI - diff2;
00375 
00376         // Prend l'angle le plus proche de sa position actuel
00377         if (diff1 < diff2)
00378             theta[1] = theta[0];
00379 
00380         tmp = links[3].a*links[3].a + links[4].d*links[4].d;
00381         if (tmp < 0)
00382             throw std::out_of_range("sqrt of negative number not allowed.");
00383 
00384         Ro = sqrt(tmp);
00385         B = atan2(links[4].d,links[3].a);
00386         C = Tobj(1,4)*Tobj(1,4) + 
00387             Tobj(2,4)*Tobj(2,4) + 
00388             (Tobj(3,4)-links[1].d)*(Tobj(3,4)-links[1].d) - 
00389             (links[2].d + links[3].d)*(links[2].d + links[3].d) - 
00390             links[2].a*links[2].a - 
00391             links[3].a*links[3].a - 
00392             links[4].d*links[4].d; 
00393         A = C / (2*links[2].a);
00394 
00395         tmp = 1-A/Ro*A/Ro;
00396         if (tmp < 0)
00397             throw std::out_of_range("sqrt of negative number not allowed.");
00398 
00399         theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
00400         theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
00401 
00402         diff1 = fabs(q_actual(3)-theta[0]);
00403         if (diff1 > M_PI)
00404             diff1 = 2*M_PI - diff1 ;
00405 
00406         diff2 = fabs(q_actual(3)-theta[3]);
00407         if (diff2 > M_PI)
00408             diff1 = 2*M_PI - diff2;
00409 
00410         //Prend l'angle le plus proche de sa position actuel
00411         if (diff1 < diff2)
00412             theta[3] = theta[0];
00413 
00414         H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
00415         L = sin(theta[3])*links[4].d + cos(theta[3])*links[3].a + links[2].a;
00416         M = cos(theta[3])*links[4].d - sin(theta[3])*links[3].a;
00417 
00418         theta[2] = atan2( M , L ) - atan2(Tobj(3,4)-links[1].d , H );
00419 
00420         theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) , 
00421                           cos(theta[2] + theta[3]) * 
00422                           (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3)) 
00423                           - (sin(theta[2]+theta[3])*Tobj(3,3)) );
00424 
00425         theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)), 
00426                          -cos(theta[2] + theta[3]) * 
00427                          (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3)) 
00428                          + (sin(theta[2]+theta[3])*Tobj(3,3)) );
00429 
00430         diff1 = fabs(q_actual(4)-theta[0]);
00431         if (diff1 > M_PI)
00432             diff1 = 2*M_PI - diff1;
00433 
00434         diff2 = fabs(q_actual(4)-theta[4]);
00435         if (diff2 > M_PI)
00436             diff2 = 2*M_PI - diff2;
00437 
00438         // Prend l'angle le plus proche de sa position actuel
00439         if (diff1 < diff2)
00440             theta[4] = theta[0];
00441 
00442         theta[5] = atan2( cos(theta[4]) * 
00443                           ( cos(theta[2] + theta[3]) * 
00444                             (cos(theta[1]) * Tobj(1,3) 
00445                              + sin(theta[1])*Tobj(2,3)) 
00446                             - (sin(theta[2]+theta[3])*Tobj(3,3)) ) + 
00447                           sin(theta[4])*(-sin(theta[1])*Tobj(1,3) 
00448                                          + cos(theta[1])*Tobj(2,3)) , 
00449                           sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3) 
00450                                                     + sin(theta[1])*Tobj(2,3) ) 
00451                           + (cos(theta[2]+theta[3])*Tobj(3,3)) );
00452 
00453         theta[6] = atan2( -sin(theta[4]) 
00454                           * ( cos(theta[2] + theta[3]) * 
00455                               (cos(theta[1]) * Tobj(1,1) 
00456                                + sin(theta[1])*Tobj(2,1)) 
00457                               - (sin(theta[2]+theta[3])*Tobj(3,1))) + 
00458                           cos(theta[4])*(-sin(theta[1])*Tobj(1,1) 
00459                                          + cos(theta[1])*Tobj(2,1)), 
00460                           -sin(theta[4]) * ( cos(theta[2] + theta[3]) * 
00461                                              (cos(theta[1]) * Tobj(1,2) 
00462                                               + sin(theta[1])*Tobj(2,2)) 
00463                                              - (sin(theta[2]+theta[3])*Tobj(3,2))) +
00464                           cos(theta[4])*(-sin(theta[1])*Tobj(1,2) 
00465                                          + cos(theta[1])*Tobj(2,2)) );
00466 
00467         qout(1) = theta[1];
00468         qout(2) = theta[2];
00469         qout(3) = theta[3];
00470         qout(4) = theta[4];
00471         qout(5) = theta[5];
00472         qout(6) = theta[6];
00473 
00474         converge = true; 
00475     }
00476     catch(std::out_of_range & e)
00477     {
00478         converge = false; 
00479         qout = q_actual;
00480     }
00481 
00482     qout.Release();
00483     return qout;
00484 }
00485 
00486 ReturnMatrix Robot::inv_kin_schilling(const Matrix & Tobj, bool & converge)
00492 {
00493     ColumnVector qout(6), q_actual;
00494     q_actual = get_q();
00495 
00496     try
00497     {
00498         Real theta[7], K=0.0, A=0.0, B=0.0, C=0.0, D=0.0, tmp=0.0, theta234 , diff1, diff2;
00499 
00500         if (links[6].d)
00501         {
00502             ColumnVector tmpd6(3); 
00503             Matrix tmp;
00504             tmpd6(1)=0;
00505             tmpd6(2)=0;
00506             tmpd6(3)=links[6].d;
00507             tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
00508             Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
00509         }
00510 
00511         theta[1] = atan2(Tobj(2,4),Tobj(1,4));
00512         //Calcul les deux angles possibles
00513         theta[0] = atan2(-Tobj(2,4),-Tobj(1,4));
00514 
00515         diff1 = fabs(q_actual(1)-theta[0]);
00516         if (diff1 > M_PI)
00517             diff1 = 2*M_PI - diff1;
00518 
00519         diff2 = fabs(q_actual(1)-theta[1]);
00520         if (diff2 > M_PI)
00521             diff2 = 2*M_PI - diff2;
00522 
00523         // Prend l'angle le plus proche de sa position actuel
00524         if (diff1 < diff2)
00525             theta[1] = theta[0];
00526 
00527         //theta2+theta3+theta4  
00528         theta234 = atan2( Tobj(3,3) , cos(theta[1])*Tobj(1,3) + sin(theta[1])*Tobj(2,3) );
00529 
00530         theta[5] = atan2( (cos(theta234)*(cos(theta[1])*Tobj(1,3) + 
00531                                           sin(theta[1])*Tobj(2,3)) + sin(theta234)*Tobj(3,3)),
00532                           (sin(theta[1])*Tobj(1,3) - cos(theta[1])*Tobj(2,3)));
00533 
00534         theta[6] = atan2( (-sin(theta234)*(cos(theta[1])*Tobj(1,1) + 
00535                                            sin(theta[1])*Tobj(2,1)) + cos(theta234)*Tobj(3,1)),
00536                           (-sin(theta234)*(cos(theta[1])*Tobj(1,2) + sin(theta[1])*Tobj(2,2)) + 
00537                            cos(theta234)*Tobj(3,2)));
00538 
00539         A= cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4) - links[1].a - 
00540             links[4].a*cos(theta234);
00541 
00542         B= Tobj(3,4) - links[1].d - links[4].a*sin(theta234);
00543 
00544         //cos(theta3)
00545         K= ( A*A + B*B - (links[3].a*links[3].a) - (links[2].a*links[2].a) ) /
00546             ( 2*links[2].a*links[3].a );
00547 
00548         tmp = 1-K*K;
00549         if (tmp < 0)
00550             throw std::out_of_range("sqrt of negative number not allowed.");
00551 
00552         theta[3] = atan2(sqrt(tmp),K);
00553         theta[0] = atan2(-sqrt(tmp),K);
00554 
00555         diff1 = fabs(q_actual(3)-theta[0]);
00556         if (diff1 > M_PI)
00557             diff1 = 2*M_PI - diff1;
00558 
00559         diff2 = fabs(q_actual(3)-theta[3]);
00560         if (diff2 > M_PI)
00561             diff2 = 2*M_PI - diff2;
00562 
00563         // Prend l'angle le plus proche de sa position actuel
00564         if (diff1 < diff2)
00565             theta[3] = theta[0];
00566 
00567         C = cos(theta[3])*links[3].a + links[2].a;
00568         D = sin(theta[3])*links[3].a;
00569 
00570         theta[2] = atan2(B,A) - atan2(D,C);
00571 
00572         theta[4] = theta234 - theta[2] - theta[3];
00573 
00574         qout(1) = theta[1];
00575         qout(2) = theta[2];
00576         qout(3) = theta[3];
00577         qout(4) = theta[4];
00578         qout(5) = theta[5];
00579         qout(6) = theta[6];
00580         converge = true; 
00581     }
00582     catch(std::out_of_range & e)
00583     {
00584         converge = false;
00585         qout = q_actual;
00586     }
00587 
00588     qout.Release();
00589     return qout;
00590 }
00591 
00592 // ----------------- M O D I F I E D  D H  N O T A T I O N ------------------
00593 
00594 
00595 ReturnMatrix mRobot::inv_kin(const Matrix & Tobj, const int mj)
00597 {
00598    bool converge = false;
00599    return inv_kin(Tobj, mj, dof, converge);
00600 }
00601 
00602 
00603 ReturnMatrix mRobot::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
00611 {
00612     switch (robotType) {
00613         case RHINO:
00614             return inv_kin_rhino(Tobj, converge);
00615             break;
00616         case PUMA:
00617             return inv_kin_puma(Tobj, converge);
00618             break;
00619         case SCHILLING:
00620             return inv_kin_schilling(Tobj, converge);
00621             break;
00622         default:
00623             return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
00624     }
00625 }
00626 
00627 
00628 ReturnMatrix mRobot::inv_kin_rhino(const Matrix & Tobj, bool & converge)
00634 {
00635     ColumnVector qout(5), q_actual;
00636     q_actual = get_q();
00637 
00638     try
00639     {
00640         Real theta[6] , diff1, diff2, tmp,
00641         angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , G=0.0;
00642 
00643         if (links[6].d > 0)
00644         {
00645             ColumnVector tmpd6(3); 
00646             tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
00647             tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
00648             Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
00649         }
00650 
00651         // Calcul les deux angles possibles
00652         theta[0] = atan2(Tobj(2,4),
00653                          Tobj(1,4));
00654 
00655         theta[1] = atan2(-Tobj(2,4),
00656                          -Tobj(1,4))  ;
00657 
00658         diff1 = fabs(q_actual(1)-theta[0]) ;            
00659         if (diff1 > M_PI)
00660             diff1 = 2*M_PI - diff1;
00661 
00662         diff2 = fabs(q_actual(1)-theta[1]);
00663         if (diff2 > M_PI)
00664             diff2 = 2*M_PI - diff2 ;
00665 
00666         // Prend l'angle le plus proche de sa position actuel
00667         if (diff1 < diff2)                 
00668             theta[1] = theta[0] ;
00669 
00670         theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1), 
00671                          sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
00672 
00673         // angle = theta1 +theta2 + theta3
00674         angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
00675                       -1*Tobj(3,3));
00676 
00677         L = cos(theta[1])*Tobj(1,4) + 
00678             sin(theta[1])*Tobj(2,4) + 
00679             links[5].d*sin(angle) - 
00680             links[5].a*cos(angle);
00681         M = links[1].d - 
00682             Tobj(3,4) - 
00683             links[5].d*cos(angle) - 
00684             links[5].a*sin(angle);
00685         K = (L*L + M*M - links[4].a*links[4].a - 
00686              links[3].a*links[3].a) / 
00687             (2 * links[4].a * links[4].a);
00688 
00689         tmp = 1-K*K;
00690         if (tmp < 0)
00691             throw std::out_of_range("sqrt of negative number not allowed.");
00692 
00693         theta[0] = atan2( sqrt(tmp) , K );
00694         theta[3] = atan2( -sqrt(tmp) , K );     
00695 
00696         diff1 = fabs(q_actual(3)-theta[0]) ;
00697         if (diff1 > M_PI)
00698             diff1 = 2*M_PI - diff1 ;
00699 
00700         diff2 = fabs(q_actual(3)-theta[3]);
00701         if (diff2 > M_PI)
00702             diff2 = 2*M_PI - diff2 ;
00703 
00704         if (diff1 < diff2)
00705             theta[3] = theta[0] ;
00706 
00707         H = cos(theta[3]) * links[4].a + links[3].a;
00708         G = sin(theta[3]) * links[4].a;
00709 
00710         theta[2] = atan2( M , L ) - atan2( G , H );
00711         theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) , 
00712                           -1*Tobj(3,3)) - theta[2] - theta[3] ;
00713 
00714         qout(1) = theta[1];
00715         qout(2) = theta[2];
00716         qout(3) = theta[3];
00717         qout(4) = theta[4];
00718         qout(5) = theta[5];
00719 
00720         converge = true;
00721     }
00722     catch(std::out_of_range & e)
00723     {
00724         converge = false; 
00725         qout = q_actual;
00726     }
00727 
00728     qout.Release();
00729     return qout;
00730 }
00731 
00732 
00733 ReturnMatrix mRobot::inv_kin_puma(const Matrix & Tobj, bool & converge)
00739 {
00740     ColumnVector qout(6), q_actual;
00741     q_actual = get_q();
00742 
00743     try
00744     {  
00745         Real theta[7] , diff1, diff2, tmp,
00746         A = 0.0 , B = 0.0 , C = 0.0 , D =0.0, Ro = 0.0,
00747         H = 0.0 , L = 0.0 , M = 0.0;
00748 
00749         // Removed d6 component because in the Puma inverse kinematics solution 
00750         // d6 = 0. 
00751         if (links[6].d)
00752         {
00753             ColumnVector tmpd6(3); Matrix tmp;
00754             tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
00755             tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
00756             Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
00757         }
00758 
00759         tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
00760         if (tmp < 0)
00761             throw std::out_of_range("sqrt of negative number not allowed.");
00762 
00763         Ro = sqrt(tmp);
00764         D = (links[2].d+links[3].d) / Ro;
00765 
00766         tmp = 1-D*D;
00767         if (tmp < 0)
00768             throw std::out_of_range("sqrt of negative number not allowed.");
00769 
00770         //Calcul les deux angles possibles
00771         theta[0] =  atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));    
00772         //Calcul les deux angles possibles
00773         theta[1] =  atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
00774 
00775         diff1 = fabs(q_actual(1)-theta[0]);
00776         if (diff1 > M_PI)
00777             diff1 = 2*M_PI - diff1;
00778 
00779         diff2 = fabs(q_actual(1)-theta[1]);
00780         if (diff2 > M_PI)
00781             diff2 = 2*M_PI - diff2;
00782 
00783         // Prend l'angle le plus proche de sa position actuel
00784         if (diff1 < diff2)
00785             theta[1] = theta[0];
00786 
00787         tmp = links[4].a*links[4].a + links[4].d*links[4].d;
00788         if (tmp < 0)
00789             throw std::out_of_range("sqrt of negative number not allowed.");
00790 
00791         Ro = sqrt(tmp);
00792         B = atan2(links[4].d,links[4].a);
00793         C = Tobj(1,4)*Tobj(1,4) + 
00794             Tobj(2,4)*Tobj(2,4) + 
00795             (Tobj(3,4)-links[1].d)*(Tobj(3,4)-links[1].d) - 
00796             (links[2].d + links[3].d)*(links[2].d + links[3].d) - 
00797             links[3].a*links[3].a - 
00798             links[4].a*links[4].a - 
00799             links[4].d*links[4].d; 
00800         A = C / (2*links[3].a);
00801 
00802         tmp = 1-A/Ro*A/Ro;
00803         if (tmp < 0)
00804             throw std::out_of_range("sqrt of negative number not allowed.");
00805 
00806         theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
00807         theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
00808 
00809         diff1 = fabs(q_actual(3)-theta[0]);
00810         if (diff1 > M_PI)
00811             diff1 = 2*M_PI - diff1 ;
00812 
00813         diff2 = fabs(q_actual(3)-theta[3]);
00814         if (diff2 > M_PI)
00815             diff2 = 2*M_PI - diff2;
00816 
00817         //Prend l'angle le plus proche de sa position actuel
00818         if (diff1 < diff2)
00819             theta[3] = theta[0];
00820 
00821         H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
00822         L = sin(theta[3])*links[4].d + cos(theta[3])*links[4].a + links[3].a;
00823         M = cos(theta[3])*links[4].d - sin(theta[3])*links[4].a;
00824 
00825         theta[2] = atan2( M , L ) - atan2(Tobj(3,4)-links[1].d , H );
00826 
00827         theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) , 
00828                           cos(theta[2] + theta[3]) * 
00829                           (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3)) 
00830                           - (sin(theta[2]+theta[3])*Tobj(3,3)) );
00831 
00832         theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)), 
00833                          -cos(theta[2] + theta[3]) * 
00834                          (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3)) 
00835                          + (sin(theta[2]+theta[3])*Tobj(3,3)) );
00836 
00837         diff1 = fabs(q_actual(4)-theta[0]);
00838         if (diff1 > M_PI)
00839             diff1 = 2*M_PI - diff1;
00840 
00841         diff2 = fabs(q_actual(4)-theta[4]);
00842         if (diff2 > M_PI)
00843             diff2 = 2*M_PI - diff2;
00844 
00845         // Prend l'angle le plus proche de sa position actuel
00846         if (diff1 < diff2)
00847             theta[4] = theta[0];
00848 
00849         theta[5] = atan2( cos(theta[4]) * 
00850                           ( cos(theta[2] + theta[3]) * 
00851                             (cos(theta[1]) * Tobj(1,3) 
00852                              + sin(theta[1])*Tobj(2,3)) 
00853                             - (sin(theta[2]+theta[3])*Tobj(3,3)) ) + 
00854                           sin(theta[4])*(-sin(theta[1])*Tobj(1,3) 
00855                                          + cos(theta[1])*Tobj(2,3)) , 
00856                           sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3) 
00857                                                     + sin(theta[1])*Tobj(2,3) ) 
00858                           + (cos(theta[2]+theta[3])*Tobj(3,3)) );
00859 
00860         theta[6] = atan2( -sin(theta[4]) 
00861                           * ( cos(theta[2] + theta[3]) * 
00862                               (cos(theta[1]) * Tobj(1,1) 
00863                                + sin(theta[1])*Tobj(2,1)) 
00864                               - (sin(theta[2]+theta[3])*Tobj(3,1))) + 
00865                           cos(theta[4])*(-sin(theta[1])*Tobj(1,1) 
00866                                          + cos(theta[1])*Tobj(2,1)), 
00867                           -sin(theta[4]) * ( cos(theta[2] + theta[3]) * 
00868                                              (cos(theta[1]) * Tobj(1,2) 
00869                                               + sin(theta[1])*Tobj(2,2)) 
00870                                              - (sin(theta[2]+theta[3])*Tobj(3,2))) +
00871                           cos(theta[4])*(-sin(theta[1])*Tobj(1,2) 
00872                                          + cos(theta[1])*Tobj(2,2)) );
00873 
00874         qout(1) = theta[1];
00875         qout(2) = theta[2];
00876         qout(3) = theta[3];
00877         qout(4) = theta[4];
00878         qout(5) = theta[5];
00879         qout(6) = theta[6];
00880 
00881         converge = true; 
00882     }
00883     catch(std::out_of_range & e)
00884     {
00885         converge = false; 
00886         qout = q_actual;
00887     }
00888 
00889     qout.Release();
00890     return qout;
00891 }
00892 
00893 ReturnMatrix mRobot::inv_kin_schilling(const Matrix & Tobj, bool & converge)
00899 {
00900     ColumnVector qout(6), q_actual;
00901     q_actual = get_q();
00902     
00903     try
00904     {
00905         Real theta[7], K=0.0, A=0.0, B=0.0, C=0.0, D=0.0, tmp=0.0, theta234 , diff1, diff2;
00906 
00907         if (links[6].d)
00908         {
00909             ColumnVector tmpd6(3); 
00910             Matrix tmp;
00911             tmpd6(1)=0;
00912             tmpd6(2)=0;
00913             tmpd6(3)=links[6].d;
00914             tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
00915             Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
00916         }
00917 
00918         theta[1] = atan2(Tobj(2,4),Tobj(1,4));
00919         //Calcul les deux angles possibles
00920         theta[0] = atan2(-Tobj(2,4),-Tobj(1,4));
00921 
00922         diff1 = fabs(q_actual(1)-theta[0]);
00923         if (diff1 > M_PI)
00924             diff1 = 2*M_PI - diff1;
00925                 
00926         diff2 = fabs(q_actual(1)-theta[1]);
00927         if (diff2 > M_PI)
00928             diff2 = 2*M_PI - diff2;
00929 
00930         // Prend l'angle le plus proche de sa position actuel
00931         if (diff1 < diff2)
00932             theta[1] = theta[0];
00933         
00934         //theta2+theta3+theta4  
00935         theta234 = atan2( Tobj(3,3) , cos(theta[1])*Tobj(1,3) + sin(theta[1])*Tobj(2,3) );
00936         
00937         theta[5] = atan2( (cos(theta234)*(cos(theta[1])*Tobj(1,3) + 
00938                                           sin(theta[1])*Tobj(2,3)) + sin(theta234)*Tobj(3,3)),
00939                           (sin(theta[1])*Tobj(1,3) - cos(theta[1])*Tobj(2,3)));
00940 
00941         theta[6] = atan2( (-sin(theta234)*(cos(theta[1])*Tobj(1,1) + 
00942                                            sin(theta[1])*Tobj(2,1)) + cos(theta234)*Tobj(3,1)),
00943                           (-sin(theta234)*(cos(theta[1])*Tobj(1,2) + sin(theta[1])*Tobj(2,2)) + 
00944                            cos(theta234)*Tobj(3,2)));
00945 
00946         A= cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4) - links[2].a - 
00947             links[5].a*cos(theta234);
00948 
00949         B= Tobj(3,4) - links[1].d - links[5].a*sin(theta234);
00950 
00951         //cos(theta3)
00952         K= ( A*A + B*B - (links[4].a*links[4].a) - (links[3].a*links[3].a) ) /
00953             ( 2*links[3].a*links[4].a );
00954 
00955         tmp = 1-K*K;
00956         if (tmp < 0)
00957             throw std::out_of_range("sqrt of negative number not allowed.");
00958 
00959         theta[3] = atan2(sqrt(tmp),K);
00960         theta[0] = atan2(-sqrt(tmp),K);
00961                 
00962         diff1 = fabs(q_actual(3)-theta[0]);
00963         if (diff1 > M_PI)
00964             diff1 = 2*M_PI - diff1;
00965                 
00966         diff2 = fabs(q_actual(3)-theta[3]);
00967         if (diff2 > M_PI)
00968             diff2 = 2*M_PI - diff2;
00969                 
00970         // Prend l'angle le plus proche de sa position actuel
00971         if (diff1 < diff2)
00972             theta[3] = theta[0];
00973 
00974         C = cos(theta[3])*links[4].a + links[3].a;
00975         D = sin(theta[3])*links[4].a;
00976 
00977         theta[2] = atan2(B,A) - atan2(D,C);
00978 
00979         theta[4] = theta234 - theta[2] - theta[3];
00980 
00981         qout(1) = theta[1];
00982         qout(2) = theta[2];
00983         qout(3) = theta[3];
00984         qout(4) = theta[4];
00985         qout(5) = theta[5];
00986         qout(6) = theta[6];
00987         
00988         converge = true; 
00989     }
00990     catch(std::out_of_range & e)
00991     {
00992         converge = false;
00993         qout = q_actual;
00994     }
00995 
00996     qout.Release();
00997     return qout;
00998 }
00999 
01000 
01001 ReturnMatrix mRobot_min_para::inv_kin(const Matrix & Tobj, const int mj)
01003 {
01004    bool converge = false;
01005    return inv_kin(Tobj, mj, dof, converge);
01006 }
01007 
01008 
01009 ReturnMatrix mRobot_min_para::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
01017 {
01018     switch (robotType) {
01019         case RHINO:
01020             return inv_kin_rhino(Tobj, converge);
01021             break;
01022         case PUMA:
01023             return inv_kin_puma(Tobj, converge);
01024             break;
01025         default:
01026             return Robot_basic::inv_kin(Tobj, mj, endlink, converge);
01027     }
01028 }
01029 
01030 
01031 ReturnMatrix mRobot_min_para::inv_kin_rhino(const Matrix & Tobj, bool & converge)
01037 {
01038     ColumnVector qout(5), q_actual;
01039     q_actual = get_q();
01040 
01041     try
01042     {
01043         Real theta[6] , diff1, diff2, tmp,
01044         angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , G=0.0 ;
01045 
01046         // Calcul les deux angles possibles
01047         theta[0] = atan2(Tobj(2,4),
01048                          Tobj(1,4));
01049 
01050         theta[1] = atan2(-Tobj(2,4),
01051                          -Tobj(1,4))  ;
01052 
01053         diff1 = fabs(q_actual(1)-theta[0]) ;            
01054         if (diff1 > M_PI)
01055             diff1 = 2*M_PI - diff1;
01056 
01057         diff2 = fabs(q_actual(1)-theta[1]);
01058         if (diff2 > M_PI)
01059             diff2 = 2*M_PI - diff2 ;
01060 
01061         // Prend l'angle le plus proche de sa position actuel
01062         if (diff1 < diff2)                 
01063             theta[1] = theta[0] ;
01064 
01065         theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1), 
01066                          sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2));
01067 
01068         // angle = theta1 +theta2 + theta3
01069         angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3),
01070                       -1*Tobj(3,3));
01071 
01072         L = cos(theta[1])*Tobj(1,4) + 
01073             sin(theta[1])*Tobj(2,4) + 
01074             links[5].d*sin(angle) - 
01075             links[5].a*cos(angle);
01076         M = links[1].d - 
01077             Tobj(3,4) - 
01078             links[5].d*cos(angle) - 
01079             links[5].a*sin(angle);
01080         K = (L*L + M*M - links[4].a*links[4].a - 
01081              links[3].a*links[3].a) / 
01082             (2 * links[4].a * links[4].a);
01083 
01084         tmp = 1-K*K;
01085         if (tmp < 0)
01086             throw std::out_of_range("sqrt of negative number not allowed.");
01087 
01088         theta[0] = atan2( sqrt(tmp) , K );
01089         theta[3] = atan2( -sqrt(tmp) , K );     
01090 
01091         diff1 = fabs(q_actual(3)-theta[0]) ;
01092         if (diff1 > M_PI)
01093             diff1 = 2*M_PI - diff1 ;
01094 
01095         diff2 = fabs(q_actual(3)-theta[3]);
01096         if (diff2 > M_PI)
01097             diff2 = 2*M_PI - diff2 ;
01098 
01099         if (diff1 < diff2)
01100             theta[3] = theta[0] ;
01101 
01102         H = cos(theta[3]) * links[4].a + links[3].a;
01103         G = sin(theta[3]) * links[4].a;
01104 
01105         theta[2] = atan2( M , L ) - atan2( G , H );
01106         theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) , 
01107                           -1*Tobj(3,3)) - theta[2] - theta[3] ;
01108 
01109         qout(1) = theta[1];
01110         qout(2) = theta[2];
01111         qout(3) = theta[3];
01112         qout(4) = theta[4];
01113         qout(5) = theta[5];
01114 
01115         converge = true;
01116     }
01117     catch(std::out_of_range & e)
01118     {
01119         converge = false; 
01120         qout = q_actual;
01121     }
01122 
01123     qout.Release();
01124     return qout;
01125 }
01126 
01127 
01128 ReturnMatrix mRobot_min_para::inv_kin_puma(const Matrix & Tobj, bool & converge)
01134 {
01135     ColumnVector qout(6), q_actual;
01136     q_actual = get_q();
01137 
01138     try
01139     {  
01140         Real theta[7] , diff1, diff2, tmp,
01141         A = 0.0 , B = 0.0 , C = 0.0 , D =0.0, Ro = 0.0,
01142         H = 0.0 , L = 0.0 , M = 0.0;
01143 
01144         // Removed d6 component because in the Puma inverse kinematics solution 
01145         // d6 = 0. 
01146         if (links[6].d > 0)
01147         {
01148             ColumnVector tmpd6(3); Matrix tmp;
01149             tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d;
01150             tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
01151             Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
01152         }
01153 
01154         tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4);
01155         if (tmp < 0)
01156             throw std::out_of_range("sqrt of negative number not allowed.");
01157 
01158         Ro = sqrt(tmp);
01159         D = (links[2].d+links[3].d) / Ro;
01160 
01161         tmp = 1-D*D;
01162         if (tmp < 0)
01163             throw std::out_of_range("sqrt of negative number not allowed.");
01164 
01165         //Calcul les deux angles possibles
01166         theta[0] =  atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp));    
01167         //Calcul les deux angles possibles
01168         theta[1] =  atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp));
01169 
01170         diff1 = fabs(q_actual(1)-theta[0]);
01171         if (diff1 > M_PI)
01172             diff1 = 2*M_PI - diff1;
01173 
01174         diff2 = fabs(q_actual(1)-theta[1]);
01175         if (diff2 > M_PI)
01176             diff2 = 2*M_PI - diff2;
01177 
01178         // Prend l'angle le plus proche de sa position actuel
01179         if (diff1 < diff2)
01180             theta[1] = theta[0];
01181 
01182         tmp = links[4].a*links[4].a + links[4].d*links[4].d;
01183         if (tmp < 0)
01184             throw std::out_of_range("sqrt of negative number not allowed.");
01185 
01186         Ro = sqrt(tmp);
01187         B = atan2(links[4].d,links[4].a);
01188         C = Tobj(1,4)*Tobj(1,4) + 
01189             Tobj(2,4)*Tobj(2,4) + 
01190             (Tobj(3,4)-links[1].d)*(Tobj(3,4)-links[1].d) - 
01191             (links[2].d + links[3].d)*(links[2].d + links[3].d) - 
01192             links[3].a*links[3].a - 
01193             links[4].a*links[4].a - 
01194             links[4].d*links[4].d; 
01195         A = C / (2*links[3].a);
01196 
01197         tmp = 1-A/Ro*A/Ro;
01198         if (tmp < 0)
01199             throw std::out_of_range("sqrt of negative number not allowed.");
01200 
01201         theta[0] = atan2(sqrt(tmp) , A/Ro) + B;
01202         theta[3] = atan2(-sqrt(tmp) , A/Ro) + B;
01203 
01204         diff1 = fabs(q_actual(3)-theta[0]);
01205         if (diff1 > M_PI)
01206             diff1 = 2*M_PI - diff1 ;
01207 
01208         diff2 = fabs(q_actual(3)-theta[3]);
01209         if (diff2 > M_PI)
01210             diff2 = 2*M_PI - diff2;
01211 
01212         //Prend l'angle le plus proche de sa position actuel
01213         if (diff1 < diff2)
01214             theta[3] = theta[0];
01215 
01216         H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4);
01217         L = sin(theta[3])*links[4].d + cos(theta[3])*links[4].a + links[3].a;
01218         M = cos(theta[3])*links[4].d - sin(theta[3])*links[4].a;
01219 
01220         theta[2] = atan2( M , L ) - atan2(Tobj(3,4)-links[1].d , H );
01221 
01222         theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) , 
01223                           cos(theta[2] + theta[3]) * 
01224                           (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3)) 
01225                           - (sin(theta[2]+theta[3])*Tobj(3,3)) );
01226 
01227         theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)), 
01228                          -cos(theta[2] + theta[3]) * 
01229                          (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3)) 
01230                          + (sin(theta[2]+theta[3])*Tobj(3,3)) );
01231 
01232         diff1 = fabs(q_actual(4)-theta[0]);
01233         if (diff1 > M_PI)
01234             diff1 = 2*M_PI - diff1;
01235 
01236         diff2 = fabs(q_actual(4)-theta[4]);
01237         if (diff2 > M_PI)
01238             diff2 = 2*M_PI - diff2;
01239 
01240         // Prend l'angle le plus proche de sa position actuel
01241         if (diff1 < diff2)
01242             theta[4] = theta[0];
01243 
01244         theta[5] = atan2( cos(theta[4]) * 
01245                           ( cos(theta[2] + theta[3]) * 
01246                             (cos(theta[1]) * Tobj(1,3) 
01247                              + sin(theta[1])*Tobj(2,3)) 
01248                             - (sin(theta[2]+theta[3])*Tobj(3,3)) ) + 
01249                           sin(theta[4])*(-sin(theta[1])*Tobj(1,3) 
01250                                          + cos(theta[1])*Tobj(2,3)) , 
01251                           sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3) 
01252                                                     + sin(theta[1])*Tobj(2,3) ) 
01253                           + (cos(theta[2]+theta[3])*Tobj(3,3)) );
01254 
01255         theta[6] = atan2( -sin(theta[4]) 
01256                           * ( cos(theta[2] + theta[3]) * 
01257                               (cos(theta[1]) * Tobj(1,1) 
01258                                + sin(theta[1])*Tobj(2,1)) 
01259                               - (sin(theta[2]+theta[3])*Tobj(3,1))) + 
01260                           cos(theta[4])*(-sin(theta[1])*Tobj(1,1) 
01261                                          + cos(theta[1])*Tobj(2,1)), 
01262                           -sin(theta[4]) * ( cos(theta[2] + theta[3]) * 
01263                                              (cos(theta[1]) * Tobj(1,2) 
01264                                               + sin(theta[1])*Tobj(2,2)) 
01265                                              - (sin(theta[2]+theta[3])*Tobj(3,2))) +
01266                           cos(theta[4])*(-sin(theta[1])*Tobj(1,2) 
01267                                          + cos(theta[1])*Tobj(2,2)) );
01268 
01269         qout(1) = theta[1];
01270         qout(2) = theta[2];
01271         qout(3) = theta[3];
01272         qout(4) = theta[4];
01273         qout(5) = theta[5];
01274         qout(6) = theta[6];
01275 
01276         converge = true; 
01277     }
01278     catch(std::out_of_range & e)
01279     {
01280         converge = false; 
01281         qout = q_actual;
01282     }
01283 
01284     qout.Release();
01285     return qout;
01286 }
01287 
01288 
01289 ReturnMatrix mRobot_min_para::inv_kin_schilling(const Matrix & Tobj, bool & converge)
01295 {
01296     ColumnVector qout(6), q_actual;
01297     q_actual = get_q();
01298 
01299     try
01300     {
01301         Real theta[7], K=0.0, A=0.0, B=0.0, C=0.0, D=0.0, tmp=0.0, theta234 , diff1, diff2;
01302 
01303         if (links[6].d)
01304         {
01305             ColumnVector tmpd6(3); 
01306             Matrix tmp;
01307             tmpd6(1)=0;
01308             tmpd6(2)=0;
01309             tmpd6(3)=links[6].d;
01310             tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6;
01311             Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6;
01312         }
01313 
01314         theta[1] = atan2(Tobj(2,4),Tobj(1,4));
01315         //Calcul les deux angles possibles
01316         theta[0] = atan2(-Tobj(2,4),-Tobj(1,4));
01317 
01318         diff1 = fabs(q_actual(1)-theta[0]);
01319         if (diff1 > M_PI)
01320             diff1 = 2*M_PI - diff1;
01321 
01322         diff2 = fabs(q_actual(1)-theta[1]);
01323         if (diff2 > M_PI)
01324             diff2 = 2*M_PI - diff2;
01325 
01326         // Prend l'angle le plus proche de sa position actuel
01327         if (diff1 < diff2)
01328             theta[1] = theta[0];
01329 
01330         //theta2+theta3+theta4  
01331         theta234 = atan2( Tobj(3,3) , cos(theta[1])*Tobj(1,3) + sin(theta[1])*Tobj(2,3) );
01332 
01333         theta[5] = atan2( (cos(theta234)*(cos(theta[1])*Tobj(1,3) + 
01334                                           sin(theta[1])*Tobj(2,3)) + sin(theta234)*Tobj(3,3)),
01335                           (sin(theta[1])*Tobj(1,3) - cos(theta[1])*Tobj(2,3)));
01336 
01337         theta[6] = atan2( (-sin(theta234)*(cos(theta[1])*Tobj(1,1) + 
01338                                            sin(theta[1])*Tobj(2,1)) + cos(theta234)*Tobj(3,1)),
01339                           (-sin(theta234)*(cos(theta[1])*Tobj(1,2) + sin(theta[1])*Tobj(2,2)) + 
01340                            cos(theta234)*Tobj(3,2)));
01341 
01342         A= cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4) - links[2].a - 
01343             links[5].a*cos(theta234);
01344 
01345         B= Tobj(3,4) - links[1].d - links[5].a*sin(theta234);
01346 
01347         //cos(theta3)
01348         K= ( A*A + B*B - (links[4].a*links[4].a) - (links[3].a*links[3].a) ) /
01349             ( 2*links[3].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[3] = atan2(sqrt(tmp),K);
01356         theta[0] = 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             diff2 = 2*M_PI - diff2;
01365 
01366         // Prend l'angle le plus proche de sa position actuel
01367         if (diff1 < diff2)
01368             theta[3] = theta[0];
01369 
01370         C = cos(theta[3])*links[4].a + links[3].a;
01371         D = sin(theta[3])*links[4].a;
01372 
01373         theta[2] = atan2(B,A) - atan2(D,C);
01374 
01375         theta[4] = theta234 - theta[2] - theta[3];
01376 
01377         qout(1) = theta[1];
01378         qout(2) = theta[2];
01379         qout(3) = theta[3];
01380         qout(4) = theta[4];
01381         qout(5) = theta[5];
01382         qout(6) = theta[6];
01383 
01384         converge = true; 
01385     }
01386     catch(std::out_of_range & e)
01387     {
01388         converge = false;
01389         qout = q_actual;
01390     }
01391 
01392     qout.Release();
01393     return qout;
01394 }
01395 
01396 
01397 #ifdef use_namespace
01398 }
01399 #endif
Generated on Thu Dec 14 08:52:18 2006 for ROBOOP, A Robotics Object Oriented Package in C++ by  doxygen 1.5.1