UR5 Robotic Arm with Suction Cup for Object Picking and Placement on Mobile Robot
-- This is a Lua script for a UR5 robotic arm with a suction cup end-effector to pick up objects and place them on a mobile robot. -- The robotic arm uses inverse kinematics to move to the position of the objects and the mobile robot. -- The mobile robot is represented by a Pioneer_p3dx model and can be in one of four places. -- The robotic arm is controlled by signals from a vision sensor that detects objects and their positions.
-- Function to enable or disable inverse kinematics for the robotic arm function enableIk(enable) sim.setObjectInt32Parameter(ikGroupHandle,sim.objintparam_visibility_layer,(1-enable)256) sim.setObjectInt32Parameter(ikTip,sim.objintparam_visibility_layer,enable256+1) end
-- Function to turn the suction cup on or off function suctionCupCmd(cmd) if (cmd=='on') then sim.setIntegerSignal('suctionPadState',1) else sim.setIntegerSignal('suctionPadState',0) end end
-- Main function for the thread function sysCall_threadmain() -- Get the handles for the joints of the robotic arm jointHandles={-1,-1,-1,-1,-1,-1} for i=1,6,1 do jointHandles[i]=sim.getObjectHandle('UR5_joint'..i..'#1') end
-- Get the handles for the IK group, the IK tip, the IK target, the vision sensor, the belt sensor, and the wait object
ikGroupHandle=sim.getIkGroupHandle('UR51')
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')
-- Set-up some of the RML vectors for the robotic arm
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}
-- Set-up some of the RML vectors for the IK
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}
-- Get the starting position and quaternion of the wait object and the quaternion of the IK tip
startPos = sim.getObjectPosition(waitHandle,-1)
workQuat = sim.getObjectQuaternion(waitHandle,-1)
stertQuat = sim.getObjectQuaternion(ikTip,-1)
-- Enable inverse kinematics for the robotic arm
enableIk(true)
-- Infinite loop to control the robotic arm
while true do
-- Wait for signals to start moving the robotic arm
if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
sim.waitForSignal('getdata')
sim.waitForSignal('arrive2')
-- Get the handle of the mobile robot based on the signal received
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
-- Clear the signal for the mobile robot handle
sim.clearIntegerSignal('arrive2')
-- Get the data about the objects and their positions from the vision sensor
local data={}
data = sim.unpackTable(sim.readCustomDataBlock(cam, 'data3'))
-- Get the position of the mobile robot
PioneerPosition= sim.getObjectPosition(Pioneer,-1)
-- Define the positions around the mobile robot where objects can be placed
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]}}
-- Define an array for checking which positions around the mobile robot are occupied
local PioneerPositionOccupy={}
for i=1,9,1 do
PioneerPositionOccupy[i]=0
end
-- If there are objects to pick up, move the robotic arm to their positions and pick them up
if(#data~=0) then
for i=4,1,-1 do
local size=0
-- Move the robotic arm to the position of the object
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.27},workQuat,nil)
-- Check if the object can be picked up based on the belt sensor
sim.wait(0.1)
if sim.readProximitySensor(sensor)>0 then
size=1
end
-- Move the robotic arm to the position of the object again at a lower height
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
-- Turn on the suction cup to pick up the object
suctionCupCmd('on')
sim.wait(0.05) --S
-- Move the robotic arm to the mobile robot's position
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.3},workQuat,nil)
-- Find an unoccupied position around the mobile robot to place the object
for k=5,8,1 do
if(PioneerPositionOccupy[k]==0) then
-- Move the robotic arm to the position around the mobile robot
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.3},workQuat,nil)
-- Check if the object can be placed based on the belt sensor
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
-- Turn off the suction cup to release the object
suctionCupCmd('off')
sim.wait(0.02)
-- Move the robotic arm back to the mobile robot's position
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.3},workQuat,nil)
-- Mark the position around the mobile robot as occupied
PioneerPositionOccupy[k]=1
break
end
end
end
else
-- If there are no objects to pick up, wait for 8 seconds before checking again
sim.wait(8)
end
-- Signal that the robotic arm has finished its task and wait for the signal to be cleared before continuing
sim.setIntegerSignal('finish2',1)
sim.setIntegerSignal('done',1)
end
end
-- Disable inverse kinematics for the robotic arm
enableIk(false)
end
原文地址: https://www.cveoy.top/t/topic/oWlI 著作权归作者所有。请勿转载和采集!