Diff: source/kinematic.c

Revision:

0:fb6e494a7656

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/kinematic.c	Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,614 @@
+#include "kinematic.h"
+#include <stdio.h>
+
+/**
+ * @defgroup MACRO_GROUP
+ *
+ * @{
+ */
+ /**
+  * @brief
+  * Macro used to build the jacobian 
+  * matrix column by column 
+  */
+#define BUILD_JACOBIAN(J, p, z, i) \
+	J->element[0][i]=p.element[0][0];\
+    J->element[1][i]=p.element[1][0];\
+    J->element[2][i]=p.element[2][0];\
+	J->element[3][i]=z.element[0][0];\
+	J->element[4][i]=z.element[1][0];\
+	J->element[5][i]=z.element[2][0];
+
+/**
+  * @brief
+  * Macro used to check if the joint angle 
+  * are in the interval between -pi and pi
+  */
+#define	CHECK_ANGLE()\
+    while(joint->element[0][0] > PI)\
+        joint->element[0][0] = joint->element[0][0] - 2*PI;\
+    while(joint->element[0][0] < -PI)\
+        joint->element[0][0] = joint->element[0][0] + 2*PI;\
+		\
+    while(joint->element[1][0] > PI)\
+        joint->element[1][0] = joint->element[1][0] - 2*PI;\
+    while(joint->element[1][0] < -PI)\
+        joint->element[1][0] = joint->element[1][0] + 2*PI;\
+		\
+	while(joint->element[2][0] > PI)\
+        joint->element[2][0] = joint->element[2][0] - 2*PI;\
+    while(joint->element[2][0] < -PI)\
+        joint->element[2][0] = joint->element[2][0] + 2*PI;\
+		\
+    while(joint->element[3][0] > PI)\
+        joint->element[3][0] = joint->element[3][0] - 2*PI;\
+    while(joint->element[3][0] < -PI)\
+        joint->element[3][0] = joint->element[3][0] + 2*PI;\
+		\
+	while(joint->element[4][0] > PI)\
+        joint->element[4][0] = joint->element[4][0] - 2*PI;\
+    while(joint->element[4][0] < -PI)\
+        joint->element[4][0] = joint->element[4][0] + 2*PI;\
+		\
+    while(joint->element[5][0] > PI)\
+        joint->element[5][0] = joint->element[5][0] - 2*PI;\
+    while(joint->element[5][0] < -PI)\
+        joint->element[5][0] = joint->element[5][0] + 2*PI;\
+ /** @} */
+
+void DHP_update(DHP* dhp,matrix* joints)
+{
+    dhp[0].d = l1;
+    dhp[0].theta = joints->element[0][0];
+    dhp[0].a = 0;
+    dhp[0].alpha = PI/2;
+
+    dhp[1].d = 0;
+    dhp[1].theta = joints->element[1][0];
+    dhp[1].a = l2;
+    dhp[1].alpha = 0;
+
+    dhp[2].d = 0;
+    dhp[2].theta = joints->element[2][0];
+    dhp[2].a = l3;
+    dhp[2].alpha = 0;
+
+    dhp[3].d = 0;
+    dhp[3].theta = joints->element[3][0];
+    dhp[3].a= l4;
+    dhp[3].alpha = -PI/2;
+
+    dhp[4].d = 0;
+    dhp[4].theta = joints->element[4][0] + PI/2;
+    dhp[4].a = 0;
+    dhp[4].alpha = PI/2;
+
+    dhp[5].d = l5;
+    dhp[5].theta = joints->element[5][0];
+    dhp[5].a = 0;
+    dhp[5].alpha = 0;
+
+    return;
+}
+
+void DH_Transf(DHP* dhp, matrix* T)
+{
+
+    T->element[0][0] = (float)cos(dhp->theta);
+    T->element[0][1] = (float)(-sin(dhp->theta)*cos(dhp->alpha));
+    T->element[0][2] = (float)(sin(dhp->theta)*sin(dhp->alpha));
+    T->element[0][3] = (float)(dhp->a * cos(dhp->theta));
+
+    T->element[1][0] = (float)sin(dhp->theta);
+    T->element[1][1] = (float)(cos(dhp->theta)*cos(dhp->alpha));
+    T->element[1][2] = (float)(-cos(dhp->theta)*sin(dhp->alpha));
+    T->element[1][3] = (float)(dhp->a * sin(dhp->theta));
+
+    T->element[2][0] = 0;
+    T->element[2][1] = (float)sin(dhp->alpha);
+    T->element[2][2] = (float)cos(dhp->alpha);
+    T->element[2][3] = dhp->d;
+
+    T->element[3][0] = 0;
+    T->element[3][1] = 0;
+    T->element[3][2] = 0;
+    T->element[3][3] = 1;
+
+    return;
+}
+
+void DPKF(matrix* joint, matrix* p)
+{
+
+    int val = 0;
+    matrix T01;
+    matrix T12;
+    matrix T23;
+    matrix T34;
+    matrix T45;
+    matrix T56;
+    matrix T_TCP;
+
+    DHP dhp[6];
+
+    /**< initialization of matrices */
+    InitMatrix(&T01,4,4);
+    InitMatrix(&T12,4,4);
+    InitMatrix(&T23,4,4);
+    InitMatrix(&T34,4,4);
+    InitMatrix(&T45,4,4);
+    InitMatrix(&T56,4,4);
+    InitMatrix(&T_TCP,4,4);
+
+    /**<  DHP initialization*/
+    DHP_update(dhp, joint);
+
+    /**< Homogeneous transformations */
+    DH_Transf(&dhp[0], &T01);
+    DH_Transf(&dhp[1], &T12);
+    DH_Transf(&dhp[2], &T23);
+    DH_Transf(&dhp[3], &T34);
+    DH_Transf(&dhp[4], &T45);
+    DH_Transf(&dhp[5], &T56);
+
+    /**<  Tool center point transformation*/
+    val = MxM(&T01, &T12, &T_TCP);
+    val += MxM(&T_TCP, &T23, &T_TCP);
+    val += MxM(&T_TCP, &T34, &T_TCP);
+    val += MxM(&T_TCP, &T45, &T_TCP);
+    val += MxM(&T_TCP, &T56, &T_TCP);
+    
+    if (val!=0)
+    {
+    	return;
+    }
+
+    /**< pose extrapolation*/
+    p->element[0][0] = T_TCP.element[0][3];
+    p->element[1][0] = T_TCP.element[1][3];
+    p->element[2][0] = T_TCP.element[2][3];
+    p->element[3][0] = (float)atan2(T_TCP.element[2][1],T_TCP.element[2][2]);
+    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]);
+    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]);
+
+
+    /**<  matrix delation*/
+    DeleteMatrix(&T01);
+    DeleteMatrix(&T12);
+    DeleteMatrix(&T23);
+    DeleteMatrix(&T34);
+    DeleteMatrix(&T45);
+    DeleteMatrix(&T56);
+    DeleteMatrix(&T_TCP);
+
+    return;
+}
+
+void JacobianG(matrix* joint, matrix* Jg, matrix* T_TCP)
+{
+    int i = 0;
+    DHP dhp[6];
+
+    matrix T01;
+    matrix T12;
+    matrix T23;
+    matrix T34;
+    matrix T45;
+    matrix T56;
+
+    matrix p0;
+    matrix p1;
+    matrix p2;
+    matrix p3;
+    matrix p4;
+    matrix p5;
+    matrix p_TCP;
+
+    matrix z0;
+    matrix z1;
+    matrix z2;
+    matrix z3;
+    matrix z4;
+    matrix z5;
+
+    matrix CrossP;
+    //p is matrix of RF origin positions
+    //z is matrix of k axis for each RF
+
+    /*Matrix creation*/
+    InitMatrix(&T01,4,4);
+    InitMatrix(&T12,4,4);
+    InitMatrix(&T23,4,4);
+    InitMatrix(&T34,4,4);
+    InitMatrix(&T45,4,4);
+    InitMatrix(&T56,4,4);
+
+    InitMatrix(&p0,3,1);
+    InitMatrix(&p1,3,1);
+    InitMatrix(&p2,3,1);
+    InitMatrix(&p3,3,1);
+    InitMatrix(&p4,3,1);
+    InitMatrix(&p5,3,1);
+    InitMatrix(&p_TCP,3,1);
+
+    InitMatrix(&z0,3,1);
+    InitMatrix(&z1,3,1);
+    InitMatrix(&z2,3,1);
+    InitMatrix(&z3,3,1);
+    InitMatrix(&z4,3,1);
+    InitMatrix(&z5,3,1);
+
+    InitMatrix(&CrossP,3,1);
+
+
+    /**<compute dh_par */
+    DHP_update(dhp,joint);
+
+    /**>compute T from dhp*/
+    DH_Transf(&dhp[0], &T01);
+    DH_Transf(&dhp[1], &T12);
+    DH_Transf(&dhp[2], &T23);
+    DH_Transf(&dhp[3], &T34);
+    DH_Transf(&dhp[4], &T45);
+    DH_Transf(&dhp[5], &T56);
+
+    /* R0 - JOINT 1
+    distance between Global RF origin & R0 origin + R0 k axis */
+    p0.element[0][0] = 0;
+    p0.element[1][0] = 0;
+    p0.element[2][0] = 0;
+
+    z0.element[0][0] = 0;
+    z0.element[1][0] = 0;
+    z0.element[2][0] = 1;
+
+
+    /* R1 -JOINT 2
+    distance between Global RF origin & R1 origin + R1 k axis */
+    for(i = 0; i < 3; i++) {
+        p1.element[i][0] = T01.element[i][3];
+    }
+    // z1 = omo2rot(T02)*[0;0;1];
+    for(i = 0; i < 3; i++) {
+        z1.element[i][0] = T01.element[i][2];
+    }
+
+    /* R2 -JOINT 3
+    distance between Global RF origin & R2 origin + R2 k axis */
+    MxM(&T01, &T12, T_TCP);
+    for(i = 0; i < 3; i++) {
+        p2.element[i][0] = T_TCP->element[i][3];
+    }
+    // z2 = omo2rot(T02)*[0;0;1];
+    for(i=0; i<3; i++) {
+        z2.element[i][0] = T_TCP->element[i][2];
+    }
+
+    /* R3 -JOINT 4
+    distance between Global RF origin & R3 origin + R3 k axis */
+    MxM(T_TCP, &T23, T_TCP);
+    for(i = 0; i < 3; i++) {
+        p3.element[i][0] = T_TCP->element[i][3];
+    }
+    // z3 = omo2rot(T03)*[0;0;1];
+    for(i = 0; i < 3; i++) {
+        z3.element[i][0] = T_TCP->element[i][2];
+    }
+
+
+    /* R4 -JOINT 5
+    distance between Global RF origin & R4 origin + R4 k axis */
+    MxM(T_TCP, &T34, T_TCP);
+    for(i = 0; i < 3; i++) {
+        p4.element[i][0] = T_TCP->element[i][3];
+    }
+    // z4 = omo2rot(T03)*[0;0;1];
+    for(i = 0; i < 3; i++) {
+        z4.element[i][0] = T_TCP->element[i][2];
+    }
+
+    /* R5 -JOINT 6
+    distance between Global RF origin & R5 origin + R5 k axis */
+    MxM(T_TCP, &T45, T_TCP);
+    for(i = 0; i < 3; i++) {
+        p5.element[i][0] = T_TCP->element[i][3];
+    }
+    // z5 = omo2rot(T03)*[0;0;1];
+    for(i = 0; i < 3; i++) {
+        z5.element[i][0] = T_TCP->element[i][2];
+    }
+
+    MxM(T_TCP, &T56, T_TCP);
+    for(i = 0; i < 3; i++) {
+        p_TCP.element[i][0] = T_TCP->element[i][3];
+    }
+
+    /**compute distance from TCP*/
+
+    Sub(&p_TCP, &p0, &p0);
+    Sub(&p_TCP, &p1, &p1);
+    Sub(&p_TCP, &p2, &p2);
+    Sub(&p_TCP, &p3, &p3);
+    Sub(&p_TCP, &p4, &p4);
+    Sub(&p_TCP, &p5, &p5);
+
+    /**compute jacobian*/
+
+    CrossProd(&z0, &p0, &CrossP);
+
+    BUILD_JACOBIAN(Jg, CrossP, z0, 0)
+
+
+    CrossProd(&z1, &p1, &CrossP);
+
+    BUILD_JACOBIAN(Jg, CrossP, z1, 1)
+
+
+    CrossProd(&z2, &p2, &CrossP);
+
+    BUILD_JACOBIAN(Jg, CrossP, z2, 2)
+
+
+    CrossProd(&z3, &p3, &CrossP);
+
+    BUILD_JACOBIAN(Jg, CrossP, z3, 3)
+
+
+    CrossProd(&z4, &p4, &CrossP);
+
+    BUILD_JACOBIAN(Jg, CrossP, z4, 4)
+
+
+
+    CrossProd(&z5, &p5, &CrossP);
+
+    BUILD_JACOBIAN(Jg, CrossP, z5, 5)
+
+
+    DeleteMatrix(&p0);
+    DeleteMatrix(&p1);
+    DeleteMatrix(&p2);
+    DeleteMatrix(&p3);
+    DeleteMatrix(&p4);
+    DeleteMatrix(&p5);
+    DeleteMatrix(&p_TCP);
+
+    DeleteMatrix(&z0);
+    DeleteMatrix(&z1);
+    DeleteMatrix(&z2);
+    DeleteMatrix(&z3);
+    DeleteMatrix(&z4);
+    DeleteMatrix(&z5);
+
+    DeleteMatrix(&T01);
+    DeleteMatrix(&T12);
+    DeleteMatrix(&T23);
+    DeleteMatrix(&T34);
+    DeleteMatrix(&T45);
+    DeleteMatrix(&T56);
+
+    DeleteMatrix(&CrossP);
+
+    return;
+}
+
+void JacobianA(matrix* joint, matrix* Ja)
+{
+
+    int i = 0;
+    int j = 0;
+
+    matrix Jg;
+    matrix M;
+    matrix Ttcp;
+    matrix JgA;
+    matrix JaA;
+
+    matrix p;
+
+    InitMatrix(&Jg, Ja->row, Ja->col );
+    InitMatrix(&M, 3 ,3 );
+    InitMatrix(&JgA, 3, 6 );
+    InitMatrix(&JaA, 3, 6 );
+    InitMatrix(&Ttcp, 4, 4 );
+    InitMatrix(&p, 6, 1);
+
+    JacobianG(joint, &Jg, &Ttcp);
+
+    p.element[0][0] = Ttcp.element[0][3];
+    p.element[1][0] = Ttcp.element[1][3];
+    p.element[2][0] = Ttcp.element[2][3];
+    p.element[3][0] = (float)atan2(Ttcp.element[2][1],Ttcp.element[2][2]);
+    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]);
+    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]);
+
+
+    // Transformation matrix
+    M.element[0][0] = (float)(cos(p.element[5][0])/cos(p.element[4][0]));
+    M.element[0][1] = (float)(sin(p.element[5][0])/cos(p.element[4][0]));
+    M.element[0][2] = 0;
+
+    M.element[1][0] = (float)-sin(p.element[5][0]);
+    M.element[1][1] = (float)cos(p.element[5][0]);
+    M.element[1][2] = 0;
+
+    M.element[2][0] = (float)(cos(p.element[5][0])*sin(p.element[4][0])/cos(p.element[4][0]));
+    M.element[2][1] = (float)(-sin(p.element[4][0])*sin(p.element[5][0])/cos(p.element[4][0]));
+    M.element[2][2] = 1;
+
+
+    for(i = 0; i < JgA.row; i++) {
+        for(j = 0; j< JgA.col; j++) {
+            JgA.element[i][j] = Jg.element[i+3][j];
+        }
+    }
+
+    MxM(&M, &JgA, &JaA);
+
+
+    for(i = 0; i < JaA.row; i++) {
+        for(j = 0; j< JaA.col; j++) {
+            Ja->element[i][j] = Jg.element[i][j];
+        }
+    }
+
+    for(i = 0; i < JaA.row; i++) {
+        for(j = 0; j< JaA.col; j++) {
+            Ja->element[i+3][j] = JaA.element[i][j];
+        }
+    }
+
+    DeleteMatrix(&M);
+    DeleteMatrix(&Jg);
+    DeleteMatrix(&JgA);
+    DeleteMatrix(&JaA);
+    DeleteMatrix(&Ttcp);
+    DeleteMatrix(&p);
+
+    return;
+}
+
+int IPKF (matrix* p, matrix* joint)
+{
+
+    float sentinel = 2;
+    float sum = 0.0;
+    float error_G = 0.5;
+    float error_N = (float)0.0001;
+    float alpha = (float)0.27; //after several trials
+    int c = 0;
+    int i = 0;
+
+
+    DHP dhp[6];
+
+    matrix f;//computed position through Direct Kinematics
+    matrix J;
+    matrix delta_p;
+    matrix delta_q;
+    matrix inv_J;
+
+
+    InitMatrix(&f, 6, 1);
+    InitMatrix(&J, 6, 6);
+    InitMatrix(&inv_J, 6, 6);
+    InitMatrix(&delta_q, 6,1);
+    InitMatrix(&delta_p, 6,1);
+
+
+    /*gradient method*/
+    while(sentinel >= error_G) {
+//
+        /*compute the direct kinematics*/
+        DPKF(joint, &f);
+
+        /*compute dh_par*/
+        DHP_update(dhp,joint);
+
+        /*compute jacobian*/
+        JacobianA(joint, &J);// to be defined
+
+        /*compute the next value of angle*/
+        Traspost(&J, &J); //Transpose of J saved in JT
+        axM(&J, alpha, &J);// Transpose of J (J) multiplied by alpha and saved in J
+
+        /*subtraction of two pose objects into matrix*/
+        Sub(p , &f, &delta_p);
+
+        //alpha*J'*(p-f)
+        MxM(&J, &delta_p, &delta_q);
+
+        Sum(joint, &delta_q, joint);
+
+        CHECK_ANGLE()
+
+        c = c+1;
+
+        /*if the gradient method does not converge break the cycle*/
+        if (c > 1000) {
+            break;
+        }
+
+        /* norm of p_minus_f*/
+
+        for(i=0; i<6; i++) {
+            sum += (delta_p.element[i][0] * delta_p.element[i][0]);
+        }
+        sentinel = (float)sqrt(sum);
+        sum = 0;
+    }
+
+//printf("c = %d\n", c);
+
+    /*Newton method*/
+    while (sentinel >= error_N) {
+
+        /*compute the direct kinematics*/
+        DPKF(joint,&f);
+
+        /*compute dh_par*/
+        DHP_update(dhp,joint);
+
+        /*compute jacobian*/
+        JacobianA(joint, &J);// to be defined
+
+        /*compute the next value of angle*/
+
+        //check if Jacobian is invertible
+        if(Inv(&J, &inv_J) == 6 ) {
+            break;
+            //printf("error due to jacobuan singularities");
+        }
+
+        Sub(p, &f, &delta_p);
+
+        MxM(&inv_J, &delta_p, &delta_q);
+
+        //q = q +  J \ (p-f);
+        Sum(joint, &delta_q, joint);
+
+        CHECK_ANGLE();
+
+        c = c+1;
+
+        /*if the newton method does not converge break the cycle*/
+        if (c > 2000) {
+            break;
+        }
+        /* norm of p_minus_f*/
+        sum = 0.0;
+
+        for(i=0; i<6; i++) {
+            sum += delta_p.element[i][0] * delta_p.element[i][0];
+        }
+        sentinel = (float)sqrt(sum);
+    }
+    
+    DeleteMatrix(&f);
+    DeleteMatrix(&J);
+    DeleteMatrix(&delta_p);
+    DeleteMatrix(&delta_q);
+    DeleteMatrix(&inv_J);
+    //printf("c = %d\n",c);
+	if(c >=2000)
+	{
+		return 1;
+	}
+	else
+    { 
+    	return 0;
+    }
+}
+
+void Print_DHP(DHP* dhp)
+{
+    printf("d = %f\n",dhp->d);
+    printf("theta = %f\n",dhp->theta);
+    printf("a = %f\n",dhp->a);
+    printf("alpha = %f\n",dhp->alpha);
+    printf("\n");
+}
+
+
+
+