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 90double 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)+ivector_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 variabledouble length;double d;double alpha;
}param_t;
#endiftypedef 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矩阵的计算并没有对程序算法做优化由于距离写这些代码的时间已有半年很多细节记不清了以后还是要及时做笔记

6轴机器人jacobian矩阵相关推荐

6轴机器人运动学（正解）
理论部分概念运动学正解,简而言之,就是给出6个关节变量,求得机械臂末端的位置和姿态即给出j1−j6j_1 - j_6j1−j6,求x,y,z,rx,ry,rzx,y,z,rx,ry,rzx, ...
6轴机器人直线插补运动
距离上一篇博客已经好长一段时间了,已知在思考一般6轴的工业机器人的逆向运动算法,有参考很多论文,里面讲到了D-H参数,然后买各种书籍,下载各种电子书,研究逆向运动算法. 慢慢地耳语目染,对D-H参数也 ...
6轴机器人运动学正解,逆解1
正解给定机器人各关节的角度,计算出机器人末端的空间位置逆解已知机器人末端的位置和姿态,计算机器人各关节的角度值常见的工业机器人正解与逆解的求解需要相应的机器人运动方程,其中关键的就是DH参数 ...
6轴机器人运动学逆解matlab,六轴机器人建模方法、正逆解、轨迹规划实例与Matalb Robotic Toolbox 的实现...
摘要本文主要是给大家一个系统的概念,如何用Matlab实现六轴机器人的建模和实现轨迹规划.以后将会给大家讲解如何手写正逆解以及轨迹插补的程序.程序是基于Matlab2016a,工具箱版本为Robot ...
matlab mdh模型,6轴机器人DH建模、仿真、正逆解代码
一.模型二.MDH模型使用修改DH模型.改进DH模型与标准DH模型主要区别在于末端,改进DH并没有建到工具坐标系,这里将4.5.6轴的坐标系都建在同一点,因此机器人连杆的参数只需要4个值a1.a2 ...
Jacobian矩阵
Jacobian矩阵 1. Jacobian 在向量分析中, 雅可比矩阵是一阶偏导数以一定方式排列成的矩阵, 其行列式称为雅可比行列式. 还有, 在代数几何中, 代数曲线的雅可比量表示雅可比簇:伴随该 ...
理解Jacobian矩阵与分布变换
理解Jacobian矩阵本文主要是以下教程的一个整理: Khan-academy: The Jacobian matrix 可能有错,欢迎指正. Locally Linear 我们考虑一下简单的函数 ...
Jacobian矩阵和Hessian矩阵的理解
深度学习中梯度向量的计算,Jacobian矩阵和Hessian矩阵是基础的知识点. 求微分其实就是线性化,导数其实就是线性空间之间的线性变换,Jaocibian矩阵本质上就是导数. 比如,映射在处的导 ...
有限元基础： Jacobian 矩阵和高斯积分
有限元基础(一) Jacobian 矩阵和高斯积分_battlestar的博客-CSDN博客_有限元高斯积分有限元的二维热传导_fpga&matlab的博客-CSDN博客_二维热传导

6轴机器人jacobian矩阵

6轴机器人jacobian矩阵相关推荐

最新文章

热门文章