计算机器人自由度的公式,六自由度机器人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++)

{ </