PUMA Inverse Kinematics

Input :

[eqn: T = bmatrix{0.890919 & -0.295633 & 0.344767 & 337.389346# 0.163166 & 0.916796 & 0.364502 & 631.419259# -0.423840&-0.268488&0.865028&94.933257# 0&0&0&1}]

Output :

[eqn: q=bmatrix{q1#q2#q3#q4#q5#q6}]

실제 나와야 할 [eqn: q]는

[eqn: q=bmatrix{50#35#15#5#-20#-40}]

- 프로그램 소스

// inv_roboto.cpp : Defines the entry point for the console application.

//

#include "stdafx.h"

#include "stdio.h"

#include "conio.h"

#include "math.h"

#include "stdlib.h"

#define PI	3.141592

#define	ERROR	0.0001

//dh parameter (a alpha d theta)

double	dh_parameter[6][4] = {

0.0,	-90.0,	0.0,	60.0,

431.8,	0.0,	149.09,	45.0,

-20.33,	90.0,	0.0,	25.0,

0.0,	-90.0,	433.07,	15.0,

0.0,	90.0,	0.0,	-10.0,

0.0,	0.0,	56.25,	-30.0

};

double	input_matrix[4][4] = {

0.890919,	-0.295633,	0.344767,	337.389346,

0.163166,	0.916796,	0.364502,	631.419259,

-0.423840,	-0.268488,	0.865028,	94.933257,

0,	0,	0,	1

};

//////////////////////////////////////////////////

//global variables

double	A_Matrix[4][4][6];

double	T_Matrix[4][4][6];

double	jacobian[6][6];

double	T_Matrix_Inverse[4][4];

double	TAInverse[4][4];

double	e[6];

double	R_Matrix[6][6];

double	Sum;

double	SumR[6];

double	V_Matrix[6][6];

double	x[6];

//////////////////////////////////////////////////

//degree to radian conversion

double	deg2rad(double deg)

{

return (deg*PI)/180;

}

//radian to degree

double	rad2deg(double rad)

{

return (rad*180)/PI;

}

//compute a matrix

void solve_a_matrix(int link)

{

A_Matrix[0][0][link-1] = cos(deg2rad(dh_parameter[link-1][3]));

A_Matrix[0][1][link-1] = -cos(deg2rad(dh_parameter[link-1][1]))*sin(deg2rad(dh_parameter[link-1][3]));

A_Matrix[0][2][link-1] = sin(deg2rad(dh_parameter[link-1][1]))*sin(deg2rad(dh_parameter[link-1][3]));

A_Matrix[0][3][link-1] = dh_parameter[link-1][0]*cos(deg2rad(dh_parameter[link-1][3]));

A_Matrix[1][0][link-1] = sin(deg2rad(dh_parameter[link-1][3]));

A_Matrix[1][1][link-1] = cos(deg2rad(dh_parameter[link-1][1]))*cos(deg2rad(dh_parameter[link-1][3]));

A_Matrix[1][2][link-1] = -sin(deg2rad(dh_parameter[link-1][1]))*cos(deg2rad(dh_parameter[link-1][3]));

A_Matrix[1][3][link-1] = dh_parameter[link-1][0]*sin(deg2rad(dh_parameter[link-1][3]));

A_Matrix[2][0][link-1] = 0;

A_Matrix[2][1][link-1] = sin(deg2rad(dh_parameter[link-1][1]));

A_Matrix[2][2][link-1] = cos(deg2rad(dh_parameter[link-1][1]));

A_Matrix[2][3][link-1] = dh_parameter[link-1][2];

A_Matrix[3][0][link-1] = 0;

A_Matrix[3][1][link-1] = 0;

A_Matrix[3][2][link-1] = 0;

A_Matrix[3][3][link-1] = 1;

}

//compute a t matrix

void solve_t_matrix(int link)

{

int i,j,k,m;

double out=0.0;

for(k=0;k<link;k++)

for(i=0;i<4;i++)

for(j=0;j<4;j++)

{

if(k==0) T_Matrix[i][j][k] = A_Matrix[i][j][k];

else

{

out = 0;

for(m=0;m<4;m++)

out+=T_Matrix[i][m][k-1]*A_Matrix[m][j][k];

T_Matrix[i][j][k] = out;

}

}

}

//compute jacobian matrix

void solve_jacobian()

{

jacobian[0][0] = 0;

jacobian[1][0] = 0;

jacobian[2][0] = 0;

jacobian[3][0] = 0;

jacobian[4][0] = 0;

jacobian[5][0] = 1;

for(int i=1;i<6;i++)

{

jacobian[0][i] = T_Matrix[1][3][i-1]*T_Matrix[2][2][i-1] - T_Matrix[2][3][i-1]*T_Matrix[1][2][i-1];

jacobian[1][i] = T_Matrix[2][3][i-1]*T_Matrix[0][2][i-1] - T_Matrix[0][3][i-1]*T_Matrix[2][2][i-1];

jacobian[2][i] = T_Matrix[0][3][i-1]*T_Matrix[1][2][i-1] - T_Matrix[1][3][i-1]*T_Matrix[0][2][i-1];

jacobian[3][i] = T_Matrix[0][2][i-1];

jacobian[4][i] = T_Matrix[1][2][i-1];

jacobian[5][i] = T_Matrix[2][2][i-1];

}

}

//print A Matrix

void print_a_matrix()

{

printf("\t************A MATRIX************\n");

for(int i=0;i<6;i++)

{

printf("%d A %d = \n",i,i+1);

for(int j=0;j<4;j++)

{

for(int k=0;k<4;k++)

{

printf("\t%.4f",A_Matrix[j][k][i]);

}

printf("\n");

}

printf("\n");

}

}

//print all T matrix

void print_t_matrix()

{

printf("\t************T MATRIX************\n");

for(int i=0;i<6;i++)

{

printf("0 T %d = \n",i+1);

for(int j=0;j<4;j++)

{

for(int k=0;k<4;k++)

{

printf("\t%.4f",T_Matrix[j][k][i]);

}

printf("\n");

}

printf("\n");

}

}

//print jacobian matrix

void print_jacobian()

{

printf("\t************JACOBIAN MATRIX************\n");

for(int i=0;i<6;i++)

{

for(int j=0;j<6;j++)

{

printf("\t%.4f",jacobian[i][j]);

}

printf("\n");

}

printf("\n");

}

//solve e

void solve_e()

{

e[0] = TAInverse[0][3];

e[1] = TAInverse[1][3];

e[2] = TAInverse[2][3];

e[3] = -(TAInverse[1][2]-TAInverse[2][1])/2;

e[4] = -(TAInverse[2][0]-TAInverse[0][2])/2;

e[5] = -(TAInverse[0][1]-TAInverse[1][0])/2;

}

//solve T inverse

void T_Inverse()

{

T_Matrix_Inverse[0][0] = T_Matrix[0][0][5];

T_Matrix_Inverse[0][1] = T_Matrix[1][0][5];

T_Matrix_Inverse[0][2] = T_Matrix[2][0][5];

T_Matrix_Inverse[0][3] = -((T_Matrix[0][0][5]*T_Matrix[0][3][5])+(T_Matrix[1][0][5]*T_Matrix[1][3][5])+(T_Matrix[2][0][5]*T_Matrix[2][3][5]));

T_Matrix_Inverse[1][0] = T_Matrix[0][1][5];

T_Matrix_Inverse[1][1] = T_Matrix[1][1][5];

T_Matrix_Inverse[1][2] = T_Matrix[2][1][5];

T_Matrix_Inverse[1][3] = -((T_Matrix[0][1][5]*T_Matrix[0][3][5])+(T_Matrix[1][1][5]*T_Matrix[1][3][5])+(T_Matrix[2][1][5]*T_Matrix[2][3][5]));

T_Matrix_Inverse[2][0] = T_Matrix[0][2][5];

T_Matrix_Inverse[2][1] = T_Matrix[1][2][5];

T_Matrix_Inverse[2][2] = T_Matrix[2][2][5];

T_Matrix_Inverse[2][3] = -((T_Matrix[0][2][5]*T_Matrix[0][3][5])+(T_Matrix[1][2][5]*T_Matrix[1][3][5])+(T_Matrix[2][2][5]*T_Matrix[2][3][5]));

T_Matrix_Inverse[3][0] = 0;

T_Matrix_Inverse[3][1] = 0;

T_Matrix_Inverse[3][2] = 0;

T_Matrix_Inverse[3][3] = 1;

}

//print to screen

void output()

{

printf("\n\n************Result************\n");

printf("Output Homogeneous Transformation Matrix\n");

printf("---------------------------------------\n");

for(int i=0;i<4;i++)

{

for(int j=0;j<4;j++)

{

printf("%.4f\t",T_Matrix[i][j][5]);

}

printf("\n");

}

printf("---------------------------------------\n");

printf("Each Joint Angle Value\n");

for(i=0;i<6;i++)	printf("%dth = %.6f\n",i+1,dh_parameter[i][3]);

}

//all variables initialization

void init()

{

Sum = 0.0;

int i,j,k;

for(i=0;i<6;i++)

{

SumR[i] = 0.0;

e[i] = 0;

x[i] = 0;

for(j=0;j<6;j++)

//init 6x6

jacobian[i][j] = 0;

R_Matrix[i][j] = 0;

V_Matrix[i][j] = 0;

}

for(k=0;k<6;k++)

for(i=0;i<4;i++)

for(j=0;j<4;j++)

{

A_Matrix[i][j][k]=0;

T_Matrix[i][j][k]=0;

T_Matrix_Inverse[i][j] = 0;

TAInverse[i][j] = 0;

}

}

//solve r matrix

void solve_r_matrix()

{

for(int i=0;i<6;i++)

{

if(i==0) for(int j=0;j<6;j++) R_Matrix[j][0] = jacobian[j][0];	//Ri=Ji when i=0

else

{

for(int k=0;k<i;k++)

{

Sum  = (R_Matrix[0][k]*jacobian[0][i] + R_Matrix[1][k]*jacobian[1][i] + R_Matrix[2][k]*jacobian[2][i] +

R_Matrix[3][k]*jacobian[3][i] + R_Matrix[4][k]*jacobian[4][i] + R_Matrix[5][k]*jacobian[5][i]) /

(R_Matrix[0][k]*R_Matrix[0][k] + R_Matrix[1][k]*R_Matrix[1][k] + R_Matrix[2][k]*R_Matrix[2][k] +

R_Matrix[3][k]*R_Matrix[3][k] + R_Matrix[4][k]*R_Matrix[4][k] + R_Matrix[5][k]*R_Matrix[5][k]);

SumR[0] += R_Matrix[0][k]*Sum;

SumR[1] += R_Matrix[1][k]*Sum;

SumR[2] += R_Matrix[2][k]*Sum;

SumR[3] += R_Matrix[3][k]*Sum;

SumR[4] += R_Matrix[4][k]*Sum;

SumR[5] += R_Matrix[5][k]*Sum;

}

for (int t=0;t<6;t++)

{

R_Matrix[t][i] = jacobian[t][i] - SumR[t];

SumR[t] = 0;	//initialization for next loop steps

}

}

}

}

//solve xi

void solve_x()

{

for (int i=0;i<6;i++){

V_Matrix[i][5] = e[i];

}

for (i=5;i>=0;i--)

{

x[i] = ( (R_Matrix[0][i]*V_Matrix[0][i] + R_Matrix[1][i]*V_Matrix[1][i] + R_Matrix[2][i]*V_Matrix[2][i] + R_Matrix[3][i]*V_Matrix[3][i] +  R_Matrix[4][i]*V_Matrix[4][i] +  R_mattrix[5][i]*V_Matrix[5][i]) / (R_Matrix[0][i]*R_Matrix[0][i] + R_Matrix[1][i]*R_Matrix[1][i] + R_Matrix[2][i]*R_Matrix[2][i] + R_Matrix[3][i]*R_Matrix[3][i] + R_Matrix[4][i]*R_Matrix[4][i] + R_Matrix[5][i]*R_Matrix[5][i]) );

for (int j=0;j<6;j++)

{

if(i>0){

V_Matrix[j][i-1] = V_Matrix[j][i] -  jacobian[j][i] * x[i] ;

}

}

}

}

void initT()

{

for(int i=0;i<6;i++)

for(int j=0;j<6;j++)

for(int k=0;k<6;k++)

T_Matrix[i][j][k] = 0.0;

}

/////////////////////////////////////////////////

//main function

void main()

{

double	cur_error = 1;	//ERROR보다 큰 값으로 초기화

int temp;

//all variables initialization

init();

while(cur_error>ERROR)

{

cur_error = 0; //for loop start

//cal a matrix

for(int i=0;i<6;i++)	solve_a_matrix(i+1);

//cal t matrix(1~6 joint)

for(i=0;i<6;i++)	solve_t_matrix(i+1);

//cal jacobianobian matrix

solve_jacobian();

//print and verification ->check

//print_a_matrix();

//print_t_matrix();

//print_jacobian();

//cal 0A6 Inverse

T_Inverse();

//cal input_matrix*T_matrix_inverse

for(i=0;i<4;i++)

for(int j=0;j<4;j++)

for(int k=0;k<4;k++)

TAInverse[i][j]+=input_matrix[i][k]*T_Matrix_Inverse[k][j];

//cal e matrix

solve_e();

for (i=0;i<4;i++)

{

for(int j=0;j<4;j++)

{

TAInverse[i][j] = 0;

}

}

//solve r matrix

solve_r_matrix();

//solve xi

solve_x();

//error

for (i=0;i<6;i++)

{

dh_parameter[i][3] = dh_parameter[i][3] + (rad2deg(x[i]));

temp=(int)(rad2deg(x[i]));

cur_error = cur_error + pow(temp,2);

}

//check

if(cur_error < ERROR) break;

//print result

output();

}

}

- 결과

************Result************

Output Homogeneous Transformation Matrix

---------------------------------------

0.4608  -0.7510 0.4729  250.1516

0.2736  0.6271  0.7293  726.3992

-0.8443 -0.2066 0.4944  -110.2936

0.0000  0.0000  0.0000  1.0000

---------------------------------------

Each Joint Angle Value

1th = 48.810370

2th = 27.507775

3th = 40.689249

4th = -15.874232

5th = -34.115339

6th = -17.649030

************Result************

Output Homogeneous Transformation Matrix

---------------------------------------

0.8740  -0.4113 0.2588  414.4046

0.2282  0.8176  0.5287  713.0371

-0.4291 -0.4030 0.8084  25.7615

0.0000  0.0000  0.0000  1.0000

---------------------------------------

Each Joint Angle Value

1th = 50.327673

2th = 36.741087

3th = 12.161279

4th = -2.951547

5th = -17.435269

6th = -30.288935

************Result************

Output Homogeneous Transformation Matrix

---------------------------------------

0.8792  -0.3517 0.3215  324.0436

0.2046  0.8880  0.4118  625.5940

-0.4304 -0.2963 0.8526  89.6550

0.0000  0.0000  0.0000  1.0000

---------------------------------------

Each Joint Angle Value

1th = 50.031004

2th = 34.833731

3th = 15.284034

4th = 6.237736

5th = -19.888831

6th = -41.285706

************Result************

Output Homogeneous Transformation Matrix

---------------------------------------

0.8887  -0.2931 0.3526  338.3458

0.1587  0.9181  0.3631  632.5240

-0.4301 -0.2667 0.8625  95.1616

0.0000  0.0000  0.0000  1.0000

---------------------------------------

Each Joint Angle Value

1th = 49.999735

2th = 34.997817

3th = 15.002501

4th = 4.992178

5th = -19.995444

6th = -39.992464