如上图,希望机器人示教圆心位置(工具tcp的z与坐标系的z对正),设定圆的半径以及相对坐标系z的夹角,实现上图画圆效果(过程中机器人tcp某个方向一直沿着圆轨迹的切线方向行进)。
可以使用以下JBI及lua代码
机器人JBI代码
NOP SETJOINT P000 0.000,-90.000,90.000,-90.000,90.000,0.000 // 设置joint点,让机器人一开始走到固定joint,避免6轴超限 SETPOSE V010 412.7645960,97.7591748,33.9600457,3.1409931,0.0000202,2.7051724 //设置圆心点。由于机器人执行轨迹时,tcp某个方向会一直沿着轨迹前进切线方向,所以6轴会旋转-360度,建议初值位置6轴在50度左右,避免最后超限 SET D010 50 //设置半径 SET D011 10 // 设置角度 SET D012 4 // 设置整圆分割点数 MOVJ P000 VJ=100% CR=0.0MM ACC=50 DEC=50 SET B010 0 TIMER T=1 S SET B010 1 WAIT B010=0 MOVEL V=100.0MM/S CR=0.0MM TOOL#(1) USER#(1) V011 MOVEC V=100.0MM/S CR=30.0MM TOOL#(1) USER#(1) V012 V013 MOVEC V=100.0MM/S CR=0.0MM TOOL#(1) USER#(1) V014 V011 // MOVEC V=100.0MM/S CR=10.0MM TOOL#(1) USER#(1) V016 V017 // MOVEC V=100.0MM/S CR=0.0MM TOOL#(1) USER#(1) V018 V011 MOVJ P000 VJ=100% CR=0.0MM ACC=50 DEC=50 END
对应lua代码如下
-- author: chenliao@elibot.cn -- Apr.17, 2023 sleep(0.3) function Calpoint(p_center,radius,angle,pointNum) -- 输入圆心坐标,半径,参考角度,以及需要计算的点个数,输出所有点的数组 local refFrame = {p_center[1],p_center[2],p_center[3],0,0,0} local refFrameTmp = {p_center[1],p_center[2],p_center[3],0,0,0} -- 以传入的p_center位置建立临时坐标系refFrame local pStart = {p_center[1]+radius,p_center[2],p_center[3],p_center[4],p_center[5],p_center[6]} -- 基准点沿着x方向偏移 radius作为第一个点 -- 将该点沿着y方向旋转指定角度(左乘) pRef = pose_mul({0,0,0,0,math.rad(angle),0},pStart) pRef[1] = pStart[1] pRef[2] = pStart[2] pRef[3] = pStart[3] pRefInUser = pose_mul(pose_inv(refFrame),pRef) -- 将pRef转到建立的临时坐标系下 out = {} -- 输出点的数组 for i=1,pointNum,1 do refFrameTmp[6] = math.rad(360/pointNum * (i-1)) -- 让临时坐标系旋转需要分割个数的角度 table.insert(out,pose_mul(refFrameTmp,pRefInUser)) -- 将在新临时坐标系下的pRefInUser 转到外部 end return out -- 输出多个点的数组,例如[V001,V002,V003,V004] end local point_arr = {} local ttl_point_num =4 while true do B010 = get_global_variable('B010') if(B010==1) then ttl_point_num = get_global_variable('D012')-- jbi中设置需要分割的点数 V010 = {get_global_variable('V010')} -- 获取示教的中心点坐标 D010 = get_global_variable('D010') --圆半径 D011 = get_global_variable('D011') --角度 point_arr = Calpoint(V010,D010,D011,ttl_point_num) for i=1,ttl_point_num,1 do point = point_arr[i] set_global_variable('V01'..i,point[1],point[2],point[3],point[4],point[5],point[6]) end -- 如果分割4个点,就存储到V011~V014 -- 如果分割8个点,就存储到V011~V018 set_global_variable('B010',0) -- 完成计算 end sleep(0.1) end