计算机器人自由度的公式,六自由度机器人Jacobian(雅克比)矩阵计算类

[j];

}

}

GetAMat();

GetEndPose();

GetTMat();

GetEndJacobian();

GetEndInvJacobian();

GetJBasetoEnd();

for (i=0;i<6;i++)

{

mInput=0;

mOutput=0;

}

mMode=BASE;

}

void GetEndPose()

{

MLGetIdentityMat(EndPose);

for (int i=0;i<6;i++)

{

MLMatMulti(AMat[5-i],EndPose);

}

}

void GetEndJacobian()

{

for (int i=0;i<6;i++)

{

EndJacobian[0]=-1*TMat[Xx][Nn]*TMat[Yy][Pp]+TMat[Yy][Nn]*TMat[Xx][Pp];

EndJacobian[1]=-1*TMat[Xx][Oo]*TMat[Yy][Pp]+TMat[Yy][Oo]*TMat[Xx][Pp];

EndJacobian[2]=-1*TMat[Xx][Aa]*TMat[Yy][Pp]+TMat[Yy][Aa]*TMat[Xx][Pp];

EndJacobian[3]=TMat[Zz][Nn];

<