希望示教2个点(起点和终点姿态不一样),可以设定运行速度,通过透传TT实现末端直线的插补(包括姿态的SLERP平滑插补),可以使用以下jbi和lua代码
JBI代码如下:
NOP SETJOINT P000 11.6717,-82.5665,70.2302,-74.1454,73.4545,12.1825 // P000 开始点 SETJOINT P001 -47.2537,-75.8249,59.7760,-68.2886,95.2118,-47.5131 // P001 结束点 SET D000 10 // 设定运动速度 10mm/s SET D002 0.02 // 设置透传采样时间 s SET D003 0.2 // 设置透传lookahead时间 s SET B000 0 MOVJ P000 VJ=10% CR=0.000MM ACC=50 DEC=50 // 走到开始点P000 TTINIT T=0.02S LOOKAHEAD=0.2S SMOOTHNESS=1.0 // 初始化, // T表示采样时间,即机器人在执行TTTARGETJOINT指令时, // 每xx秒从对应位置变量处获取位置并存入透传队列最后 // 队列最前的数据会被剔除 // LOOKAHEAD表示前瞻时间,LOOKAHEAD/T 得到队列长度 // 例如0.2/0.02=10,即队列长度为10,机器人会对队列内的位置数据进行平滑处理 // SMOOTHNESS表示平滑度,范围0-1 // TTSTARTJOINT P000 // 检查当前位置是否是P000,如果是开始透传运动 // 机器人在开始透传时,会用当前位置将队列内所有数据填满 SET B000 2 // lua开始计算并将最新的数据填入V001 WAIT B000 =3 LABEL *TT // 透传的循环 TTTARGETJOINT V001 // 按照设定的采样时间,从V001获取新位置数据并填入队列 // 此时若有新数据进入队列,机器人就会运动 JUMP *TT IF B000 = 3 // lua重将B000置为非3,机器人停止透传运动 TTSTOP MOVJ P000 VJ=10% CR=0.000MM ACC=50 DEC=50 SET B000 0 END
lua代码如下
--author:chenliao@elibot.cn -- June.25, 2023 sleep(0.3) function checkPos(p,delta) -- 检查当前位置和目标位置p的xyz的差值是否小于delta p_curr = get_tcp_pose() if (math.abs(p_curr[1]-p[1])<delta) and (math.abs(p_curr[2]-p[2])<delta) and (math.abs(p_curr[3]-p[3])<delta) then return true else return false end end function TT_line_move(ptarget,speed) local t_sample = get_global_variable("D002") --获取采样时间 local t_lookahead = get_global_variable("D003") --获取lookahead时间 local pStart = get_tcp_pose() -- 获取开始位置的笛卡尔坐标 local dis = get_point_distance(pStart, ptarget) -- 计算首末点的距离 local ttl_time = dis/speed --根据距离和设定速度计算总时间 local count = math.floor(ttl_time/t_sample) --根据总时间和采样时间,计算插补周期数 for i = 1, count, 1 do Pout = get_interp_pose(pStart ,ptarget,i/count) --使用基于pStart,ptarget点和比例进行插补,姿态采用SLERP插补 set_global_variable("V001", Pout[1], Pout[2], Pout[3], Pout[4], Pout[5], Pout[6]) --将插补的位姿放入V001中 sleep(t_sample) end while checkPos(ptarget,2)==false do -- 如果当前位置与目标位置xyz某个差值大于2mm,继续发送目标位置 set_global_variable("V001", Pout[1], Pout[2], Pout[3], Pout[4], Pout[5], Pout[6]) sleep(t_sample) end set_global_variable("B000", 0) end set_global_variable("B000", 0) while true do B000 = get_global_variable("B000") if (B000 == 2) then speed = get_global_variable('D000') --读取速度mm/s p_curr = get_tcp_pose() set_global_variable('V001',p_curr[1],p_curr[2],p_curr[3],p_curr[4],p_curr[5],p_curr[6])--等机器人走到开始位置后,先获取一次当前位置并赋值到V001,防止位置误差 set_global_variable('B000',3) j_target = {get_global_variable('P001')} target = get_fwd_kinematics(j_target) TT_line_move(target,speed) end sleep(0.1) end