aubo_kin.cpp
Go to the documentation of this file.
00001 #include <aubo_kinematics/aubo_kin.h>
00002 #include "ros/ros.h"
00003 #include <math.h>
00004 #include <stdio.h>
00005 
00006 
00007 namespace aubo_kinematics {
00008 
00009   namespace {
00010 
00011     int SIGN(double x) {
00012       return (x > 0) - (x < 0);
00013     }
00014 
00015     float  antiSinCos(float sA,float cA){
00016         float angle,r;
00017         r = sqrt(sA*sA+cA*cA);
00018         sA /= r;
00019         cA /= r;
00020 
00021         if (fabs(cA) < 1.75e-5)
00022             angle = M_PI/2*SIGN(sA);
00023         else
00024         {
00025             r = sA/cA;
00026             angle = atan(r);
00027             if (cA < 0) angle -= M_PI*SIGN(angle);
00028         }
00029         return angle;
00030     }
00031   
00032     #ifdef AUBO_I5_PARAMS
00033 
00034     const double EPS_DEF = 0.0001;
00035     const double EPS_DEF_J1 = 0.0125;
00036     const double EPS_DEF_J3 = 0.046;
00037 
00038     const double d1 =  0.0985;
00039     const double d2 =  0.1215;
00040     const double a2 =  0.408;
00041     const double a3 =  0.376;
00042     const double d4 =  0.0;
00043     const double d5 =  0.1025;
00044     const double d6 =  0.094;
00045 
00046     #endif
00047    
00048   }
00049 
00050   void forward(const double* q, double* T){
00051       float s1 = sin(*q + M_PI), c1 = cos(*q + M_PI); q++;
00052       float s2 = sin(*q - M_PI/2), c2 = cos(*q - M_PI/2); q++;
00053       float s3 = sin(*q), c3 = cos(*q); q++;
00054       float s4 = sin(*q - M_PI/2), c4 = cos(*q - M_PI/2); q++;
00055       float s5 = sin(*q), c5 = cos(*q); q++;
00056       float s6 = sin(*q), c6 = cos(*q);
00057 
00058       float tmp0 = c2*c3;
00059       float tmp1 = s2*s3;
00060       float tmp2 = c1*tmp0 + c1*tmp1;
00061       float tmp3 = c3*s2;
00062       float tmp4 = c2*s3;
00063       float tmp5 = c1*tmp3 - c1*tmp4;
00064       float tmp6 = c4*tmp2 - s4*tmp5;
00065       float tmp7 = c5*tmp6 + s1*s5;
00066       float tmp8 = -c4*tmp5 - s4*tmp2;
00067       float tmp9 = s5*tmp6;
00068       float tmp10 = c5*s1;
00069       float tmp11 = a2*c2;
00070       float tmp12 = s1*tmp0 + s1*tmp1;
00071       float tmp13 = s1*tmp3 - s1*tmp4;
00072       float tmp14 = c4*tmp12 - s4*tmp13;
00073       float tmp15 = -c1*s5 + c5*tmp14;
00074       float tmp16 = -c4*tmp13 - s4*tmp12;
00075       float tmp17 = c1*c5;
00076       float tmp18 = s5*tmp14;
00077       float tmp19 = tmp0 + tmp1;
00078       float tmp20 = -tmp3 + tmp4;
00079       float tmp21 = -c4*tmp19 - s4*tmp20;
00080       float tmp22 = c4*tmp20 - s4*tmp19;
00081       float tmp23 = c5*tmp22;
00082       float tmp24 = s5*tmp22;
00083 
00084       *T = c6*tmp7 + s6*tmp8;   T++;
00085       *T = c6*tmp8 - s6*tmp7;   T++;
00086       *T = -tmp10 + tmp9;       T++;
00087       *T = a3*tmp2 + c1*tmp11 - d2*s1 - d4*s1 + d5*tmp8 - d6*(tmp10 - tmp9); T++;
00088       *T = c6*tmp15 + s6*tmp16; T++;
00089       *T = c6*tmp16 - s6*tmp15; T++;
00090       *T = tmp17 + tmp18;       T++;
00091       *T = a3*tmp12 + c1*d2 + c1*d4 + d5*tmp16 - d6*(-tmp17 - tmp18) + s1*tmp11;        T++;
00092       *T = c6*tmp23 + s6*tmp21; T++;
00093       *T = c6*tmp21 - s6*tmp23; T++;
00094       *T = tmp24;       T++;
00095       *T = -a2*s2 + a3*tmp20 + d1 + d5*tmp21 + d6*tmp24;        T++;
00096       *T = 0;   T++;
00097       *T = 0;   T++;
00098       *T = 0;   T++;
00099       *T = 1;
00100   }
00101 
00102 
00103 
00104   int inverse(const double* T, double *jointResults) {
00105       float R06[9], positions[3];
00106       int i, j, j1_idx,j3_idx,j5_idx, num_j1,num_j3,num_j5,solution_num=0;
00107       float J1[2],J2,J3[2],J4,J5[2],J6,J234,J6234;
00108       float aCos, bSin,abCos,alpha,SumSJ23,SumCJ23,temp,temp1,temp2;
00109       float CA_1,SA_1,CA_5,SA_5,CA_3,SA_3,CA_6,SA_6,SA_J234,CA_J234,SA_2,CA_2;
00110       bool loop_again = false;
00111       char nof_j5=0;
00112 
00113       for (i=0;i<3;i++)
00114       {
00115           for (j=0;j<3;j++)
00116           {
00117                R06[i*3 + j] = T[i*4 + j];
00118           }
00119           positions[i] = T[i*4 + 3];
00120       }
00121 
00122       for (i=0;i<3;i++)
00123       {
00124           R06[i] = -R06[i];
00125           R06[3+i] = -R06[3+i];
00126       }
00127 
00128       positions[0]= -positions[0];
00129       positions[1]= -positions[1];
00130 
00131       /******************************************************
00132       * j1
00133       ******************************************************/
00134       /* calc alpha first */
00135       aCos = positions[1]-d6*R06[5];
00136       bSin = positions[0]-d6*R06[2];
00137       abCos = sqrt(aCos*aCos+bSin*bSin);
00138       if (abCos < d2-EPS_DEF_J1)
00139       {
00140           return solution_num;
00141       }
00142       CA_1 = aCos/abCos;
00143       SA_1 = bSin/abCos;
00144       alpha = antiSinCos(SA_1,CA_1);
00145 
00146       //calc j1
00147       CA_1 = d2/abCos;
00148       if (CA_1 > 0.9999999998) //0.001 degree
00149       {
00150           num_j1 = 1;
00151           temp = 0;
00152       }
00153       else
00154       {
00155           num_j1 = 2;
00156           temp = acos(CA_1);
00157 
00158           J1[1] = alpha-temp;
00159           if (J1[1] <=-M_PI) J1[1] += 2*M_PI;
00160       }
00161 
00162       J1[0] = temp+alpha;
00163       if (J1[0] > M_PI) J1[0] -= 2*M_PI;
00164 
00165 
00166       for (j1_idx=0;j1_idx<num_j1;j1_idx++)
00167       {
00168 
00169           CA_1 = cos(J1[j1_idx]);
00170           SA_1 = sin(J1[j1_idx]);
00171           CA_5 = R06[5]*CA_1+R06[2]*SA_1;
00172           if (!loop_again)
00173           {
00174           temp = fabs(CA_5);
00175           if (temp > 1+EPS_DEF)
00176           {
00177               continue;
00178           }
00179           else if (temp > 1-6e-8)
00180           {
00181               J5[0] = M_PI*(1-SIGN(CA_5))/2;
00182               num_j5 = 1;
00183           }
00184           else
00185           {
00186               J5[0] = acos(CA_5);
00187               J5[1] = -J5[0];
00188               num_j5 = 2;
00189           }
00190           nof_j5 |= (num_j5>1)<<j1_idx;
00191           }
00192           else
00193           {
00194               if (nof_j5&(1<<j1_idx)) num_j5 = 2;
00195               else num_j5 = 0;
00196           }
00197 
00198           for (j5_idx=0;j5_idx<num_j5;j5_idx++)
00199           {
00200 
00201               if (num_j5 == 1)
00202               {
00203                   J6234 = antiSinCos( -R06[6], R06[7]);
00204 
00205 
00206                   SumSJ23 = (-positions[0]+d6*R06[2])*CA_1+(positions[1]-d6*R06[5])*SA_1;
00207                   SumCJ23 = positions[2]-d1-d6*R06[8];
00208                   temp = fabs(SumSJ23)-(a2+a3+d5);
00209                   temp1 = fabs(SumCJ23)-(a2+a3+d5);
00210 
00211                   if (fabs(temp)<EPS_DEF || fabs(temp1)<EPS_DEF)
00212                   {
00213 
00214                       if (fabs(temp)<EPS_DEF) J2 = M_PI/4*SIGN(SumSJ23);
00215                       else J2 = (1-SIGN(SumCJ23))*M_PI/2;
00216 
00217                       J6 = J6234-J2*SIGN(CA_5);
00218 
00219                       jointResults[solution_num*6] = J1[j1_idx];
00220                       jointResults[solution_num*6 + 1] = J2;
00221                       jointResults[solution_num*6 + 2] = 0;
00222                       jointResults[solution_num*6 + 3] = 0;
00223                       jointResults[solution_num*6 + 4] = J5[0];
00224                       jointResults[solution_num*6 + 5] = J6;
00225                       solution_num++;
00226                   }
00227                   else
00228                   {
00229 
00230                       J2 = 0.0; // maintain J2;
00231                       //calc alpha
00232                       temp = SumSJ23*SumSJ23+SumCJ23*SumCJ23;
00233 
00234                       abCos=sqrt(temp);
00235                       alpha = antiSinCos(SumSJ23/abCos, SumCJ23/abCos);
00236                       temp1 = (temp+d5*d5-a2*a2-a3*a3)/(2*a2*a3);
00237                       temp2 = -d5*abCos/(a2*a3); // negative
00238 
00239                       if (fabs(temp1) > 1-temp2+EPS_DEF)
00240                       {
00241                           continue;
00242                       }
00243                       else if (fabs(temp1) > 1-temp2-EPS_DEF)
00244                       {
00245 
00246                           J3[0] = (1-SIGN(temp1))*M_PI/2;
00247                           J234 = J3[0]+alpha; //calc J234
00248                           while (J234 > M_PI) J234 -= 2*M_PI;
00249                           while (J234 <= -M_PI) J234 += 2*M_PI;
00250                       }
00251                       else
00252                       {
00253 
00254                           //last j3 in range or not
00255                           CA_3 = cos(0);
00256                           temp = temp1+temp2;
00257                           temp1 -= temp2;
00258                           if (CA_3 >= temp && CA_3 <= temp1)
00259                           {
00260                               J3[0] = 0;
00261                           }
00262                           else
00263                           {
00264                               if (CA_3 < temp && fabs(temp)<=1) temp2 = temp;
00265                               else if (CA_3 > temp1 && fabs(temp1)<=1) temp2 = temp1;
00266                               else
00267                               {
00268                                   continue;
00269                               }
00270                               J3[0] = acos(temp2);
00271                               //min change
00272                               if (fabs(J3[0])>fabs(J3[0])) J3[0] = -J3[0];
00273                           }
00274                           //calc J234
00275                           SA_J234 = (SumSJ23-a2*sin(J2)-a3*sin(J2-J3[0]))/d5;
00276                           CA_J234 = (SumCJ23-a2*cos(J2)-a3*cos(J2-J3[0]))/d5;
00277                           J234 = antiSinCos(SA_J234,CA_J234);
00278                       }
00279                       J4 = J234-J2+J3[0];
00280                       J6 = J6234-J234*SIGN(CA_5);
00281                       jointResults[solution_num*6] = J1[j1_idx];
00282                       jointResults[solution_num*6 + 1] = J2;
00283                       jointResults[solution_num*6 + 2] = J3[0];
00284                       jointResults[solution_num*6 + 3] = J4;
00285                       jointResults[solution_num*6 + 4] = J5[0];
00286                       jointResults[solution_num*6 + 5] = J6;
00287                       solution_num++;
00288                   }
00289               }
00290               else
00291               {
00292 
00293                   /*-----------------------------------------------------
00294                   * j6
00295                   *-----------------------------------------------------*/
00296                   SA_5 = sin(J5[j5_idx]);
00297                   CA_6 = (R06[3]*CA_1+R06[0]*SA_1)/SA_5;
00298                   SA_6 = (R06[4]*CA_1+R06[1]*SA_1)/SA_5;
00299 
00300 
00301                   temp = sqrt(CA_6*CA_6+SA_6*SA_6);
00302                   SA_6 /= temp;
00303                   CA_6 /= temp;
00304                   J6 = antiSinCos(SA_6, CA_6);
00305 
00306                   /*-----------------------------------------------------
00307                   * j234
00308                   *-----------------------------------------------------*/
00309                   SA_J234 = R06[8]*SA_5-R06[6]*CA_5*CA_6-R06[7]*CA_5*SA_6;
00310                   CA_J234 = R06[7]*CA_6-R06[6]*SA_6;
00311                   J234 = antiSinCos(SA_J234,CA_J234);
00312 
00313 
00314                   SumSJ23 = -positions[0]*CA_1 + positions[1]*SA_1 + d6*R06[2]*CA_1 - d6*R06[5]*SA_1 - d5*SA_J234;
00315                   SumCJ23 =  positions[2] - d1 - d6*R06[8] - d5*CA_J234;
00316                   CA_3 = (SumSJ23*SumSJ23+SumCJ23*SumCJ23-a2*a2-a3*a3)/(2*a2*a3);
00317 
00318                   if (fabs(CA_3) > 1+(loop_again ? EPS_DEF_J3 : EPS_DEF))
00319                   {
00320                       if (loop_again && !solution_num && j5_idx == 1) ;
00321                       continue;
00322                   }
00323                   else if (fabs(CA_3) > 1-5*10e-14)
00324                   {
00325                       J3[0] = M_PI*(1-SIGN(CA_3))/2;
00326                       num_j3 = 1;
00327                   }
00328                   else
00329                   {
00330                       J3[0] = acos(CA_3);
00331                       J3[1] = -J3[0];
00332                       num_j3 = 2;
00333                   }
00334 
00335                   for (j3_idx=0;j3_idx<num_j3;j3_idx++)
00336                   {
00337                       SA_3 = sin(J3[j3_idx]);
00338                       temp = a2+a3*CA_3;
00339                       temp1 = a3*SA_3;
00340                       temp2 = temp*temp+temp1*temp1;
00341                       CA_2 = (temp*SumCJ23-temp1*SumSJ23)/temp2;
00342                       SA_2 = (temp*SumSJ23+temp1*SumCJ23)/temp2;
00343                       if (CA_2*CA_2+SA_2*SA_2 > 1+EPS_DEF)
00344                       {
00345                           if (loop_again && !solution_num && j5_idx == 1) ;
00346                           continue;
00347                       }
00348                       J2 = antiSinCos(SA_2,CA_2);
00349                       J4 = J234-J2+J3[j3_idx];
00350                       while (J4 <= -M_PI) J4 += 2*M_PI;
00351                       while (J4 > M_PI) J4 -= 2*M_PI;
00352 
00353                       jointResults[solution_num*6] = J1[j1_idx];
00354                       jointResults[solution_num*6 + 1] = J2;
00355                       jointResults[solution_num*6 + 2] = J3[j3_idx];
00356                       jointResults[solution_num*6 + 3] = J4;
00357                       jointResults[solution_num*6 + 4] = J5[j5_idx];
00358                       jointResults[solution_num*6 + 5] = J6;
00359                       solution_num++;
00360                   }
00361               }
00362           }
00363           if (j1_idx == num_j1-1 && solution_num == 0 && nof_j5 && !loop_again)
00364           {
00365               j1_idx = -1;
00366               loop_again = true;
00367           }
00368       }
00369 
00370       //JOINT_REVERSE
00371       for (int i=0;i<solution_num*6;i++)
00372           jointResults[i] = -jointResults[i];
00373       return solution_num;
00374   }
00375 };
00376 
00377 
00378 #define IKFAST_HAS_LIBRARY
00379 #include <aubo_kinematics/ikfast.h>
00380 using namespace ikfast;
00381 
00382 // check if the included ikfast version matches what this file was compiled with
00383 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
00384 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);
00385 
00386 #ifdef IKFAST_NAMESPACE
00387 namespace IKFAST_NAMESPACE {
00388 #endif
00389 
00390 void to_mat44(double * mat4_4, const IkReal* eetrans, const IkReal* eerot)
00391 {
00392     for(int i=0; i< 3;++i){
00393         mat4_4[i*4+0] = eerot[i*3+0];
00394         mat4_4[i*4+1] = eerot[i*3+1];
00395         mat4_4[i*4+2] = eerot[i*3+2];
00396         mat4_4[i*4+3] = eetrans[i];
00397     }
00398     mat4_4[3*4+0] = 0;
00399     mat4_4[3*4+1] = 0;
00400     mat4_4[3*4+2] = 0;
00401     mat4_4[3*4+3] = -1;
00402 }
00403 
00404 void from_mat44(const double * mat4_4, IkReal* eetrans, IkReal* eerot)
00405 {
00406     for(int i=0; i< 3;++i){
00407         eerot[i*3+0] = mat4_4[i*4+0];
00408         eerot[i*3+1] = mat4_4[i*4+1];
00409         eerot[i*3+2] = mat4_4[i*4+2];
00410         eetrans[i] = mat4_4[i*4+3];
00411     }
00412 }
00413 
00414 
00415 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00416   if(!pfree) return false;
00417 
00418   int n = GetNumJoints();
00419   double q_sols[8*6];
00420   double T[16];
00421 
00422   to_mat44(T, eetrans, eerot);
00423 
00424   int num_sols = aubo_kinematics::inverse(T,q_sols);
00425 
00426   std::vector<int> vfree(0);
00427 
00428   for (int i=0; i < num_sols; ++i){
00429     std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(n);
00430     for (int j=0; j < n; ++j) vinfos[j].foffset = q_sols[i*n+j];
00431     solutions.AddSolution(vinfos,vfree);
00432   }
00433   return num_sols > 0;
00434 }
00435 
00436 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
00437 {
00438     double T[16];
00439     aubo_kinematics::forward(j,T);
00440     from_mat44(T,eetrans,eerot);
00441 }
00442 
00443 IKFAST_API int GetNumFreeParameters() { return 1; }
00444 IKFAST_API int* GetFreeParameters() { static int freeparams[] = {5}; return freeparams; }
00445 IKFAST_API int GetNumJoints() { return 6; }
00446 
00447 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00448 
00449 #ifdef IKFAST_NAMESPACE
00450 } // end namespace
00451 #endif
00452 
00453 
00454 #ifndef IKFAST_NO_MAIN
00455 
00456 using namespace std;
00457 using namespace aubo_kinematics;
00458 
00459 int main(int argc, char* argv[])
00460 {
00461   double q[6] = {0.0, 0.0, 1.0, 0.0, 1.0, 0.0};
00462   double T[16];
00463   forward(q, T);
00464   for(int i=0;i<4;i++) {
00465     for(int j=i*4;j<(i+1)*4;j++)
00466       printf("%1.3f ", T[j]);
00467     printf("\n");
00468   }
00469   double q_sols[8*6];
00470   int num_sols;
00471   num_sols = inverse(T,q_sols);
00472   for(int i=0;i<num_sols;i++) 
00473     printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\n", 
00474        q_sols[i*6+0], q_sols[i*6+1], q_sols[i*6+2], q_sols[i*6+3], q_sols[i*6+4], q_sols[i*6+5]);
00475   for(int i=0;i<=4;i++)
00476     printf("%f ", M_PI/2.0*i);
00477   printf("\n");
00478   return 0;
00479 }
00480 #endif


aubo_kinematics
Author(s): liuxin
autogenerated on Wed Sep 6 2017 03:06:35