六自由度机器人Jacobian雅克比矩阵计算类Word文档下载推荐.docx

1、 double AMat644; ; double A0to144; double A1to244; double A2to344; double A3to444; double A4to544; double A5to644; double TMat644; double T0to644; double T1to644; double T2to644; double T3to644; double T4to644; double T5to644; /末端位姿! double EndPose44; /D-H参数表! double DHParam64;/顺序为：Angle d_L a_L a_A

2、! /雅克比矩阵! double EndJacobian66; /逆雅克比矩阵! double EndInvJacobian66; /基坐标的笛卡尔微分运动到末端坐标的传递矩阵! double JBasetoEnd66; double T_1to644;/该矩阵的姿态与基坐标一致，位置与末端坐标一致! /以便可以按照基坐标进行平动和绕基坐标轴方向转动! double mInput6; /输入! double mOutput6;/输出! int mMode; void GetAMat（） for （int i=0;i MLGetDHTransMat（AMati,DHParami0,DHParam

3、i1,DHParami2,DHParami3）; void GetTMat（） MLGetIdentityMat（T5to6）; MLMatMulti（AMat5,T5to6）; MLMatMulti（AMat4,T5to6,T4to6）; MLMatMulti（AMat3,T4to6,T3to6）; MLMatMulti（AMat2,T3to6,T2to6）; MLMatMulti（AMat1,T2to6,T1to6）; MLMatMulti（AMat0,T1to6,T0to6）; void UpdateAngle（double Angles6）/Angles为弧度! DHParami0=A

4、nglesi; GetAMat（）; GetEndPose（）; GetTMat（）; GetEndJacobian（）; GetEndInvJacobian（）; GetJBasetoEnd（）; void Inti（double DHparameter64） for （int j=0;j DHParamij=DHparameterij; for （i=0; mInputi=0; mOutputi=0; mMode=BASE; void GetEndPose（） MLGetIdentityMat（EndPose）; MLMatMulti（AMat5-i,EndPose）; void GetE

5、ndJacobian（） EndJacobian0i=-1*TMatiXxNn*TMatiYyPp+TMatiYyNn*TMatiXxPp; EndJacobian1i=-1*TMatiXxOo*TMatiYyPp+TMatiYyOo*TMatiXxPp; EndJacobian2i=-1*TMatiXxAa*TMatiYyPp+TMatiYyAa*TMatiXxPp; EndJacobian3i=TMatiZzNn; EndJacobian4i=TMatiZzOo; EndJacobian5i=TMatiZzAa; void GetEndInvJacobian（） double Data13

6、6; CvMat Mat1 = cvMat（ 6,6,CV_64FC1,Data1）; double Data236; CvMat Mat2 = cvMat（ 6,6,CV_64FC1,Data2）; cvmSet（&Mat1,i,j,EndJacobianij）; cvInvert（&Mat1,&Mat2,CV_SVD）; EndInvJacobianij=cvmGet（&Mat2,i,j）; void EndOutput（double Input6, double Output6）/Output为角速度! MLMatMulti_3（EndInvJacobian,Input,Output）;

7、 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_1to6YyNn; JBasetoEnd02=T_1to6ZzNn; JBasetoEnd10=T_1to6XxOo; JBasetoEnd11=T_1to6

8、YyOo; JBasetoEnd12=T_1to6ZzOo; JBasetoEnd20=T_1to6XxAa; JBasetoEnd21=T_1to6YyAa; JBasetoEnd22=T_1to6ZzAa; for （int i=3; JBasetoEndij=0; JBasetoEnd33=T_1to6XxNn; JBasetoEnd34=T_1to6YyNn; JBasetoEnd35=T_1to6ZzNn; JBasetoEnd43=T_1to6XxOo; JBasetoEnd44=T_1to6YyOo; JBasetoEnd45=T_1to6ZzOo; JBasetoEnd53=T

9、_1to6XxAa; JBasetoEnd54=T_1to6YyAa; JBasetoEnd55=T_1to6ZzAa; JBasetoEnd03=T_1to6YyPp*T_1to6ZzNn-T_1to6ZzPp*T_1to6YyNn;/（PN）x JBasetoEnd04=T_1to6ZzPp*T_1to6XxNn-T_1to6XxPp*T_1to6ZzNn;N）y JBasetoEnd05=T_1to6XxPp*T_1to6YyNn-T_1to6YyPp*T_1to6 XxNn;N）z JBasetoEnd13=T_1to6YyPp*T_1to6ZzOo-T_1to6ZzPp*T_1to6

10、YyOo;O）x JBasetoEnd14=T_1to6ZzPp*T_1to6XxOo-T_1to6XxPp*T_1to6ZzOo;O）y JBasetoEnd15=T_1to6XxPp*T_1to6YyOo-T_1to6YyPp*T_1to6XxOo;O）z JBasetoEnd23=T_1to6YyPp*T_1to6ZzAa-T_1to6ZzPp*T_1to6YyAa;A）x JBasetoEnd24=T_1to6ZzPp*T_1to6XxAa-T_1to6XxPp*T_1to6ZzAa;A）y JBasetoEnd25=T_1to6XxPp*T_1to6YyAa-T_1to6YyPp*T_1to6XxAa;A）z void BaseOutput（double BaseInput6, double Output6）/Output为角速度! double EndInput6; MLMatMulti_3（JBasetoEnd,BaseInput,EndInput）; EndOutput（EndInput,Output）; void SetInput（double Input6） mInputi=Inputi; void SetMode（int mode） mMode=mode; void GetOutput（int mode=BASE） in