希望示教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