How to program a dobot to have dynamically changing drop poses.

[Dobot23]

Hello. I could use some help with a Lua script I am using in Coppeliasim for a project. The lua script is used to program a robot arm, the type is dobot. Within CoppeliaSim, a conveyor belt brings boxes from left to right to the robot station. The boxes are aligned one by one, and it means there is no two boxes arriving at robot station at the same time.The boxes on conveyor have two types of colors – green and red.
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
]

[fferri]

If you need to stack objects in a pick and place task, you'll need to use IK, since the drop location will have each time a different Z, and -via IK- that will result in a different robot configuration.

See simIK.getConfigForTipPose or set up a standard IK script on the robot to work directly by moving the target dummy.