6轴机器人jacobian矩阵

jacobian矩阵
计算出的结果是4*6的矩阵
机器人关节参数和角度值 看之前的文章
程序验证用到matlab 显示的结果与matlab机器人工具箱的结果差大概0.1左右

/*计算机器人jacobian矩阵 
 *机器人关节参数在文件 param_table中
 *角度值在文件 dt_jacob中
 *在屏幕输出 jacobian矩阵 */

#include <stdio.h>
#include <math.h>
#include <string.h>

#include "matrix_opt.h"

#define JOINT_V "./dt_jacob"
#define DESIGN_DT "./param_table"

#define ANGLE_OFFSET_J2 90
#define ANGLE_OFFSET_J3 90



double jacobian[MATRIX_J][MATRIX_J];
double matrix_R[MATRIX_N][MATRIX_N];
double matrix_T[MATRIX_N][MATRIX_N];

int matrix_jacob(double mtx_R[MATRIX_N][MATRIX_N], 
            double mtx_Te[MATRIX_N][MATRIX_N], 
            double mtx_Ti[MATRIX_N][MATRIX_N]);

//int fun_calcu_vct(double matrix_A[MATRIX_N][MATRIX_N], 
int fun_calcu_vct(double (*p_matrix)[], 
                double matrix_vct[MATRIX_N][MATRIX_N], 
                double (*jacobian)[MATRIX_J], int sign);

//int fun_calcu_pos();

//int vector_mul_ex(double vct_a[VECTOR_N], double vct_b[VECTOR_N], 
//          double vct_ret[VECTOR_N]);
int vector_mul_ex(double (*jacobian)[MATRIX_J], double vct_b[VECTOR_N], 
            double (*ret)[MATRIX_J]); 

//int vector_sub(double vct_a[VECTOR_N], double vct_b[VECTOR_N], 
//          double vct_ret[VECTOR_N], int n);
int vector_sub(double vct_a[VECTOR_N], double (*jacobian)[MATRIX_J], 
            double vct_ret[VECTOR_N], int n);

double matrix_E[4][4] = {
  
  1, 0, 0, 0,
                        0, 1, 0, 0,
                        0, 0, 1, 0, 
                        0, 0, 0, 1 };


int main()
{
    /*
    */
    int i;
    param_t param_table[6] ={
  
  0};
    double z_offset=0;

    FILE * fp=NULL;
    fp=fopen(JOINT_V, "r");
    if(fp== NULL){
        perror("open dt_jacob file error\n");
        return 0;
    }
    fscanf(fp, "%lf%lf%lf%lf%lf%lf", 
                &param_table[0].joint_v, 
                &param_table[1].joint_v, 
                &param_table[2].joint_v, 
                &param_table[3].joint_v, 
                &param_table[4].joint_v, 
                &param_table[5].joint_v 
          );

    param_table[1].joint_v += ANGLE_OFFSET_J2;
    param_table[2].joint_v += ANGLE_OFFSET_J3;

    param_table[0].joint_v *= RAD2ANG;
    param_table[1].joint_v *= RAD2ANG;
    param_table[2].joint_v *= RAD2ANG;
    param_table[3].joint_v *= RAD2ANG;
    param_table[4].joint_v *= RAD2ANG;
    param_table[5].joint_v *= RAD2ANG;

    fclose(fp);

    fp=fopen(DESIGN_DT, "r");
    if( fp== NULL){
        perror("open param_table file error\n");
        return 0;
    }   

    for(i=0; i<6; i++){
        fscanf(fp, "%lf%lf%lf",
                    &param_table[i].length,
                    &param_table[i].d,
                    &param_table[i].alpha );
    }
    fscanf(fp, "%lf", &z_offset );
    fclose(fp);

    param_table[0].alpha *= RAD2ANG;
    param_table[1].alpha *= RAD2ANG;
    param_table[2].alpha *= RAD2ANG;
    param_table[3].alpha *= RAD2ANG;
    param_table[