UR5机械臂抓取流程分析：sysCall_threadmain() 函数详解
该函数为线程主函数，主要是控制机械臂的运动和抓取动作。
在函数开始时，先初始化一些变量，包括关节的句柄、运动的参数、位置信息等等。
接着进入一个无限循环while true，不断监听信号，并根据信号进行机械臂的运动和抓取动作。
当收到arrive5信号时，首先读取相机的数据，如果有物品，则根据物品的位置进行机械臂的移动和抓取，否则直接跳过。
机械臂的移动是通过调用sim.rmlMoveToPosition函数来实现的，该函数可以控制机械臂运动的速度和加速度等参数，保证机械臂的运动平稳。抓取动作是通过调用suctionCupCmd函数来实现的，该函数可以控制机械臂上的吸盘的开合。
当完成所有的抓取任务后，机械臂会回到等待位置。如果还有物品没有处理完，则继续进行抓取；否则停止循环。
最后，关闭机械臂的运动控制。
function sysCall_threadmain()
jointHandles={-1,-1,-1,-1,-1,-1}
for i=1,6,1 do
jointHandles[i]=sim.getObjectHandle('UR5_joint'..i)
end
ikGroupHandle=sim.getIkGroupHandle('UR5')
ikTip=sim.getObjectHandle('UR5_ik')
ikTarget=sim.getObjectHandle('UR5_target')
waitHandle = sim.getObjectHandle('wait')
cam = sim.getObjectHandle('Vision_sensor0')
sensor=sim.getObjectHandle('Beltsensor')
-- Set-up some of the RML vectors:
vel=180
accel=40
jerk=80
currentVel={0,0,0,0,0,0,0}
currentAccel={0,0,0,0,0,0,0}
maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
targetVel={0,0,0,0,0,0}
ikMaxVel0={0.6,0.6,0.6,1.8}
ikMaxAccel0={0.8,0.8,0.8,0.9}
ikMaxVel={0.9,0.9,0.9,2.7}
ikMaxAccel={1.2,1.2,1.2,1.35}
ikMaxVel2={18,18,18,48}
ikMaxAccel2={24,24,24,27}
ikMaxJerk={0.6,0.6,0.6,0.8}
position = {{-0.35,1.6},{-0.35,1.675},{-0.35,1.75},
{-0.425,1.6},{-0.425,1.675},{-0.425,1.75},
{-0.5,1.6},{-0.5,1.675},{-0.5,1.75}}
tempposition = {-0.7,1.6}
occupy={}
for i=1,9,1 do
occupy[i]=0
end
tempoccupy=0
local waitPos = sim.getObjectPosition(waitHandle,-1)
waitQuat = sim.getObjectQuaternion(waitHandle,-1)
enableIk(true)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)
while true do
if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
sim.waitForSignal('arrive5')
local data = {}
data = sim.unpackTable(sim.readCustomDataBlock(cam, 'data'))
if(#data~=0) then
for i=1,#data,1 do
local size=0
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.33},waitQuat,nil)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel0,ikMaxAccel0,ikMaxJerk,{data[i][1],data[i][2],0.30},waitQuat,nil)
sim.wait(0.1)
if sim.readProximitySensor(sensor)>0 then
size=1
end
if(size==1) then
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{data[i][1],data[i][2],0.29},waitQuat,nil)
else
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{data[i][1],data[i][2],0.28},waitQuat,nil)
end
suctionCupCmd('on')
sim.wait(0.08)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.38},waitQuat,nil)
if(i==#data) then
sim.setIntegerSignal('finish5',1)
sim.clearIntegerSignal('arrive5')
end
sim.setObjectOrientation(waitHandle, -1, {0,0,-data[i][3]})
local temp = sim.getObjectQuaternion(waitHandle, -1)
local targetQuat = {temp[1], temp[2], temp[3], temp[4]}
sim.setObjectOrientation(waitHandle, -1, {0,0,0})
if(size==0) then
local n=1
for j=1,9,1 do
if(occupy[j]~=2) then
n=n+1
else
break
end
end
if(n==10) then
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],0.35},targetQuat,nil)
if(tempoccupy==0) then
height=0.04
else
height=0.04*tempoccupy+0.04
end
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],height+0.1},targetQuat,nil)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{tempposition[1],tempposition[2],height+0.01},targetQuat,nil)
sim.wait(0.08)
suctionCupCmd('off')
sim.wait(0.05)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],0.33},targetQuat,nil)
tempoccupy=tempoccupy+1
else
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.35},targetQuat,nil)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.22},targetQuat,nil)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{position[n][1],position[n][2],0.14+0.005},targetQuat,nil)
sim.wait(0.05)
suctionCupCmd('off')
sim.wait(0.05)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.33},targetQuat,nil)
occupy[n]=3
end
else
local m=1
for j=1,9,1 do
if(occupy[j]>=2) then
m=m+1
else
break
end
end
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[m][1],position[m][2],0.35},targetQuat,nil)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[m][1],position[m][2],0.22},targetQuat,nil)
if(occupy[m]==0) then
height=0.05
else
height=0.05*occupy[m]+0.05
end
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{position[m][1],position[m][2],height+0.005},targetQuat,nil)
sim.wait(0.05)
suctionCupCmd('off')
sim.wait(0.05)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[m][1],position[m][2],0.33},targetQuat,nil)
occupy[m]=occupy[m]+1
end
end
else
sim.setIntegerSignal('finish5',1)
sim.clearIntegerSignal('arrive5')
end
if(tempoccupy~=0) then
local p=tempoccupy
for i=1,p,1 do
local n=1
for j=1,9,1 do
if(occupy[j]~=2) then
n=n+1
else
break
end
end
if(n==10) then
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)
break
else
high=0.04*tempoccupy
if(high>0.3) then
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],high+0.1},waitQuat,nil)
else
--sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],0.3},waitQuat,nil)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],high+0.1},waitQuat,nil)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{tempposition[1],tempposition[2],high+0.005},waitQuat,nil)
sim.wait(0.05)
suctionCupCmd('on')
sim.wait(0.05)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],0.3},waitQuat,nil)
--sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.3},waitQuat,nil)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.22},waitQuat,nil)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{position[n][1],position[n][2],0.14+0.005},waitQuat,nil)
sim.wait(0.05)
suctionCupCmd('off')
sim.wait(0.05)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.33},waitQuat,nil)
tempoccupy=tempoccupy-1
occupy[n]=3
end
end
end
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)
else
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)
end
end
end
enableIk(false)
end
原文地址: https://www.cveoy.top/t/topic/oVEd 著作权归作者所有。请勿转载和采集!