六自由度机器人Jacobian雅克比矩阵计算类Word文档下载推荐.docx
《六自由度机器人Jacobian雅克比矩阵计算类Word文档下载推荐.docx》由会员分享，可在线阅读，更多相关《六自由度机器人Jacobian雅克比矩阵计算类Word文档下载推荐.docx（8页珍藏版）》请在冰豆网上搜索。
doubleAMat[6][4][4];
};
doubleA0to1[4][4];
doubleA1to2[4][4];
doubleA2to3[4][4];
doubleA3to4[4][4];
doubleA4to5[4][4];
doubleA5to6[4][4];
doubleTMat[6][4][4];
doubleT0to6[4][4];
doubleT1to6[4][4];
doubleT2to6[4][4];
doubleT3to6[4][4];
doubleT4to6[4][4];
doubleT5to6[4][4];
//末端位姿!
doubleEndPose[4][4];
//D-H参数表!
doubleDHParam[6][4];
//顺序为：
Angled_La_La_A!
//雅克比矩阵!
doubleEndJacobian[6][6];
//逆雅克比矩阵!
doubleEndInvJacobian[6][6];
//基坐标的笛卡尔微分运动到末端坐标的传递矩阵!
doubleJBasetoEnd[6][6];
doubleT_1to6[4][4];
//该矩阵的姿态与基坐标一致，位置与末端坐标一致!
//以便可以按照基坐标进行平动和绕基坐标轴方向转动!
doublemInput[6];
//输入!
doublemOutput[6];
//输出!
intmMode;
voidGetAMat（）
for（inti=0;
i
MLGetDHTransMat（AMat[i],DHParam[i][0],DHParam[i][1],DHParam[i][2],DHParam[i][3]）;
}
voidGetTMat（）
MLGetIdentityMat（T5to6）;
MLMatMulti（AMat[5],T5to6）;
MLMatMulti（AMat[4],T5to6,T4to6）;
MLMatMulti（AMat[3],T4to6,T3to6）;
MLMatMulti（AMat[2],T3to6,T2to6）;
MLMatMulti（AMat[1],T2to6,T1to6）;
MLMatMulti（AMat[0],T1to6,T0to6）;
voidUpdateAngle（doubleAngles[6]）//Angles为弧度!
DHParam[i][0]=Angles[i];
GetAMat（）;
GetEndPose（）;
GetTMat（）;
GetEndJacobian（）;
GetEndInvJacobian（）;
GetJBasetoEnd（）;
voidInti（doubleDHparameter[6][4]）
for（intj=0;
j
DHParam[i][j]=DHparameter[i][j];
for（i=0;
mInput[i]=0;
mOutput[i]=0;
mMode=BASE;
voidGetEndPose（）
MLGetIdentityMat（EndPose）;
MLMatMulti（AMat[5-i],EndPose）;
voidGetEndJacobian（）
EndJacobian[0][i]=-1*TMat[i][Xx][Nn]*TMat[i][Yy][Pp]+TMat[i][Yy][Nn]*TMat[i][Xx][Pp];
EndJacobian[1][i]=-1*TMat[i][Xx][Oo]*TMat[i][Yy][Pp]+TMat[i][Yy][Oo]*TMat[i][Xx][Pp];
EndJacobian[2][i]=-1*TMat[i][Xx][Aa]*TMat[i][Yy][Pp]+TMat[i][Yy][Aa]*TMat[i][Xx][Pp];
EndJacobian[3][i]=TMat[i][Zz][Nn];
EndJacobian[4][i]=TMat[i][Zz][Oo];
EndJacobian[5][i]=TMat[i][Zz][Aa];
voidGetEndInvJacobian（）
doubleData1[36];
CvMatMat1=cvMat（6,6,CV_64FC1,Data1）;
doubleData2[36];
CvMatMat2=cvMat（6,6,CV_64FC1,Data2）;
cvmSet（&
Mat1,i,j,EndJacobian[i][j]）;
cvInvert（&
Mat1,&
Mat2,CV_SVD）;
EndInvJacobian[i][j]=cvmGet（&
Mat2,i,j）;
voidEndOutput（doubleInput[6],doubleOutput[6]）//Output为角速度!
MLMatMulti_3（EndInvJacobian,Input,Output）;
voidGetJBasetoEnd（）
doubleTransMat[4][4];
MLGetIdentityMat（TransMat）;
TransMat[0][3]=-1*EndPose[0][3];
TransMat[1][3]=-1*EndPose[1][3];
TransMat[2][3]=-1*EndPose[2][3];
MLMatMulti（TransMat,EndPose,T_1to6）;
JBasetoEnd[0][0]=T_1to6[Xx][Nn];
JBasetoEnd[0][1]=T_1to6[Yy][Nn];
JBasetoEnd[0][2]=T_1to6[Zz][Nn];
JBasetoEnd[1][0]=T_1to6[Xx][Oo];
JBasetoEnd[1][1]=T_1to6[Yy][Oo];
JBasetoEnd[1][2]=T_1to6[Zz][Oo];
JBasetoEnd[2][0]=T_1to6[Xx][Aa];
JBasetoEnd[2][1]=T_1to6[Yy][Aa];
JBasetoEnd[2][2]=T_1to6[Zz][Aa];
for（inti=3;
JBasetoEnd[i][j]=0;
JBasetoEnd[3][3]=T_1to6[Xx][Nn];
JBasetoEnd[3][4]=T_1to6[Yy][Nn];
JBasetoEnd[3][5]=T_1to6[Zz][Nn];
JBasetoEnd[4][3]=T_1to6[Xx][Oo];
JBasetoEnd[4][4]=T_1to6[Yy][Oo];
JBasetoEnd[4][5]=T_1to6[Zz][Oo];
JBasetoEnd[5][3]=T_1to6[Xx][Aa];
JBasetoEnd[5][4]=T_1to6[Yy][Aa];
JBasetoEnd[5][5]=T_1to6[Zz][Aa];
JBasetoEnd[0][3]=T_1to6[Yy][Pp]*T_1to6[Zz][Nn]-T_1to6[Zz][Pp]*T_1to6[Yy][Nn];
//（P×
N）x
JBasetoEnd[0][4]=T_1to6[Zz][Pp]*T_1to6[Xx][Nn]-T_1to6[Xx][Pp]*T_1to6[Zz][Nn];
N）y
JBasetoEnd[0][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Nn]-T_1to6[Yy][Pp]*T_1to6[
Xx][Nn];
N）z
JBasetoEnd[1][3]=T_1to6[Yy][Pp]*T_1to6[Zz][Oo]-T_1to6[Zz][Pp]*T_1to6[Yy][Oo];
O）x
JBasetoEnd[1][4]=T_1to6[Zz][Pp]*T_1to6[Xx][Oo]-T_1to6[Xx][Pp]*T_1to6[Zz][Oo];
O）y
JBasetoEnd[1][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Oo]-T_1to6[Yy][Pp]*T_1to6[Xx][Oo];
O）z
JBasetoEnd[2][3]=T_1to6[Yy][Pp]*T_1to6[Zz][Aa]-T_1to6[Zz][Pp]*T_1to6[Yy][Aa];
A）x
JBasetoEnd[2][4]=T_1to6[Zz][Pp]*T_1to6[Xx][Aa]-T_1to6[Xx][Pp]*T_1to6[Zz][Aa];
A）y
JBasetoEnd[2][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Aa]-T_1to6[Yy][Pp]*T_1to6[Xx][Aa];
A）z
voidBaseOutput（doubleBaseInput[6],doubleOutput[6]）//Output为角速度!
doubleEndInput[6];
MLMatMulti_3（JBasetoEnd,BaseInput,EndInput）;
EndOutput（EndInput,Output）;
voidSetInput（doubleInput[6]）
mInput[i]=Input[i];
voidSetMode（intmode）
mMode=mode;
voidGetOutput（intmode=BASE）
in
升级会员 