kinematic.c

00001 #include "kinematic.h"
00002 #include <stdio.h>
00003 
00004 /**
00005  * @defgroup MACRO_GROUP
00006  *
00007  * @{
00008  */
00009  /**
00010   * @brief
00011   * Macro used to build the jacobian 
00012   * matrix column by column 
00013   */
00014 #define BUILD_JACOBIAN(J, p, z, i) \
00015     J->element[0][i]=p.element[0][0];\
00016     J->element[1][i]=p.element[1][0];\
00017     J->element[2][i]=p.element[2][0];\
00018     J->element[3][i]=z.element[0][0];\
00019     J->element[4][i]=z.element[1][0];\
00020     J->element[5][i]=z.element[2][0];
00021 
00022 /**
00023   * @brief
00024   * Macro used to check if the joint angle 
00025   * are in the interval between -pi and pi
00026   */
00027 #define CHECK_ANGLE()\
00028     while(joint->element[0][0] > PI)\
00029         joint->element[0][0] = joint->element[0][0] - 2*PI;\
00030     while(joint->element[0][0] < -PI)\
00031         joint->element[0][0] = joint->element[0][0] + 2*PI;\
00032         \
00033     while(joint->element[1][0] > PI)\
00034         joint->element[1][0] = joint->element[1][0] - 2*PI;\
00035     while(joint->element[1][0] < -PI)\
00036         joint->element[1][0] = joint->element[1][0] + 2*PI;\
00037         \
00038     while(joint->element[2][0] > PI)\
00039         joint->element[2][0] = joint->element[2][0] - 2*PI;\
00040     while(joint->element[2][0] < -PI)\
00041         joint->element[2][0] = joint->element[2][0] + 2*PI;\
00042         \
00043     while(joint->element[3][0] > PI)\
00044         joint->element[3][0] = joint->element[3][0] - 2*PI;\
00045     while(joint->element[3][0] < -PI)\
00046         joint->element[3][0] = joint->element[3][0] + 2*PI;\
00047         \
00048     while(joint->element[4][0] > PI)\
00049         joint->element[4][0] = joint->element[4][0] - 2*PI;\
00050     while(joint->element[4][0] < -PI)\
00051         joint->element[4][0] = joint->element[4][0] + 2*PI;\
00052         \
00053     while(joint->element[5][0] > PI)\
00054         joint->element[5][0] = joint->element[5][0] - 2*PI;\
00055     while(joint->element[5][0] < -PI)\
00056         joint->element[5][0] = joint->element[5][0] + 2*PI;\
00057  /** @} */
00058 
00059 void DHP_update(DHP* dhp,matrix* joints)
00060 {
00061     dhp[0].d = l1;
00062     dhp[0].theta = joints->element[0][0];
00063     dhp[0].a = 0;
00064     dhp[0].alpha = PI/2;
00065 
00066     dhp[1].d = 0;
00067     dhp[1].theta = joints->element[1][0];
00068     dhp[1].a = l2;
00069     dhp[1].alpha = 0;
00070 
00071     dhp[2].d = 0;
00072     dhp[2].theta = joints->element[2][0];
00073     dhp[2].a = l3;
00074     dhp[2].alpha = 0;
00075 
00076     dhp[3].d = 0;
00077     dhp[3].theta = joints->element[3][0];
00078     dhp[3].a= l4;
00079     dhp[3].alpha = -PI/2;
00080 
00081     dhp[4].d = 0;
00082     dhp[4].theta = joints->element[4][0] + PI/2;
00083     dhp[4].a = 0;
00084     dhp[4].alpha = PI/2;
00085 
00086     dhp[5].d = l5;
00087     dhp[5].theta = joints->element[5][0];
00088     dhp[5].a = 0;
00089     dhp[5].alpha = 0;
00090 
00091     return;
00092 }
00093 
00094 void DH_Transf(DHP* dhp, matrix* T)
00095 {
00096 
00097     T->element[0][0] = (float)cos(dhp->theta);
00098     T->element[0][1] = (float)(-sin(dhp->theta)*cos(dhp->alpha));
00099     T->element[0][2] = (float)(sin(dhp->theta)*sin(dhp->alpha));
00100     T->element[0][3] = (float)(dhp->a * cos(dhp->theta));
00101 
00102     T->element[1][0] = (float)sin(dhp->theta);
00103     T->element[1][1] = (float)(cos(dhp->theta)*cos(dhp->alpha));
00104     T->element[1][2] = (float)(-cos(dhp->theta)*sin(dhp->alpha));
00105     T->element[1][3] = (float)(dhp->a * sin(dhp->theta));
00106 
00107     T->element[2][0] = 0;
00108     T->element[2][1] = (float)sin(dhp->alpha);
00109     T->element[2][2] = (float)cos(dhp->alpha);
00110     T->element[2][3] = dhp->d;
00111 
00112     T->element[3][0] = 0;
00113     T->element[3][1] = 0;
00114     T->element[3][2] = 0;
00115     T->element[3][3] = 1;
00116 
00117     return;
00118 }
00119 
00120 void DPKF(matrix* joint, matrix* p)
00121 {
00122 
00123     int val = 0;
00124     matrix T01;
00125     matrix T12;
00126     matrix T23;
00127     matrix T34;
00128     matrix T45;
00129     matrix T56;
00130     matrix T_TCP;
00131 
00132     DHP dhp[6];
00133 
00134     /**< initialization of matrices */
00135     InitMatrix(&T01,4,4);
00136     InitMatrix(&T12,4,4);
00137     InitMatrix(&T23,4,4);
00138     InitMatrix(&T34,4,4);
00139     InitMatrix(&T45,4,4);
00140     InitMatrix(&T56,4,4);
00141     InitMatrix(&T_TCP,4,4);
00142 
00143     /**<  DHP initialization*/
00144     DHP_update(dhp, joint);
00145 
00146     /**< Homogeneous transformations */
00147     DH_Transf(&dhp[0], &T01);
00148     DH_Transf(&dhp[1], &T12);
00149     DH_Transf(&dhp[2], &T23);
00150     DH_Transf(&dhp[3], &T34);
00151     DH_Transf(&dhp[4], &T45);
00152     DH_Transf(&dhp[5], &T56);
00153 
00154     /**<  Tool center point transformation*/
00155     val = MxM(&T01, &T12, &T_TCP);
00156     val += MxM(&T_TCP, &T23, &T_TCP);
00157     val += MxM(&T_TCP, &T34, &T_TCP);
00158     val += MxM(&T_TCP, &T45, &T_TCP);
00159     val += MxM(&T_TCP, &T56, &T_TCP);
00160     
00161     if (val!=0)
00162     {
00163         return;
00164     }
00165 
00166     /**< pose extrapolation*/
00167     p->element[0][0] = T_TCP.element[0][3];
00168     p->element[1][0] = T_TCP.element[1][3];
00169     p->element[2][0] = T_TCP.element[2][3];
00170     p->element[3][0] = (float)atan2(T_TCP.element[2][1],T_TCP.element[2][2]);
00171     p->element[4][0] = (float)atan2(-T_TCP.element[2][0],sin(p->element[3][0])*T_TCP.element[2][1] + cos(p->element[3][0])*T_TCP.element[2][2]);
00172     p->element[5][0] = (float)atan2(-cos(p->element[3][0])*T_TCP.element[0][1]+ sin(p->element[3][0]) * T_TCP.element[0][2],cos(p->element[3][0]) * T_TCP.element[1][1]- sin(p->element[3][0]) * T_TCP.element[1][2]);
00173 
00174 
00175     /**<  matrix delation*/
00176     DeleteMatrix(&T01);
00177     DeleteMatrix(&T12);
00178     DeleteMatrix(&T23);
00179     DeleteMatrix(&T34);
00180     DeleteMatrix(&T45);
00181     DeleteMatrix(&T56);
00182     DeleteMatrix(&T_TCP);
00183 
00184     return;
00185 }
00186 
00187 void JacobianG(matrix* joint, matrix* Jg, matrix* T_TCP)
00188 {
00189     int i = 0;
00190     DHP dhp[6];
00191 
00192     matrix T01;
00193     matrix T12;
00194     matrix T23;
00195     matrix T34;
00196     matrix T45;
00197     matrix T56;
00198 
00199     matrix p0;
00200     matrix p1;
00201     matrix p2;
00202     matrix p3;
00203     matrix p4;
00204     matrix p5;
00205     matrix p_TCP;
00206 
00207     matrix z0;
00208     matrix z1;
00209     matrix z2;
00210     matrix z3;
00211     matrix z4;
00212     matrix z5;
00213 
00214     matrix CrossP;
00215     //p is matrix of RF origin positions
00216     //z is matrix of k axis for each RF
00217 
00218     /*Matrix creation*/
00219     InitMatrix(&T01,4,4);
00220     InitMatrix(&T12,4,4);
00221     InitMatrix(&T23,4,4);
00222     InitMatrix(&T34,4,4);
00223     InitMatrix(&T45,4,4);
00224     InitMatrix(&T56,4,4);
00225 
00226     InitMatrix(&p0,3,1);
00227     InitMatrix(&p1,3,1);
00228     InitMatrix(&p2,3,1);
00229     InitMatrix(&p3,3,1);
00230     InitMatrix(&p4,3,1);
00231     InitMatrix(&p5,3,1);
00232     InitMatrix(&p_TCP,3,1);
00233 
00234     InitMatrix(&z0,3,1);
00235     InitMatrix(&z1,3,1);
00236     InitMatrix(&z2,3,1);
00237     InitMatrix(&z3,3,1);
00238     InitMatrix(&z4,3,1);
00239     InitMatrix(&z5,3,1);
00240 
00241     InitMatrix(&CrossP,3,1);
00242 
00243 
00244     /**<compute dh_par */
00245     DHP_update(dhp,joint);
00246 
00247     /**>compute T from dhp*/
00248     DH_Transf(&dhp[0], &T01);
00249     DH_Transf(&dhp[1], &T12);
00250     DH_Transf(&dhp[2], &T23);
00251     DH_Transf(&dhp[3], &T34);
00252     DH_Transf(&dhp[4], &T45);
00253     DH_Transf(&dhp[5], &T56);
00254 
00255     /* R0 - JOINT 1
00256     distance between Global RF origin & R0 origin + R0 k axis */
00257     p0.element[0][0] = 0;
00258     p0.element[1][0] = 0;
00259     p0.element[2][0] = 0;
00260 
00261     z0.element[0][0] = 0;
00262     z0.element[1][0] = 0;
00263     z0.element[2][0] = 1;
00264 
00265 
00266     /* R1 -JOINT 2
00267     distance between Global RF origin & R1 origin + R1 k axis */
00268     for(i = 0; i < 3; i++) {
00269         p1.element[i][0] = T01.element[i][3];
00270     }
00271     // z1 = omo2rot(T02)*[0;0;1];
00272     for(i = 0; i < 3; i++) {
00273         z1.element[i][0] = T01.element[i][2];
00274     }
00275 
00276     /* R2 -JOINT 3
00277     distance between Global RF origin & R2 origin + R2 k axis */
00278     MxM(&T01, &T12, T_TCP);
00279     for(i = 0; i < 3; i++) {
00280         p2.element[i][0] = T_TCP->element[i][3];
00281     }
00282     // z2 = omo2rot(T02)*[0;0;1];
00283     for(i=0; i<3; i++) {
00284         z2.element[i][0] = T_TCP->element[i][2];
00285     }
00286 
00287     /* R3 -JOINT 4
00288     distance between Global RF origin & R3 origin + R3 k axis */
00289     MxM(T_TCP, &T23, T_TCP);
00290     for(i = 0; i < 3; i++) {
00291         p3.element[i][0] = T_TCP->element[i][3];
00292     }
00293     // z3 = omo2rot(T03)*[0;0;1];
00294     for(i = 0; i < 3; i++) {
00295         z3.element[i][0] = T_TCP->element[i][2];
00296     }
00297 
00298 
00299     /* R4 -JOINT 5
00300     distance between Global RF origin & R4 origin + R4 k axis */
00301     MxM(T_TCP, &T34, T_TCP);
00302     for(i = 0; i < 3; i++) {
00303         p4.element[i][0] = T_TCP->element[i][3];
00304     }
00305     // z4 = omo2rot(T03)*[0;0;1];
00306     for(i = 0; i < 3; i++) {
00307         z4.element[i][0] = T_TCP->element[i][2];
00308     }
00309 
00310     /* R5 -JOINT 6
00311     distance between Global RF origin & R5 origin + R5 k axis */
00312     MxM(T_TCP, &T45, T_TCP);
00313     for(i = 0; i < 3; i++) {
00314         p5.element[i][0] = T_TCP->element[i][3];
00315     }
00316     // z5 = omo2rot(T03)*[0;0;1];
00317     for(i = 0; i < 3; i++) {
00318         z5.element[i][0] = T_TCP->element[i][2];
00319     }
00320 
00321     MxM(T_TCP, &T56, T_TCP);
00322     for(i = 0; i < 3; i++) {
00323         p_TCP.element[i][0] = T_TCP->element[i][3];
00324     }
00325 
00326     /**compute distance from TCP*/
00327 
00328     Sub(&p_TCP, &p0, &p0);
00329     Sub(&p_TCP, &p1, &p1);
00330     Sub(&p_TCP, &p2, &p2);
00331     Sub(&p_TCP, &p3, &p3);
00332     Sub(&p_TCP, &p4, &p4);
00333     Sub(&p_TCP, &p5, &p5);
00334 
00335     /**compute jacobian*/
00336 
00337     CrossProd(&z0, &p0, &CrossP);
00338 
00339     BUILD_JACOBIAN(Jg, CrossP, z0, 0)
00340 
00341 
00342     CrossProd(&z1, &p1, &CrossP);
00343 
00344     BUILD_JACOBIAN(Jg, CrossP, z1, 1)
00345 
00346 
00347     CrossProd(&z2, &p2, &CrossP);
00348 
00349     BUILD_JACOBIAN(Jg, CrossP, z2, 2)
00350 
00351 
00352     CrossProd(&z3, &p3, &CrossP);
00353 
00354     BUILD_JACOBIAN(Jg, CrossP, z3, 3)
00355 
00356 
00357     CrossProd(&z4, &p4, &CrossP);
00358 
00359     BUILD_JACOBIAN(Jg, CrossP, z4, 4)
00360 
00361 
00362 
00363     CrossProd(&z5, &p5, &CrossP);
00364 
00365     BUILD_JACOBIAN(Jg, CrossP, z5, 5)
00366 
00367 
00368     DeleteMatrix(&p0);
00369     DeleteMatrix(&p1);
00370     DeleteMatrix(&p2);
00371     DeleteMatrix(&p3);
00372     DeleteMatrix(&p4);
00373     DeleteMatrix(&p5);
00374     DeleteMatrix(&p_TCP);
00375 
00376     DeleteMatrix(&z0);
00377     DeleteMatrix(&z1);
00378     DeleteMatrix(&z2);
00379     DeleteMatrix(&z3);
00380     DeleteMatrix(&z4);
00381     DeleteMatrix(&z5);
00382 
00383     DeleteMatrix(&T01);
00384     DeleteMatrix(&T12);
00385     DeleteMatrix(&T23);
00386     DeleteMatrix(&T34);
00387     DeleteMatrix(&T45);
00388     DeleteMatrix(&T56);
00389 
00390     DeleteMatrix(&CrossP);
00391 
00392     return;
00393 }
00394 
00395 void JacobianA(matrix* joint, matrix* Ja)
00396 {
00397 
00398     int i = 0;
00399     int j = 0;
00400 
00401     matrix Jg;
00402     matrix M;
00403     matrix Ttcp;
00404     matrix JgA;
00405     matrix JaA;
00406 
00407     matrix p;
00408 
00409     InitMatrix(&Jg, Ja->row, Ja->col );
00410     InitMatrix(&M, 3 ,3 );
00411     InitMatrix(&JgA, 3, 6 );
00412     InitMatrix(&JaA, 3, 6 );
00413     InitMatrix(&Ttcp, 4, 4 );
00414     InitMatrix(&p, 6, 1);
00415 
00416     JacobianG(joint, &Jg, &Ttcp);
00417 
00418     p.element[0][0] = Ttcp.element[0][3];
00419     p.element[1][0] = Ttcp.element[1][3];
00420     p.element[2][0] = Ttcp.element[2][3];
00421     p.element[3][0] = (float)atan2(Ttcp.element[2][1],Ttcp.element[2][2]);
00422     p.element[4][0] = (float)atan2(-Ttcp.element[2][0],sin(p.element[3][0])*Ttcp.element[2][1]+cos(p.element[3][0])*Ttcp.element[2][2]);
00423     p.element[5][0] = (float)atan2(-cos(p.element[3][0])*Ttcp.element[0][1]+ sin(p.element[3][0])*Ttcp.element[0][2],cos(p.element[3][0])*Ttcp.element[1][1]-sin(p.element[3][0])*Ttcp.element[1][2]);
00424 
00425 
00426     // Transformation matrix
00427     M.element[0][0] = (float)(cos(p.element[5][0])/cos(p.element[4][0]));
00428     M.element[0][1] = (float)(sin(p.element[5][0])/cos(p.element[4][0]));
00429     M.element[0][2] = 0;
00430 
00431     M.element[1][0] = (float)-sin(p.element[5][0]);
00432     M.element[1][1] = (float)cos(p.element[5][0]);
00433     M.element[1][2] = 0;
00434 
00435     M.element[2][0] = (float)(cos(p.element[5][0])*sin(p.element[4][0])/cos(p.element[4][0]));
00436     M.element[2][1] = (float)(-sin(p.element[4][0])*sin(p.element[5][0])/cos(p.element[4][0]));
00437     M.element[2][2] = 1;
00438 
00439 
00440     for(i = 0; i < JgA.row; i++) {
00441         for(j = 0; j< JgA.col; j++) {
00442             JgA.element[i][j] = Jg.element[i+3][j];
00443         }
00444     }
00445 
00446     MxM(&M, &JgA, &JaA);
00447 
00448 
00449     for(i = 0; i < JaA.row; i++) {
00450         for(j = 0; j< JaA.col; j++) {
00451             Ja->element[i][j] = Jg.element[i][j];
00452         }
00453     }
00454 
00455     for(i = 0; i < JaA.row; i++) {
00456         for(j = 0; j< JaA.col; j++) {
00457             Ja->element[i+3][j] = JaA.element[i][j];
00458         }
00459     }
00460 
00461     DeleteMatrix(&M);
00462     DeleteMatrix(&Jg);
00463     DeleteMatrix(&JgA);
00464     DeleteMatrix(&JaA);
00465     DeleteMatrix(&Ttcp);
00466     DeleteMatrix(&p);
00467 
00468     return;
00469 }
00470 
00471 int IPKF (matrix* p, matrix* joint)
00472 {
00473 
00474     float sentinel = 2;
00475     float sum = 0.0;
00476     float error_G = 0.5;
00477     float error_N = (float)0.0001;
00478     float alpha = (float)0.27; //after several trials
00479     int c = 0;
00480     int i = 0;
00481 
00482 
00483     DHP dhp[6];
00484 
00485     matrix f;//computed position through Direct Kinematics
00486     matrix J;
00487     matrix delta_p;
00488     matrix delta_q;
00489     matrix inv_J;
00490 
00491 
00492     InitMatrix(&f, 6, 1);
00493     InitMatrix(&J, 6, 6);
00494     InitMatrix(&inv_J, 6, 6);
00495     InitMatrix(&delta_q, 6,1);
00496     InitMatrix(&delta_p, 6,1);
00497 
00498 
00499     /*gradient method*/
00500     while(sentinel >= error_G) {
00501 //
00502         /*compute the direct kinematics*/
00503         DPKF(joint, &f);
00504 
00505         /*compute dh_par*/
00506         DHP_update(dhp,joint);
00507 
00508         /*compute jacobian*/
00509         JacobianA(joint, &J);// to be defined
00510 
00511         /*compute the next value of angle*/
00512         Traspost(&J, &J); //Transpose of J saved in JT
00513         axM(&J, alpha, &J);// Transpose of J (J) multiplied by alpha and saved in J
00514 
00515         /*subtraction of two pose objects into matrix*/
00516         Sub(p , &f, &delta_p);
00517 
00518         //alpha*J'*(p-f)
00519         MxM(&J, &delta_p, &delta_q);
00520 
00521         Sum(joint, &delta_q, joint);
00522 
00523         CHECK_ANGLE()
00524 
00525         c = c+1;
00526 
00527         /*if the gradient method does not converge break the cycle*/
00528         if (c > 1000) {
00529             break;
00530         }
00531 
00532         /* norm of p_minus_f*/
00533 
00534         for(i=0; i<6; i++) {
00535             sum += (delta_p.element[i][0] * delta_p.element[i][0]);
00536         }
00537         sentinel = (float)sqrt(sum);
00538         sum = 0;
00539     }
00540 
00541 //printf("c = %d\n", c);
00542 
00543     /*Newton method*/
00544     while (sentinel >= error_N) {
00545 
00546         /*compute the direct kinematics*/
00547         DPKF(joint,&f);
00548 
00549         /*compute dh_par*/
00550         DHP_update(dhp,joint);
00551 
00552         /*compute jacobian*/
00553         JacobianA(joint, &J);// to be defined
00554 
00555         /*compute the next value of angle*/
00556 
00557         //check if Jacobian is invertible
00558         if(Inv(&J, &inv_J) == 6 ) {
00559             break;
00560             //printf("error due to jacobuan singularities");
00561         }
00562 
00563         Sub(p, &f, &delta_p);
00564 
00565         MxM(&inv_J, &delta_p, &delta_q);
00566 
00567         //q = q +  J \ (p-f);
00568         Sum(joint, &delta_q, joint);
00569 
00570         CHECK_ANGLE();
00571 
00572         c = c+1;
00573 
00574         /*if the newton method does not converge break the cycle*/
00575         if (c > 2000) {
00576             break;
00577         }
00578         /* norm of p_minus_f*/
00579         sum = 0.0;
00580 
00581         for(i=0; i<6; i++) {
00582             sum += delta_p.element[i][0] * delta_p.element[i][0];
00583         }
00584         sentinel = (float)sqrt(sum);
00585     }
00586     
00587     DeleteMatrix(&f);
00588     DeleteMatrix(&J);
00589     DeleteMatrix(&delta_p);
00590     DeleteMatrix(&delta_q);
00591     DeleteMatrix(&inv_J);
00592     //printf("c = %d\n",c);
00593     if(c >=2000)
00594     {
00595         return 1;
00596     }
00597     else
00598     { 
00599         return 0;
00600     }
00601 }
00602 
00603 void Print_DHP(DHP* dhp)
00604 {
00605     printf("d = %f\n",dhp->d);
00606     printf("theta = %f\n",dhp->theta);
00607     printf("a = %f\n",dhp->a);
00608     printf("alpha = %f\n",dhp->alpha);
00609     printf("\n");
00610 }
00611 
00612 
00613 
00614