from robolink import *
from robodk import *

LOCAL_PATH = None


def addUR5Robot2014350238():
    station = RDK.ActiveStation()
    robot = station.AddFile(LOCAL_PATH + 'UR5 2014350238.robot')
    station.setVisible(False)
    ur5_2014350238_base = robot.Parent()

    ur5_2014350238_base = RDK.Item('UR5 2014350238 Base', ITEM_TYPE_FRAME)
    ur5_2014350238_base.setPose(eye(4))
    ur5_2014350238_base.setVisible(False)

    ur5_2014350238 = RDK.Item('UR5 2014350238', ITEM_TYPE_ROBOT)
    ur5_2014350238.setVisible(True, visible_frame=False)

def addTooling():
    ur5_2014350238 = RDK.Item('UR5 2014350238', ITEM_TYPE_ROBOT)

    master_tool = ur5_2014350238.AddFile(LOCAL_PATH + 'Tool Files/Master Tool Geometry.STL')
    master_tool.setGeometryPose(rotz(-40 * pi / 180))
    master_tool.setPoseTool(transl(0, 0, 48.94))
    master_tool.setVisible(True, visible_frame=False)


    grinder_tool = ur5_2014350238.AddFile(LOCAL_PATH + 'Tool Files/Grinder Tool Geometry.STL')
    grinder_tool.setGeometryPose(transl(0,0,48.94) * rotz(-90 * pi / 180))
    grinder_tool.setPoseTool(transl(0,0,152.27))
    grinder_tool.setVisible(True, visible_frame=False)

RDK = Robolink()

LOCAL_PATH = RDK.getParam('PATH_OPENSTATION') + '/'

addUR5Robot2014350238()
addTooling()
