//初始化
public static int MF_INITMATHFUN(HCOORDtyp inTOOL, double[] JV, double[] JA, double[] HOME) //初始化
{
TOOL = inTOOL;
for (int i = 0; i < 4; i++)
{
Vmax[i] = JV[i];
Amax[i] = JA[i];
HJ[i] = HOME[i];
}
return(0);
}
//初始化一个单位阵
public static HCOORDtyp MF_HCOORDINIT()
{
HCOORDtyp HCOORD = new HCOORDtyp();
HCOORD.C = COORDtyp.Robot;
HCOORD.X = 0;
HCOORD.Y = 0;
HCOORD.Z = 0;
HCOORD.R11 = 1;
HCOORD.R12 = 0;
HCOORD.R13 = 0;
HCOORD.R21 = 0;
HCOORD.R22 = 1;
HCOORD.R23 = 0;
HCOORD.R31 = 0;
HCOORD.R32 = 0;
HCOORD.R33 = 1;
return(HCOORD);
}
//齐次坐标求逆
public static HCOORDtyp inv(HCOORDtyp HCoord)
{
//由于齐次坐标矩阵的特殊性，所以求逆可以采用更简单的方式
//A(1,2,3;1,2,3)构成单位正交矩阵，而A矩阵的第4行为[0 0 0 1]所以可以用比较简单的方式求逆。
HCOORDtyp temp = new HCOORDtyp();
temp.C = HCoord.C;
//旋转分量是单位正交阵，转置即求逆
temp.R11 = HCoord.R11; temp.R12 = HCoord.R21; temp.R13 = HCoord.R31;
temp.R21 = HCoord.R12; temp.R22 = HCoord.R22; temp.R23 = HCoord.R32;
temp.R31 = HCoord.R13; temp.R32 = HCoord.R23; temp.R33 = HCoord.R33;
//平移分量
temp.X = -(HCoord.R11 * HCoord.X + HCoord.R21 * HCoord.Y + HCoord.R31 * HCoord.Z);
temp.Y = -(HCoord.R12 * HCoord.X + HCoord.R22 * HCoord.Y + HCoord.R32 * HCoord.Z);
temp.Z = -(HCoord.R13 * HCoord.X + HCoord.R23 * HCoord.Y + HCoord.R33 * HCoord.Z);
return(temp);
}
public static int PVTIndex = 0; //PVT表索引
/* 数学函数 */
//齐次坐标绕y轴旋转
private static HCOORDtyp roty(HCOORDtyp POS, double Ang)
{
//POS原位姿，Ang旋转角度
HCOORDtyp trans;
trans.C = POS.C;
trans.R11 = Math.Cos(Ang);
trans.R12 = 0;
trans.R13 = -Math.Sin(Ang);
trans.R21 = 0;
trans.R22 = 1;
trans.R23 = 0;
trans.R31 = Math.Sin(Ang);
trans.R32 = 0;
trans.R33 = Math.Cos(Ang);
trans.X = 0;
trans.Y = 0;
trans.Z = 0;
return(POS * trans);
}
//机器人坐标转换为关节坐标 //逆解
public static int MF_ROBOT2JOINT(HCOORDtyp HPOS, ref JPOStyp JPOS, int FLAG)
{
//运动学反解，工具点->关节坐标
//HPOS齐次坐标，JPOS关节坐标，FLAG对应多解标志：bit0=1对应1关节采用overhead姿态，bit1=1对应3关节低肘型，bit2=1对应5关节>0（腕上扬），bit3=1确定强制设定多解姿态（否则根据当前姿态自动设定）
double q1, q2, q3, q4, q123, x3, z3;
HPOS = HPOS * HCOORDtyp.inv(TOOL); //变换腕点坐标
q4 = Math.Atan2(-HPOS.R21, -HPOS.R22);
q123 = Math.Atan2(HPOS.R31, -HPOS.R33);
x3 = HPOS.X - Math.Sin(q123) * d[3];
z3 = HPOS.Z + Math.Cos(q123) * d[3];
q2 = Math.Acos((x3 * x3 + z3 * z3 - a[0] * a[0] - a[1] * a[1]) / (2.0 * a[0] * a[1])); //余弦定理
if ((FLAG & 0x00000008) != 0) //指定肘型
{
if ((FLAG & 0x00000002) == 0) //高肘型,q2<0
{
q2 = -q2;
}
}
else //不指定
{
if (curJPOS.J2 < 0) //按上次计算确定肘型
{
q2 = -q2;
}
}
q1 = -Math.Atan2((-z3 * a[1] * Math.Cos(q2) + x3 * a[1] * Math.Sin(q2) - z3 * a[0]), (z3 * a[1] * Math.Sin(q2) + x3 * a[1] * Math.Cos(q2) + x3 * a[0]));
q3 = q123 - q2 - q1;
JPOS.J1 = q1; JPOS.J2 = q2; JPOS.J3 = q3; JPOS.J4 = q4;
//将结果保存为当前点，作为下次多解时的选择依据，当前点可由用户随时设定
curJPOS.J1 = JPOS.J1; curJPOS.J2 = JPOS.J2; curJPOS.J3 = JPOS.J3; curJPOS.J4 = JPOS.J4;
return(0);
}
//关节坐标转换为机器人坐标
public static HCOORDtyp MF_JOINT2ROBOT(JPOStyp JPOS)
{
//运动学正解，关节坐标->工具点
HCOORDtyp HPOS = new HCOORDtyp();
HPOS.C = COORDtyp.Robot;
HPOS.X = Math.Cos(JPOS.J1) * a[0] + Math.Cos(JPOS.J1 + JPOS.J2) * a[1] + Math.Sin(JPOS.J1 + JPOS.J2 + JPOS.J3) * d[3];
HPOS.Y = 0;
HPOS.Z = Math.Sin(JPOS.J1) * a[0] + Math.Sin(JPOS.J1 + JPOS.J2) * a[1] - Math.Cos(JPOS.J1 + JPOS.J2 + JPOS.J3) * d[3];
HPOS.R11 = 0.5 * Math.Cos(JPOS.J1 + JPOS.J2 + JPOS.J3 - JPOS.J4) + 0.5 * Math.Cos(JPOS.J1 + JPOS.J2 + JPOS.J3 + JPOS.J4);
HPOS.R12 = -0.5 * Math.Sin(JPOS.J1 + JPOS.J2 + JPOS.J3 + JPOS.J4) + 0.5 * Math.Sin(JPOS.J1 + JPOS.J2 + JPOS.J3 - JPOS.J4);
HPOS.R13 = Math.Sin(JPOS.J1 + JPOS.J2 + JPOS.J3);
HPOS.R21 = -Math.Sin(JPOS.J4);
HPOS.R22 = -Math.Cos(JPOS.J4);
HPOS.R23 = 0;
HPOS.R31 = 0.5 * Math.Sin(JPOS.J1 + JPOS.J2 + JPOS.J3 + JPOS.J4) + 0.5 * Math.Sin(JPOS.J1 + JPOS.J2 + JPOS.J3 - JPOS.J4);
HPOS.R32 = -0.5 * Math.Cos(JPOS.J1 + JPOS.J2 + JPOS.J3 - JPOS.J4) + 0.5 * Math.Cos(JPOS.J1 + JPOS.J2 + JPOS.J3 + JPOS.J4);
HPOS.R33 = -Math.Cos(JPOS.J1 + JPOS.J2 + JPOS.J3);
HPOS = HPOS * TOOL; //变换工具点坐标
return(HPOS);
}
//重载“*”，（矩阵乘法）
public static HCOORDtyp operator *(HCOORDtyp firHCoord, HCOORDtyp secHCoord)
{
HCOORDtyp temp = new HCOORDtyp();
temp.C = firHCoord.C;
temp.R11 = firHCoord.R11 * secHCoord.R11 + firHCoord.R12 * secHCoord.R21 + firHCoord.R13 * secHCoord.R31;
temp.R12 = firHCoord.R11 * secHCoord.R12 + firHCoord.R12 * secHCoord.R22 + firHCoord.R13 * secHCoord.R32;
temp.R13 = firHCoord.R11 * secHCoord.R13 + firHCoord.R12 * secHCoord.R23 + firHCoord.R13 * secHCoord.R33;
temp.R21 = firHCoord.R21 * secHCoord.R11 + firHCoord.R22 * secHCoord.R21 + firHCoord.R23 * secHCoord.R31;
temp.R22 = firHCoord.R21 * secHCoord.R12 + firHCoord.R22 * secHCoord.R22 + firHCoord.R23 * secHCoord.R32;
temp.R23 = firHCoord.R21 * secHCoord.R13 + firHCoord.R22 * secHCoord.R23 + firHCoord.R23 * secHCoord.R33;
temp.R31 = firHCoord.R31 * secHCoord.R11 + firHCoord.R32 * secHCoord.R21 + firHCoord.R33 * secHCoord.R31;
temp.R32 = firHCoord.R31 * secHCoord.R12 + firHCoord.R32 * secHCoord.R22 + firHCoord.R33 * secHCoord.R32;
temp.R33 = firHCoord.R31 * secHCoord.R13 + firHCoord.R32 * secHCoord.R23 + firHCoord.R33 * secHCoord.R33;
temp.X = firHCoord.R11 * secHCoord.X + firHCoord.R12 * secHCoord.Y + firHCoord.R13 * secHCoord.Z + firHCoord.X;
temp.Y = firHCoord.R21 * secHCoord.X + firHCoord.R22 * secHCoord.Y + firHCoord.R23 * secHCoord.Z + firHCoord.Y;
temp.Z = firHCoord.R31 * secHCoord.X + firHCoord.R32 * secHCoord.Y + firHCoord.R33 * secHCoord.Z + firHCoord.Z;
return(temp);
}
public static extern int MF_EUL2TR(EPOStyp EPOS, ref HCOORDtyp HPOS); //欧拉坐标->齐次坐标
public static extern int MF_COORDTRANS(HCOORDtyp SOURCE, HCOORDtyp TRANS, ref HCOORDtyp RESULT); //坐标变换
public static extern int MF_GETREMAIN(ref JPOStyp JPOS, ref JPOStyp JV, ref HCOORDtyp HPOS, double[] HV); //读取残余的位置和速度（用于连续过渡段）
public static extern int MF_GETTOOL(ref HCOORDtyp TOOL); //读取工具坐标
public static extern int MF_TOOL2WRIST(HCOORDtyp TOOL, HCOORDtyp TRANS, ref HCOORDtyp WRIST); //已知工具点坐标求腕点坐标
public static extern int MF_MOVL(HCOORDtyp HPOS1, HCOORDtyp HPOS2, double A, double V, double AF, double W, int MOD, double HEADLEN, double TAILLEN); //空间直线插补
public static extern int MF_ROBOT2JOINT(HCOORDtyp HPOS, ref JPOStyp JPOS, int FLAG); //机器人坐标转换为关节坐标
public static extern int MF_SETTOOLCOORD(JPOStyp[] JPOS, ref HCOORDtyp TOOLCOORD); //3点建立工具坐标系
public static extern int MF_SETWORLD(HCOORDtyp WORLD); //设定世界坐标
public static extern int MF_INITMATHFUN(double[] LIMITP, double[] LIMITN, HCOORDtyp WORLD, HCOORDtyp USER, HCOORDtyp TOOL, double[] JV, double[] JA, double V, double W, double A, double AF, double[] HOME, double DT); //初始化
public static extern int MF_WRIST2TOOL(HCOORDtyp WRIST, HCOORDtyp TRANS, ref HCOORDtyp TOOL); //已知腕点坐标求工具点坐标
public static extern int MF_TR2QP(HCOORDtyp HPOS, ref QPOStyp QPOS); //齐次坐标转伪四元坐标
public static extern int MF_TR2EUL(HCOORDtyp HCOORD, ref EPOStyp EPOS, int FLAG); //齐次坐标->欧拉坐标
public static extern int MF_HCOORDINIT(ref HCOORDtyp HCOORD); //初始化一个单位阵
public static extern int MF_MOVCW(HCOORDtyp HPOS1, HCOORDtyp HPOS2, HCOORDtyp HPOS3, double A, double V, double AF, double W, int MOD, double HEAD, double TAIL); //空间圆弧插补
public static extern int MF_GETUSER(ref HCOORDtyp USER); //读取用户坐标
public static extern int MF_QP2TR(QPOStyp QPOS, ref HCOORDtyp HPOS); //伪四元坐标转齐次坐标
public static extern int MF_SETTOOL(HCOORDtyp TOOL); //设定工具坐标
public static extern int MF_GETWORLD(ref HCOORDtyp WORLD); //读取世界坐标
public static extern int MF_SETUSERCOORD(JPOStyp[] JPOS, ref HCOORDtyp USERCOORD); //3点确定用户坐标系
public static extern int MF_SETUSER(HCOORDtyp USER); //设定用户坐标
public static extern int MF_JOINT2ROBOT(JPOStyp JPOS, ref HCOORDtyp HPOS); //关节坐标转换为机器人坐标
