UR5 机械臂抓取任务线程主函数代码解析
该函数为一个线程主函数，用于控制机械臂运动和执行抓取任务。
第 1-10 行，初始化机械臂 6 个关节的句柄，以及一些变量。
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')
第 12-27 行，设置机械臂的运动参数。
-- 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}
第 29-40 行，设置货架的位置信息和占用状态，并初始化为全部未占用。
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
第 42-46 行，获取等待位置的位置和姿态信息，并开启逆运动学。
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)
第 47-50 行，将机械臂移动到等待位置。
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
第 51-84 行，循环执行抓取任务，直到收到结束信号。
第 53-54 行，等待信号触发。
第 55-62 行，获取相机传输的数据并处理。
第 63-64 行，判断传感器的信号是否为 1。
第 65-71 行，根据传感器信号执行相应的抓取动作。
第 72-76 行，根据抓取结果设置货架状态。
第 78-82 行，检查是否还有待抓取的物品，如果没有则结束抓取任务。
第 84-93 行，如果还有待抓取的物品，则将剩余物品抓取并放回货架。
第 85-86 行，依次抓取每个物品并放回货架。
第 87-94 行，将抓取的物品移动到指定位置并执行放置动作。
第 95-96 行，将货架占用状态清零。
第 98-100 行，将机械臂移动到等待位置。
第 102 行，关闭逆运动学。
原文地址: https://www.cveoy.top/t/topic/oVEg 著作权归作者所有。请勿转载和采集!