from robodk.robolink import *
from robodk.robodialogs import *
from robodk.robofileio import *
from robodk.robomath import *


#----------------------------
# Global variables:

# LOAD_AS_PROGRAM flag:
# Set to True to generate a program in the UI: we can modify targets manually and properly see the program
# Set to False, it will simulate or generate the robot program directly when running the macro
LOAD_AS_PROGRAM = True

# Set the name of the reference frame to place the targets:
REFERENCE_NAME = 'UR3e Base'

# Set the name of the reference target
# (orientation will be kept constant with respect to this target)
TARGET_NAME = 'Home'

#---------------------------
# Start the RoboDK API
RDK = Robolink()

target1 = RDK.Item('Target 1')

Home = RDK.Item('Home')



data1=[[0,0,0,0,0,pi],
       [0,0,0,0,0,0],
       [0,0,0,0,0,0]]


data2=[[0,0,0,0,0,0],
       [0,0,0,0,0,0],
       [0,0,0,0,0,0]]



# Select the robot (the popup is diplayed only if there are 2 or more robots)
robot = RDK.ItemUserPick('Select a robot', ITEM_TYPE_ROBOT)


# Get the reference frame to generate the path
frame = RDK.Item(REFERENCE_NAME, ITEM_TYPE_FRAME)
print('Frame Fifi: ',frame)
if not frame.Valid():
    raise Exception("Reference frame not found. Use name: %s" % REFERENCE_NAME)

# Use the home target as a reference
target = RDK.Item(TARGET_NAME, ITEM_TYPE_TARGET)
print("Target à atteindre:",target)
if not target.Valid():
    raise Exception("Home target is not valid. Set a home target named: %s" % TARGET_NAME)

# Set the robot to the home position
robot.setJoints(target.Joints())

robot.MoveJ(Home)
# Get the pose reference from the home target

pose_ref = robot.Pose()


if LOAD_AS_PROGRAM:


    # Remember the speed so that we don't set it with every instruction
    current_speed = 1000
    target = None

    # Iterate through all the points
    for i in range(len(data1)):
        posei=pose_ref*transl(data1[i][0],data1[i][1],data1[i][2])*rotz(data1[i][5])
        print("Declacement",data1[i])
        robot.MoveJ(posei)

        posei=pose_ref*transl(data2[i][0],data2[i][1],data2[i][2])*rotz(data2[i][5])
        print("Declacement",data2[i])
        robot.MoveJ(posei)






print("Done")