public static double[] JPOS2Array(JPOStyp JPOS)
{
double[] J = new double[4];
J[0] = JPOS.J1;
J[1] = JPOS.J2;
J[2] = JPOS.J3;
J[3] = JPOS.J4;
return(J);
}
//重载“-”，（按比例缩放）
public static JPOStyp operator -(JPOStyp JPos1, JPOStyp JPos2)
{
JPOStyp temp = new JPOStyp();
temp.J1 = JPos1.J1 - JPos2.J1;
temp.J2 = JPos1.J2 - JPos2.J2;
temp.J3 = JPos1.J3 - JPos2.J3;
temp.J4 = JPos1.J4 - JPos2.J4;
return(temp);
}
//重载“*”，（按比例缩放）
public static JPOStyp operator *(double k, JPOStyp JPos)
{
JPOStyp temp = new JPOStyp();
temp.J1 = k * JPos.J1;
temp.J2 = k * JPos.J2;
temp.J3 = k * JPos.J3;
temp.J4 = k * JPos.J4;
return(temp);
}
//机器人坐标转换为关节坐标 //逆解
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 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_CYLINDER2JOINT(CPOStyp CPOS, ref JPOStyp JPOS, int FLAG); //圆柱坐标转换为关节坐标
public static extern int MF_MOVJS(JPOStyp[] JPOS, JPOStyp JV, double[] V, int NUM, int PT, int FIN); //连续PTP运动（关节空间上的样条）
public static extern int MF_MOVJ(JPOStyp JPOS1, JPOStyp JPOS2, JPOStyp JVO, double V, double TAIL, int PT); //点位运动插补
public static extern int MF_ROBOT2JOINT(HCOORDtyp HPOS, ref JPOStyp JPOS, int FLAG); //机器人坐标转换为关节坐标
public static extern int MF_PULSE2JOINT(double[] ENC, ref JPOStyp JPOS); //脉冲数转关节坐标
public static extern int MF_ISINTERFERENCE(JPOStyp JPOS, double[] PP, double[] PN); //检查是否处于干涉区内
public static extern int MF_JOINT2CYLINDER(JPOStyp JPOS, ref CPOStyp CPOS); //关节坐标转换为圆柱坐标
public static extern int MF_JOINT2ROBOT(JPOStyp JPOS, ref HCOORDtyp HPOS); //关节坐标转换为机器人坐标
//点位运动插补 //。。。。
public static int MF_MOVJ(JPOStyp JPOS1, JPOStyp JPOS2, JPOStyp JVO, double V, double TAIL)
{
//通用的单段PTP运动规划，5段法，加速度连续
//初速度指定（前段剩余），连续度指定（即规划的剩余残差，0-100%，按时间算），本段始末加速度为零
//可用于连续PTP控制，如果初速度为零，TAIL为1，即变为单独PTP运动
//全部用最大速度和加速度计算各轴时间（1阶速度曲线），取其最大者，并按给定速度V（%）放大，再按连续度TAIL（%）截取
//然后各轴按此时间做定时间运动规划
//最终结果为PVT表
//JPOS1起点，JPOS2终点，JVO初速度，V速度（%），TAIL残余度（%）
int i, rtn;
double Tl, Tcut, TK, Vr, Scut;
double[] SL, jP1, jP2, jVo, jPf = new double[4], jVf = new double[4], t = new double[3], s = new double[3];
double[,] PS = new double[4, 3]; //各轴位移段
double[,] PT = new double[4, 3]; //各轴时间段
if ((V > 1) || (V <= 0) || (TAIL >= 1) || (TAIL < 0)) //数据不合理
{
return(-1);
}
//关节角和关节速度都转为数组
SL = JPOStyp.JPOS2Array(JPOS2 - JPOS1); //位移
jP1 = JPOStyp.JPOS2Array(JPOS1);
jP2 = JPOStyp.JPOS2Array(JPOS2);
jVo = JPOStyp.JPOS2Array(JVO);
if ((Math.Abs(SL[0]) < 0.000001) && (Math.Abs(SL[1]) < 0.000001) && (Math.Abs(SL[2]) < 0.000001) && (Math.Abs(SL[3]) < 0.000001))
{
return(-1); //位移不可全为零
}
if ((Math.Abs(jVo[0]) > Vmax[0]) || (Math.Abs(jVo[1]) > Vmax[1]) || (Math.Abs(jVo[2]) > Vmax[2]) || (Math.Abs(jVo[3]) > Vmax[3]))
{
return(-1); //初速度不可大于最大速度
}
Vr = 0; //实际最高速度
Tl = 0; //时间最大值
//考虑1阶速度曲线的情况计算运动时间（为了下一步升2阶，所有Amax减半使用）
for (i = 0; i < 4; i++)
{
PLANV(SL[i], jVo[i], 0, Amax[i] / 2, Vmax[i], ref t, ref s);
PS[i, 0] = s[0]; PS[i, 1] = s[1]; PS[i, 2] = s[2];
PT[i, 0] = t[0]; PT[i, 1] = t[1]; PT[i, 2] = t[2];
if (Tl < (t[0] + t[1] + t[2])) //取其最大时间
{
Tl = t[0] + t[1] + t[2];
}
}
//按给定速度（%）放大时间
Tl = Tl / V;
Tcut = Tl * (1 - TAIL); //截断时间
for (i = 0; i < 4; i++)
{
TK = (PT[i, 0] + PT[i, 1] + PT[i, 2]) / Tl;
if (TK >= 0.000001)
{
PT[i, 0] /= TK;
PT[i, 1] /= TK;
PT[i, 2] /= TK;
}
else
{
PT[i, 0] = Tl / 2;
PT[i, 1] = 0;
PT[i, 2] = Tl / 2;
}
}
//按照统一时间重新规划1阶速度
for (i = 0; i < 4; i++)
{
if (Tcut < PT[i, 0]) //加速段就截断，只有1段
{
Scut = jVo[i] * Tcut + (PS[i, 0] - jVo[i] * PT[i, 0]) * Tcut * Tcut / PT[i, 0] / PT[i, 0];
jVf[i] = 2 * Scut / Tcut - jVo[i];
PS[i, 0] = jP1[i] + Scut; PT[i, 0] = Tcut;
PS[i, 1] = PS[i, 0]; PT[i, 1] = Tcut;
PS[i, 2] = PS[i, 0]; PT[i, 2] = Tcut;
}
else if (Tcut >= (PT[i, 0] + PT[i, 1])) //减速段截断（或者不截断）,共3段
{
Scut = PS[i, 0] + PS[i, 1] + PS[i, 2] * (1 - (Tl - Tcut) * (Tl - Tcut) / PT[i, 2] / PT[i, 2]);
if (TAIL > 0.000001)
{
jVf[i] = 2 * (PS[i, 0] + PS[i, 1] + PS[i, 2] - Scut) / (Tl - Tcut);
}
else
{
jVf[i] = 0;
}
PS[i, 0] += jP1[i];
PS[i, 1] += PS[i, 0]; PT[i, 1] += PT[i, 0];
PS[i, 2] = jP1[i] + Scut; PT[i, 2] = Tcut;
}
else //匀速段截断, 共2段
{
Scut = PS[i, 0] + PS[i, 1] * (Tcut - PT[i, 0]) / PT[i, 1];
jVf[i] = PS[i, 1] / PT[i, 1];
PS[i, 0] += jP1[i];
PS[i, 1] = PS[i, 0] + Scut; PT[i, 1] = Tcut;
PS[i, 2] = PS[i, 1]; PT[i, 2] = Tcut;
}
}
//写入PVT表。MOVJ总是写在PVT头部，然后读取清空，只有MOVJS才会产生长的PVT
PVTbusy = 1; //锁定PVT表
PVTready = 0;
for (i = 0; i < 4; i++)
{
//写入起始点
Ptab[i, PVTIndex] = jP1[i]; Ttab[i, PVTIndex] += 0;
//写入1分段
Ptab[i, PVTIndex + 1] = PS[i, 0]; Ttab[i, PVTIndex + 1] = Ttab[i, PVTIndex] + PT[i, 0];
//写入2分段
Ptab[i, PVTIndex + 2] = PS[i, 1]; Ttab[i, PVTIndex + 2] = Ttab[i, PVTIndex] + PT[i, 1];
//写入3分段
Ptab[i, PVTIndex + 3] = PS[i, 2]; Ttab[i, PVTIndex + 3] = Ttab[i, PVTIndex] + PT[i, 2];
}
PVTIndex += 3;
PVTbusy = 0;
JVF.J1 = jVf[0]; JVF.J2 = jVf[1]; JVF.J3 = jVf[2]; JVF.J4 = jVf[3];
JPF.J1 = PS[0, 2]; JPF.J2 = PS[1, 2]; JPF.J3 = PS[2, 2]; JPF.J4 = PS[3, 2];
if (TAIL == 0)
{
PVTready = 1; //计算完毕，非PT模式，PVT可读
}
return(0);
}
public static extern int MF_GETREMAIN(ref JPOStyp JPOS, ref JPOStyp JV, ref HCOORDtyp HPOS, double[] HV); //读取残余的位置和速度（用于连续过渡段）
public static extern int MF_SETCURJPOS(JPOStyp CURJPOS); //设定当前位置（用于反解中选多解等）
public static extern int MF_INITPOS(JPOStyp CURPOS); //将当前位置作为初始位置（消除上段规划残余）
public static extern int MF_IMOVL(JPOStyp JPOS, EPOStyp DIR, double V, ref JPOStyp JV); //增量直线（手动用）
public static extern int MF_JOINT2PULSE(JPOStyp JPOS, double[] ENC); //关节坐标转换为脉冲数
