
如上图,希望机器人示教圆心位置(工具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