Revision 0:fb6e494a7656, committed 2016-04-20

Comitter:

LudovicoDani

Date:

Wed Apr 20 10:03:58 2016 +0000

Commit message:

IPKF_Test_nucleo

Changed in this revision

diff -r 000000000000 -r fb6e494a7656 header/kinematic.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/header/kinematic.h	Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,129 @@
+/********************************************//**
+ * @brief
+ * @file kinematic.h
+ * @author Daniele Ludovico, Fiorella Sibona
+ * @date 12/04/2016
+ * @brief File containing the function to compute the
+ * kinematic function of a robotic arm.
+ *
+ */
+
+#ifndef KINEMATIC_H_INCLUDED
+#define KINEMATIC_H_INCLUDED
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <math.h>
+#include "matrix.h"
+#include "params.h"
+
+
+/********************************************//**
+ * @brief struct DHP
+ * denavit hartenberg parameter
+ ***********************************************/
+typedef struct s_DHP{
+float d;
+float theta;
+float a;
+float alpha;
+} DHP;
+
+
+/********************************************//**
+ * \brief this function allows to update
+ * the current dh parameter with the
+ * actual joints angle
+ *
+ * \param pointer to the DH parameter to
+ * update
+ * \param pointer to the new joint angle,
+ * value which update the DHP
+ *
+ * \return void
+ *
+ ***********************************************/
+
+void DHP_update(DHP* dhp, matrix* joints);
+
+/********************************************//**
+ * \brief this function allows to compute
+ * the homogeneouse trasformation matrix
+ * starting from DH parameters
+ *
+ * \param pointer to the actual DH parameter
+ * \param pointer to the trasformation matrix
+ * to compute
+ *
+ * \return void
+ *
+ ***********************************************/
+
+void DH_Transf(DHP* dhp, matrix* T);
+
+/********************************************//**
+ * \brief Function which performs the direct
+ * position kinematic function. Given the
+ * joints values the TCP pose is computed
+ *
+ * \param pointer to the structure containing
+ * the actual joints values
+ * \param pointer to the pose structure where
+ * the result will be stored
+ *
+ * \return void
+ ************************************************/
+
+void DPKF (matrix* joint, matrix* p);
+
+/********************************************//**
+ * \brief Function which returns the
+ * geometric Jacobian given the joints values
+ *
+ * \param pointer to the structure containing
+ * the joints values
+ * \param Jg is the address where the
+ * geometric Jacobian will be stored
+ * \param T_TCP is the matrix in which the
+ * homogeneous transformation from 0 to TCP
+ * is stored once computed
+ *
+ * \return void
+ ***********************************************/
+void JacobianG(matrix* joint, matrix* Jg, matrix* T_TCP);
+
+/********************************************//**
+ * \brief Function which returns the Analytical Jacobian given the joints values
+ *
+ * \param joint is the address to the structure containing the joints values
+ * \param Ja is the address where the Analytical Jacobian will be stored
+ *
+ * \return void
+ *
+ ***********************************************/
+void JacobianA(matrix* joint, matrix * Ja);
+
+/********************************************//**
+ * \brief Computes the inverse kinematics (returns the joints values for a desired pose).
+ *
+ * \param p is the pointer to the desired pose
+ * \param joint is where the result will be saved
+ *
+ * \return 0 if the inverse kinematics is computed 
+ * correctly
+ * \return 1 if the point to reach is not in the 
+ * task space
+ ***********************************************/
+int IPKF (matrix* p, matrix* joint);
+
+void Print_DHP(DHP* dhp);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // KINEMATIC_H_INCLUDED
+
+

