UR5机械臂抓取和堆叠Lua脚本代码解析
该脚本是一个Lua脚本，主要实现了一个UR5机械臂的控制，该机械臂通过视觉传感器和传送带传感器来完成物品的抓取和堆叠。代码主要分为三个函数：
suctionCupCmd(command)
该函数用于控制机械臂的吸盘，当参数command为'on'时，机械臂的吸盘打开；当参数command为'off'时，机械臂的吸盘关闭。
enableIk(enable)
该函数用于控制机械臂的运动学逆解器（IK），当参数enable为true时，机械臂进入IK模式；当参数enable为false时，机械臂退出IK模式。
sysCall_threadmain()
该函数是脚本的主函数，其中包含了机械臂的运动控制和物品的抓取和堆叠过程。代码首先获取了机械臂的关节句柄和IK相关的句柄，同时设置了机械臂的运动参数。接着进入了一个while循环，不断等待视觉传感器的信号，并根据传感器的信号控制机械臂的运动，完成对物品的抓取和堆叠。其中，机械臂的运动状态受到传送带传感器信号的影响，当传送带上有物品时，机械臂会将物品抓取并堆叠；当传送带上没有物品时，机械臂会将已堆叠的物品取出并重新堆叠。最后，当仿真结束时，机械臂退出IK模式。
function suctionCupCmd(command)
modelBase=sim.getObjectAssociatedWithScript(sim.handle_self)
modelName=sim.getObjectName(modelBase)
if(command=='on') then
sim.setIntegerSignal(modelName ..'_suctionCup',1)
else if(command=='off') then
sim.setIntegerSignal(modelName ..'_suctionCup',0)
end
end
end
enableIk=function(enable)
if enable then
sim.setObjectMatrix(ikTarget,-1,sim.getObjectMatrix(ikTip,-1))
for i=1,#jointHandles,1 do
sim.setJointMode(jointHandles[i],sim.jointmode_ik,1)
end
sim.setExplicitHandling(ikGroupHandle,0)
else
sim.setExplicitHandling(ikGroupHandle,1)
for i=1,#jointHandles,1 do
sim.setJointMode(jointHandles[i],sim.jointmode_force,0)
end
end
end
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/oVD4 著作权归作者所有。请勿转载和采集!