JBI和LUA中实现基于透传的直线插补(可任意设置速度)

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

点击显示全文
赞同0
发表评论
分享

手机扫码分享
0
349
收藏
举报
收起
登录
  • 密码登录
  • 验证码登录
还没有账号,立即注册
还没有账号,立即注册
注册
已有账号,立即登录
选择发帖板块
上传文件
举报
请选择举报理由
举报
举报说明