逐句翻译-- ON OFFfunction suctionCupCmdcommand modelBase=simgetObjectAssociatedWithScriptsimhandle_self modelName=simgetObjectNamemodelBase ifcommand==on then simsetIntegerSignalmodelName

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 -- 将ikTarget的矩阵设置为ikTip的矩阵 sim.setObjectMatrix(ikTarget,-1,sim.getObjectMatrix(ikTip,-1)) -- 遍历关节句柄，将关节模式设置为IK模式 for i=1,#jointHandles,1 do sim.setJointMode(jointHandles[i],sim.jointmode_ik,1) end -- 禁用IK组 sim.setExplicitHandling(ikGroupHandle,0) else -- 启用IK组 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..'#1') end -- 获取IK组句柄 ikGroupHandle=sim.getIkGroupHandle('UR51') -- 获取IK目标和IK末端句柄 ikTip=sim.getObjectHandle('UR5_ik1') ikTarget=sim.getObjectHandle('UR5_target1') -- 获取相机、传感器、等待句柄和机器人句柄 cam=sim.getObjectHandle('Vision_sensor0#0') sensor=sim.getObjectHandle('Beltsensor#1') waitHandle = sim.getObjectHandle('wait#0') -- 初始化一些RML向量 vel=180 accel=40 jerk=80 currentVel={0,0,0,0,0,0,0} currentAccel={0,0,0,0,0,0,0} maxVel={velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180} maxAccel={accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180} maxJerk={jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180} targetVel={0,0,0,0,0,0} -- 初始化IK最大速度、加速度和Jerk ikMaxVel={1.2,1.2,1.2,3.6} ikMaxAccel={1.6,1.6,1.6,1.8} ikMaxVel2={12,12,12,36} ikMaxAccel2={16,16,16,18} ikMaxJerk={0.6,0.6,0.6,0.8} -- 获取等待句柄的初始位置和四元数 startPos = sim.getObjectPosition(waitHandle,-1) workQuat = sim.getObjectQuaternion(waitHandle,-1) stertQuat = sim.getObjectQuaternion(ikTip,-1) -- 启用IK enableIk(true) while true do if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then -- 等待获取数据信号 sim.waitForSignal('getdata') -- 等待到达信号 sim.waitForSignal('arrive2') if(sim.getIntegerSignal('arrive2')==1) then Pioneer=sim.getObjectHandle('P1') end if(sim.getIntegerSignal('arrive2')==2) then Pioneer=sim.getObjectHandle('P2') end if(sim.getIntegerSignal('arrive2')==3) then Pioneer=sim.getObjectHandle('P3') end if(sim.getIntegerSignal('arrive2')==4) then Pioneer=sim.getObjectHandle('P4') end -- 清除到达信号 sim.clearIntegerSignal('arrive2') -- 获取相机数据 local data={} data = sim.unpackTable(sim.readCustomDataBlock(cam, 'data3')) -- 获取机器人位置 PioneerPosition= sim.getObjectPosition(Pioneer,-1) -- 计算占据矩阵 PioneerPositionMatrix={{PioneerPosition[1]+0.075,PioneerPosition[2]+0.075},{PioneerPosition[1],PioneerPosition[2]+0.075},{PioneerPosition[1]-0.075,PioneerPosition[2]+0.075}, {PioneerPosition[1]+0.075,PioneerPosition[2]},{PioneerPosition[1]-0.075,PioneerPosition[2]},{PioneerPosition[1]+0.075,PioneerPosition[2]-0.075}, {PioneerPosition[1],PioneerPosition[2]-0.075},{PioneerPosition[1]-0.075,PioneerPosition[2]-0.075},{PioneerPosition[1],PioneerPosition[2]}} local PioneerPositionOccupy={} for i=1,9,1 do PioneerPositionOccupy[i]=0 end if(#data~=0) then for i=4,1,-1 do local size=0 -- 移动IK目标到物体位置 sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.27},workQuat,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.155},workQuat,nil) else sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{data[i][1],data[i][2],0.145},workQuat,nil) end -- 开启吸盘 suctionCupCmd('on') sim.wait(0.05) -- 移动IK目标到机器人位置 sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.3},workQuat,nil) for k=5,8,1 do if(PioneerPositionOccupy[k]==0) then -- 移动IK目标到机器人占据位置 sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.3},workQuat,nil) if(size==1) then -- 根据物体大小调整移动高度 sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.29},workQuat,nil) else sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.28},workQuat,nil) end -- 关闭吸盘 suctionCupCmd('off') sim.wait(0.02) -- 移动IK目标到机器人位置 sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.3},workQuat,nil) PioneerPositionOccupy[k]=1 break end end end else sim.wait(8) end -- 发送完成信号 sim.setIntegerSignal('finish2',1) sim.setIntegerSignal('done',1) end end -- 禁用IK enableIk(false) en