cob_kinematics: ikfast_ur10.cpp Source File

Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 #define IKFAST_HAS_LIBRARY
00021 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
00022 using namespace ikfast;
00023 
00024 #include <math.h>
00025 #include <stdio.h>
00026 
00027 #define ZERO_THRESH 0.00000001
00028 #define SIGN(x) ( ( (x) > 0 ) - ( (x) < 0 ) )
00029 #define PI M_PI
00030 #define UR10_PARAMS
00031 
00032 #ifdef UR10_PARAMS
00033 #define d1  0.1273
00034 #define a2 -0.612
00035 #define a3 -0.5723
00036 #define d4  0.163941
00037 #define d5  0.1157
00038 #define d6  0.0922
00039 #endif
00040 
00041 namespace ur_kinematics {
00042 
00043   void forward(const double* q, double* T) {
00044     double s1 = sin(*q), c1 = cos(*q); q++;
00045     double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
00046     double s3 = sin(*q), c3 = cos(*q); q234 += *q; q++;
00047     q234 += *q; q++;
00048     double s5 = sin(*q), c5 = cos(*q); q++;
00049     double s6 = sin(*q), c6 = cos(*q); 
00050     double s234 = sin(q234), c234 = cos(q234);
00051     *T = ((c1*c234-s1*s234)*s5)/2.0 - c5*s1 + ((c1*c234+s1*s234)*s5)/2.0; T++;
00052     *T = (c6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0) - 
00053           (s6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0); T++;
00054     *T = (-(c6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0 - 
00055           s6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0)); T++;
00056     *T = ((d5*(s1*c234-c1*s234))/2.0 - (d5*(s1*c234+c1*s234))/2.0 - 
00057           d4*s1 + (d6*(c1*c234-s1*s234)*s5)/2.0 + (d6*(c1*c234+s1*s234)*s5)/2.0 - 
00058           a2*c1*c2 - d6*c5*s1 - a3*c1*c2*c3 + a3*c1*s2*s3); T++;
00059     *T = c1*c5 + ((s1*c234+c1*s234)*s5)/2.0 + ((s1*c234-c1*s234)*s5)/2.0; T++;
00060     *T = (c6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0) + 
00061           s6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0)); T++;
00062     *T = (c6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0) - 
00063           s6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0)); T++;
00064     *T = ((d5*(c1*c234-s1*s234))/2.0 - (d5*(c1*c234+s1*s234))/2.0 + d4*c1 + 
00065           (d6*(s1*c234+c1*s234)*s5)/2.0 + (d6*(s1*c234-c1*s234)*s5)/2.0 + d6*c1*c5 - 
00066           a2*c2*s1 - a3*c2*c3*s1 + a3*s1*s2*s3); T++;
00067     *T = ((c234*c5-s234*s5)/2.0 - (c234*c5+s234*s5)/2.0); T++;
00068     *T = ((s234*c6-c234*s6)/2.0 - (s234*c6+c234*s6)/2.0 - s234*c5*c6); T++;
00069     *T = (s234*c5*s6 - (c234*c6+s234*s6)/2.0 - (c234*c6-s234*s6)/2.0); T++;
00070     *T = (d1 + (d6*(c234*c5-s234*s5))/2.0 + a3*(s2*c3+c2*s3) + a2*s2 - 
00071          (d6*(c234*c5+s234*s5))/2.0 - d5*c234); T++;
00072     *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
00073   }
00074 
00075   int inverse(const double* T, double* q_sols, double q6_des) {
00076     int num_sols = 0;
00077     double T02 = -*T; T++; double T00 =  *T; T++; double T01 =  *T; T++; double T03 = -*T; T++; 
00078     double T12 = -*T; T++; double T10 =  *T; T++; double T11 =  *T; T++; double T13 = -*T; T++; 
00079     double T22 =  *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 =  *T;
00080 
00082     double q1[2];
00083     {
00084       double A = d6*T12 - T13;
00085       double B = d6*T02 - T03;
00086       double R = A*A + B*B;
00087       if(fabs(A) < ZERO_THRESH) {
00088         double div;
00089         if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
00090           div = -SIGN(d4)*SIGN(B);
00091         else
00092           div = -d4/B;
00093         double arcsin = asin(div);
00094         if(fabs(arcsin) < ZERO_THRESH)
00095           arcsin = 0.0;
00096         if(arcsin < 0.0)
00097           q1[0] = arcsin + 2.0*PI;
00098         else
00099           q1[0] = arcsin;
00100         q1[1] = PI - arcsin;
00101       }
00102       else if(fabs(B) < ZERO_THRESH) {
00103         double div;
00104         if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
00105           div = SIGN(d4)*SIGN(A);
00106         else
00107           div = d4/A;
00108         double arccos = acos(div);
00109         q1[0] = arccos;
00110         q1[1] = 2.0*PI - arccos;
00111       }
00112       else if(d4*d4 > R) {
00113         return num_sols;
00114       }
00115       else {
00116         double arccos = acos(d4 / sqrt(R)) ;
00117         double arctan = atan2(-B, A);
00118         double pos = arccos + arctan;
00119         double neg = -arccos + arctan;
00120         if(fabs(pos) < ZERO_THRESH)
00121           pos = 0.0;
00122         if(fabs(neg) < ZERO_THRESH)
00123           neg = 0.0;
00124         if(pos >= 0.0)
00125           q1[0] = pos;
00126         else
00127           q1[0] = 2.0*PI + pos;
00128         if(neg >= 0.0)
00129           q1[1] = neg; 
00130         else
00131           q1[1] = 2.0*PI + neg;
00132       }
00133     }
00135 
00137     double q5[2][2];
00138     {
00139       for(int i=0;i<2;i++) {
00140         double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4);
00141         double div;
00142         if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
00143           div = SIGN(numer) * SIGN(d6);
00144         else
00145           div = numer / d6;
00146         double arccos = acos(div);
00147         q5[i][0] = arccos;
00148         q5[i][1] = 2.0*PI - arccos;
00149       }
00150     }
00152 
00153     {
00154       for(int i=0;i<2;i++) {
00155         for(int j=0;j<2;j++) {
00156           double c1 = cos(q1[i]), s1 = sin(q1[i]);
00157           double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
00158           double q6;
00160           if(fabs(s5) < ZERO_THRESH)
00161             q6 = q6_des;
00162           else {
00163             q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1), 
00164                        SIGN(s5)*(T00*s1 - T10*c1));
00165             if(fabs(q6) < ZERO_THRESH)
00166               q6 = 0.0;
00167             if(q6 < 0.0)
00168               q6 += 2.0*PI;
00169           }
00171 
00172           double q2[2], q3[2], q4[2];
00174           double c6 = cos(q6), s6 = sin(q6);
00175           double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1));
00176           double x04y = c5*(T20*c6 - T21*s6) - T22*s5;
00177           double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) + 
00178                         T03*c1 + T13*s1;
00179           double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6);
00180 
00181           double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);
00182           if(fabs(fabs(c3) - 1.0) < ZERO_THRESH)
00183             c3 = SIGN(c3);
00184           else if(fabs(c3) > 1.0) {
00185             // TODO NO SOLUTION
00186             continue;
00187           }
00188           double arccos = acos(c3);
00189           q3[0] = arccos;
00190           q3[1] = 2.0*PI - arccos;
00191           double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;
00192           double s3 = sin(arccos);
00193           double A = (a2 + a3*c3), B = a3*s3;
00194           q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);
00195           q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);
00196           double c23_0 = cos(q2[0]+q3[0]);
00197           double s23_0 = sin(q2[0]+q3[0]);
00198           double c23_1 = cos(q2[1]+q3[1]);
00199           double s23_1 = sin(q2[1]+q3[1]);
00200           q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);
00201           q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);
00203           for(int k=0;k<2;k++) {
00204             if(fabs(q2[k]) < ZERO_THRESH)
00205               q2[k] = 0.0;
00206             else if(q2[k] < 0.0) q2[k] += 2.0*PI;
00207             if(fabs(q4[k]) < ZERO_THRESH)
00208               q4[k] = 0.0;
00209             else if(q4[k] < 0.0) q4[k] += 2.0*PI;
00210             q_sols[num_sols*6+0] = q1[i];    q_sols[num_sols*6+1] = q2[k]; 
00211             q_sols[num_sols*6+2] = q3[k];    q_sols[num_sols*6+3] = q4[k]; 
00212             q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6; 
00213             num_sols++;
00214           }
00215 
00216         }
00217       }
00218     }
00219     return num_sols;
00220   }
00221 };
00222 
00223 using namespace std;
00224 using namespace ur_kinematics;
00225 
00226 #ifdef IKFAST_NAMESPACE
00227 namespace IKFAST_NAMESPACE {
00228 #endif
00229 
00230 void to_mat44(double * mat4_4, const IkReal* eetrans, const IkReal* eerot)
00231 {
00232     for(int i=0; i< 3;++i){
00233         mat4_4[i*4+0] = eerot[i*3+0];
00234         mat4_4[i*4+1] = eerot[i*3+1];
00235         mat4_4[i*4+2] = eerot[i*3+2];
00236         mat4_4[i*4+3] = eetrans[i];
00237     }
00238     mat4_4[3*4+0] = 0;
00239     mat4_4[3*4+1] = 0;
00240     mat4_4[3*4+2] = 0;
00241     mat4_4[3*4+3] = 1;
00242     
00243 }
00244 
00245 void from_mat44(const double * mat4_4, IkReal* eetrans, IkReal* eerot)
00246 {
00247     for(int i=0; i< 3;++i){
00248         eerot[i*3+0] = mat4_4[i*4+0];
00249         eerot[i*3+1] = mat4_4[i*4+1];
00250         eerot[i*3+2] = mat4_4[i*4+2];
00251         eetrans[i] = mat4_4[i*4+3];
00252     }
00253 }
00254 
00255 
00256 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
00257   if(!pfree) return false;
00258   
00259   int n = GetNumJoints();
00260   double q_sols[8*6];
00261   double T[16];
00262   to_mat44(T, eetrans, eerot);
00263   int num_sols = 0;
00264   num_sols = inverse(T, q_sols,pfree[0]);
00265   for (int i=0; i < num_sols; ++i){
00266     std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(n);
00267     for (int j=0; j < n; ++j) vinfos[j].foffset = q_sols[i*n+j];
00268     std::vector<int> vfree(0);
00269     solutions.AddSolution(vinfos,vfree);
00270   }
00271   //printf("WAAAAAAAAAAAAAAAAAAAAAAAAAAAAH  %d\n", num_sols);
00272   return num_sols > 0;
00273 }
00274 
00275 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
00276 {
00277     double T[16];
00278     forward(j,T);
00279     from_mat44(T,eetrans,eerot);
00280 }
00281 
00282 IKFAST_API int GetNumFreeParameters() { return 1; }
00283 IKFAST_API int* GetFreeParameters() { static int freeparams[] = {5}; return freeparams; }
00284 IKFAST_API int GetNumJoints() { return 6; }
00285 
00286 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
00287 
00288 #ifdef IKFAST_NAMESPACE
00289 } // end namespace
00290 #endif
00291 
00292 #ifndef IKFAST_NO_MAIN
00293 /*
00294 int main(int argc, char* argv[])
00295 {
00296   double q[6] = {0.0, 0.0, 1.0, 0.0, 1.0, 0.0};
00297   double* T = new double[16];
00298   forward(q, T);
00299   for(int i=0;i<4;i++) {
00300     for(int j=i*4;j<(i+1)*4;j++)
00301       printf("%1.3f ", T[j]);
00302     printf("\n");
00303   }
00304   double q_sols[8*6];
00305   int num_sols;
00306   num_sols = inverse(T, q_sols);
00307   for(int i=0;i<num_sols;i++) 
00308     printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\n", 
00309        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]);
00310   for(int i=0;i<=4;i++)
00311     printf("%f ", PI/2.0*i);
00312   printf("\n");
00313   return 0;
00314 }
00315 */
00316 #endif