At the end of conveyor belt, depth camera sensor is installed to detect the box’s color and position to send the information to robot arm controller.
The dobot, namedRobotic arm picks the box at the end of the conveyor belt and place the box on the green or red table, according to the color of the box (e.g., green box to Green Table, and red box to Red table).
The robot arm places all the red boxes at the same co-ordinates on the red table and places the green boxes on the same co-ordinates on the green table. This causes the boxes to collide into each other. How can I fix this issue?
Code: Select all
[/--RobotArm
function sysCall_init()
flagSet = false
homePose = {0 *math.pi/180, 0*math.pi/180, 0*math.pi/180, 0*math.pi/180}
pickUpPose1 = {-20*math.pi/180, 0*math.pi/180, 0*math.pi/180, 0*math.pi/180}
pickUpPose2 = {-20*math.pi/180, 50*math.pi/180, 47*math.pi/180, 0*math.pi/180}
corout=coroutine.create(coroutineMain)
prevActionComplete = true
end
function sysCall_actuation()
if coroutine.status(corout)~='dead' then
local ok,errorMsg=coroutine.resume(corout)
if errorMsg then
error(debug.traceback(corout,errorMsg),2)
end
else
corout=coroutine.create(coroutineMain)
end
end
function movCallback(config, vel, accel, handles)
for i=1,#handles,1 do
if sim.isDynamicallyEnabled(handles[i]) then
sim.setJointTargetPosition(handles[i], config[i])
else
sim.setJointPosition(handles[i], config[i])
end
end
end
function moveToConfig(handles, maxVel, maxAccel, maxJerk, targetConf, enable)
local currentConf={}
for i=1,#handles,1 do
currentConf[i]=sim.getJointPosition(handles[i])
targetConf[i]=targetConf[i]
end
sim.moveToConfig(-1, currentConf, nil, nil, maxVel, maxAccel, maxJerk, targetConf, nil, movCallback, handles)
if enable then
sim.writeCustomDataBlock(gripperHandle, 'activity', 'on')
else
sim.writeCustomDataBlock(gripperHandle, 'activity', 'off')
end
end
function coroutineMain()
modelBase=sim.getObject('.')
gripperHandle=sim.getObject('./suctionCup_link2')
motorHandles = {}
for i=1,4,1 do
motorHandles[i]=sim.getObject('./motor'..i)
end
auxMotor1=sim.getObject('./auxMotor1')
auxMotor2=sim.getObject('./auxMotor2')
local vel=22
local accel=40
local jerk=80
local maxVel={vel*math.pi/180, vel*math.pi/180, vel*math.pi/180, vel*math.pi/180}
local maxAccel={accel*math.pi/180, accel*math.pi/180, accel*math.pi/180, accel*math.pi/180}
local maxJerk={jerk*math.pi/180, jerk*math.pi/180, jerk*math.pi/180, jerk*math.pi/180}
if flagSet then
moveToConfig(motorHandles, maxVel, maxAccel, maxJerk, pickUpPose1, false)
moveToConfig(motorHandles, maxVel, maxAccel, maxJerk, pickUpPose2, true)
moveToConfig(motorHandles, maxVel, maxAccel, maxJerk, pickUpPose1, true)
moveToConfig(motorHandles, maxVel, maxAccel, maxJerk, dropPose1, true)
moveToConfig(motorHandles, maxVel, maxAccel, maxJerk, dropPose2, false)
moveToConfig(motorHandles, maxVel, maxAccel, maxJerk, dropPose1, false)
moveToConfig(motorHandles, maxVel, maxAccel, maxJerk, homePose, false)
prevActionComplete = true
flagSet = false
end
end
function sysCall_joint(inData)
local jointError = 0 -- Initialize jointError to zero
if inData.handle == auxMotor1 then
local t2 = sim.getJointPosition(motorHandles[2])
local t3 = sim.getJointPosition(motorHandles[3])
jointError = t3 - t2 - inData.pos
end
if inData.handle == auxMotor2 then
local t3 = sim.getJointPosition(motorHandles[3])
jointError = -t3 - inData.pos
end
local ctrl = jointError * 20
local maxVelocity = ctrl
if (maxVelocity > inData.maxVel) then
maxVelocity = inData.maxVel
end
if (maxVelocity < -inData.maxVel) then
maxVelocity = -inData.maxVel
end
local forceOrTorqueToApply = inData.maxForce
local outData = {vel = maxVelocity, force = forceOrTorqueToApply}
return outData
end
function sampleFunc(color)
if prevActionComplete then
flagSet = true
currentBoxColor = color
homePose = {0 *math.pi/180, 0*math.pi/180, 0*math.pi/180, 0*math.pi/180}
pickUpPose1 = {-20*math.pi/180, 0*math.pi/180, 0*math.pi/180, 0*math.pi/180}
pickUpPose2 = {-20*math.pi/180, 50*math.pi/180, 47*math.pi/180, 0*math.pi/180}
-- Set drop poses based on the box color (in radians)
if color == "red" then
dropPose1 = {-110*math.pi/180, 0*math.pi/180, 0*math.pi/180, 0*math.pi/180}
dropPose2 = {-110*math.pi/180, 50*math.pi/180, 47*math.pi/180, 0*math.pi/180}
elseif color == "green" then
dropPose1 = {110*math.pi/180, 0*math.pi/180, 0*math.pi/180, 0*math.pi/180}
dropPose2 = {110*math.pi/180, 50*math.pi/180, 47*math.pi/180, 0*math.pi/180}
end
prevActionComplete = false
return flagSet
else
return false
end
end
]