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

小咪咪 2022-08-29 04:21 119阅读 0赞

\[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;

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

发表评论取消回复

还没有评论，来说两句吧...

相关阅读

相关雅克比矩阵

相关一种三自由度机器人的设计（CAD+Solidworks+文档）

相关公式计算 ln

相关 java Double计算公式类

相关 qps计算公式

相关计算机器人正反运动学方程,机器人运动学方程

相关计算机器人自由度的公式,六自由度机器人Jacobian(雅克比)矩阵计算类

相关计算公式

相关百度机器人

相关雅克比公式求矩阵最大特征值（C语言版）