Tekkotsu Homepage

Demos

Overview

Downloads

Dev. Resources

Reference

Credits

invkine.cpp Go to the documentation of this file. /* ROBOOP -- A robotics object oriented package in C++ Copyright (C) 2004 Etienne Lachance This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA Report problems and direct all questions to: Richard Gourdeau Professeur Agrege Departement de genie electrique Ecole Polytechnique de Montreal C.P. 6079, Succ. Centre-Ville Montreal, Quebec, H3C 3A7 email: richard.gourdeau@polymtl.ca ------------------------------------------------------------------------------- Revision_history: 2004/04/19: Vincent Drolet -Added Robot::inv_kin_rhino and Robot::inv_kin_puma member functions. 2004/04/20: Etienne Lachance -Added try, throw, catch statement in Robot::inv_kin_rhino and Robot::inv_kin_puma in order to avoid singularity. 2004/05/21: Etienne Lachance -Added Doxygen documentation. 2004/07/01: Ethan Tira-Thompson -Added support for newmat's use_namespace #define, using ROBOOP namespace -Fixed warnings regarding atan2 when using float as Real type 2004/07/16: Ethan Tira-Thompson -If USING_FLOAT is set from newmat's include.h, ITOL is 1e-4 instead of 1e-6 Motivation was occasional failures to converge when requiring 1e-6 precision from floats using prismatic joints with ranges to 100's -A few modifications to support only solving for mobile joints in chain -Can now do inverse kinematics for frames other than end effector 2004/07/21: Ethan Tira-Thompson -Added inv_kin_pos() for times when you only care about position -Added inv_kin_orientation() for times when you only care about orientation ------------------------------------------------------------------------------- */ /*! @file invkine.cpp @brief Inverse kinematics solutions. */ #include "robot.h" #include <sstream> using namespace std; #ifdef use_namespace namespace ROBOOP { using namespace NEWMAT; #endif //! @brief RCS/CVS version. static const char rcsid[] __UNUSED__ = "$Id: invkine.cpp,v 1.29 2007/11/16 16:46:45 ejt Exp $"; #define NITMAX 1000 //!< def maximum number of iterations in inv_kin #ifdef USING_FLOAT //from newmat's include.h # define ITOL 1e-4f //!< def tolerance for the end of iterations in inv_kin #else # define ITOL 1e-6 //!< def tolerance for the end of iterations in inv_kin #endif ReturnMatrix Robot_basic::inv_kin(const Matrix & Tobj, const int mj) //! @brief Overload inv_kin function. { bool converge = false; return inv_kin(Tobj, mj, dof, converge); } ReturnMatrix Robot_basic::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge) /*! @brief Numerical inverse kinematics. @param Tobj: Transformation matrix expressing the desired end effector pose. @param mj: Select algorithm type, 0: based on Jacobian, 1: based on derivative of T. @param endlink: the link to pretend is the end effector @param converge: Indicate if the algorithm converge. The joint position vector before the inverse kinematics is returned if the algorithm does not converge. */ { ColumnVector qout, dq, q_tmp; UpperTriangularMatrix U; qout = get_available_q(); if(qout.nrows()==0) { converge=true; return qout; } q_tmp = qout; converge = false; if(mj == 0) { // Jacobian based Matrix Ipd, A, B(6,1), M; for(int j = 1; j <= NITMAX; j++) { Ipd = (kine(endlink)).i()*Tobj; B(1,1) = Ipd(1,4); B(2,1) = Ipd(2,4); B(3,1) = Ipd(3,4); B(4,1) = Ipd(3,2); B(5,1) = Ipd(1,3); B(6,1) = Ipd(2,1); A = jacobian(endlink,endlink); QRZ(A,U); QRZ(A,B,M); dq = U.i()*M; while(dq.MaximumAbsoluteValue() > 1) dq /= 10; for(int k = 1; k<= dq.nrows(); k++) qout(k)+=dq(k); set_q(qout); if (dq.MaximumAbsoluteValue() < ITOL) { converge = true; break; } } } else { // using partial derivative of T int adof=get_available_dof(endlink); Matrix A(12,adof),B,M; for(int j = 1; j <= NITMAX; j++) { B = (Tobj-kine(endlink)).SubMatrix(1,3,1,4).AsColumn(); int k=1; for(int i = 1; i<=dof && k<=adof; i++) { if(links[i].immobile) continue; A.SubMatrix(1,12,k,k) = dTdqi(i,endlink).SubMatrix(1,3,1,4).AsColumn(); k++; } if(A.ncols()==0) { converge=true; break; } QRZ(A,U); QRZ(A,B,M); dq = U.i()*M; while(dq.MaximumAbsoluteValue() > 1) dq /= 10; for(k = 1; k<=adof; k++) qout(k)+=dq(k); set_q(qout); if (dq.MaximumAbsoluteValue() < ITOL) { converge = true; break; } } } if(converge) { // Make sure that: -pi < qout <= pi for revolute joints int j=1; for(int i = 1; i <= dof; i++) { if(links[i].immobile) continue; if(links[i].get_joint_type() == 0) { while(qout(j) >= M_PI) qout(j) -= 2*M_PI; while(qout(j) <= -M_PI) qout(j) += 2*M_PI; } j++; } set_q(qout); qout.Release(); return qout; } else { set_q(q_tmp); q_tmp.Release(); return q_tmp; } } //void serrprintf(const char *, int a, int b, int c); //#define DEBUG_ET { int debl=deb++; for(int a=0; a<50; a++) serrprintf("%d: %d %d\n",a,debl,__LINE__); } #define DEBUG_ET ; //cerr << a << ": " << debl << ' ' << __LINE__ << endl; } ReturnMatrix Robot_basic::inv_kin_pos(const ColumnVector & Pobj, const int mj, const int endlink, const ColumnVector& /*Plink*/, bool & converge) /*! @brief Numerical inverse kinematics. @param Pobj: Vector expressing the desired end effector position; can be homogenous @param mj: Select algorithm type, 0: based on Jacobian, 1: based on derivative of T. @param endlink: the link to pretend is the end effector @param Plink: ignored for now @param converge: Indicate if the algorithm converge. The joint position vector before the inverse kinematics is returned if the algorithm does not converge. */ { ColumnVector qout, dq, q_tmp; UpperTriangularMatrix U; //int deb=0; DEBUG_ET; qout = get_available_q(); if(qout.nrows()==0) { converge=true; return qout; } q_tmp = qout; ColumnVector PHobj(4); if(Pobj.nrows()!=4) { PHobj.SubMatrix(1,Pobj.nrows(),1,1)=Pobj; PHobj.SubMatrix(Pobj.nrows()+1,4,1,1)=1; } else { PHobj=Pobj; } DEBUG_ET; converge = false; if(mj == 0) { // Jacobian based DEBUG_ET; Matrix Ipd, A, B(3,1),M; for(int j = 1; j <= NITMAX; j++) { Ipd = (kine(endlink)).i()*PHobj; B(1,1) = Ipd(1,1); B(2,1) = Ipd(2,1); B(3,1) = Ipd(3,1); A = jacobian(endlink,endlink); A = A.SubMatrix(1,3,1,A.ncols()); //need to remove any joints which do not affect position //otherwise those joints's q go nuts int removeCount=0; for(int k=1; k<= A.ncols(); k++) if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()<ITOL) removeCount++; Matrix A2(3,A.ncols()-removeCount); if(removeCount==0) A2=A; else for(int k=1,m=1; k<= A.ncols(); k++) { if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()<ITOL) continue; A2.SubMatrix(1,3,m,m)=A.SubMatrix(1,3,k,k); m++; } //ok... on with the show, using A2 now if(A2.ncols()==0) { converge=true; break; } { stringstream ss; ss << "A2-pre:\n"; for(int r=1; r<=A2.nrows(); r++) { for(int c=1; c<=A2.ncols(); c++) { ss << A2(r,c) << ' '; } ss << '\n'; } QRZ(A2,U); /* ss << "A2-mid:\n"; for(int r=1; r<=A2.nrows(); r++) { for(int c=1; c<=A2.ncols(); c++) { ss << A2(r,c) << ' '; } ss << '\n'; }*/ QRZ(A2,B,M); //serrprintf(ss.str().c_str(),0,0,0); } DEBUG_ET; /*stringstream ss; ss << "A2-post:\n"; for(int r=1; r<=A2.nrows(); r++) { for(int c=1; c<=A2.ncols(); c++) { ss << A2(r,c) << ' '; } ss << '\n'; } ss << "U:\n"; for(int r=1; r<=4; r++) { for(int c=r; c<=4; c++) { ss << U(r,c) << ' '; } ss << '\n'; } ss << "M: "; for(int r=1; r<=M.nrows(); r++) { ss << M(r,1) << ' '; } ss << '\n';*/ //serrprintf(ss.str().c_str(),0,0,0); DEBUG_ET; dq = U.i()*M; DEBUG_ET; while(dq.MaximumAbsoluteValue() > 1) dq /= 10; for(int k = 1,m=1; m<= dq.nrows(); k++) if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()>=ITOL) qout(k)+=dq(m++); set_q(qout); if (dq.MaximumAbsoluteValue() < ITOL) { converge = true; break; } } } else { // using partial derivative of T int adof=get_available_dof(endlink); Matrix A(3,adof),Rcur,B,M; ColumnVector pcur; bool used[adof]; for(int j = 1; j <= NITMAX; j++) { kine(Rcur,pcur,endlink); B = (PHobj.SubMatrix(1,3,1,1)-pcur); int k=1,m=1; for(int i = 1; m<=adof; i++) { if(links[i].immobile) continue; Matrix Atmp=dTdqi(i,endlink).SubMatrix(1,3,4,4); used[m]=(Atmp.MaximumAbsoluteValue()>=ITOL); if(!used[m++]) continue; A.SubMatrix(1,3,k,k) = Atmp; k++; } Matrix A2=A.SubMatrix(1,3,1,k-1); if(A2.ncols()==0) { converge=true; break; } QRZ(A2,U); QRZ(A2,B,M); dq = U.i()*M; while(dq.MaximumAbsoluteValue() > 1) dq /= 10; for(k = m = 1; k<=adof; k++) if(used[k]) qout(k)+=dq(m++); set_q(qout); if (dq.MaximumAbsoluteValue() < ITOL) { converge = true; break; } } } DEBUG_ET; if(converge) { DEBUG_ET; // Make sure that: -pi < qout <= pi for revolute joints int j=1; for(int i = 1; i <= dof; i++) { if(links[i].immobile) continue; unsigned int * test=(unsigned int*)&qout(j); if(((*test)&(255U<<23))==(255U<<23)) { //serrprintf("qout %d is not-finite\n",j,0,0); set_q(q_tmp); q_tmp.Release(); return q_tmp; } /* if(links[i].get_joint_type() == 0 && finite(qout(j))) { while(qout(j) >= M_PI) qout(j) -= 2*M_PI; while(qout(j) <= -M_PI) qout(j) += 2*M_PI; } */ j++; } DEBUG_ET; set_q(qout); qout.Release(); DEBUG_ET; return qout; } else { DEBUG_ET; set_q(q_tmp); q_tmp.Release(); DEBUG_ET; return q_tmp; } } ReturnMatrix Robot_basic::inv_kin_orientation(const Matrix & Robj, const int mj, const int endlink, bool & converge) /*! @brief Numerical inverse kinematics. @param Robj: Rotation matrix expressing the desired end effector orientation w.r.t base frame @param mj: Select algorithm type, 0: based on Jacobian, 1: based on derivative of T. @param endlink: the link to pretend is the end effector @param converge: Indicate if the algorithm converge. The joint position vector before the inverse kinematics is returned if the algorithm does not converge. */ { ColumnVector qout, dq, q_tmp; UpperTriangularMatrix U; qout = get_available_q(); if(qout.nrows()==0) { converge=true; return qout; } q_tmp = qout; Matrix RHobj(4,3); RHobj.SubMatrix(1,3,1,3)=Robj; RHobj.SubMatrix(4,4,1,3)=0; converge = false; if(mj == 0) { // Jacobian based Matrix Ipd, A, B(3,1),M; for(int j = 1; j <= NITMAX; j++) { Ipd = kine(endlink).i()*RHobj; B(1,1) = Ipd(3,2); B(2,1) = Ipd(1,3); B(3,1) = Ipd(2,1); A = jacobian(endlink,endlink); A = A.SubMatrix(4,6,1,A.ncols()); //need to remove any joints which do not affect position //otherwise those joints's q go nuts int removeCount=0; for(int k=1; k<= A.ncols(); k++) if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()<ITOL) removeCount++; Matrix A2(3,A.ncols()-removeCount); if(removeCount==0) A2=A; else for(int k=1,m=1; k<= A.ncols(); k++) { if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()<ITOL) continue; A2.SubMatrix(1,3,m,m)=A.SubMatrix(1,3,k,k); m++; } //ok... on with the show, using A2 now if(A2.ncols()==0) { converge=true; break; } QRZ(A2,U); QRZ(A2,B,M); dq = U.i()*M; while(dq.MaximumAbsoluteValue() > 1) dq /= 10; for(int k = 1,m=1; m<= dq.nrows(); k++) if(A.SubMatrix(1,3,k,k).MaximumAbsoluteValue()>=ITOL) qout(k)+=dq(m++); set_q(qout); if (dq.MaximumAbsoluteValue() < ITOL) { converge = true; break; } } } else { // using partial derivative of T int adof=get_available_dof(endlink); Matrix A(9,adof),Rcur,B,M; ColumnVector pcur; bool used[adof]; for(int j = 1; j <= NITMAX; j++) { kine(Rcur,pcur,endlink); B = (Robj-Rcur).AsColumn(); int k=1,m=1; for(int i = 1; m<=adof; i++) { if(links[i].immobile) continue; Matrix Atmp=dTdqi(i,endlink).SubMatrix(1,3,1,3).AsColumn(); used[m]=(Atmp.MaximumAbsoluteValue()>=ITOL); if(!used[m++]) continue; A.SubMatrix(1,9,k,k) = Atmp; k++; } Matrix A2=A.SubMatrix(1,9,1,k-1); if(A2.ncols()==0) { converge=true; break; } QRZ(A2,U); QRZ(A2,B,M); dq = U.i()*M; while(dq.MaximumAbsoluteValue() > 1) dq /= 10; for(k = m = 1; k<=adof; k++) if(used[k]) qout(k)+=dq(m++); set_q(qout); if (dq.MaximumAbsoluteValue() < ITOL) { converge = true; break; } } } if(converge) { // Make sure that: -pi < qout <= pi for revolute joints int j=1; for(int i = 1; i <= dof; i++) { if(links[i].immobile) continue; if(links[i].get_joint_type() == 0) { while(qout(j) >= M_PI) qout(j) -= 2*M_PI; while(qout(j) <= -M_PI) qout(j) += 2*M_PI; } j++; } set_q(qout); qout.Release(); return qout; } else { set_q(q_tmp); q_tmp.Release(); return q_tmp; } } // --------------------- R O B O T DH N O T A T I O N -------------------------- ReturnMatrix Robot::inv_kin(const Matrix & Tobj, const int mj) //! @brief Overload inv_kin function. { bool converge = false; return inv_kin(Tobj, mj, dof, converge); } ReturnMatrix Robot::inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge) /*! @brief Inverse kinematics solutions. The solution is based on the analytic inverse kinematics if robot type (familly) is Rhino or Puma, otherwise used the numerical algoritm defined in Robot_basic class. */ { switch (robotType) { case RHINO: return inv_kin_rhino(Tobj, converge); case PUMA: return inv_kin_puma(Tobj, converge); case ERS_LEG: case ERS2XX_HEAD: case ERS7_HEAD: //no specializations yet... :( case PANTILT: case GOOSENECK: return inv_kin_goose_neck(Tobj); case CRABARM: default: return Robot_basic::inv_kin(Tobj, mj, endlink, converge); } } ReturnMatrix Robot::inv_kin_pos(const ColumnVector & Pobj, const int mj, const int endlink, const ColumnVector& Plink, bool & converge) /*! @brief Inverse kinematics solutions. The solution is based on the analytic inverse kinematics if robot type (familly) is Rhino or Puma, otherwise used the numerical algoritm defined in Robot_basic class. */ { switch (robotType) { case ERS_LEG: case ERS2XX_HEAD: case ERS7_HEAD: return inv_kin_ers_pos(Pobj, endlink, Plink, converge); case RHINO: case PUMA: //no specializations yet... :( case PANTILT: return inv_kin_pan_tilt(Pobj, converge); case GOOSENECK: return inv_kin_goose_neck_pos(Pobj); case CRABARM: default: return Robot_basic::inv_kin_pos(Pobj, mj, endlink, Plink, converge); } } ReturnMatrix Robot::inv_kin_orientation(const Matrix & Robj, const int mj, const int endlink, bool & converge) /*! @brief Inverse kinematics solutions. The solution is based on the analytic inverse kinematics if robot type (familly) is Rhino or Puma, otherwise used the numerical algoritm defined in Robot_basic class. */ { switch (robotType) { case ERS_LEG: case ERS2XX_HEAD: case ERS7_HEAD: case RHINO: case PUMA: //no specializations yet... :( case PANTILT: case GOOSENECK: // return inv_kin_goose_neck_orientation(Robj) case CRABARM: default: return Robot_basic::inv_kin_orientation(Robj, mj, endlink, converge); } } ReturnMatrix Robot::inv_kin_rhino(const Matrix & Tobj, bool & converge) /*! @brief Analytic Rhino inverse kinematics. converge will be false if the desired end effector pose is outside robot range. */ { ColumnVector qout(5), q_actual; q_actual = get_q(); try { Real theta[6] , diff1, diff2, tmp, angle , L=0.0 , M=0.0 , K=0.0 , H=0.0 , Gl=0.0 ; // Calcul les deux angles possibles theta[0] = atan2(Tobj(2,4), Tobj(1,4)); theta[1] = atan2(-Tobj(2,4), -Tobj(1,4)) ; diff1 = fabs(q_actual(1)-theta[0]) ; if (diff1 > M_PI) diff1 = 2*M_PI - diff1; diff2 = fabs(q_actual(1)-theta[1]); if (diff2 > M_PI) diff1 = 2*M_PI - diff2 ; // Prend l'angle le plus proche de sa position actuel if (diff1 < diff2) theta[1] = theta[0] ; theta[5] = atan2(sin(theta[1])*Tobj(1,1) - cos(theta[1])*Tobj(2,1), sin(theta[1])*Tobj(1,2) - cos(theta[1])*Tobj(2,2)); // angle = theta1 +theta2 + theta3 angle = atan2(-1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3), -1*Tobj(3,3)); L = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4) + links[5].d*sin(angle) - links[4].a*cos(angle); M = links[1].d - Tobj(3,4) - links[5].d*cos(angle) - links[4].a*sin(angle); K = (L*L + M*M - links[3].a*links[3].a - links[2].a*links[2].a) / (2 * links[3].a * links[2].a); tmp = 1-K*K; if (tmp < 0) throw std::out_of_range("sqrt of negative number not allowed."); theta[0] = atan2( sqrt(tmp) , K ); theta[3] = atan2( -sqrt(tmp) , K ); diff1 = fabs(q_actual(3)-theta[0]) ; if (diff1 > M_PI) diff1 = 2*M_PI - diff1 ; diff2 = fabs(q_actual(3)-theta[3]); if (diff2 > M_PI) diff1 = 2*M_PI - diff2 ; if (diff1 < diff2) theta[3] = theta[0] ; H = cos(theta[3]) * links[3].a + links[2].a; Gl = sin(theta[3]) * links[3].a; theta[2] = atan2( M , L ) - atan2( Gl , H ); theta[4] = atan2( -1*cos(theta[1])*Tobj(1,3) - sin(theta[1])*Tobj(2,3) , -1*Tobj(3,3)) - theta[2] - theta[3] ; qout(1) = theta[1]; qout(2) = theta[2]; qout(3) = theta[3]; qout(4) = theta[4]; qout(5) = theta[5]; set_q(qout); converge = true; } catch(std::out_of_range & e) { converge = false; set_q(q_actual); qout = q_actual; } qout.Release(); return qout; } ReturnMatrix Robot::inv_kin_puma(const Matrix & Tobj, bool & converge) /*! @brief Analytic Puma inverse kinematics. converge will be false if the desired end effector pose is outside robot range. */ { ColumnVector qout(6), q_actual; q_actual = get_q(); try { Real theta[7] , diff1, diff2, tmp, A = 0.0 , B = 0.0 , Cl = 0.0 , D =0.0, Ro = 0.0, H = 0.0 , L = 0.0 , M = 0.0; // Removed d6 component because in the Puma inverse kinematics solution // d6 = 0. if (links[6].d) { ColumnVector tmpd6(3); tmpd6(1)=0; tmpd6(2)=0; tmpd6(3)=links[6].d; tmpd6 = Tobj.SubMatrix(1,3,1,3)*tmpd6; Tobj.SubMatrix(1,3,4,4) = Tobj.SubMatrix(1,3,4,4) - tmpd6; } tmp = Tobj(2,4)*Tobj(2,4) + Tobj(1,4)*Tobj(1,4); if (tmp < 0) throw std::out_of_range("sqrt of negative number not allowed."); Ro = sqrt(tmp); D = (links[2].d+links[3].d) / Ro; tmp = 1-D*D; if (tmp < 0) throw std::out_of_range("sqrt of negative number not allowed."); //Calcul les deux angles possibles theta[0] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D, sqrt(tmp)); //Calcul les deux angles possibles theta[1] = atan2(Tobj(2,4),Tobj(1,4)) - atan2(D , -sqrt(tmp)); diff1 = fabs(q_actual(1)-theta[0]); if (diff1 > M_PI) diff1 = 2*M_PI - diff1; diff2 = fabs(q_actual(1)-theta[1]); if (diff2 > M_PI) diff1 = 2*M_PI - diff2; // Prend l'angle le plus proche de sa position actuel if (diff1 < diff2) theta[1] = theta[0]; tmp = links[3].a*links[3].a + links[4].d*links[4].d; if (tmp < 0) throw std::out_of_range("sqrt of negative number not allowed."); Ro = sqrt(tmp); B = atan2(links[4].d,links[3].a); Cl = Tobj(1,4)*Tobj(1,4) + Tobj(2,4)*Tobj(2,4) + Tobj(3,4)*Tobj(3,4) - (links[2].d + links[3].d)*(links[2].d + links[3].d) - links[2].a*links[2].a - links[3].a*links[3].a - links[4].d*links[4].d; A = Cl / (2*links[2].a); tmp = 1-A/Ro*A/Ro; if (tmp < 0) throw std::out_of_range("sqrt of negative number not allowed."); theta[0] = atan2(sqrt(tmp) , A/Ro) + B; theta[3] = atan2(-sqrt(tmp) , A/Ro) + B; diff1 = fabs(q_actual(3)-theta[0]); if (diff1 > M_PI) diff1 = 2*M_PI - diff1 ; diff2 = fabs(q_actual(3)-theta[3]); if (diff2 > M_PI) diff1 = 2*M_PI - diff2; //Prend l'angle le plus proche de sa position actuel if (diff1 < diff2) theta[3] = theta[0]; H = cos(theta[1])*Tobj(1,4) + sin(theta[1])*Tobj(2,4); L = sin(theta[3])*links[4].d + cos(theta[3])*links[3].a + links[2].a; M = cos(theta[3])*links[4].d - sin(theta[3])*links[3].a; theta[2] = atan2( M , L ) - atan2(Tobj(3,4) , H ); theta[0] = atan2( -sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3) , cos(theta[2] + theta[3]) * (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3)) - (sin(theta[2]+theta[3])*Tobj(3,3)) ); theta[4] = atan2(-1*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)), -cos(theta[2] + theta[3]) * (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3)) + (sin(theta[2]+theta[3])*Tobj(3,3)) ); diff1 = fabs(q_actual(4)-theta[0]); if (diff1 > M_PI) diff1 = 2*M_PI - diff1; diff2 = fabs(q_actual(4)-theta[4]); if (diff2 > M_PI) diff1 = 2*M_PI - diff2; // Prend l'angle le plus proche de sa position actuel if (diff1 < diff2) theta[4] = theta[0]; theta[5] = atan2( cos(theta[4]) * ( cos(theta[2] + theta[3]) * (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3)) - (sin(theta[2]+theta[3])*Tobj(3,3)) ) + sin(theta[4])*(-sin(theta[1])*Tobj(1,3) + cos(theta[1])*Tobj(2,3)) , sin(theta[2]+theta[3]) * (cos(theta[1]) * Tobj(1,3) + sin(theta[1])*Tobj(2,3) ) + (cos(theta[2]+theta[3])*Tobj(3,3)) ); theta[6] = atan2( -sin(theta[4]) * ( cos(theta[2] + theta[3]) * (cos(theta[1]) * Tobj(1,1) + sin(theta[1])*Tobj(2,1)) - (sin(theta[2]+theta[3])*Tobj(3,1))) + cos(theta[4])*(-sin(theta[1])*Tobj(1,1) + cos(theta[1])*Tobj(2,1)), -sin(theta[4]) * ( cos(theta[2] + theta[3]) * (cos(theta[1]) * Tobj(1,2) + sin(theta[1])*Tobj(2,2)) - (sin(theta[2]+theta[3])*Tobj(3,2))) + cos(theta[4])*(-sin(theta[1])*Tobj(1,2) + cos(theta[1])*Tobj(2,2)) ); qout(1) = theta[1]; qout(2) = theta[2]; qout(3) = theta[3]; qout(4) = theta[4]; qout(5) = theta[5]; qout(6) = theta[6]; set_q(qout); converge = true; } catch(std::out_of_range & e) { converge = false; set_q(q_actual); qout = q_actual; } qout.Release(); return qout; } ReturnMatrix Robot::inv_kin_pan_tilt(const ColumnVector & Pobj, bool & converge) /* @brief Inverse Kinematics of Pan Tilt Robot @param Pobj: Vector expressing the desired end effector position @param converge: Indicate if the algorithm converge. */ { //std::cout << "Inv_Kine Pan Tilt!" << std::endl; ColumnVector qout, qtmp, /* Joint angle vectors */ xyz1, xyz2, /* Frame 1 and 2 coordinates */ xyz0 = Pobj; /* Base frame coordinates */ Matrix M01, /* Matrix to convert coordinates from base (frame 0) to frame 1 */ M12; /* Matrix to convert coordinates from frame 1 to frame 2 */ qtmp = get_q(); M01 = convertFrame(1,2); //std::cout << "Matrix M01" << std::endl << setw(5) << M01 << std::endl; xyz1 = M01*xyz0; //std::cout << xyz1(1) << " " << xyz1(2) << " " << xyz1(3) << std::endl; qtmp(2)=atan2(xyz1(2),xyz1(1)); set_q(qtmp); M12 = convertFrame(2,3); //std::cout << "Matrix M12" << std::endl << setw(5) << M12 << std::endl; xyz2 = M12*xyz1; //std::cout << xyz2(1) << " " << xyz2(2) << " " << xyz2(3) << std::endl; qtmp(3)=atan2(xyz2(2),xyz2(1)); set_q(qtmp); //Now I have to check to see if the angles lie within the ranges of the pan and tilt //If not, set angles to closest min/max value. if( qtmp(2) < links[2].get_theta_min() ) qtmp(2) = links[2].get_theta_min(); if( qtmp(2) > links[2].get_theta_max() ) qtmp(2) = links[2].get_theta_max(); if( qtmp(3) < links[3].get_theta_min() ) qtmp(3) = links[3].get_theta_min(); if( qtmp(3) > links[3].get_theta_max() ) qtmp(3) = links[3].get_theta_max(); //std::cout << "inv_kin_pan_tilt computed the values:" << endl; //std::cout << "Pan: "<< qtmp(2) << " Tilt: " << qtmp(3) << std::endl; qout = qtmp; converge=true; return qout; } ReturnMatrix Robot::inv_kin_goose_neck(const Matrix & Tobj) /* */ { //std::cout << "inv_kin_goose_neck was called!" << std::endl; ColumnVector qout, Pobj(3); Real phi; Pobj(1) = Tobj(1,4); Pobj(2) = Tobj(2,4); Pobj(3) = Tobj(3,4); phi = atan2(Tobj(3,3),Tobj(1,3)); qout = goose_neck_angles(Pobj, phi); return qout; } ReturnMatrix Robot::inv_kin_goose_neck_pos(const ColumnVector & Pobj) /* */ { //std::cout << "inv_kin_goose_neck_pos was called!" << std::endl; ColumnVector qout; qout = goose_neck_angles(Pobj, -1.57079632679); return qout; } /*ReturnMatrix Robot::inv_kin_goose_neck_orientation(const Matrix & Robj) { } */ ReturnMatrix Robot::goose_neck_angles(const ColumnVector & Pobj, Real phi) /* */ { Real c2, qp, q1, q2, q3, //phi=-1.57079632679, //phi is the Tool Orientation //this phi puts the tool above the target K1, K2, L1=links[2].get_a(),//Planar Link Lengths L2=links[3].get_a(),