kni: invkine.cpp Source File

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  //!< 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