enableIk=functionenable if enable then simsetObjectMatrixikTarget-1simgetObjectMatrixikTip-1 for i=1#jointHandles1 do simsetJointModejointHandlesisimjointmode_ik1 en

-- 初始化一些值 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('Vsup')

-- 设置一些 RML 向量 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}

-- 定义函数 enableIk，用于启用或禁用逆向运动学 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

-- 定义函数 pendingForGripper，用于等待夹持器完成松开或夹紧 function pendingForGripper(attachHandle, signal) if signal == true then -- 如果信号为 true，则等待夹持器完成松开 while true do child = sim.getObjectChild(attachHandle,0) -- 如果存在，返回子对象编号，否则返回 -1 if child==-1 then --sim.addStatusbarMessage(child) return 0 end wait(500) end else -- 夹紧 while true do child = sim.getObjectChild(attachHandle,0) -- 如果存在，返回子对象编号，否则返回 -1 if child~=-1 then --sim.addStatusbarMessage(child) return 0 end wait(500) end end end

-- 定义函数 loosegripper，用于夹紧或松开夹持器 function loosegripper(signal) if signal==true then -- 松开夹持器 sim.setIntegerSignal('RG2l_open', 1) else -- 夹紧夹持器 sim.setIntegerSignal('RG2l_open', 0) sim.waitForSignal('RG2lRing') sim.clearIntegerSignal('RG2lRing') end pendingForGripper(attachPoint, signal) end

-- 定义函数 backToWait，用于移动到等待位置 function backToWait(waitPos, waitQuat) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,waitPos,waitQuat,nil) end

-- 定义函数 gripCube，用于抓取方块 function gripCube(gripPos) -- 检查视觉传感器 'Vsup' local gripQuat = sim.unpackTable(sim.readCustomDataBlock(topSensor, 'data')) --sim.addStatusbarMessage(string.format('direction: %f, %f, %f, %f',gripQuat[1], gripQuat[2], gripQuat[3], gripQuat[4]))

-- 最后选择一个方向来夹取它
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,gripPos,gripQuat,nil)
wait(500)
loosegripper(false)

end

-- 定义函数 deliverCube，用于将方块交付到指定位置 function deliverCube(exchangePos, exchangeQuat) while true do -- 等待 ur5r 完成接收（清除 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) ------------------------------------------------------------------------- -- 最后，ur5l 将一个方块交付给平台，现在需要发送信号通知 ur5r 取走它 ------------------------------------------------------------------------- sim.setIntegerSignal('doorRing', 1) 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('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('Vsup')

-- 设置一些 RML 向量：
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')   -- 阻塞等待 beltRing 信号，如果收到信号，则夹取方块并清除信号以便下一次等待
        gripCube(gripPos)
        backToWait(waitPos, waitQuat)
        sim.clearIntegerSignal('beltRing')
        deliverCube(exchangePos, exchangeQuat)
    end
end
enableIk(false)

en