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

ROBOOP v1.21a
Generated Tue Aug 16 16:32:15 2005 by Doxygen 1.4.4