UR5机器人吸取物体并放置到Pioneer机器人上的Lua脚本代码

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) end