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); //关节坐标转换为脉冲数