from robolink import *
from robodk import *

PATH = 'C:/RoboDK/User/'

def addUR5Robot2014350238():
    station = RDK.ActiveStation()
    station.AddFile(PATH + 'UR5 2014350238.robot')
    
    ur5_2014350238_base = RDK.Item('UR5 2014350238 Base', ITEM_TYPE_FRAME)
    ur5_2014350238_base.setPose(eye(4))
    ur5_2014350238_base.setVisible(False)

def addTooling():
    ur5_2014350238 = RDK.Item('UR5 2014350238', ITEM_TYPE_ROBOT)
    
    ur5_2014350238.AddFile(PATH + 'Tool Files/Master Tool Geometry.STL')
    master_tool_geometry = RDK.Item('Master Tool Geometry', ITEM_TYPE_OBJECT)
    ur5_2014350238.AddTool(UR_2_Pose([0, 0, 48.94, 0, 0, -0.873]), 'Master Tool')
    master_tool = RDK.Item('Master Tool', ITEM_TYPE_TOOL)
    
    ur5_2014350238.AddFile(PATH + 'Tool Files/Grinder Tool Geometry.STL')
    grinder_tool_geometry = RDK.Item('Grinder Tool Geometry', ITEM_TYPE_OBJECT)
    grinder_tool_geometry.setPose(UR_2_Pose([0, 0, 48.94, 0, 0, -0.873]))
    ur5_2014350238.AddTool(UR_2_Pose([0, 0, 152.27, 0, 0, -0.873]), 'Grinder Tool')
    grinder_tool = RDK.Item('Grinder Tool', ITEM_TYPE_TOOL)

RDK = Robolink()

addUR5Robot2014350238()
addTooling()
