六自由度機(jī)器人Jacobian雅克比矩陣計(jì)算類

1、六自由度機(jī)器人 Jacobian（ 雅克比 ） 矩陣計(jì)算類六自由度機(jī)器人 Jacobian（ 雅克比 ） 矩陣計(jì)算類！作者：想飛的豬說(shuō)明： MLGetIdentityMat 為獲得單位矩陣函數(shù)MLMatMulti 為矩陣相乘函數(shù)typedef和OpenCV求逆矩陣函數(shù)cvlnvert沒有給出請(qǐng)大家自己寫一下！很簡(jiǎn)單的! struct RobotJacobian6/ 變量 !/ 各關(guān)節(jié)傳遞矩陣 !unionstructdouble AMat644;double A0to144;double A1to244;double A2to344;double A3to444;double A4to544;

2、double A5to644;unionstructdouble TMat644;structdouble T0to644;double T1to644;double T2to644;double T3to644;double T4to644;double T5to644;/ 末端位姿 !double EndPose44;/D-H 參數(shù)表 !double DHParam64;/順序?yàn)椋?Angle d_L a_L a_A!/ 雅克比矩陣 !double EndJacobian66;/ 逆雅克比矩陣 !double EndInvJacobian66;/ 基坐標(biāo)的笛卡爾微分運(yùn)動(dòng)到末端坐標(biāo)的傳遞矩陣

3、 !double JBasetoEnd66;double T_1to644;/ 該矩陣的姿態(tài)與基坐標(biāo)一致，位置與末端坐標(biāo)一致 ! / 以便 可以按照基坐標(biāo)進(jìn)行平動(dòng)和繞基坐標(biāo)軸方向轉(zhuǎn)動(dòng) !double mInput6; /輸入 !double mOutput6;/輸出 !int mMode;void GetAMat()for (int i=0;iMLGetDHTransMat(AMati,DHParami0,DHParami1,DHParami2,DHParami3);void GetTMat()MLGetIdentityMat(T5to6);MLMatMulti(AMat5,T5to6);ML

4、MatMulti(AMat4,T5to6,T4to6);MLMatMulti(AMat3,T4to6,T3to6);MLMatMulti(AMat2,T3to6,T2to6);MLMatMulti(AMat1,T2to6,T1to6);MLMatMulti(AMat0,T1to6,T0to6);void UpdateAngle(double Angles6)/Angles為弧度 !for (int i=0;iDHParami0=Anglesi;GetAMat();GetEndPose();GetTMat();GetEndJacobian();GetEndInvJacobian();GetJBa

5、setoEnd();void Inti(double DHparameter64)for (int i=0;ifor (int j=0;jDHParamij=DHparameterij;GetAMat();GetEndPose();GetTMat();GetEndJacobian();GetEndInvJacobian();GetJBasetoEnd();for (i=0;imInputi=0;mOutputi=0;mMode=BASE;void GetEndPose()MLGetIdentityMat(EndPose);for (int i=0;iMLMatMulti(AMat5-i,End

6、Pose);void GetEndJacobian()for (int i=0;iEndJacobian0i=-1*TMatiXxNn*TMatiYyPp+TMatiYyNn*TMatiXxPp;EndJacobian1i=-1*TMatiXxOo*TMatiYyPp+TMatiYyOo*TMatiXxPp;EndJacobian2i=-1*TMatiXxAa*TMatiYyPp+TMatiYyAa*TMatiXxPp;EndJacobian3i=TMatiZzNn;EndJacobian4i=TMatiZzOo;EndJacobian5i=TMatiZzAa;void GetEndInvJa

7、cobian()double Data136;CvMat Mat1 = cvMat( 6,6,CV_64FC1,Data1);double Data236;CvMat Mat2 = cvMat( 6,6,CV_64FC1,Data2); for (int i=0;ifor (int j=0;jcvmSet(&Mat1,i,j,EndJacobianij);cvInvert(&Mat1,&Mat2,CV_SVD);for (i=0;ifor (int j=0;jEndInvJacobianij=cvmGet(&Mat2,i,j);為角速度 ! void EndOu

8、tput(double Input6, double Output6)/OutputMLMatMulti_3(EndInvJacobian,Input,Output);void GetJBasetoEnd()double TransMat44;MLGetIdentityMat(TransMat);TransMat03=-1*EndPose03;TransMat13=-1*EndPose13;TransMat23=-1*EndPose23;MLMatMulti(TransMat,EndPose,T_1to6);JBasetoEnd00=T_1to6XxNn; JBasetoEnd01=T_1to

9、6YyNn;JBasetoEnd02=T_1to6ZzNn;JBasetoEnd10=T_1to6XxOo; JBasetoEnd11=T_1to6YyOo;JBasetoEnd12=T_1to6ZzOo;JBasetoEnd20=T_1to6XxAa; JBasetoEnd21=T_1to6YyAa;JBasetoEnd22=T_1to6ZzAa;for (int i=3;ifor (int j=0;jJBasetoEndij=0;JBasetoEnd33=T_1to6XxNn; JBasetoEnd34=T_1to6YyNn;JBasetoEnd35=T_1to6ZzNn;JBasetoE

10、nd43=T_1to6XxOo; JBasetoEnd44=T_1to6YyOo;JBasetoEnd45=T_1to6ZzOo;JBasetoEnd53=T_1to6XxAa; JBasetoEnd54=T_1to6YyAa;JBasetoEnd55=T_1to6ZzAa;JBasetoEnd03=T_1to6YyPp*T_1to6ZzNn-T_1to6ZzPp*T_1to6YyNn;/(Px N)xJBasetoE nd04=T_1to6ZzPp*T_1to6XxN n-T_1to6XxPp*T_1to6ZzNn;/(Px N)yJBasetoEnd05=T_1to6XxPp*T_1to6

11、YyNn-T_1to6YyPp*T_1to6XxNn;/(P x N)zJBasetoEnd13=T_1to6YyPp*T_1to6ZzOo-T_1to6ZzPp*T_1to6YyOo;/(Px O)xJBasetoEnd14=T_1to6ZzPp*T_1to6XxOo-T_1to6XxPp*T_1to6ZzOo;/(Px O)yJBasetoEnd15=T_1to6XxPp*T_1to6YyOo-T_1to6YyPp*T_1to6XxOo;/(Px O)zJBasetoEnd23=T_1to6YyPp*T_1to6ZzAa-T_1to6ZzPp*T_1to6YyAa;/(Px A)xJBas

12、etoEnd24=T_1to6ZzPp*T_1to6XxAa-T_1to6XxPp*T_1to6ZzAa;/(P x A)yJBasetoEnd25=T_1to6XxPp*T_1to6YyAa-T_1to6YyPp*T_1to6XxAa;/(P x A)z為角速度 !void BaseOutput(double BaseInput6, double Output6)/Outputdouble EndInput6;MLMatMulti_3(JBasetoEnd,BaseInput,EndInput);EndOutput(EndInput,Output);void SetInput(double Input6)for (int i=0;imInputi=Inputi;void SetMode(int mode)mMode=mode;void GetOutput(int mode=BASE)i