# This macro shows an example to draw a polygon of radius R and n_sides vertices using the RoboDK API for Python
# More information about the RoboDK API here:
# https://robodk.com/doc/en/RoboDK-API.html
from robodk.robolink import *  # API to communicate with RoboDK
from robodk.robomath import *  # basic matrix operations
import math
# Any interaction with RoboDK must be done through RDK:
RDK = Robolink()

# Select a robot (popup is displayed if more than one robot is available)
# robot = RDK.ItemUserPick('Select a robot', 'KUKA KR 6 R900 sixx')
robot = RDK.Item('UR5')
if not robot.Valid():
    raise Exception('No robot selected or available')

# get the current position of the TCP with respect to the reference frame:
# (4x4 matrix representing position and orientation)
target_ref = robot.Pose()
pos_ref = target_ref.Pos()

# move the robot to the first point:
#robot.MoveJ([-12.311461, -104.158923, -109.080493, 213.507031, -2.141485, -0.267428])
#robot.MoveJ([-23.830000, -90.000000, -90.000000, -180.000000, -90.000000, 0.000000])
target_ref_ur_1 = robot.SolveFK([-33.893261, -79.980439, -114.848932, -162.149405, -42.860394, 225.336967],robot.PoseTool())
target_ref_ur_1_copy = robomath.Pose_2_UR(target_ref_ur_1)
# It is important to provide the reference frame and the tool frames when generating programs offline
#robot.setPoseFrame(robot.PoseFrame())
#robot.setPoseTool(robot.PoseTool())
robot.setRounding(
    1)  # Set the rounding parameter (Also known as: CNT, APO/C_DIS, ZoneData, Blending radius, cornering, ...)
 # Set linear speed in mm/s
#robot.setSpeed(180)
robot.setSpeed(8)
robot.MoveJ([-42.497571, -104.765776, -108.841797, -146.339777, -91.515185, 224.963716])

for i in range(80):
    target_ref_ur = robot.Pose()
    target_ref_ur_fanuc = robomath.Pose_2_Fanuc(target_ref_ur)
    Pose_with_fanuc_5_2 = [x + y for x, y in zip(target_ref_ur_fanuc, [-1.1, 0, 0, 0, 0, 0])]
    target_ref_ur_2 = robomath.Fanuc_2_Pose(Pose_with_fanuc_5_2)
    robot.MoveL(target_ref_ur_2)

target_ref_ur = robot.Pose()
target_ref_ur_fanuc = robomath.Pose_2_Fanuc(target_ref_ur)
Pose_with_fanuc_5_2 = [x + y for x, y in zip(target_ref_ur_fanuc, [0, 2.1, 0, 0, 0, 0])]
target_ref_ur_2 = robomath.Fanuc_2_Pose(Pose_with_fanuc_5_2)
robot.MoveL(target_ref_ur_2)

for i in range(30):
    target_ref_ur = robot.Pose()
    target_ref_ur_fanuc = robomath.Pose_2_Fanuc(target_ref_ur)
    Pose_with_fanuc_5_2 = [x + y for x, y in zip(target_ref_ur_fanuc, [1.1, 0, 0, 0, 0, 0])]
    target_ref_ur_2 = robomath.Fanuc_2_Pose(Pose_with_fanuc_5_2)
    robot.MoveL(target_ref_ur_2)

for i in range(5):
    target_ref_ur = robot.Pose()
    target_ref_ur_fanuc = robomath.Pose_2_Fanuc(target_ref_ur)
    Pose_with_fanuc_5_2 = [x + y for x, y in zip(target_ref_ur_fanuc, [1.1, 0, 0, 0, 0, -8])]
    target_ref_ur_2 = robomath.Fanuc_2_Pose(Pose_with_fanuc_5_2)
    robot.MoveL(target_ref_ur_2)

for i in range(45):
    target_ref_ur = robot.Pose()
    target_ref_ur_fanuc = robomath.Pose_2_Fanuc(target_ref_ur)
    Pose_with_fanuc_5_2 = [x + y for x, y in zip(target_ref_ur_fanuc, [1.1, 0, 0, 0, 0, 0])]
    target_ref_ur_2 = robomath.Fanuc_2_Pose(Pose_with_fanuc_5_2)
    robot.MoveL(target_ref_ur_2)

target_ref_ur = robot.Pose()
target_ref_ur_fanuc = robomath.Pose_2_Fanuc(target_ref_ur)
Pose_with_fanuc_5_2 = [x + y for x, y in zip(target_ref_ur_fanuc, [0, 2.1, 0, 0, 0, 0])]
target_ref_ur_2 = robomath.Fanuc_2_Pose(Pose_with_fanuc_5_2)
robot.MoveL(target_ref_ur_2)

for i in range(80):
    target_ref_ur = robot.Pose()
    target_ref_ur_fanuc = robomath.Pose_2_Fanuc(target_ref_ur)
    Pose_with_fanuc_5_2 = [x + y for x, y in zip(target_ref_ur_fanuc, [-1.1, 0, 0, 0, 0, 0])]
    target_ref_ur_2 = robomath.Fanuc_2_Pose(Pose_with_fanuc_5_2)
    robot.MoveL(target_ref_ur_2)

target_ref_ur = robot.Pose()
target_ref_ur_fanuc = robomath.Pose_2_Fanuc(target_ref_ur)
Pose_with_fanuc_5_2 = [x + y for x, y in zip(target_ref_ur_fanuc, [0, 2.1, 0, 0, 0, 0])]
target_ref_ur_2 = robomath.Fanuc_2_Pose(Pose_with_fanuc_5_2)
robot.MoveL(target_ref_ur_2)

for i in range(80):
    target_ref_ur = robot.Pose()
    target_ref_ur_fanuc = robomath.Pose_2_Fanuc(target_ref_ur)
    Pose_with_fanuc_5_2 = [x + y for x, y in zip(target_ref_ur_fanuc, [1.1, 0, 0, 0, 0, 0])]
    target_ref_ur_2 = robomath.Fanuc_2_Pose(Pose_with_fanuc_5_2)
    robot.MoveL(target_ref_ur_2)
#UR_pose = robomath.Pose_2_UR(target_ref_ur)
'''target_ref_ur.setPos(
    [210.159,-440.674,317.189, 0.893 * 180 / math.pi, 1.725 * 180 / math.pi,
     -1.877 * 180 / math.pi])'''
'''UR_pose.setPos(
    [272.383, -341.334, 271.431, -0.001, 1.621, -1.184])'''

#robot.MoveL(target_ref_ur)