UR5L机械臂抓取视觉传感器检测到的红绿色块并放到指定位置

function enableIk(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 pendingForGripper(attachHandle, signal) if signal == true then -- if true ,wait for gripper finish loosing while true do child = sim.getObjectChild(attachHandle,0) -- if exist, return child Number, otherwise -1 if child==-1 then --sim.addStatusbarMessage(child) return 0 end wait(500) end else -- tighting while true do child = sim.getObjectChild(attachHandle,0) -- if exist, return child Number, otherwise -1 if child~=-1 then --sim.addStatusbarMessage(child) return 0 end wait(500) end end end

function loosegripper(signal) if signal==true then -- loose gripper sim.setIntegerSignal('RG2l_open', 1) else -- tighten gripper sim.setIntegerSignal('RG2l_open', 0) sim.waitForSignal('RG2lRing') sim.clearIntegerSignal('RG2lRing') end pendingForGripper(attachPoint, signal) end

function backToWait(waitPos, waitQuat) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,waitPos,waitQuat,nil) end

function gripCube(gripPos) -- check vision sensor 'Vision_sensor'
local detectedObjects = sim.readVisionSensor(topSensor) local objectCount = detectedObjects[2] for i=1,objectCount do local objectName = detectedObjects[7][i] if objectName == 'redCube' or objectName == 'greenCube' then local objectPos = detectedObjects[2i+1] local objectQuat = detectedObjects[2i+2] sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,objectPos,objectQuat,nil) wait(500) loosegripper(false) break end end end

function deliverCube(exchangePos, exchangeQuat) while true do -- wait for ur5r to finish receiving(clearing the doorRing) local s = sim.getIntegerSignal('doorRing') if s == nil then break end wait(500) end sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{exchangePos[1], exchangePos[2], exchangePos[3]+0.03}, exchangeQuat,nil) loosegripper(true) ------------------------------------------------------------------------- -- finally, ur5l delievered a cube to the platform and now needs to -- send a signal to notify ur5r to get it ------------------------------------------------------------------------- sim.setIntegerSignal('doorRing', 1) end

function sysCall_threadmain() -- Initialize some values: jointHandles={-1,-1,-1,-1,-1,-1} for i=1,6,1 do jointHandles[i]=sim.getObjectHandle('UR5_joint'..i) end

ikGroupHandle=sim.getIkGroupHandle('UR5L_ik')
ikTip=sim.getObjectHandle('UR5L_ikTip')
attachPoint = sim.getObjectHandle('RG2_attachPoint')
ikTarget=sim.getObjectHandle('UR5L_ikTarget')
exchange=sim.getObjectHandle('exchangePos')
waitHandle = sim.getObjectHandle('waitPosL')
topSensor=sim.getObjectHandle('Vision_sensor')

-- Set-up some of the RML vectors:
ikMaxVel={0.4,0.4,0.4,1.8}
ikMaxAccel={0.8,0.8,0.8,0.9}
ikMaxJerk={0.6,0.6,0.6,0.8}

local waitPos = sim.getObjectPosition(waitHandle,-1)
local waitQuat = sim.getObjectQuaternion(waitHandle,-1)
local gripPos = sim.getObjectPosition(ikTarget,-1)
local gripQuat = sim.getObjectQuaternion(ikTarget,-1)
local exchangePos = sim.getObjectPosition(exchange, -1)
local exchangeQuat = sim.getObjectQuaternion(exchange,-1)

enableIk(true)
if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
    loosegripper(true)
    while true do
        backToWait(waitPos, waitQuat)
        sim.waitForSignal('beltRing')   -- block for beltRing, ifso grip cube 
                                        -- and clear signal for next waiting
        gripCube(gripPos)
        backToWait(waitPos, waitQuat)
        sim.clearIntegerSignal('beltRing')
        deliverCube(exchangePos, exchangeQuat)
    end
end
enableIk(false)

end