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

六自由度机器人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

{

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

{

DHParam[i][0]=Angles[i];

}

GetAMat();

GetEndPose();

GetTMat();

GetEndJacobian();

GetEndInvJacobian();

GetJBasetoEnd();

}

void Inti(double DHparameter[6][4])

{

for (int i=0;i

{

for (int j=0;j

{

DHParam[i][j]=DHparameter[i][j];

}

}

GetAMat();

GetEndPose();

GetTMat();

GetEndJacobian();

GetEndInvJacobian();

GetJBasetoEnd();

for (i=0;i

{

mInput[i]=0;

mOutput[i]=0;

}

mMode=BASE;

}

void GetEndPose()

{

MLGetIdentityMat(EndPose);

for (int i=0;i

{

MLMatMulti(AMat[5-i],EndPose);

}

}

void GetEndJacobian()

{

for (int i=0;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

{

for (int j=0;j

{

cvmSet(&Mat1,i,j,EndJacobian[i][j]);

}

}

cvInvert(&Mat1,&Mat2,CV_SVD);

for (i=0;i

{

for (int j=0;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

{

for (int j=0;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

{

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;

六自由度机器人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

{

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

{

DHParam[i][0]=Angles[i];

}

GetAMat();

GetEndPose();

GetTMat();

GetEndJacobian();

GetEndInvJacobian();

GetJBasetoEnd();

}

void Inti(double DHparameter[6][4])

{

for (int i=0;i

{

for (int j=0;j

{

DHParam[i][j]=DHparameter[i][j];

}

}

GetAMat();

GetEndPose();

GetTMat();

GetEndJacobian();

GetEndInvJacobian();

GetJBasetoEnd();

for (i=0;i

{

mInput[i]=0;

mOutput[i]=0;

}

mMode=BASE;

}

void GetEndPose()

{

MLGetIdentityMat(EndPose);

for (int i=0;i

{

MLMatMulti(AMat[5-i],EndPose);

}

}

void GetEndJacobian()

{

for (int i=0;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

{

for (int j=0;j

{

cvmSet(&Mat1,i,j,EndJacobian[i][j]);

}

}

cvInvert(&Mat1,&Mat2,CV_SVD);

for (i=0;i

{

for (int j=0;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

{

for (int j=0;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

{

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;


相关内容

  • 一种可重构模块化机器人系统的运动学研究

    • 机械设计与制造 122 文章编号:lOOt一3997(2012)10-0122-03 Machinery Design&Manufacture 第10期2012年10月 一种可重构模块化机器人系统的运动学研究丰 高文斌1'2王洪光1潘新安I,2 (1．中国科学院沈阳自动化研究所机器人学重点实 ...

  • 什么是有限元

    • Chapter 1 什么是有限元 by caoer 1.1 PDE 有限元是一种求解问题的数值方法,求解什么问题呢?--求解PDE(偏微分方程). 那么PDE 是做什么用的呢?--描述客观物理世界.我想如果这两个问题搞清楚了也就明白了为什么要用fem,fem 可以做那些东西. PDE 可以描述很多物 ...

  • 带时延补偿的图像雅可比矩阵在线估计方法

    • ComputerEngineeringandApplications计算机工程与应用 2010,46(21)181 带时延补偿的图像雅可比矩阵在线估计方法 刘文芳1,邴志刚2,卢胜利2,路海龙2 LIUWen-fang1,BINGZhi-gang2,LUSheng-li2,LUHai-long2 1 ...

  • 关于显式和隐式

    • 隐式算法和显式算法是两个比较范的概念,无论是静力学问题,还是动力学问题,都可以应用这两种方法.隐式方法包括Newton-Raphsonmethod,Tangetial stiffness method,radial return method等等:显式算法包括central difference m ...

  • 机器人学复习题

    • *(一)概念 1. 什么是机器人? 科幻作家阿西莫夫机器人三原则:1.不伤害人类:2.在原则下服从人给出的命令:3.在与上两个原则不矛盾的前提下保护自身. 我国科学家对机器人的定义是:"机器人是一种自动化的机器,所不同的是这种机器具备一些与人或生物相似的智能能力,如感知能力.规划能力.动作 ...

  • 非线性系统控制论文

    • 非线性系统的状态反馈线性化 汪赛 (北京信息科技大学 自动化学院,北京 100192) 摘要:本文从微分几何概念入手,介绍了单输入仿射非线性系统状态反馈线性化成立的条件与多输入仿射非线性系统状态反馈线性化,并以锅炉电机为例利用多输入多输出状态反馈线性化理论将其转化为一个线性系统.最后指出非线性系统状 ...

  • 矩阵微积分

    • 在网上看到有人贴了如下求导公式: Y = A * X --> DY/DX = A' Y = X * A --> DY/DX = A Y = A' * X * B --> DY/DX = A * B' Y = A' * X' * B --> DY/DX = B * A' 于是把 ...

  • 机器人正运动学方程的D-H表示法

    • 2.7 机器人正运动学方程的D-H 表示法 在1955年,Denavit 和Hartenberg 在"ASME Journal of Applied Mechanics"发表了一篇论文,后来利用那个这篇论文来对机器人进行表示和建模,并导出了它们的运动方程,这已成为表示机器人和对机 ...

  • 雅克比迭代法

    • 雅克比迭代法的应用 一． 题目:用雅克比迭代法求解下列方程组 10x1x22x37.2x110x22x38.3 xx5x4.2123 二．引言: 用雅克比迭代法对方程组进行求解是,突出算法简单的优点,因此编程比较简单,但是应用此法求解方程组是也有很大的局限性,如它要求 ...

热门内容

标签

调研报告格式及范文   个人年终工作总结范文大全   求保证书范文和格式   高中物理教案范文   单位迟到检讨书范文   销售实习月小结范文   质量保证书范文   往来公函范文   销售会议感想范文   任职书范文   桥梁顶岗实习对安全意识的收货   项目公示范文   申请牧路修路