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[4].alpha *= RAD2ANG;
    param_table[5].alpha *= RAD2ANG;

    initmatrix_A(param_table);  

    for(i=0; i<6; i++){
        printf("matrix_a%d \n", i+1);
        print_mtx(union_A.matrix_A[i], MATRIX_N, MATRIX_N);
    }

    double matrix_tmp[MATRIX_N][MATRIX_N] ={0};
    double (*p_matrix)[MATRIX_N] = NULL;
    matrix_copy(matrix_E, matrix_R, MATRIX_N, MATRIX_N);

    /*计算旋转矩阵 并保存向量到 jacobian[3] -> [5] */
    for(i=0; i<6; i++){
            printf("i= %d \n", i);
        fun_calcu_vct(p_matrix, matrix_tmp, 
                    ( double(*)[] )(*(jacobian+3)+i), 1);
        if(i ==0){
            p_matrix = matrix_R;
        }
        matrix_copy(union_A.matrix_A[i], matrix_tmp, 
                    MATRIX_N-1, MATRIX_N-1);
        print_mtx(matrix_tmp, MATRIX_N, MATRIX_N);
    }

    /*计算转换矩阵T1 -> T4 并保存向量到 jacobian[0] -> [2] */
    matrix_copy(matrix_E, matrix_T, MATRIX_N, MATRIX_N);
    p_matrix = NULL;
    for(i=0; i<5; i++){
            printf("i= %d \n", i);
        fun_calcu_vct(p_matrix, matrix_tmp, 
                    ( double(*)[] )(*(jacobian+0)+i), 2);
        if(i ==0){
            p_matrix = matrix_T;
        }
        matrix_copy(union_A.matrix_A[i], matrix_tmp, 
                    MATRIX_N, MATRIX_N);
        print_mtx(matrix_tmp, MATRIX_N, MATRIX_N);
    }

    /*保存jacobian[][4] 到 jacobian[][5] -> [][5] 
     * T4[][3]=T5[][3]=T6[][3]  */
    double pos_end[3];

    jacobian[0][5] = jacobian[0][4];    
    jacobian[1][5] = jacobian[1][4];    
    jacobian[2][5] = jacobian[2][4];    
    //matrix_mul(p_matrix, matrix_tmp, p_matrix, MATRIX_N, MATRIX_N);
    pos_end[0] = jacobian[0][4];
    pos_end[1] = jacobian[1][4];
    pos_end[2] = jacobian[2][4];

    //pos_end[0] = p_matrix[0][3];
    //pos_end[1] = p_matrix[1][3];
    //pos_end[2] = p_matrix[2][3];
    printf("pos_end %lf %lf %lf \n", pos_end[0], pos_end[1], pos_end[2]);

    /*计算向量差 向量积 并保存到 jacobian[0][] -> [2][] */
    fun_sub_mulex(jacobian, pos_end);

    printf("\n jacobian matrix \n");
    print_mtx(jacobian, 3, 6);
    print_mtx(&jacobian[3], 3, 6);

}

int fun_sub_mulex(double (*jacobian)[MATRIX_J], double *pos_end )
{
    double vector[3];
    int i;

    for(i=0; i<6; i++){
        vector_sub(pos_end, (double (*)[])&jacobian[0][i], 
                    vector, 3);
printf("vector %lf %lf %lf \n", vector[0], vector[1], vector[2]);
//  *(jacobian+3)+i
        vector_mul_ex( (double (*)[])&jacobian[3][i], 
                    vector, (double (*)[])&jacobian[0][i] );
    }
}

//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])
{
    double vector[VECTOR_N];

    vector[0] = jacobian[1][0]*vct_b[2] - jacobian[2][0]*vct_b[1];
    vector[1] = jacobian[2][0]*vct_b[0] - jacobian[0][0]*vct_b[2];
    vector[2] = jacobian[0][0]*vct_b[1] - jacobian[1][0]*vct_b[0];

    ret[0][0] =  vector[0];
    ret[1][0] =  vector[1];
    ret[2][0] =  vector[2];

    printf("jcb vct %lf %lf %lf \n", vector[0], vector[1], vector[2]);

}

//int vector_sub(double vct_a[], double vct_b[], double vct_ret[], int n)
int vector_sub(double vct_a[VECTOR_N], double (*jacobian)[MATRIX_J], 
            double vct_ret[VECTOR_N], int n)
{
    int i;
    for(i=0; i<n; i++){
        vct_ret[i] = vct_a[i] - **jacobian++;//vct_b[i]
    }
}

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

    if(p_matrix == NULL){
        if(sign ==1){
            **jacobian++ = 0;
            **jacobian++ = 0;
            **jacobian = 1;
            return;
        }else{
            **jacobian++ = 0;
            **jacobian++ = 0;
            **jacobian = 0;
            return;
        }
    }

    matrix_mul(p_matrix, matrix_vct, p_matrix, MATRIX_N, MATRIX_N);

    if(sign == 1){
        **jacobian++ = p_matrix[0][2];
        **jacobian++ = p_matrix[1][2];
        **jacobian = p_matrix[2][2];
    }else{
        **jacobian++ = p_matrix[0][3];
        **jacobian++ = p_matrix[1][3];
        **jacobian = p_matrix[2][3];
    }   

}

