    sim=require'sim'

    function moveToConfig(handles,maxVel,maxAccel,maxJerk,targetConf,enable)
        for i=1,#handles,1 do
            targetConf[i]=targetConf[i]*math.pi/180
        end
        local params = {
            joints = handles,
            targetPos = targetConf,
            maxVel = maxVel,
            maxAccel = maxAccel,
            maxJerk = maxJerk,
        }
        sim.moveToConfig(params)

        if enable then
            sim.writeCustomStringData(gripperHandle,'activity','on')
        else
            sim.writeCustomStringData(gripperHandle,'activity','off')
        end
    end

    function sysCall_thread()
        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}

        while true do
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{0,40,45,0},false) -- menuju cuboid1
            sim.writeCustomStringData(gripperHandle,'activity','on') -- ambil
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{30,10,20,0},true) -- angkat
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{-20,30,10,0},true) -- ke bowl
            sim.writeCustomStringData(gripperHandle,'activity','off') -- lepas

            -- Cuboid 2
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{10,25,35,0},false)
            sim.writeCustomStringData(gripperHandle,'activity','on')
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{30,10,20,0},true)
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{-20,30,10,0},true)
            sim.writeCustomStringData(gripperHandle,'activity','off')

            -- Cuboid 3
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{15,30,40,0},false)
            sim.writeCustomStringData(gripperHandle,'activity','on')
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{30,10,20,0},true)
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{-20,30,10,0},true)
            sim.writeCustomStringData(gripperHandle,'activity','off')

            -- Cuboid 4
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{20,35,45,0},false)
            sim.writeCustomStringData(gripperHandle,'activity','on')
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{30,10,20,0},true)
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{-20,30,10,0},true)
            sim.writeCustomStringData(gripperHandle,'activity','off')

            -- Cuboid 5
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{25,40,50,0},false)
            sim.writeCustomStringData(gripperHandle,'activity','on')
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{30,10,20,0},true)
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{-20,30,10,0},true)
            sim.writeCustomStringData(gripperHandle,'activity','off')

            -- kembali ke home
            moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{0,0,0,0},false)
        end
    end

    function sysCall_joint(inData)
        if inData.handle==auxMotor1 then
            local t2=sim.getJointPosition(motorHandles[2])
            local t3=sim.getJointPosition(motorHandles[3])
            error=t3-t2-inData.pos
        end
        if inData.handle==auxMotor2 then
            local t3=sim.getJointPosition(motorHandles[3])
            error=-t3-inData.pos
        end

        local ctrl=error*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