public double PSI;//3个姿态（Z-Y-Z欧拉角）
//重载“*”，（按比例缩放）
public static EPOStyp operator *(double k, EPOStyp EPos)
{
EPOStyp temp = new EPOStyp();
temp.C = EPos.C;
temp.X = k * EPos.X;
temp.Y = k * EPos.Y;
temp.Z = k * EPos.Z;
temp.PHI = k * EPos.PHI;
temp.THETA = k * EPos.THETA;
temp.PSI = k * EPos.PSI;
return(temp);
}
//增量直线（手动用）
public static int MF_IMOVL(JPOStyp JPOS, EPOStyp DIR, double V, ref JPOStyp JV)
{
//返回当前瞬时各关节速度分量，供运动线程JOG方式使用
//JPOS为当前点，DIR为增量（方向/旋转），V为线速度或角速度，JV指明各关节速度
HCOORDtyp hpos;
JPOStyp dP, P2 = new JPOStyp();
hpos = MF_JOINT2ROBOT(JPOS); //到腕点
hpos.X = hpos.X + DIR.X / 1000; //指向前方1mm处（注意长度单位是m）
hpos.Y = hpos.Y + DIR.Y / 1000;
hpos.Z = hpos.Z + DIR.Z / 1000;
hpos = roty(hpos, DIR.THETA / 1000); //指向前方0.001弧度处。此处借用欧拉角传递数据，并非真正欧拉角，数据表示分别绕坐标系xyz轴的转动
if (MF_ROBOT2JOINT(hpos, ref P2, 0) != 0)
{
return(-1); //反解不存在
}
dP = P2 - JPOS;
JV = (V * 1000) * dP; //关节坐标增量/时间 = 各关节速度，单位rad/s
return(0);
}
public static extern int MF_EUL2TR(EPOStyp EPOS, ref HCOORDtyp HPOS); //欧拉坐标->齐次坐标
public static extern int MF_IMOVL(JPOStyp JPOS, EPOStyp DIR, double V, ref JPOStyp JV); //增量直线（手动用）
public static extern int MF_TR2EUL(HCOORDtyp HCOORD, ref EPOStyp EPOS, int FLAG); //齐次坐标->欧拉坐标