/*  */
#include <stdio.h>
 #include <math.h>


#include "matrix_opt.h"

/*
union
{
    struct 
    {
        double matrix_A[6][MATRIX_N][MATRIX_N];
    };
    struct
    {
        double matrix_a1[MATRIX_N][MATRIX_N];
        double matrix_a2[MATRIX_N][MATRIX_N];
        double matrix_a3[MATRIX_N][MATRIX_N];
        double matrix_a4[MATRIX_N][MATRIX_N];
        double matrix_a5[MATRIX_N][MATRIX_N];
        double matrix_a6[MATRIX_N][MATRIX_N];
    };
};
*/

void calculate_matrix_R(double angle_r, double angle_p, double angle_y,
            double (*matrix_R)[MATRIX_N])
{
    /*计算RPY旋转矩阵 */
    int i,j;
    double mtmp;
    double r_c, r_s, p_c, p_s, y_c, y_s;

    angle_r *= RAD2ANG;
    angle_p *= RAD2ANG;
    angle_y *= RAD2ANG;
    r_c = cos( angle_r );
    IS_ZERO(r_c);
    r_s = sin( angle_r );
    IS_ZERO(r_s);
    p_c = cos( angle_p );
    IS_ZERO(p_c);
    p_s = sin( angle_p );
    IS_ZERO(p_s);
    y_c = cos( angle_y );
    IS_ZERO(p_c);
    y_s = sin( angle_y );
    IS_ZERO(y_s);

    matrix_R[0][0] = r_c * p_c;
    matrix_R[0][1] = r_c * p_s * y_s - r_s * y_c;
    matrix_R[0][2] = r_c * p_s * y_c + r_s * y_s;

    matrix_R[1][0] = r_s * p_c;
    matrix_R[1][1] = r_s * p_s * y_s + r_c * y_c;
    matrix_R[1][2] = r_s * p_s * y_c - r_c * y_s;

    matrix_R[2][0] = -p_s;
    matrix_R[2][1] = p_c * y_s;
    matrix_R[2][2] = p_c * y_c;

}

void calculate_matrix_A(double matrix[MATRIX_N][MATRIX_N], param_t *p_param)
{
    double *pmatrix=(double *)matrix;
    double value, var_c, var_s, angle_c, angle_s;

    //  printf("calculate_matrix_A: %p \n", pmatrix);
    var_c = cos(p_param->joint_v);
    IS_ZERO(var_c);
    var_s = sin(p_param->joint_v);
    IS_ZERO(var_s);
    angle_c = cos(p_param->alpha);
    IS_ZERO(angle_c);
    angle_s = sin(p_param->alpha);
    IS_ZERO(angle_s);

    *pmatrix++ = var_c;
    *pmatrix++ = -var_s * angle_c;

    *pmatrix++ = var_s * angle_s;
    *pmatrix++ = p_param->length * var_c;

    *pmatrix++ = var_s;
    *pmatrix++ = var_c * angle_c;
    *pmatrix++ = -var_c *angle_s;
    *pmatrix++ = p_param->length * var_s;

    *pmatrix++ =0;
    *pmatrix++ = angle_s;
    *pmatrix++ = angle_c;
    *pmatrix++ = p_param->d;

    *pmatrix++ =0;
    *pmatrix++ =0;
    *pmatrix++ =0;
    *pmatrix =1;

}

void matrix_copy(double matrix_src[MATRIX_N][MATRIX_N],
            double matrix_dest[MATRIX_N][MATRIX_N], int m, int n)
{
    int i,j;
    for(i=0; i<m; i++){
        for(j=0; j<n; j++){
            matrix_dest[i][j] = matrix_src[i][j];
        }
    }
}

int matrix_mul(double matrix_a[MATRIX_N][MATRIX_N],
            double matrix_b[MATRIX_N][MATRIX_N],
            double matrix_result[MATRIX_N][MATRIX_N], int m, int n)
{
    int i,j,k;
    double sum;
    double matrix_tmp[MATRIX_N][MATRIX_N]={0};

    /*嵌套循环计算结果矩阵（m*p）的每个元素*/
    for(i=0; i<m; i++)
      for(j=0; j<n; j++)
      {
          /*按照矩阵乘法的规则计算结果矩阵的i*j元素*/
          sum=0;
          for(k=0; k<n; k++)
            sum += matrix_a[i][k] * matrix_b[k][j];
          matrix_tmp[i][j] = sum;
      }
    matrix_copy(matrix_tmp, matrix_result, MATRIX_N, MATRIX_N);
    return 0;
}