diff -r 000000000000 -r fb6e494a7656 header/matrix.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/header/matrix.h	Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,227 @@
+/** @brief
+ * @file matrix.h
+ * @author Daniele Ludovico
+ * @date 20/3/2016
+ * @brief File containing the function to manipulates matrices.
+ *
+ */
+
+
+#ifndef MATRIX_H_INCLUDED
+#define MATRIX_H_INCLUDED
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+
+/** @brief struct matrix
+ *  allows to execute matrix computation
+ */
+typedef struct s_matrix
+{
+	int row;/**< number of row of the matrix */
+	int col;/**<  number of column of the matrix */
+	float** element; /**< this field allows to read and write the elements of the matrix */
+}matrix;
+
+/********************************************//**
+ * \brief this function allows to allocate a
+ * matrix in memory
+ *
+ * \param matrix pointer to the object to
+ * allocate
+ *
+ * \return 0 if the allocation is done
+ * correctly
+ * \return 1 if there is not enough space in
+ * memory to create a new matrix
+ *
+ ***********************************************/
+
+int CreateMatrix(matrix* m);
+
+/********************************************//**
+ * \brief this function allows to delete from
+ * memory a matrix struct
+ *
+ * \param matrix pointer to the object to
+ * delete
+ * \return void
+ *
+ ***********************************************/
+
+void DeleteMatrix(matrix* m);
+
+/********************************************//**
+ * \brief this function initialize a matrix
+ *
+ * \param m is the pointer to the matrix to
+ * initialize
+ * \param row is the number of row of the matrix
+ * \param col is the number of column of the matrix
+ *
+ * \return 0 if the allocation is done
+ * correctly
+ * \return 1 if there is not enough space in
+ * memory to create a new matrix
+ *
+ ***********************************************/
+
+int InitMatrix(matrix* m, int row, int col);
+
+/********************************************//**
+ * \brief  this function allows to compute
+ * the product between 2 matrices
+ *
+ * \param a is the first element of the product
+ * \param b is the second element of the product
+ * \param r is the matrix which will contain
+ * the result
+ *
+ * \return 0 if the product is computed correctly
+ * \return 1 if the number of column of a is different
+ * from the row of b
+ * \return 2 if the number of row of a is different
+ * from number of row of r
+ * \return 3 if the number of column of b is different
+ * from number of column of r
+ *
+ ***********************************************/
+
+int MxM (matrix* a, matrix* b, matrix* r);
+
+/********************************************//**
+ * \brief this function allows to compute the
+ * product between a scalar and a matrix
+ *
+ * \param m is the pointer to the matrix
+ * \param a is the scalar number
+ * \param r is the pointer to the result
+ *
+ * \return 0 if the the product is computed
+ * correctly
+ * \return 1 if the number of row of a is different
+ * from number of row of r
+ * \return 2 if the number of col of a is different
+ * from number of col of r
+ *
+ ***********************************************/
+
+int axM (matrix* m, float a, matrix* r);
+
+/********************************************//**
+ * \brief this function allows to compute the
+ * sum between two matrices
+ *
+ * \param a is the pointer to the first matrix
+ * \param b is the pointer to the second matrix
+ * \param c is the pointer to the result
+ *
+ * \return 0 if the sum is computed correctly
+ * \return 1 if the number of column of a is different
+ * from the column of b
+ * \return 2 if the number of row of a is different
+ * from number of row of b
+ * \return 3 if the number of row of a is different
+ * from number of row of r
+ * \return 4 if the number of column of a is different
+ * from number of column of r
+ *
+ ***********************************************/
+
+int Sum (matrix* a, matrix* b, matrix* r);
+
+/********************************************//**
+ * \brief this function allows to compute the
+ * subtraction between two matrices
+ *
+ * \param a is the pointer to the first matrix
+ * \param b is the pointer to the second matrix
+ * \param c is the pointer to the result
+ *
+ * \return 0 if the subtraction is computed correctly
+ * \return 1 if the number of column of a is different
+ * from the column of b
+ * \return 2 if the number of row of a is different
+ * from number of row of b
+ * \return 3 if the number of row of a is different
+ * from number of row of r
+ * \return 4 if the number of column of a is different
+ * from number of column of r
+ *
+ ***********************************************/
+
+int Sub (matrix* a, matrix* b, matrix* r);
+
+/********************************************//**
+ * \brief this function allows to compute the
+ * traspost of a matrix
+ *
+ * \param m is the pointer to the matrix to
+ * be trasposed
+ * \param  r is the pointer to the result
+ *
+ * \return 0 if the operation is done correctly
+ * \return 1 if the number of row of m is different
+ * from the column of r
+ * \return 2 if the number of column of m is different
+ * from the row of r
+ *
+ ***********************************************/
+
+int Traspost(matrix* m, matrix* r);
+
+/********************************************//**
+ * \brief this function allows to compute the
+ * inverse of a matrix using the Gauss algorithm
+ *
+ * \param m is the pointer to the original matrix
+ * \param inv_m is the pointer to the result
+ *
+ * \return 0 if the inversion is done correctly
+ * \return 1 if m is not a square matrix
+ * \return 2 if the number of row of m is different
+ * from the row of inv_m
+ * \return 3 if the number of column of m is different
+ * from the column of inv_m
+ * \return 4 if m is not invertible
+ *
+ ***********************************************/
+
+int Inv (matrix* m, matrix* inv_m);
+
+/********************************************//**
+ * \brief Computes the cross product between two vectors
+ *
+ * \param v is the first vector
+ * \param w is the second vector
+ * \param r is address to the resulting matrix
+ * \return void
+ *
+ ***********************************************/
+
+void CrossProd(matrix* v, matrix* w, matrix* r);
+
+/********************************************//**
+ * \brief print the matrix in the prompt
+ *
+ * \param m is the pointer to the matrix to print
+ *
+ * \return void
+ *
+ ***********************************************/
+
+void Print_Matrix(matrix* m);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // MATRIX_H_INCLUDED
+
+
+

