File content as of revision 0:fb6e494a7656:

#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");
}