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

ROBOOP v1.21a
Generated Thu Nov 22 00:51:28 2007 by Doxygen 1.5.4