# 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 numpy as np
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()

target_ref.setPos([355.48, -487.43, 58.20,  0.511*180/math.pi, -3.647*180/math.pi, 2.265*180/math.pi])
# move the robot to the first point:
robot.MoveJ(target_ref)

target_ref.setPos([355.48, -487.43, 53.20,  0.511*180/math.pi, -3.647*180/math.pi, 2.265*180/math.pi])


# 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(10)
robot.MoveL(target_ref)
for i in range(400):
    target_ref = robomath.transl(1, 0, 0) * target_ref
    robot.MoveL(target_ref)


'''target_ref.setPos([355.48, -487.43, 58.20,  0.511*180/math.pi, -3.647*180/math.pi, 2.265*180/math.pi])
robot.MoveJ(target_ref)'''

'''robot.setSpeedJoints(60)
robot.MoveL([179.77, -89.42, -96.30, -78.50, 81.88, 185.50])

robot.MoveL([175.59, -90.75, -95.08, -77.81, 82.33, 181.30])

robot.MoveL([175.88, -95.31, -90.25, -78.12, 82.29, 181.60])

robot.MoveL([179.79, -94.08, -91.48, -78.66, 81.88, 185.52])

robot.MoveL([179.77, -89.42, -96.30, -78.50, 81.88, 185.50])

robot.MoveL([175.59, -90.75, -95.08, -77.81, 82.33, 181.30])

robot.MoveL([175.88, -95.31, -90.25, -78.12, 82.29, 181.60])

robot.MoveL([179.79, -94.08, -91.48, -78.66, 81.88, 185.52])'''