from robodk.robolink import *
from time import sleep

RDK = Robolink()
RDK.setRunMode(RUNMODE_RUN_ROBOT)

UR5 = RDK.Item('UR5', ITEM_TYPE_ROBOT)

robot_program = RDK.Item('Reset_Simulation', ITEM_TYPE_PROGRAM)
robot_program.RunCode()
robot_program.WaitFinished()

robot_program = RDK.Item('Cup_Tool_Attach_(ATI)', ITEM_TYPE_PROGRAM)
robot_program.RunCode()
robot_program.WaitFinished()

time.sleep(2)

UR5.MoveJ(RDK.Item('Home', ITEM_TYPE_TARGET), True)

time.sleep(2)

robot_program = RDK.Item('Cup_Tool_Open_(UR5)', ITEM_TYPE_PROGRAM)
robot_program.RunCode()
robot_program.WaitFinished()

time.sleep(2)

robot_program = RDK.Item('Cup_Tool_Close_(UR5)', ITEM_TYPE_PROGRAM)
robot_program.RunCode()
robot_program.WaitFinished()

time.sleep(2)

robot_program = RDK.Item('Cup_Tool_Detach_(ATI)', ITEM_TYPE_PROGRAM)
robot_program.RunCode()
robot_program.WaitFinished()

time.sleep(2)

UR5.MoveJ(RDK.Item('Home', ITEM_TYPE_TARGET), True)