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

作者：想飞的猪

说明：MLGetIdentityMat为获得单位矩阵函数

MLMatMulti为矩阵相乘函数

和OpenCV求逆矩阵函数cvInvert没有给出请大家自己写一下！很简单的！

typedef struct RobotJacobian6
{
//变量!
 //各关节传递矩阵!
 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参数表!
 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];//该矩阵的姿态与基坐标一致，位置与末端坐标一致!
                     //以便可以按照基坐标进行平动和绕基坐标轴方向转动!
 
 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;