# Type help("robolink") or help("robodk") for more information
# Press F5 to run the script
# Note: you do not need to keep a copy of this file, your python script is saved with the station
from robolink import *    # API to communicate with RoboDK
from robodk import *      # basic matrix operations

# Use RoboDK API as RL
RDK = Robolink()

# define default approach distance
APPROACH = 100

# gather robot, tool and reference frames from the station
robot               = RDK.Item('UR10e', ITEM_TYPE_ROBOT)
tool                = RDK.Item('Grijper', ITEM_TYPE_TOOL)
frame_pallet        = RDK.Item('PalletFrame', ITEM_TYPE_FRAME)
null_position_holder = RDK.Item('NullPositionHolder', ITEM_TYPE_FRAME)

# gather targets
target_pallet_safe = RDK.Item('PalletApproach', ITEM_TYPE_TARGET)
target_null_safe = RDK.Item('NullPositionApproach', ITEM_TYPE_TARGET)


# get variable parameters
SIZE_SLOTKLOS = RDK.getParam('SizeSlotKlos')
SIZE_PALLET = RDK.getParam('SizePallet')
SIZE_SLOTKLOS_XYZ = [float(x.replace(' ','')) for x in SIZE_SLOTKLOS.split(',')]
SIZE_PALLET_XYZ = [float(x.replace(' ','')) for x in SIZE_PALLET.split(',')]
SIZE_SLOTKLOS_Z = SIZE_SLOTKLOS_XYZ[2] # the height of the boxes is important to take into account when approaching the positions

def box_calc(size_xyz, pallet_xyz):
    """Calculates a list of points to store parts in a pallet"""
    [size_x, size_y, size_z] = size_xyz
    [pallet_x, pallet_y, pallet_z] = pallet_xyz    
    xyz_list = []
    for h in range(int(pallet_z)):
        for j in range(int(pallet_y)):
            for i in range(int(pallet_x)):
                xyz_list = xyz_list + [[(i+0.5)*size_x, (j+0.5)*size_y, (h+0.5)*size_z]]
    return xyz_list

def TCP_On(toolitem):
    """Attaches the closest object to the toolitem Htool pose,
    It will also output appropriate function calls on the generated robot program (call to TCP_On)"""
    toolitem.AttachClosest()
    toolitem.RDK().RunMessage('Set air valve on')
    toolitem.RDK().RunProgram('TCP_On()');
        
def TCP_Off(toolitem, itemleave=0):
    """Detaches the closest object attached to the toolitem Htool pose,
    It will also output appropriate function calls on the generated robot program (call to TCP_Off)"""
    toolitem.DetachAll(itemleave)
    toolitem.RDK().RunMessage('Set air valve off')
    toolitem.RDK().RunProgram('TCP_Off()');

# calculate an array of positions to get/store the parts
parts_positions = box_calc(SIZE_SLOTKLOS_XYZ, SIZE_PALLET_XYZ)

# Calculate a new TCP that takes into account the size of the part (targets are set to the center of the box)
tool_xyzrpw = tool.PoseTool()*transl(0,0,SIZE_SLOTKLOS_Z/2)
tool_tcp = robot.AddTool(tool_xyzrpw, 'TCP A')

robot.setPoseTool(tool_tcp)
nparts = len(parts_positions)
i = nparts-1

while i >= 0:
    # ----------- take a part from the pallet ------
    # get the xyz position of part i
    robot.setPoseFrame(frame_pallet)
    part_position_i = parts_positions[i]
    target_i = transl(part_position_i)*rotx(pi)*rotz(0.5*pi)
    target_i_app = target_i * transl(0,0,-(APPROACH+SIZE_SLOTKLOS_Z))    
    # approach to the pallet
    robot.MoveJ(target_pallet_safe)
    # get the box i
    robot.MoveJ(target_i_app)
    robot.MoveL(target_i)
    TCP_On(tool) # attach the closest part
    robot.MoveL(target_i_app)
    robot.MoveJ(target_pallet_safe)

    # ----------- place the box i on the convegor ------
    # approach to the conveyor
    robot.setPoseFrame(null_position_holder)
    target_conv_pose = null_position_holder.Pose()
    target_conv_app = target_conv_pose
    robot.MoveJ(target_null_safe)
    #robot.MoveJ(target_conv_pose)
   
    TCP_Off(tool, null_position_holder) # detach an place the object in the moving reference frame of the conveyor
       
    i = i - 1