# 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('UR10')
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])

# 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)
tra_ref = robot.SolveFK([-21.593557, -79.144719, 126.877740, -47.349909, 38.527052, 45], robot.PoseTool())
robot.setSpeed(8)
robot.MoveJ([-22.021143, -81.324863, 127.348339, -45.644532, 38.110733, 45])
robot.MoveL(robot.SolveFK([-22.021143, -81.324863, 127.348339, -45.644532, 38.110733, 45], robot.PoseTool()))

#robot.MoveJ([-21.593540, -79.138666, 126.880308, -47.358529, 38.527016, 44.864549])

robot.MoveL(tra_ref)

for i in range(800):
    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.0724, -0.0724, 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, [-1.485, 1.485, 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, [0.724, 0.724, 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(7):
    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.724, 0.724, 0, 0, 0, -10])]
    target_ref_ur_2 = robomath.Fanuc_2_Pose(Pose_with_fanuc_5_2)
    robot.MoveL(target_ref_ur_2)

for i in range(18):
    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.724, 0.724, 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(7):
    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.724, 0.724, 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(18):
    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.724, 0.724, 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, [-1.485, 1.485, 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, [-0.724, -0.724, 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, [-1.485, 1.485, 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)
robot.MoveJ([-22.029730, -83.761470, 126.062909, -41.922708, 38.125026, 45])