MLMatMulti為矩陣相乘函數(shù)

和OpenCV求逆矩陣函數(shù)cvInvert沒有給出請大家自己寫一下！很簡單的！

typedef struct RobotJacobian6
{
//變量!
//各關節(jié)傳遞矩陣!
union
{
struct
{
double AMat[6][4][4];
};
double A0to1[4][4];
double A1to2[4][4];
double A2to3[4][4];
double A3to4[4][4];
double A4to5[4][4];
double A5to6[4][4];
};

union
{
struct
{
double TMat[6][4][4];
};
struct
{
double T0to6[4][4];
double T1to6[4][4];
double T2to6[4][4];
double T3to6[4][4];
double T4to6[4][4];
double T5to6[4][4];
};
};

//末端位姿!
double EndPose[4][4];

//D-H參數(shù)表!
double DHParam[6][4];//順序為：Angle d_L a_L a_A!

//雅克比矩陣!
double EndJacobian[6][6];
//逆雅克比矩陣!
double EndInvJacobian[6][6];

//基坐標的笛卡爾微分運動到末端坐標的傳遞矩陣!
double JBasetoEnd[6][6];
double T_1to6[4][4];//該矩陣的姿態(tài)與基坐標一致，位置與末端坐標一致!
//以便可以按照基坐標進行平動和繞基坐標軸方向轉動!

double mInput[6]; //輸入!
double mOutput[6];//輸出!
int mMode;

void GetAMat()
{
for (int i=0;i<6;i++)
{
MLGetDHTransMat(AMat[i],DHParam[i][0],DHParam[i][1],DHParam[i][2],DHParam[i][3]);
}
}

void GetTMat()
{
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);
}

void UpdateAngle(double Angles[6])//Angles為弧度!
{
for (int i=0;i<6;i++)
{
DHParam[i][0]=Angles[i];
}
GetAMat();
GetEndPose();
GetTMat();
GetEndJacobian();
GetEndInvJacobian();

GetJBasetoEnd();
}

void Inti(double DHparameter[6][4])
{
for (int i=0;i<6;i++)
{
for (int j=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;
}

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][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];
}
}

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[i][j]);
}
}

cvInvert(&Mat1,&Mat2,CV_SVD);

for (i=0;i<6;i++)
{
for (int j=0;j<6;j++)
{
EndInvJacobian[i][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[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
}

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[i]=Input[i];
}
}

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;