diff -r 000000000000 -r fb6e494a7656 header/params.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/header/params.h	Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,36 @@
+/********************************************//**
+ * @brief
+ * @file params.h
+ * @author Daniele Ludovico, Fiorella Sibona
+ * @date 19/04/2016
+ * @brief File containing the parameter which 
+ * describe the robot.
+ *
+ */
+ 
+#ifndef PARAMS_H_INCLUDED
+#define PARAMS_H_INCLUDED
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**< height of the rotating base*/
+#define l1 (float)0.1
+/**< first arm lenght*/
+#define l2 (float)0.7
+/**< second arm lenght*/
+#define l3 (float)0.5
+/**< wrist lenght*/
+#define l4 (float)0.1
+/**< hand lenght*/
+#define l5 (float)0.1
+
+#define PI (float)3.141593
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // PARAMS_H_INCLUDED
+

diff -r 000000000000 -r fb6e494a7656 header/pathPlanning.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/header/pathPlanning.h	Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,16 @@
+#ifndef PATHPLANNING_H
+#define PATHPLANNING_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "matrix.h"
+
+void GeneratePath(matrix* p0, matrix* pf, matrix* path);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

diff -r 000000000000 -r fb6e494a7656 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/082adc85693f
\ No newline at end of file

diff -r 000000000000 -r fb6e494a7656 source/kinematic.c
--- /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");
+}
+
+
+
+

