Homepage Demos Overview Downloads Tutorials Reference
Credits

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

ROBOOP v1.21a
Generated Tue Nov 23 16:35:51 2004 by Doxygen 1.3.9.1