void matrix_translate(double matrix[MATRIX_M][MATRIX_N], int m, int n)
{
    double m_tmp;
    int i, j, k;

    for(i=0, j=0; i<m; i++, j++){
        for(k=j; k<n; k++){
            if(i == k) continue;
            m_tmp = matrix[i][k];
            matrix[i][k] = matrix[k][i];
            matrix[k][i] = m_tmp;
        }
    }
}

void printmatrix(double matrix[MATRIX_N][MATRIX_N], int m, int n)
{
    int i, j;

    for(i=0; i<m; i++){
        for(j=0; j<n; j++){
            printf(" %lf ", matrix[i][j]);
        }
        printf("\n");
    }
    printf("\n");
}

void matrix_copy2(double (*matrix_s)[], double (*matrix_d)[], 
            int m, int n)
{
    double *p_s = (double *)matrix_s;
    double *p_d = (double *)matrix_d;

    int i, j;
    /*
    for(i=0; i<m; i++){
        for(j=0; j<n; j++){
            *(p_d + i*n+j)  = *(p_s + i*n+j);
        }
    }
*/
///*
    for(i=0; i<m*n; i++){
        (*matrix_d)[i] = (*matrix_s)[i];
    }
//*/

}

void print_mtx(double (*p_matrix)[], int m, int n)
{
    int i, j;
    double *p = (double *)p_matrix;

    /*
    for(i=0; i<m; i++){
        for(j=0; j<n; j++){
            printf(" %lf ", *(p + i*n+j) ); 
        }
        printf("\n");
    }
    printf("\n");
*/

    ///*
    for(j=0; j<m*n; j++){
        printf(" %lf ", (*p_matrix)[j] );   
        if( (j+1)%n == 0 ) printf("\n");
    }
    printf("\n");
    //*/

}

void initmatrix_A(param_t *p_table)
{
    int i;
    for(i=0; i<6; i++){
    //  printf(" %p \n", union_A.matrix_A[i]);
        calculate_matrix_A(union_A.matrix_A[i], p_table+i); 
    }
}

/*matrix_opt.h文件*/
#ifndef MATRIX_OPT
#define MATRIX_OPT

#define MATRIX_1 1
#define MATRIX_M 4
#define MATRIX_N 4
#define MATRIX_J 6
#define VECTOR_N 3

#define ANGLE_OFFSET_J2 90
#define ANGLE_OFFSET_J3 90

#define PI (3.1415926535898)
#define ANG2RAD_EQU(N) (N *= (180.0/3.1415926535898) )
#define ANG2RAD(N) ( (N) * (180.0/3.1415926535898) )
#define RAD2ANG (3.1415926535898/180.0)
#define IS_ZERO(var) if(var < 0.0000000001 && var > -0.0000000001){var = 0;} 


#ifndef DH_PARAM
#define DH_PARAM
typedef struct {
    double joint_v;  //joint variable
    double length;
    double d;
    double alpha;
}param_t;
#endif

typedef union
{
    //struct{   
        double matrix_A[6][MATRIX_N][MATRIX_N];
    //};  
    struct{   
        double matrix_a1[MATRIX_N][MATRIX_N];
        double matrix_a2[MATRIX_N][MATRIX_N];
        double matrix_a3[MATRIX_N][MATRIX_N];
        double matrix_a4[MATRIX_N][MATRIX_N];
        double matrix_a5[MATRIX_N][MATRIX_N];
        double matrix_a6[MATRIX_N][MATRIX_N];
    };  
}matrix_union_t;

matrix_union_t union_A;

void printmatrix(double matrix[MATRIX_N][MATRIX_N], int m, int n);

void matrix_translate(double matrix[MATRIX_M][MATRIX_N], int m, int n);

int matrix_mul(double matrix_a[MATRIX_N][MATRIX_N],
            double matrix_b[MATRIX_N][MATRIX_N],
            double matrix_result[MATRIX_N][MATRIX_N], int m, int n);

void matrix_copy(double matrix_a[MATRIX_N][MATRIX_N],
            double matrix_b[MATRIX_N][MATRIX_N], int m, int n);

void calculate_matrix_A(double matrix[MATRIX_N][MATRIX_N], 
            param_t *p_param);

void calculate_matrix_R(double angle_r, double angle_p, double angle_y,
            double (*matrix_R)[MATRIX_N]);

void matrix_copy2(double (*matrix_s)[], double (*matrix_d)[], int m, int n);

void print_mtx(double (*p_matrix)[], int m, int n);

void initmatrix_A(param_t *p_table);

今天被家里人介绍了个姑娘…..

这3篇文章仅仅是完成了机器人正解逆解及jacobian矩阵的计算并没有对程序算法做优化由于距离写这些代码的时间已有半年很多细节记不清了以后还是要及时做笔记

原文链接：https://blog.csdn.net/weixin_37942267/article/details/79344327

你可能也喜欢