六自由度机器人Jacobian雅克比矩阵计算类.docx
- 文档编号:2852369
- 上传时间:2022-11-15
- 格式:DOCX
- 页数:6
- 大小:15.63KB
六自由度机器人Jacobian雅克比矩阵计算类.docx
《六自由度机器人Jacobian雅克比矩阵计算类.docx》由会员分享,可在线阅读,更多相关《六自由度机器人Jacobian雅克比矩阵计算类.docx(6页珍藏版)》请在冰豆网上搜索。
六自由度机器人Jacobian雅克比矩阵计算类
六自由度机器人Jacobian(雅克比)矩阵计算类!
作者:
想飞的猪
说明:
MLGetIdentityMat为获得单位矩阵函数
MLMatMulti为矩阵相乘函数
和OpenCV求逆矩阵函数cvInvert没有给出请大家自己写一下!
很简单的!
typedefstructRobotJacobian6
{
//变量!
//各关节传递矩阵!
union
{
struct
{
doubleAMat[6][4][4];
};
doubleA0to1[4][4];
doubleA1to2[4][4];
doubleA2to3[4][4];
doubleA3to4[4][4];
doubleA4to5[4][4];
doubleA5to6[4][4];
};
union
{
struct
{
doubleTMat[6][4][4];
};
struct
{
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<6;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为弧度!
{
for(inti=0;i<6;i++)
{
DHParam[i][0]=Angles[i];
}
GetAMat();
GetEndPose();
GetTMat();
GetEndJacobian();
GetEndInvJacobian();
GetJBasetoEnd();
}
voidInti(doubleDHparameter[6][4])
{
for(inti=0;i<6;i++)
{
for(intj=0;j<4;j++)
{
DHParam[i][j]=DHparameter[i][j];
}
}
GetAMat();
GetEndPose();
GetTMat();
GetEndJacobian();
GetEndInvJacobian();
GetJBasetoEnd();
for(i=0;i<6;i++)
{
mInput[i]=0;
mOutput[i]=0;
}
mMode=BASE;
}
voidGetEndPose()
{
MLGetIdentityMat(EndPose);
for(inti=0;i<6;i++)
{
MLMatMulti(AMat[5-i],EndPose);
}
}
voidGetEndJacobian()
{
for(inti=0;i<6;i++)
{
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);
for(inti=0;i<6;i++)
{
for(intj=0;j<6;j++)
{
cvmSet(&Mat1,i,j,EndJacobian[i][j]);
}
}
cvInvert(&Mat1,&Mat2,CV_SVD);
for(i=0;i<6;i++)
{
for(intj=0;j<6;j++)
{
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;i<6;i++)
{
for(intj=0;j<3;j++)
{
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];//(P×N)y
JBasetoEnd[0][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Nn]-T_1to6[Yy][Pp]*T_1to6[Xx][Nn];//(P×N)z
JBasetoEnd[1][3]=T_1to6[Yy][Pp]*T_1to6[Zz][Oo]-T_1to6[Zz][Pp]*T_1to6[Yy][Oo];//(P×O)x
JBasetoEnd[1][4]=T_1to6[Zz][Pp]*T_1to6[Xx][Oo]-T_1to6[Xx][Pp]*T_1to6[Zz][Oo];//(P×O)y
JBasetoEnd[1][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Oo]-T_1to6[Yy][Pp]*T_1to6[Xx][Oo];//(P×O)z
JBasetoEnd[2][3]=T_1to6[Yy][Pp]*T_1to6[Zz][Aa]-T_1to6[Zz][Pp]*T_1to6[Yy][Aa];//(P×A)x
JBasetoEnd[2][4]=T_1to6[Zz][Pp]*T_1to6[Xx][Aa]-T_1to6[Xx][Pp]*T_1to6[Zz][Aa];//(P×A)y
JBasetoEnd[2][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Aa]-T_1to6[Yy][Pp]*T_1to6[Xx][Aa];//(P×A)z
}
voidBaseOutput(doubleBaseInput[6],doubleOutput[6])//Output为角速度!
{
doubleEndInput[6];
MLMatMulti_3(JBasetoEnd,BaseInput,EndInput);
EndOutput(EndInput,Output);
}
voidSetInput(doubleInput[6])
{
for(inti=0;i<6;i++)
{
mInput[i]=Input[i];
}
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 自由度 机器人 Jacobian 矩阵 计算
![提示](https://static.bdocx.com/images/bang_tan.gif)