计算机器人自由度的公式,六自由度机器人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];
EndJacobian[4]=TMat[Zz][Oo];
EndJacobian[5]=TMat[Zz][Aa];
}
}
void GetEndInvJacobian()
{
double Data1[36];
CvMat Mat1 = cvMat( 6,6,CV_64FC1,Data1);
double Data2[36];
CvMat Mat2 = cvMat( 6,6,CV_64FC1,Data2);
for (int i=0;i<6;i++)
{
for (int j=0;j<6;j++)
{
cvmSet(&Mat1,i,j,EndJacobian[j]);
}
}
cvInvert(&Mat1,&Mat2,CV_SVD);
for (i=0;i<6;i++)
{
for (int j=0;j<6;j++)
{
EndInvJacobian[j]=cvmGet(&Mat2,i,j);
}
}
}
void EndOutput(double Input[6], double Output[6])//Output为角速度!
{
MLMatMulti_3(EndInvJacobian,Input,Output);
}
void GetJBasetoEnd()
{
double TransMat[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 (int i=3;i<6;i++)
{
for (int j=0;j<3;j++)
{
JBasetoEnd[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
}
void BaseOutput(double BaseInput[6], double Output[6])//Output为角速度!
{
double EndInput[6];
MLMatMulti_3(JBasetoEnd,BaseInput,EndInput);
EndOutput(EndInput,Output);
}
void SetInput(double Input[6])
{
for (int i=0;i<6;i++)
{
mInput=Input;
}
}
void SetMode(int mode)
{
mMode=mode;
}
void GetOutput(int mode=BASE)
{
int i=0;
int j=0;
switch(mode)
{
case eND:
EndOutput(mInput,mOutput);
break;
case BASE:
BaseOutput(mInput,mOutput);
break;
}
}
RobotJacobian6()
{
}
RobotJacobian6(double DHparameter[6][4])
{
Inti(DHparameter);
}
}MLJacobian;