diff -r 000000000000 -r fb6e494a7656 source/main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/main.cpp	Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,94 @@
+#include "mbed.h"
+#include "params.h"
+#include "matrix.h"
+#include "kinematic.h"
+#include "pathPlanning.h"
+
+DigitalIn sw2(PC_13);
+Serial pc(USBTX, USBRX);
+//DigitalOut led_green(LED_GREEN);
+
+matrix path;
+matrix p0;
+matrix pf;
+
+matrix p;
+matrix q;
+
+Timer t;
+
+void check_sw2(void)
+{
+    if (sw2 == 0) {
+        //pc.printf("SW2 button pressed. \n");
+        //led_green = 0;
+        t.start();
+        for(int k = 0; k < 1 ; k++) {
+            for(int j = 0; j < path.col; j++) {
+                for(int i = 0; i < 6; i++) {
+                    p.element[i][0] = path.element[i][j];
+                    //pc.printf("%.4f    ", p.element[i][0]);
+                }
+
+                //pc.printf("\n");
+                IPKF(&p, &q);
+                
+                pc.printf("%f    ", q.element[0][0]);
+                pc.printf("\n");
+                
+                //for(int i = 0; i < 6; i++) {
+                //    pc.printf("%.4f    ", q.element[i][0]);
+                //}
+                //pc.printf("\n");
+
+                //DPKF(&q,&p);
+
+                //for(int i = 0; i < 6; i++) {
+                //    pc.printf("%.4f    ", p.element[i][0]);
+                //}
+                //pc.printf("\nj=%d\n", j);
+            }
+        }
+        wait(2);
+        pc.printf("%f", 0.0);
+        wait(2);
+        t.stop();
+        //pc.printf("%f s\n", t.read());
+        t.reset();
+    }
+    //led_green = 1;
+    return;
+}
+
+int main()
+{
+    //led_green = 1;
+    pc.baud(115200);
+
+    InitMatrix(&path, 6, 100);
+    InitMatrix(&p0, 6, 1);
+    InitMatrix(&pf, 6, 1);
+
+    InitMatrix(&p, 6, 1);
+    InitMatrix(&q, 6, 1);
+
+    p0.element[0][0]= 0.4;
+    p0.element[1][0]= 0;
+    p0.element[2][0]= 0.4;
+    p0.element[3][0]= 0;
+    p0.element[4][0]= 0;
+    p0.element[5][0]= 0;
+
+    pf.element[0][0]= 0.7;
+    pf.element[1][0]= 0.4;
+    pf.element[2][0]= -0.1;
+    pf.element[3][0]= PI/4;
+    pf.element[4][0]= PI/6;
+    pf.element[5][0]= -PI/3;
+
+    GeneratePath(&p0,&pf,&path);
+    while(true) {
+        check_sw2();
+        wait(0.1);
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r fb6e494a7656 source/matrix.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/matrix.c	Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,402 @@
+#include "matrix.h"
+
+int CreateMatrix(matrix *m)
+{
+	int i;
+	int k ;
+
+	m->element = (float**) malloc(m->row * (sizeof (float*)));
+	if(m->element == NULL)
+	{
+		return 1;
+	}
+	else
+	{
+		for(i = 0; i < m->row; i++)
+		{
+			m->element[i]= (float*) malloc(m->col * (sizeof (float)));
+			if(m->element[i] == NULL)
+			{
+				return 1;
+			}
+		}
+	}
+	for(i = 0; i < m->row; i++)
+	{
+		for(k = 0; k < m->col; k++)
+		{
+			m->element[i][k] = 0;
+		}
+	}
+	return 0;
+}
+
+void DeleteMatrix(matrix *m)
+{
+    int i;
+	for(i = 0; i< m->row; i ++)
+	{
+		if (m->element[i]!= NULL)
+		{
+			free(m->element[i]);
+			m->element[i] = NULL;
+		}
+	}
+	if (m->element[i]!= NULL)
+	{
+		free(m->element);
+		m->element = NULL;
+	}
+}
+
+int InitMatrix(matrix* m, int row, int col)
+{
+    m->row = row;
+    m->col = col;
+    return CreateMatrix(m);
+}
+
+int MxM (matrix* a, matrix* b, matrix* r)
+{
+    int i;
+    int z;
+    int j;
+    matrix temp;
+    InitMatrix(&temp, a->row, b->col);
+
+
+    if (a->col != b->row)
+	{
+		DeleteMatrix(&temp);
+		return 1;
+	}
+	else if(a->row!=r->row)
+    {
+		DeleteMatrix(&temp);
+        return 2;
+    }
+    else if(b->col!=r->col)
+    {
+        		DeleteMatrix(&temp);
+        return 3;
+    }
+	else
+	{
+
+		for(i = 0; i < a->row; i++)
+		{
+			for(j = 0; j < b->col; j++)
+			{
+				for(z = 0; z < a->col; z++)
+				{
+					temp.element[i][j]+= a->element[i][z] * b->element[z][j];
+				}
+			}
+		}
+
+		for(i = 0; i < temp.row; i++)
+		{
+			for(j = 0; j < temp.col; j++)
+			{
+			    r->element[i][j]=temp.element[i][j];
+			}
+		}
+        DeleteMatrix(&temp);
+		return 0;
+	}
+}
+
+int axM(matrix *m,float a, matrix* r)
+{
+    int i = 0;
+    int j = 0;
+    matrix temp;
+	
+	InitMatrix(&temp,m->row,m->col);
+	
+    if(m->row!=r->row)
+    {
+        		DeleteMatrix(&temp);
+        return 1;
+    }
+    else if(m->col!=r->col)
+    {
+        		DeleteMatrix(&temp);
+        return 2;
+    }
+    else
+    {
+        
+
+        for(i = 0; i < m->row; i++)
+        {
+            for(j = 0; j < m->col; j++)
+            {
+                temp.element[i][j] =  (m->element[i][j]) * a;
+            }
+        }
+        for(i = 0; i < temp.row; i++)
+		{
+			for(j = 0; j < temp.col; j++)
+			{
+			    r->element[i][j]=temp.element[i][j];
+			}
+		}
+        DeleteMatrix(&temp);
+
+        return 0;
+    }
+}
+
+int Sum(matrix *a, matrix *b, matrix* r)
+{
+    int i = 0;
+    int j = 0;
+
+    if(a->col != b->col)
+    {
+        return 1;
+    }
+    else if(a->row != b->row)
+    {
+        return 2;
+    }
+    else if(a->row != r->row)
+    {
+        return 3;
+    }
+    else if(a->col != r->col)
+    {
+        return 4;
+    }
+    else
+    {
+        for(i = 0; i < r->row; i++)
+        {
+            for(j = 0; j < r->col; j++)
+            {
+                r->element[i][j] =  (a->element[i][j]) + (b->element[i][j]);
+            }
+        }
+
+        return 0;
+    }
+}
+
+int Sub(matrix *a, matrix *b, matrix* r)
+{
+    int i = 0;
+    int j = 0;
+
+    if(a->col != b->col)
+    {
+        return 1;
+    }
+    else if(a->row != b->row)
+    {
+        return 2;
+    }
+    else if(a->row != r->row)
+    {
+        return 3;
+    }
+    else if(a->col != r->col)
+    {
+        return 4;
+    }
+    else
+    {
+        for(i = 0; i < r->row; i++)
+        {
+            for(j = 0; j < r->col; j++)
+            {
+                r->element[i][j] =  (a->element[i][j]) - (b->element[i][j]);
+            }
+        }
+
+        return 0;
+    }
+}
+
+int Traspost(matrix *m, matrix *r)
+{
+    int i;
+    int j;
+
+    matrix temp;
+	InitMatrix(&temp,m->row,m->col);
+	
+    if (m->row!=r->col)
+    {
+        DeleteMatrix(&temp);
+        return 1;
+    }
+    else if (m->col!=r->row)
+    {
+        DeleteMatrix(&temp);
+        return 2;
+    }
+    else
+    {
+        for(i = 0; i < m->row; i++)
+        {
+            for(j = 0; j < m->col; j++)
+            {
+                temp.element[j][i] = m->element[i][j];
+            }
+        }
+
+        for(i = 0; i < r->row; i++)
+        {
+            for(j = 0; j < r->col; j++)
+            {
+                r->element[i][j] = temp.element[i][j];
+            }
+        }
+
+        DeleteMatrix(&temp);
+        return 0;
+    }
+}
+
+int Inv(matrix *m, matrix *inv_m)
+{
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    float regTemp = 0;
+    
+    matrix temp;
+    temp.row = m->row;
+    temp.col = (m->col) * 2;
+    CreateMatrix(&temp);
+
+    if (m->col != m->row)
+    {	DeleteMatrix(&temp);
+        return 1;
+    }
+    else if (m->row != inv_m->row)
+    {	DeleteMatrix(&temp);
+        return 2;
+    }
+    else if (m->col != inv_m->col)
+    {	DeleteMatrix(&temp);
+        return 3;
+    }
+    else
+    {
+
+        for(i = 0; i < m->row; i++)
+        {
+            for(j = 0 ; j < m->col; j++)
+            {
+                temp.element[i][j] = m->element[i][j];
+                if(j == 0)
+                {
+                    temp.element[i][i + m->col] = 1;
+                }
+            }
+        }
+
+        /*start gauss algorithm*/
+        for(k = 0; k < temp.row; k++)
+        {
+            /*check if element [k][k] != 0*/
+            if(temp.element[k][k] == 0)
+            {
+                /*find an element [j][k] != 0*/
+                for(i = k; i < temp.row; i++)
+                {
+                    if(temp.element[i][k] != 0)
+                    {
+                        break;
+                    }
+                }
+                /*if not the matrix has no inverse*/
+                if(i == temp.row)
+                {
+                    DeleteMatrix(&temp);
+                    return 4;
+                }
+                /*else exchange row*/
+                for(j = 0; j < temp.col; j++)
+                {
+                    regTemp = temp.element[k][j];
+                    temp.element[k][j] = temp.element[i][j];
+                    temp.element[i][j] = regTemp;
+                }
+            }
+            /*row k divided by element[k][k]*/
+            regTemp = temp.element[k][k];
+            for(j = 0; j<temp.col; j++)
+            {
+                temp.element[k][j] = temp.element[k][j]/regTemp;
+            }
+            /*compute the other value of matrix*/
+            for (i = 0; i < temp.row; i++)
+            {
+                if (i != k)
+                {
+                    regTemp = temp.element[i][k];
+                    for(j = 0; j < temp.col; j++)
+                    {
+                        temp.element[i][j] = temp.element[i][j] - temp.element[k][j] * regTemp;
+                    }
+                }
+            }
+        }
+        for(i = 0; i < inv_m->row; i++)
+        {
+            for(j = 0; j < inv_m->col; j++)
+            {
+                inv_m->element[i][j] = temp.element[i][inv_m->col + j];
+            }
+        }
+        DeleteMatrix(&temp);
+        return 0;
+    }
+}
+
+void CrossProd(matrix* v, matrix* w, matrix* r)
+{
+    matrix s;
+
+	InitMatrix(&s, 3,3);
+
+    //Sv = [0,-v(3),v(2);v(3),0,-v(1);-v(2),v(1),0];
+    s.element[0][0] = 0;
+	s.element[0][1] = -(v->element[2][0]);
+	s.element[0][2] = v->element[1][0];
+	s.element[1][0] = v->element[2][0];
+	s.element[1][1] = 0;
+	s.element[1][2] = -v->element[0][0];
+	s.element[2][0] = -v->element[1][0];
+	s.element[2][1] = v->element[0][0];
+	s.element[2][2] = 0;
+
+    //k = Sv*w;
+    MxM(&s, w, r);
+
+    DeleteMatrix(&s);
+
+    return;
+}
+
+void Print_Matrix(matrix* m)
+{
+    int i;
+    int j;
+    for (i = 0; i<m->row;i++)
+    {
+        for( j = 0; j < m->col;j++)
+        {
+            printf("%f  ",m->element[i][j]);
+        }
+            printf("\n");
+
+    }
+    printf("\n");
+    return;
+}
+
+

diff -r 000000000000 -r fb6e494a7656 source/pathPlanning.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/pathPlanning.c	Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,24 @@
+#include "pathPlanning.h"
+
+void GeneratePath(matrix* p0, matrix* pf, matrix* path)
+{
+    float delta_x = (pf->element[0][0] - p0->element[0][0]) / path->col;
+    float delta_y = (pf->element[1][0] - p0->element[1][0]) / path->col;
+    float delta_z = (pf->element[2][0] - p0->element[2][0]) / path->col;
+    float delta_theta_x = (pf->element[3][0] - p0->element[3][0]) / path->col;
+    float delta_theta_y = (pf->element[4][0] - p0->element[4][0]) / path->col;
+    float delta_theta_z = (pf->element[5][0] - p0->element[5][0]) / path->col;
+    
+    for(int i = 0; i < path->col; i++)
+    {
+        path->element[0][i] = p0->element[0][0] + i*delta_x;
+        path->element[1][i] = p0->element[1][0] + i*delta_y;
+        path->element[2][i] = p0->element[2][0] + i*delta_z;
+        path->element[3][i] = p0->element[3][0] + i*delta_theta_x;
+        path->element[4][i] = p0->element[4][0] + i*delta_theta_y;
+        path->element[5][i] = p0->element[5][0] + i*delta_theta_z;
+        
+    } 
+    
+    return;
+}
\ No newline at end of file