invalid-file이게 PUMA560인지 600인지.. 기억은 안나지만 PUMA였던건 확실하다.
Homogeneous Matrix를 input으로 넣어주면 inverse를 풀어 q matrix를 구하는것.
(여기서 q matrix는 inverse 해석의 결과인 각 관절의 angle들입니다.)
아래는 그 소스입니다.
나머지는 보시는 분들 몫...질문 사절.ㅋ(몇년전꺼라 기억도 안남..)
// 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.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
'Robotics > Software Tech.' 카테고리의 다른 글
| IOCP 사용하기 (0) | 2008.01.01 |
|---|---|
| [MFC]TRACE 파일 저장 (0) | 2008.01.01 |
| 인터넷폰(internet phone) 논문 (0) | 2007.12.20 |
| Windows MIDI and Digital Audio Programming (0) | 2007.12.20 |
| [mfc]마이크 입력 레벨메터 소스 (0) | 2007.12.16 |