6. Examples¶

# 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

# 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', ITEM_TYPE_ROBOT)
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()
print("Drawing a polygon around the target: ")
print(Pose_2_TxyzRxyz(target_ref))

# move the robot to the first point:
robot.MoveJ(target_ref)

# 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(10)  # Set the rounding parameter (Also known as: CNT, APO/C_DIS, ZoneData, Blending radius, cornering, ...)
robot.setSpeed(200)  # Set linear speed in mm/s

# Set the number of sides of the polygon:
n_sides = 6
R = 100

# make a hexagon around reference target:
for i in range(n_sides + 1):
    ang = i * 2 * pi / n_sides  #angle: 0, 60, 120, ...

    #-----------------------------
    # Movement relative to the reference frame
    # Create a copy of the target
    target_i = Mat(target_ref)
    pos_i = target_i.Pos()
    pos_i[0] = pos_i[0] + R * cos(ang)
    pos_i[1] = pos_i[1] + R * sin(ang)
    target_i.setPos(pos_i)
    print("Moving to target %i: angle %.1f" % (i, ang * 180 / pi))
    print(str(Pose_2_TxyzRxyz(target_i)))
    robot.MoveL(target_i)

    #-----------------------------
    # Post multiply: relative to the tool
    #target_i = target_ref * rotz(ang) * transl(R,0,0) * rotz(-ang)
    #robot.MoveL(target_i)

# move back to the center, then home:
robot.MoveL(target_ref)

print('Done')

# This example shows how RoboDK and the Python GUI tkinter can display graphical user interface to customize program generation according to certain parameters
# This example is an improvement of the weld Hexagon
# 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 *  # robodk robotics toolbox

# Set up default parameters
PROGRAM_NAME = "DoWeld"  # Name of the program
APPROACH = 300  # Approach distance
RADIUS = 200  # Radius of the polygon
SPEED_WELD = 50  # Speed in mn/s of the welding path
SPEED_MOVE = 200  # Speed in mm/s of the approach/retract movements
SIDES = 8  # Number of sides for the polygon
DRY_RUN = 1  # If 0, it will generate SprayOn/SprayOff program calls, otherwise it will not activate the tool
RUN_MODE = RUNMODE_SIMULATE  # Simulation behavior (simulate, generate program or generate the program and send it to the robot)

# use RUNMODE_SIMULATE to simulate only
# use RUNMODE_MAKE_ROBOTPROG to generate the program
# use RUNMODE_MAKE_ROBOTPROG_AND_UPLOAD to generate the program and send it to the robot


# Main program
def RunProgram():
    # Use default global variables
    global PROGRAM_NAME
    global APPROACH
    global RADIUS
    global SPEED_WELD
    global SPEED_MOVE
    global SIDES
    global DRY_RUN
    global RUN_MODE

    # Any interaction with RoboDK must be done through RDK:
    RDK = Robolink()

    # get the home target and the welding targets:
    home = RDK.Item('Home')
    target = RDK.Item('Target Reference')

    # get the robot as an item:
    robot = RDK.Item('', ITEM_TYPE_ROBOT)

    # impose the run mode
    RDK.setRunMode(RUN_MODE)
    # set the name of the generated program
    RDK.ProgramStart(PROGRAM_NAME, "", "", robot)

    # get the pose of the reference target (4x4 matrix representing position and orientation):
    poseref = target.Pose()

    # move the robot to home, then to an approach position
    robot.MoveJ(home)
    robot.setSpeed(SPEED_MOVE)
    robot.MoveJ(transl(0, 0, APPROACH) * poseref)

    # make an polygon of n SIDES around the reference target
    for i in range(SIDES + 1):
        ang = i * 2 * pi / SIDES  #angle: 0, 60, 120, ...
        # Calculate next position
        posei = poseref * rotz(ang) * transl(RADIUS, 0, 0) * rotz(-ang)
        robot.MoveL(posei)

        # Impose weld speed only for the first point
        if i == 0:
            # Set weld speed and activate the gun after reaching the first point
            robot.setSpeed(SPEED_WELD)
            if not DRY_RUN:
                # Activate the spray right after we reached the first point
                robot.RunCodeCustom("SprayOn", INSTRUCTION_CALL_PROGRAM)

    # Stop the tool if we are not doing a dry run
    if not DRY_RUN:
        robot.RunCodeCustom("SprayOff", INSTRUCTION_CALL_PROGRAM)
    robot.setSpeed(SPEED_MOVE)

    # move back to the approach point, then home:
    robot.MoveL(transl(0, 0, APPROACH) * poseref)
    robot.MoveJ(home)

    # Provoke program generation (automatic when RDK is finished)
    RDK.Finish()


# Use tkinter to display GUI menus
from tkinter import *

# Generate the main window
root = Tk()
root.title("Program settings")

# Use variables linked to the global variables
runmode = IntVar()
runmode.set(RUN_MODE)  # setting up default value

dryrun = IntVar()
dryrun.set(DRY_RUN)  # setting up default value

entry_name = StringVar()
entry_name.set(PROGRAM_NAME)

entry_speed = StringVar()
entry_speed.set(str(SPEED_WELD))


# Define feedback call
def ShowRunMode():
    print("Selected run mode: " + str(runmode.get()))


# Define a label and entry text for the program name
Label(root, text="Program name").pack()
Entry(root, textvariable=entry_name).pack()

# Define a label and entry text for the weld speed
Label(root, text="Weld speed (mm/s)").pack()
Entry(root, textvariable=entry_speed).pack()

# Define a check box to do a dry run
Checkbutton(root, text="Dry run", variable=dryrun, onvalue=1, offvalue=0, height=5, width=20).pack()

# Add a display label for the run mode
Label(root, text="Run mode", justify=LEFT, padx=20).pack()

# Set up the run modes (radio buttons)
runmodes = [("Simulate", RUNMODE_SIMULATE), ("Generate program", RUNMODE_MAKE_ROBOTPROG), ("Send program to robot", RUNMODE_MAKE_ROBOTPROG_AND_START)]
for txt, val in runmodes:
    Radiobutton(root, text=txt, padx=20, variable=runmode, command=ShowRunMode, value=val).pack(anchor=W)


# Add a button and default action to execute the current choice of the user
def ExecuteChoice():
    global DRY_RUN
    global RUN_MODE
    global SPEED_WELD
    global PROGRAM_NAME
    DRY_RUN = dryrun.get()
    RUN_MODE = runmode.get()
    SPEED_WELD = float(entry_speed.get())
    PROGRAM_NAME = entry_name.get()
    # Run the main program once all the global variables have been set
    RunProgram()


Button(root, text='Simulate/Generate', command=ExecuteChoice).pack()

# Important to display the graphical user interface
root.mainloop()

# This macro shows an example to run a program on the robot from the Python API (online programming)
#
# Important: By default, right clicking a program on the RoboDK API and selecting "Run On Robot" has the same effect as running this example.
# Use the Example_OnlineProgramming.py instead if the program is run from the RoboDK Station Tree
#
# This example forces the connection to the robot by using:
# robot.Connect()
# and
# RDK.setRunMode(RUNMODE_RUN_ROBOT)

# In this script, if the variable RUN_ON_ROBOT is set to True, an attempt will be made to connect to the robot
# Alternatively, if the RUN_ON_ROBOT variable is set to False, the program will be simulated (offline programming)
#
# 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

# 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', ITEM_TYPE_ROBOT)
if not robot.Valid():
    raise Exception('No robot selected or available')

RUN_ON_ROBOT = True

# Important: by default, the run mode is RUNMODE_SIMULATE
# If the program is generated offline manually the runmode will be RUNMODE_MAKE_ROBOTPROG,
# Therefore, we should not run the program on the robot
if RDK.RunMode() != RUNMODE_SIMULATE:
    RUN_ON_ROBOT = False

if RUN_ON_ROBOT:
    # Update connection parameters if required:
    # robot.setConnectionParams('192.168.2.35',30000,'/', 'anonymous','')

    # Connect to the robot using default IP
    success = robot.Connect()  # Try to connect once
    #success robot.ConnectSafe() # Try to connect multiple times
    status, status_msg = robot.ConnectedState()
    if status != ROBOTCOM_READY:
        # Stop if the connection did not succeed
        print(status_msg)
        raise Exception("Failed to connect: " + status_msg)

    # This will set to run the API programs on the robot and the simulator (online programming)
    RDK.setRunMode(RUNMODE_RUN_ROBOT)
    # Note: This is set automatically when we Connect() to the robot through the API

#else:
# This will run the API program on the simulator (offline programming)
# RDK.setRunMode(RUNMODE_SIMULATE)
# Note: This is the default setting if we do not execute robot.Connect()
# We should not set the RUNMODE_SIMULATE if we want to be able to generate the robot programm offline

# Get the current joint position of the robot
# (updates the position on the robot simulator)
joints_ref = robot.Joints()

# 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()
print("Drawing a polygon around the target: ")
print(Pose_2_TxyzRxyz(target_ref))

# move the robot to the first point:
robot.MoveJ(target_ref)

# It is important to provide the reference frame and the tool frames when generating programs offline
# It is important to update the TCP on the robot mostly when using the driver
robot.setPoseFrame(robot.PoseFrame())
robot.setPoseTool(robot.PoseTool())
robot.setRounding(10)  # Set the rounding parameter (Also known as: CNT, APO/C_DIS, ZoneData, Blending radius, cornering, ...)
robot.setSpeed(200)  # Set linear speed in mm/s

# Set the number of sides of the polygon:
n_sides = 6
R = 100

# make a hexagon around reference target:
for i in range(n_sides + 1):
    ang = i * 2 * pi / n_sides  #angle: 0, 60, 120, ...

    #-----------------------------
    # Movement relative to the reference frame
    # Create a copy of the target
    target_i = Mat(target_ref)
    pos_i = target_i.Pos()
    pos_i[0] = pos_i[0] + R * cos(ang)
    pos_i[1] = pos_i[1] + R * sin(ang)
    target_i.setPos(pos_i)
    print("Moving to target %i: angle %.1f" % (i, ang * 180 / pi))
    print(str(Pose_2_TxyzRxyz(target_i)))
    robot.MoveL(target_i)

    #-----------------------------
    # Post multiply: relative to the tool
    #target_i = target_ref * rotz(ang) * transl(R,0,0) * rotz(-ang)
    #robot.MoveL(target_i)

# move back to the center, then home:
robot.MoveL(target_ref)

print('Done')

## Example to run a program created using the GUI from the API
#prog = RDK.Item('MainProgram', ITEM_TYPE_PROGRAM)
## prog.setRunType(PROGRAM_RUN_ON_ROBOT) # Run on robot option
## prog.setRunType(PROGRAM_RUN_ON_SIMULATOR) # Run on simulator (default on startup)
#prog.RunProgram()
#while prog.Busy() == 1:
#    pause(0.1)
#print("Program done")

# Move a robot along a line given a start and end point by steps
# This macro shows three different ways of programming a robot using a Python script and the RoboDK API

# More information about the RoboDK API here:
# https://robodk.com/doc/en/RoboDK-API.html
# For more information visit:
# https://robodk.com/doc/en/PythonAPI/robodk.html#robolink-py

# Default parameters:
P_START = [1755, -500, 2155]  # Start point with respect to the robot base frame
P_END = [1755, 600, 2155]  # End point with respect to the robot base frame
NUM_POINTS = 10  # Number of points to interpolate


# Function definition to create a list of points (line)
def MakePoints(xStart, xEnd, numPoints):
    """Generates a list of points"""
    if len(xStart) != 3 or len(xEnd) != 3:
        raise Exception("Start and end point must be 3-dimensional vectors")
    if numPoints < 2:
        raise Exception("At least two points are required")

    # Starting Points
    pt_list = []
    x = xStart[0]
    y = xStart[1]
    z = xStart[2]

    # How much we add/subtract between each interpolated point
    x_steps = (xEnd[0] - xStart[0]) / (numPoints - 1)
    y_steps = (xEnd[1] - xStart[1]) / (numPoints - 1)
    z_steps = (xEnd[2] - xStart[2]) / (numPoints - 1)

    # Incrementally add to each point until the end point is reached
    for i in range(numPoints):
        point_i = [x, y, z]  # create a point
        #append the point to the list
        pt_list.append(point_i)
        x = x + x_steps
        y = y + y_steps
        z = z + z_steps
    return pt_list


#---------------------------------------------------
#--------------- PROGRAM START ---------------------
from robodk.robolink import *  # API to communicate with RoboDK
from robodk.robomath import *  # basic matrix operations

# Generate the points curve path
POINTS = MakePoints(P_START, P_END, NUM_POINTS)

# Initialize the RoboDK API
RDK = Robolink()

# turn off auto rendering (faster)
RDK.Render(False)

# Automatically delete previously generated items (Auto tag)
list_names = RDK.ItemList()  # list all names
for item_name in list_names:
    if item_name.startswith('Auto'):
        RDK.Item(item_name).Delete()

# Promt the user to select a robot (if only one robot is available it will select that robot automatically)
robot = RDK.ItemUserPick('Select a robot', ITEM_TYPE_ROBOT)

# Turn rendering ON before starting the simulation
RDK.Render(True)

# Abort if the user hits Cancel
if not robot.Valid():
    quit()

# Retrieve the robot reference frame
reference = robot.Parent()

# Use the robot base frame as the active reference
robot.setPoseFrame(reference)

# get the current orientation of the robot (with respect to the active reference frame and tool frame)
pose_ref = robot.Pose()
print(Pose_2_TxyzRxyz(pose_ref))
# a pose can also be defined as xyzwpr / xyzABC
#pose_ref = TxyzRxyz_2_Pose([100,200,300,0,0,pi])

#-------------------------------------------------------------
# Option 1: Move the robot using the Python script

# We can automatically force the "Create robot program" action using a RUNMODE state
# RDK.setRunMode(RUNMODE_MAKE_ROBOTPROG)

# Iterate through all the points
for i in range(NUM_POINTS):
    # update the reference target with the desired XYZ coordinates
    pose_i = pose_ref
    pose_i.setPos(POINTS[i])

    # Move the robot to that target:
    robot.MoveJ(pose_i)

# Done, stop program execution
quit()

#-------------------------------------------------------------
# Option 2: Create the program on the graphical user interface
# Turn off rendering
RDK.Render(False)
prog = RDK.AddProgram('AutoProgram')

# Iterate through all the points
for i in range(NUM_POINTS):
    # add a new target and keep the reference to it
    ti = RDK.AddTarget('Auto Target %i' % (i + 1))
    # use the reference pose and update the XYZ position
    pose_i = pose_ref
    pose_i.setPos(POINTS[i])
    ti.setPose(pose_i)
    # force to use the target as a Cartesian target
    ti.setAsCartesianTarget()

    # Optionally, add the target as a Linear/Joint move in the new program
    prog.MoveL(ti)

# Turn rendering ON before starting the simulation
RDK.Render(True)

# Run the program on the simulator (simulate the program):
prog.RunProgram()
# prog.WaitFinished() # wait for the program to finish

# We can create the program automatically
# prog.MakeProgram()

# Also, if we have the robot driver we could use the following call to provoke a "Run on robot" action (simulation and the robot move simultaneously)
# prog.setRunType(PROGRAM_RUN_ON_ROBOT)

# Done, stop program execution
quit()

#-------------------------------------------------------------
# Option 3: Move the robot using the Python script and detect if movements can be linear
# This is an improved version of option 1
#
# We can automatically force the "Create robot program" action using a RUNMODE state
# RDK.setRunMode(RUNMODE_MAKE_ROBOTPROG)

# Iterate through all the points
ROBOT_JOINTS = None
for i in range(NUM_POINTS):
    # update the reference target with the desired XYZ coordinates
    pose_i = pose_ref
    pose_i.setPos(POINTS[i])

    # Move the robot to that target:
    if i == 0:
        # important: make the first movement a joint move!
        robot.MoveJ(pose_i)
        ROBOT_JOINTS = robot.Joints()
    else:
        # test if we can do a linear movement from the current position to the next point
        if robot.MoveL_Collision(ROBOT_JOINTS, pose_i) == 0:
            robot.MoveL(pose_i)
        else:
            robot.MoveJ(pose_i)
        ROBOT_JOINTS = robot.Joints()

# Done, stop program execution
quit()

#-------------------------------------------------------------
# Option 4: Create a follow curve project

# First we need to create an object from the provided points or add the points to an existing object and optionally project them on the surface

# Create a new object given the list of points (the 3xN vector can be extended to 6xN to provide the normal)
object_points = RDK.AddPoints(POINTS)

# Alternatively, we can project the points on the object surface
# object = RDK.Item('Object', ITEM_TYPE_OBJECT)
# object_points = object.AddPoints(POINTS, PROJECTION_ALONG_NORMAL_RECALC)
# Place the points at the same location as the reference frame of the object
# object_points.setParent(object.Parent())

# Set the name of the object containing points
object_points.setName('AutoPoints n%i' % NUM_POINTS)

path_settings = RDK.AddMillingProject("AutoPointFollow settings")
prog, status = path_settings.setMillingParameters(part=object_points)
# At this point, we may have to manually adjust the tool object or the reference frame

# Run the create program if success
prog.RunProgram()

# Done
quit()

#-------------------------------------------------------------
# Option 5: Create a follow points project (similar to Option 4)

# First we need to create an object from the provided points or add the points to an existing object and optionally project them on the surface

# Create a new object given the list of points:
object_curve = RDK.AddCurve(POINTS)

# Alternatively, we can project the points on the object surface
# object = RDK.Item('Object', ITEM_TYPE_OBJECT)
# object_curve = object.AddCurve(POINTS, PROJECTION_ALONG_NORMAL_RECALC)
# Place the curve at the same location as the reference frame of the object
# object_curve.setParent(object.Parent())

# Set the name of the object containing points
object_curve.setName('AutoPoints n%i' % NUM_POINTS)

# Create a new "Curve follow project" to automatically follow the curve
path_settings = RDK.AddMillingProject("AutoCurveFollow settings")
prog, status = path_settings.setMillingParameters(part=object_curve)
# At this point, we may have to manually adjust the tool object or the reference frame

# Run the create program if success
prog.RunProgram()

# Done
quit()

#-------------------------------------------------------------
# Option 6: Move the robot using the Python script and measure using an external measurement system
# This example is meant to help validating robot accuracy through distance measurements and using a laser tracker or a stereo camera

if robot.ConnectSafe() <= 0:
    raise Exception("Can't connect to robot")

RDK.setRunMode(RUNMODE_RUN_ROBOT)  # It is redundant if connection worked successfully

POINTS_NOMINAL = []
POINTS_ACCURATE = []
STABILIZATION_TIME = 2  # stabilization time in seconds before taking a measurement
for i in range(NUM_POINTS):
    # build a target using the reference orientation and the XYZ coordinates
    pose_i = pose_ref
    pose_i.setPos(LINE[i])

    # Move to the target with the nomrinal kinematics
    RDK.RunMessage('Moving to point %i (Nominal kinematics)' % (i + 1))
    RDK.RunMessage(str(Pose_2_KUKA(pose_i)))
    # Solve nominal inverse kinematics
    robot.setAccuracyActive(False)
    ji = robot.SolveIK(pose_i)
    robot.MoveL(ji)
    robot.Pause(STABILIZATION_TIME)

    # Take the measurement
    while True:
        pose1, pose2, npoints1, npoints2, time, errors = RDK.StereoCamera_Measure()
        if errors != 0 or npoints1 < 4 or npoints2 < 4:
            print('Invalid measurement. Retrying in 2 seconds...')
            pause(2)
        else:
            # calculate the pose of the tool with respect to the reference frame
            measured = invH(pose1) * pose2
            # retrieve XYZ value of the measurement
            POINTS_NOMINAL = measured.Pos()
            break

    # Move to the target with the accurate kinematics
    RDK.RunMessage('Moving to point %i (Accurate kinematics)' % (i + 1))
    robot.setAccuracyActive(True)
    ji = robot.SolveIK(pose_i)
    robot.MoveL(ji)
    robot.Pause(STABILIZATION_TIME)
    while True:
        pose1, pose2, npoints1, npoints2, time, errors = RDK.StereoCamera_Measure()
        if errors != 0 or npoints1 < 4 or npoints2 < 4:
            print('Invalid measurement. Retrying in 2 seconds...')
            pause(2)
        else:
            # calculate the pose of the tool with respect to the reference frame
            measured = invH(pose1) * pose2
            # retrieve XYZ value of the measurement
            POINTS_ACCURATE = measured.Pos()
            break

# At this point we can check the accurate vs the nominal kinematics and create the following table:
print('pa\tpb\tdist\tdist nom\tdist acc\terror nom\terror acc')
for i in range(numPoints):
    for j in range(numPoints + 1, numPoints):
        dist_program = distance(LINE[i], LINE[j])
        dist_nominal = distance(POINTS_NOMINAL[i], POINTS_NOMINAL[j])
        dist_accurate = distance(POINTS_ACCURATE[i], POINTS_ACCURATE[j])
        error_nominal = abs(dist_nominal - dist_program)
        error_accurate = abs(dist_accurate - dist_program)
        print('%i\t%i\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f' % (i + 1, j + 1, dist_program, dist_nominal, dist_accurate, error_nominal, error_accurate))

quit()

# Type help("robolink") or help("robodk") for more information
# Press F5 to run the script
# Documentation: https://robodk.com/doc/en/RoboDK-API.html
# Reference:     https://robodk.com/doc/en/PythonAPI/index.html
#
# Move a robot along a line given a start and end point by steps
# This macro shows different ways of programming a robot using a Python script and the RoboDK API

# Default parameters:
P_START = [1755, -500, 2155]  # Start point with respect to the robot base frame
P_END = [1755, 600, 2155]  # End point with respect to the robot base frame
NUM_POINTS = 10  # Number of points to interpolate


# Function definition to create a list of points (line)
def MakePoints(xStart, xEnd, numPoints):
    """Generates a list of points"""
    if len(xStart) != 3 or len(xEnd) != 3:
        raise Exception("Start and end point must be 3-dimensional vectors")
    if numPoints < 2:
        raise Exception("At least two points are required")

    # Starting Points
    pt_list = []
    x = xStart[0]
    y = xStart[1]
    z = xStart[2]

    # How much we add/subtract between each interpolated point
    x_steps = (xEnd[0] - xStart[0]) / (numPoints - 1)
    y_steps = (xEnd[1] - xStart[1]) / (numPoints - 1)
    z_steps = (xEnd[2] - xStart[2]) / (numPoints - 1)

    # Incrementally add to each point until the end point is reached
    for i in range(numPoints):
        point_i = [x, y, z]  # create a point
        #append the point to the list
        pt_list.append(point_i)
        x = x + x_steps
        y = y + y_steps
        z = z + z_steps
    return pt_list


#---------------------------------------------------
#--------------- PROGRAM START ---------------------
from robodk.robolink import *  # API to communicate with RoboDK for simulation and offline/online programming
from robodk.robomath import *  # Robotics toolbox for industrial robots

# Generate the points curve path
POINTS = MakePoints(P_START, P_END, NUM_POINTS)

# Initialize the RoboDK API
RDK = Robolink()

# turn off auto rendering (faster)
RDK.Render(False)

# Automatically delete previously generated items (Auto tag)
list_items = RDK.ItemList()  # list all names
for item in list_items:
    if item.Name().startswith('Auto'):
        item.Delete()

# Promt the user to select a robot (if only one robot is available it will select that robot automatically)
robot = RDK.ItemUserPick('Select a robot', ITEM_TYPE_ROBOT)

# Turn rendering ON before starting the simulation
RDK.Render(True)

# Abort if the user hits Cancel
if not robot.Valid():
    quit()

# Retrieve the robot reference frame
reference = robot.Parent()

# Use the robot base frame as the active reference
robot.setPoseFrame(reference)

# get the current orientation of the robot (with respect to the active reference frame and tool frame)
pose_ref = robot.Pose()
print(Pose_2_TxyzRxyz(pose_ref))
# a pose can also be defined as xyzwpr / xyzABC
#pose_ref = TxyzRxyz_2_Pose([100,200,300,0,0,pi])

#-------------------------------------------------------------
# Option 1: Create a curve follow project

# First we need to create an object from the provided points or add the points to an existing object and optionally project them on the surface

# Create a new object given the list of points (the 3xN vector can be extended to 6xN to provide the normal)
object_points = RDK.AddPoints(POINTS)

# Alternatively, we can project the points on the object surface
# object = RDK.Item('Object', ITEM_TYPE_OBJECT)
# object_points = object.AddPoints(POINTS, PROJECTION_ALONG_NORMAL_RECALC)
# Place the points at the same location as the reference frame of the object
# object_points.setParent(object.Parent())

# Set the name of the object containing points
object_points.setName('AutoPoints n%i' % NUM_POINTS)

path_settings = RDK.AddMillingProject("AutoPointFollow settings")
prog, status = path_settings.setMillingParameters(part=object_points)
# At this point, we may have to manually adjust the tool object or the reference frame

# Run the create program if success
prog.RunProgram()

# Done
quit()

#-------------------------------------------------------------
# Option 2: Create a point follow project (similar to Option 4)

# First we need to create an object from the provided points or add the points to an existing object and optionally project them on the surface

# Create a new object given the list of points:
object_curve = RDK.AddCurve(POINTS)

# Alternatively, we can project the points on the object surface
# object = RDK.Item('Object', ITEM_TYPE_OBJECT)
# object_curve = object.AddCurve(POINTS, PROJECTION_ALONG_NORMAL_RECALC)
# Place the curve at the same location as the reference frame of the object
# object_curve.setParent(object.Parent())

# Set the name of the object containing points
object_curve.setName('AutoPoints n%i' % NUM_POINTS)

# Create a new "Curve follow project" to automatically follow the curve
path_settings = RDK.AddMillingProject("AutoCurveFollow settings")
prog, status = path_settings.setMillingParameters(part=object_curve)
# At this point, we may have to manually adjust the tool object or the reference frame

# Run the create program if success
prog.RunProgram()

# Done
quit()

# This macro shows how to load a list of XYZ points including speed
# The list must be provided as X,Y,Z,Speed. Units must be mm and mm/s respectively
from robodk.robolink import *
from robodk.robodialogs import *
from robodk.robofileio 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 = 'Reference CSV'

# 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()

# Ask the user to pick a file:
rdk_file_path = RDK.getParam("PATH_OPENSTATION")
path_file = getOpenFileName(rdk_file_path + "/")
if not path_file:
    print("Nothing selected")
    quit()

# Get the program name from the file path
program_name = getFileName(path_file)

# Load the CSV file as a list of list [[x,y,z,speed],[x,y,z,speed],...]
data = LoadList(path_file)

# Delete previously generated programs that follow a specific naming
#list_names = RDK.ItemList(False)
#for item_name in list_names:
#    if item_name.startswith('Auto'):
#        RDK.Item(item_name).Delete()

# Select the robot (the popup is diplayed only if there are 2 or more robots)
robot = RDK.ItemUserPick('Select a robot', ITEM_TYPE_ROBOT)
if not robot.Valid():
    raise Exception("Robot not selected or not valid")
    quit()

# Get the reference frame to generate the path
frame = RDK.Item(REFERENCE_NAME, ITEM_TYPE_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)
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())

# Get the pose reference from the home target
pose_ref = robot.Pose()

if LOAD_AS_PROGRAM:
    # Add a new program
    program = RDK.AddProgram(program_name, robot)

    # Turn off rendering (faster)
    RDK.Render(False)

    # Speed up by not showing the instruction:
    program.ShowInstructions(False)

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

    # Very important: Make sure we set the reference frame and tool frame so that the robot is aware of it
    program.setPoseFrame(frame)
    program.setPoseTool(robot.PoseTool())

    # Iterate through all the points
    for i in range(len(data)):
        pi = pose_ref
        pi.setPos(data[i])

        # Update speed if there is a 4th column
        if len(data[i]) >= 3:
            speed = data[i][3]
            # Update the program if the speed is different than the previously set speed
            if type(speed) != str and speed != current_speed:
                program.setSpeed(speed)
                current_speed = speed

        target = RDK.AddTarget('T%i' % i, frame)
        target.setPose(pi)
        pi = target

        # Add a linear movement (with the exception of the first point which will be a joint movement)
        if i == 0:
            program.MoveJ(pi)
        else:
            program.MoveL(pi)

        # Update from time to time to notify the user about progress
        if i % 100 == 0:
            program.ShowTargets(False)
            RDK.ShowMessage("Loading %s: %.1f %%" % (program_name, 100 * i / len(data)), False)
            RDK.Render()

    program.ShowTargets(False)

else:
    # Very important: Make sure we set the reference frame and tool frame so that the robot is aware of it
    robot.setPoseFrame(frame)
    robot.setPoseTool(robot.PoseTool())

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

    # Iterate through all the points
    for i in range(len(data)):
        pi = pose_ref
        pi.setPos(data[i][0:3])

        # Update speed if there is a 4th column
        if len(data[i]) >= 3:
            speed = data[i][3]
            # Update the program if the speed is different than the previously set speed
            if type(speed) != str and speed != current_speed:
                robot.setSpeed(speed)
                current_speed = speed

        # Add a linear movement (with the exception of the first point which will be a joint movement)
        if i == 0:
            robot.MoveJ(pi)
        else:
            robot.MoveL(pi)

        # Update from time to time to notify the user about progress
        #if i % 200 == 0:
        RDK.ShowMessage("Program %s: %.1f %%" % (program_name, 100 * i / len(data)), False)

RDK.ShowMessage("Done", False)
print("Done")

# This macro can load CSV files from Denso programs in RoboDK.
# Supported types of files are:
#  1-Tool data : Tool.csv
#  2-Work object data: Work.csv
#  3-Target data: P_Var.csv
# This macro can also filter a given targets file

# Type help("robodk.robolink") or help("robodk.robomath") for more information
# Press F5 to run the script
# Visit: http://www.robodk.com/doc/PythonAPI/
# For RoboDK API documentation

from robodk.robolink import *  # API to communicate with RoboDK
from robodk.robomath import *  # Robot toolbox
from robodk.robodialogs import *
from robodk.robofileio import *

# Start communication with RoboDK
RDK = Robolink()

# Ask the user to select the robot (ignores the popup if only
ROBOT = RDK.ItemUserPick('Select a robot', ITEM_TYPE_ROBOT)

# Check if the user selected a robot
if not ROBOT.Valid():
    quit()

# Automatically retrieve active reference and tool
FRAME = ROBOT.getLink(ITEM_TYPE_FRAME)
TOOL = ROBOT.getLink(ITEM_TYPE_TOOL)

#FRAME = RDK.ItemUserPick('Select a reference frame', ITEM_TYPE_FRAME)
#TOOL = RDK.ItemUserPick('Select a tool', ITEM_TYPE_TOOL)

if not FRAME.Valid() or not TOOL.Valid():
    raise Exception("Select appropriate FRAME and TOOL references")


# Function to convert XYZWPR to a pose
# Important! Specify the order of rotation
def xyzwpr_to_pose(xyzwpr):
    x, y, z, rx, ry, rz = xyzwpr
    return transl(x, y, z) * rotz(rz * pi / 180) * roty(ry * pi / 180) * rotx(rx * pi / 180)
    #return transl(x,y,z)*rotx(rx*pi/180)*roty(ry*pi/180)*rotz(rz*pi/180)
    #return KUKA_2_Pose(xyzwpr)


# csv_file = 'C:/Users/Albert/Desktop/Var_P.csv'
csv_file = getOpenFileName(RDK.getParam('PATH_OPENSTATION'))

# Specify file codec
codec = 'utf-8'  #'ISO-8859-1'


# Load P_Var.CSV data as a list of poses, including links to reference and tool frames
def load_targets(strfile):
    csvdata = LoadList(strfile, ',', codec)
    poses = []
    idxs = []
    for i in range(0, len(csvdata)):
        x, y, z, rx, ry, rz = csvdata[i][0:6]
        poses.append(xyzwpr_to_pose([x, y, z, rx, ry, rz]))
        #idxs.append(csvdata[i][6])
        idxs.append(i)

    return poses, idxs


# Load and display Targets from P_Var.CSV in RoboDK
def load_targets_GUI(strfile):
    poses, idxs = load_targets(strfile)
    program_name = getFileName(strfile)
    program_name = program_name.replace('-', '_').replace(' ', '_')
    program = RDK.Item(program_name, ITEM_TYPE_PROGRAM)
    if program.Valid():
        program.Delete()
    program = RDK.AddProgram(program_name, ROBOT)
    program.setFrame(FRAME)
    program.setTool(TOOL)
    ROBOT.MoveJ(ROBOT.JointsHome())

    for pose, idx in zip(poses, idxs):
        name = '%s-%i' % (program_name, idx)
        target = RDK.Item(name, ITEM_TYPE_TARGET)
        if target.Valid():
            target.Delete()
        target = RDK.AddTarget(name, FRAME, ROBOT)
        target.setPose(pose)

        try:
            program.MoveJ(target)
        except:
            print('Warning: %s can not be reached. It will not be added to the program' % name)


def load_targets_move(strfile):
    poses, idxs = load_targets(strfile)

    ROBOT.setFrame(FRAME)
    ROBOT.setTool(TOOL)

    ROBOT.MoveJ(ROBOT.JointsHome())

    for pose, idx in zip(poses, idxs):
        try:
            ROBOT.MoveJ(pose)
        except:
            RDK.ShowMessage('Target %i can not be reached' % idx, False)


# Force just moving the robot after double clicking
#load_targets_move(csv_file)
#quit()

# Recommended mode of operation:
# 1-Double click the python file creates a program in RoboDK station
# 2-Generate program generates the program directly

MAKE_GUI_PROGRAM = False

ROBOT.setFrame(FRAME)
ROBOT.setTool(TOOL)

if RDK.RunMode() == RUNMODE_SIMULATE:
    MAKE_GUI_PROGRAM = True
    # MAKE_GUI_PROGRAM = mbox('Do you want to create a new program? If not, the robot will just move along the tagets', 'Yes', 'No')
else:
    # if we run in program generation mode just move the robot
    MAKE_GUI_PROGRAM = False

if MAKE_GUI_PROGRAM:
    RDK.Render(False)  # Faster if we turn render off
    load_targets_GUI(csv_file)
else:
    load_targets_move(csv_file)

# This macro shows how to load a KUKA SRC file
# PTP movements with joint coordinates and LIN movements with Cartesian information (XYZABC) will be imported as a program.
# This macro also supports defining the tool and the base inline and changing the speed using the VEL.CP global variable

## Example program:
# DEF Milling ( )
#
# $BASE = {FRAME: X 0.000,Y -1000.000,Z 0.000,A 0.000,B 0.000,C 0.000}
# $TOOL = {FRAME: X 466.604,Y -4.165,Z 109.636,A -0.000,B 90.000,C 0.000}
#
# $VEL.CP = 1.00000
#
# PTP {A1 107.78457,A2 -44.95260,A3 141.64681,A4 107.66839,A5 -87.93467,A6 6.37710}
# LIN {X -0.000,Y -0.000,Z 6.350,A -180.000,B 0.000,C -180.000}
#
# $VEL.CP = 0.02117
# LIN {X 276.225,Y 0.000,Z 6.350,A 180.000,B 0.000,C -180.000}
# LIN {X 276.225,Y 323.850,Z 6.350,A -160.000,B 0.000,C 180.000}
# LIN {X -0.000,Y 323.850,Z 6.350,A -180.000,B -0.000,C -180.000}
# LIN {X -0.000,Y -0.000,Z 6.350,A -180.000,B 0.000,C -180.000}
# $VEL.CP = 1.00000
# LIN {X -0.000,Y -0.000,Z 106.350,A -180.000,B 0.000,C -180.000}
#
# END

from robodk.robomath import *  # Robot toolbox
from robodk.robodialogs import *
from robodk.robofileio import *
from robodk.robolink import *

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

# Ask the user to pick an SRC file:
rdk_file_path = RDK.getParam("PATH_OPENSTATION")
src_file_path = getOpenFileName(rdk_file_path + "/")
if not src_file_path:
    print("Nothing selected")
    quit()

# Get the program name from the file path
program_name = getFileName(src_file_path)
print("Loading program: " + program_name)

if not src_file_path.lower().endswith(".src"):
    raise Exception("Invalid file selected. Select an SRC file.")


def GetValues(line):
    """Get all the numeric values from a line"""
    # LIN {X 1671.189,Y -562.497,Z -243.070,A 173.363,B -8.525,C -113.306} C_DIS
    line = line.replace(",", " ")
    line = line.replace("}", " ")
    values = line.split(" ")

    list_values = []
    for value in values:
        try:
            value = float(value)
        except:
            continue

        list_values.append(value)

    return list_values


# Ask the user to select a robot (if more than a robot is available)
robot = RDK.ItemUserPick('Select a robot', ITEM_TYPE_ROBOT)
if not robot.Valid():
    raise Exception("Robot not selected or not valid")

# Get the active reference frame
frame = robot.getLink(ITEM_TYPE_FRAME)
if not frame.Valid():
    # If there is no active reference frame, use the robot base
    frame = robot.Parent()

# Get the active tool frame
tool = robot.getLink(ITEM_TYPE_TOOL)

# Add a new program
program = RDK.AddProgram(program_name, robot)

# Turn off rendering (faster)
RDK.Render(False)

# Speed up by not showing the instruction:
program.ShowInstructions(False)

# Open the file and iterate through each line
with open(src_file_path) as f:
    count = 0
    for line in f:
        # Remove empty characters:
        line = line.strip()
        print("Loading line: " + line)

        # Get all the numeric values in order
        values = GetValues(line)

        # Increase the counter
        count = count + 1

        # Update TCP speed (KUKA works in m/s, RoboDK works in mm/s)
        if line.startswith("$VEL.CP"):
            program.setSpeed(values[0]*1000)
            continue
            
        # Check operations that involve a pose
        if len(values) < 6:
            print("Warning! Invalid line: " + line)
            continue

        # Check what instruction we need to add:
        if line.startswith("LIN"):
            target = RDK.AddTarget('T%i'% count, frame)
            
            # Check if we have external axes information, if so, provided it to joints E1 to En
            if len(values) > 6:
                target.setJoints([0,0,0,0,0,0] + values[6:])
                
            target.setPose(KUKA_2_Pose(values[:6]))
            program.MoveL(target)

        # Check PTP move
        elif line.startswith("PTP"):
            target = RDK.AddTarget('T%i'% count, frame)
            target.setAsJointTarget()
            target.setJoints(values)
            program.MoveJ(target)

        # Set the tool
        elif line.startswith("$TOOL"):
            pose = KUKA_2_Pose(values[:6])
            tool = robot.AddTool(pose, "SRC TOOL")
            program.setTool(tool)

        # Set the reference frame
        elif line.startswith("$BASE"):
            frame = RDK.AddFrame("SRC BASE", robot.Parent())
            frame.setPose(KUKA_2_Pose(values[:6]))
            program.setFrame(frame)

# Hide the targets
program.ShowTargets(False)

# Show the instructions
program.ShowInstructions(True)

RDK.ShowMessage("Done", False)
print("Done")

# This macro shows how you can create a program that moves the robot through a set of points
# The points are automatically created as a cube grid around a reference target
# If a linear movement can't be done from one point to the next one the robot will follow a joint movement
from robodk.robolink import *  # API to communicate with RoboDK
from robodk.robomath import *  # basic matrix operations
from random import uniform  # to randomly calculate rz (rotation around the Z axis)

# Name of the reference target
REFERENCE_TARGET = 'RefTarget'

# Check for collisions
CHECK_COLLISIONS = False

#Start the RoboDK API
RDK = Robolink()

# Set collision checking ON or OFF
RDK.setCollisionActive(COLLISION_ON if CHECK_COLLISIONS else COLLISION_OFF)

# Run on robot: Force the program to run on the connected robot (same behavior as right clicking the program, then, selecting "Run on Robot")
# RDK.setRunMode(RUNMODE_RUN_ROBOT)

# Get the main/only robot in the station
robot = RDK.Item('', ITEM_TYPE_ROBOT)
if not robot.Valid():
    raise Exception("Robot not valid or not available")

# Get the active reference frame
frame = robot.getLink(ITEM_TYPE_FRAME)
if not frame.Valid():
    frame = robot.Parent()
    robot.setPoseFrame(frame)

# Get the reference pose with respect to the robot
frame_pose = robot.PoseFrame()

# Get the active tool
tool = robot.getLink(ITEM_TYPE_TOOL)
if not tool.Valid():
    tool = robot.AddTool(transl(0, 0, 75), "Tool Grid")
    robot.setPoseTool(tool)

# Get the target reference RefTarget
target_ref = RDK.Item(REFERENCE_TARGET, ITEM_TYPE_TARGET)
if not target_ref.Valid():
    target_ref = RDK.AddTarget(REFERENCE_TARGET, frame, robot)

# Get the reference position (pose=4x4 matrix of the target with respect to the reference frame)
pose_ref = target_ref.Pose()
startpoint = target_ref.Joints()
config_ref = robot.JointsConfig(startpoint)

# Retrieve the tool pose
tool_pose = tool.PoseTool()

# Retrieve the degrees of freedom or axes (num_dofs = 6 for a 6 axis robot)
num_dofs = len(robot.JointsHome().list())

# Get the reference frame of the target reference
ref_frame = target_ref.Parent()


# Function definition to check if 2 robot configurations are the same
# Configurations are set as [Rear/Front,LowerArm/UpperArm,Flip/NonFlip] bits (int values)
def config_equal(config1, config2):
    if config1[0] != config2[0] or config1[1] != config2[1] or config1[2] != config2[2]:
        return False
    return True


# Create a new program
prog = RDK.AddProgram('AutoCreated')

# This should make program generation slightly faster
#prog.ShowInstructions(False)

# Start creating the program or moving the robot:
program_or_robot = prog
program_or_robot.setPoseTool(tool_pose)

program_or_robot.MoveJ(target_ref)
lastjoints = startpoint
rz = 0
ntargets = 0
for tz in range(-100, 101, 100):
    for ty in range(0, 401, 200):
        for tx in range(100, -5001, -250):
            ntargets = ntargets + 1
            # calculate a random rotation around the Z axis of the tool
            #rz = uniform(-20*pi/180, 20*pi/180)

            # Calculate the position of the new target: translate with respect to the robot base and rotate around the tool
            newtarget_pose = transl(tx, ty, tz) * pose_ref * rotz(rz)

            # First, make sure the target is reachable:
            newtarget_joints = robot.SolveIK(newtarget_pose, lastjoints, tool_pose, frame_pose)
            if len(newtarget_joints.list()) < num_dofs:
                print('...target not reachable!! Skipping target')
                continue

            # Create a new target:
            newtarget_name = 'Auto T%.0f,%.0f,%.0f Rz=%.1f' % (tx, ty, tz, rz)
            print('Creating target %i: %s' % (ntargets, newtarget_name))
            newtarget = RDK.AddTarget(newtarget_name, ref_frame, robot)

            # At this point, the target is reachable.
            # We have to check if we can do a linear move or not. We have 2 methods:
            can_move_linear = True

            # ------------------------------
            # Validation method 1: check the joints at the destination target
            # and make sure we have the same configuration
            # A quick way to validate (it may not be perfect if robot joints can move more than 1 turn)
            # To improve this method we would have to check configurations on all possible solutions
            # from the inverse kinematics, using SolveIK_All()
            if False:
                target_joints_config = robot.JointsConfig(newtarget_joints)
                if not config_equal(config_ref, target_joints_config):
                    # We can't do a linear movement
                    can_move_linear = False
                    print("Warning! configuration is not the same as the reference target! Linear move will not be possible")

                    # update the reference configuration to the new one
                    config_ref = target_joints_config
            # -------------------------------

            # -------------------------------
            # Validation method 2: use the robot.MoveL_Test option to check if the robot can make a linear movement
            # This method is more robust and should provide a 100% accurate result but it may take more time
            # robot.MoveL_Test can also take collisions into account if collision checking is activated
            issues = robot.MoveL_Test(lastjoints, newtarget_pose)
            can_move_linear = (issues == 0)
            # We can retrieve the final joint position by retrieving the robot joints
            if can_move_linear:
                newtarget_joints = robot.Joints()

            # ---------------------------------

            if can_move_linear:
                # All good, we don't need to modify the target.
                # However, we could set the joints in the target as this may allow us to retrieve the robot configuration if we ever need it
                newtarget.setAsCartesianTarget()  # default behavior
                newtarget.setJoints(newtarget_joints)
                # It is important to have setPose after setJoints as it may recalculate the joints to match the target
                newtarget.setPose(newtarget_pose)

                # Add the linear movement
                program_or_robot.MoveL(newtarget)

            else:
                #print(newtarget_joints)
                # Check if we can do a joint movement (check for collisions)
                issues = robot.MoveJ_Test(lastjoints, newtarget_joints)
                can_move_joints = (issues == 0)
                if not can_move_joints:
                    # Skip this point
                    print("Skipping movement to: " + str(newtarget_joints))
                    continue

                # Make sure we have a joint target and a joint movement
                newtarget.setAsJointTarget()  # default behavior

                # Setting the pose for a joint target is not important unless we want to retrieve the pose later
                # or we want to use the Cartesian coordinates for post processing
                newtarget.setPose(newtarget_pose)

                # Make sure we set the joints after the pose for a joint taget as it may recalculate the pose
                newtarget.setJoints(newtarget_joints)

                # Add the joint movement
                program_or_robot.MoveJ(newtarget)

            # Remember the joint poisition of the last movement
            lastjoints = newtarget_joints

# Showing the instructions at the end is faster:
prog.ShowInstructions(True)

# Hiding the targets is cleaner and more difficult to accidentaly move a target
#prog.ShowTargets(False)

print('Program done with %i targets' % ntargets)

from tkinter import *
from robodk.robolink import *
import tkinter
import threading

# Create a new window
window = Tk()


# Close the window
def onClose():
    window.destroy()
    quit(0)


# Trigger Select button
# IMPORTANT: We need to run the action on a separate thread because
# (otherwise, if we want to interact with RoboDK window it will freeze)
def on_btnSelect():

    def thread_btnSelect():
        # Run button action (example to select an item and display its name)
        RDK = Robolink()
        item = RDK.ItemUserPick('Select an item')
        if item.Valid():
            RDK.ShowMessage("You selected the item: " + item.Name())

    threading.Thread(target=thread_btnSelect).start()


# Set the window title (must be unique for the docking to work, try to be creative)
window_title = 'RoboDK API Docked Window'
window.title(window_title)

# Delete the window when we close it
window.protocol("WM_DELETE_WINDOW", onClose)

# Add a button (Select action)
btnSelect = Button(window, text='Trigger on_btnSelect', height=5, width=60, command=on_btnSelect)
btnSelect.pack(fill=X)

# Embed the window
EmbedWindow(window_title)

# Run the window event loop. This is like an app and will block until we close the window
window.mainloop()

# This example shows how to quickly calculate the cycle time of a program
# as a function of the linear and joint speeds
#
# Important notes and tips for accurate cycle time calculation:
# https://robodk.com/doc/en/General.html#CycleTime

# Start the RoboDK API
from robodk.robolink import *  # RoboDK API

RDK = Robolink()

# Ask the user to select a program
program = RDK.ItemUserPick('Select a program (make sure the program does not change the robot speed)', ITEM_TYPE_PROGRAM)

# Retrieve the robot linked to the selected program
robot = program.getLink(ITEM_TYPE_ROBOT)

# Output the linear speed, joint speed and time (separated by tabs)
writeline = "Linear Speed (mm/s)\tJoint Speed (deg/s)\tCycle Time(s)"
print(writeline)
# Prepare an HTML message we can show to the user through the RoboDK API:
msg_html = "<table border=1><tr><td>" + writeline.replace('\t', '</td><td>') + "</td></tr>"

for speed_lin in [1, 5, 10, 20, 50, 100, 200, 500]:
    for speed_joints in [1, 5, 10, 20, 50, 100, 200, 500]:
        # Set the robot speed
        robot.setSpeed(speed_lin, speed_joints)

        # Update the program and retrieve updated information:
        # https://robodk.com/doc/en/PythonAPI/robodk.html#robodk.robolink.Item.Update
        result = program.Update()
        instructions, time, travel, ok, error = result

        # Print the information
        newline = "%.1f\t%.1f\t%.1f" % (speed_lin, speed_joints, time)
        print(newline)
        msg_html = msg_html + '<tr><td>' + newline.replace('\t', '</td><td>') + '</td></tr>'

msg_html = msg_html + '</table>'

RDK.ShowMessage(msg_html)

# This macro allows updating the tool given an ID that is passed as an argument
# If no ID is passed as argument it will pop up a message
# This macro can be used together with a robot machining project to change the tool as it simulates
# Double click your robot machining project, select Program Events, and enter SetTool(%1) on change tool event
# https://robodk.com/doc/en/RoboDK-API.html
import sys  # allows getting the passed argument parameters
from robodk.robolink import *
from robodk.robodialogs import *

RDK = Robolink()

TOOL_ID = 0
if len(sys.argv) >= 2:
    TOOL_ID = int(sys.argv[1])
else:
    tool_str = mbox("Enter the tool number:\n(for example, for Tool 1 set 1)", entry='1')
    if not tool_str:
        # No input
        quit()
    TOOL_ID = int(tool_str)

# Select a robot
robot = RDK.Item('', ITEM_TYPE_ROBOT)
if not robot.Valid():
    raise Exception("Robot not available")

# Create the tool name
tool_name = 'Tool ' + str(TOOL_ID)
print("Using robot: " + robot.Name())
print("Setting tool: " + tool_name)

# Select the tool
tool = RDK.Item(tool_name, ITEM_TYPE_TOOL)
if not tool.Valid():
    raise Exception("Tool %s does not exist!" % tool_name)

# Update the robot to use the tool
robot.setTool(tool)

print("Done!")

# This example takes 2 objects:
# 1 - Object with features (curves and/or points)
# 2 - Object with surface (additional features are ignored)
# This example projects the features (points/curves) to the reference surface and calculates the normals to the surface

from robodk.robolink import *  # RoboDK API

RDK = Robolink()

# Set to True to invert the normals (flip the normals)
FlipNormals = False

# Set the type of projection
ProjectionType = PROJECTION_ALONG_NORMAL_RECALC
# Available values include:
#PROJECTION_NONE                = 0 # No curve projection
#PROJECTION_CLOSEST             = 1 # The projection will be the closest point on the surface
#PROJECTION_ALONG_NORMAL        = 2 # The projection will be done along the normal.
#PROJECTION_ALONG_NORMAL_RECALC = 3 # The projection will be done along the normal. Furthermore, the normal will be recalculated according to the surface normal.
#PROJECTION_CLOSEST_RECALC      = 4 # The projection will be the closest point on the surface and the normals will be recalculated
#PROJECTION_RECALC              = 5 # The normals are recalculated according to the surface normal of the closest projection

#-------------------------------------------------------------
# Ask the user to provide the object with the features
object_features = RDK.ItemUserPick("Select object with the features to project (curves and/or points)", ITEM_TYPE_OBJECT)
if not object_features.Valid():
    quit()

# Ask the user to provide the object with the surface used as a reference
object_surface = RDK.ItemUserPick("Select Surface Object to project features", ITEM_TYPE_OBJECT)
if not object_surface.Valid():
    quit()

# Create a duplicate copy of the surface object
object_surface.Copy()
new_object = RDK.Paste(object_surface.Parent())
new_object.setName("Recalculated Normals")
new_object.setVisible(True)

# Hide the objects used to build the new object with the desired curves
object_features.setVisible(False)
object_surface.setVisible(False)

# Turn Off rendering (faster)
RDK.Render(False)

# Add all curves, projected as desired (iterate through all curves until no more curves are found)
curve_id = 0
while True:
    # Retrieve the curve points
    curve_points, name_feature = object_features.GetPoints(FEATURE_CURVE, curve_id)
    print(name_feature)
    curve_id = curve_id + 1
    npoints = len(curve_points)
    if npoints == 0:
        break

    print("Adding curve %s with %i points" % (name_feature, npoints))
    curve_points_proj = RDK.ProjectPoints(curve_points, object_surface, ProjectionType)

    # Optionally flip the normals (ijk vector)
    if FlipNormals:
        for ci in range(len(curve_points_proj)):
            x, y, z, i, j, k = curve_points_proj[ci]
            curve_points_proj[ci] = [x, y, z, -i, -j, -k]

    RDK.AddCurve(curve_points_proj, new_object, True, PROJECTION_NONE)

# Add all points projected
point_list, name_feature = object_features.GetPoints(FEATURE_POINT)
npoints = len(point_list)
print("Adding %i points" % npoints)
if npoints > 0:
    #RDK.AddPoints(point_list, new_object, True, PROJECTION_ALONG_NORMAL_RECALC)
    point_list_proj = RDK.ProjectPoints(point_list, object_surface, ProjectionType)
    RDK.AddPoints(point_list_proj, new_object, True, PROJECTION_NONE)
    #RDK.AddCurve(curve_points, new_object, True, PROJECTION_NONE)

# Set the curve width
new_object.setValue('DISPLAY', 'LINEW=2')
# Set the curve color
new_object.setColorCurve([0.0, 0.5, 0.5])

# Turn On rendering (Optional)
RDK.Render(True)
print("Done")

# This macro shows how to average the normals of an object containing curves.
# This macro can also filter points that are too close to each other.
# The use must select an object, then, a copy of this object is created with the normals averaged.

# For more information about the RoboDK API:
# Documentation: https://robodk.com/doc/en/RoboDK-API.html
# Reference:     https://robodk.com/doc/en/PythonAPI/index.html
#-------------------------------------------------------

# Enter the size of the average filter, in number of samples.
# If this value is set to -1 it will popup a message asking the user to enter a value
FilterNormalSamples = -1  # in samples

# Enter the distance, in mm, to filter close points.
# For example, if we want one point each 2 mm at most, we should enter 2.
# Set to -1 to not filter the number of points.
FilterPointDistance = -1  # in mm

# ------------------------------------------------------
# Start the RoboDK API
from robodk.robolink import *  # RoboDK API
from robodk.robomath import *  # Robot toolbox
from robodk.robodialogs import *

RDK = Robolink()

# Ask the user to select the object
obj = RDK.ItemUserPick("Select the object or the tool to filter curves")  # we can optionally filter by ITEM_TYPE_OBJECT or ITEM_TYPE_TOOL (not both)
# Exit if the user selects cancel
if not obj.Valid():
    quit()

# Ask the user to enter the filter size
if FilterNormalSamples <= 0:
    str_avg_filter = mbox("Enter the filter size (the number of points/normals used for the average filter).\nFor example, if the filter size is 10 units, the 10 closest normals are used to average each individual normal.", entry="10")
    if not str_avg_filter:
        # The user selected cancel
        quit()
    # Convert the user input to an integer
    FilterNormalSamples = int(str_avg_filter)
    if FilterNormalSamples <= 0:
        RDK.ShowMessage("Invalid Filter value. Enter a value >= 1", False)
        raise Exception(msg)

# Iterate through all object curves, extract the curve points and average the normals
curve_id = 0
obj_filtered = None
while True:
    points, name_feature = obj.GetPoints(FEATURE_CURVE, curve_id)
    # points is a double array of float with np points and xyzijk data for each point
    # point[np] = [x,y,z,i,j,k] # where xyz is the position and ijk is the tool orientation (Z axis, usually the normal to the surface)
    np = len(points)
    # when curve_id is out of bounds, an empty double array is returned
    if np == 0 or len(points[0]) < 6:
        break

    msg = "Filtering: " + name_feature
    print(msg)
    RDK.ShowMessage(msg, False)
    curve_id = curve_id + 1

    # For each point, average the normals in the range of points [-FilterNormalSamples/2 ; +FilterNormalSamples/2]
    new_normals = []
    for i in range(np):
        id_avg_from = round(max(0, i - 0.5 * FilterNormalSamples))
        id_avg_to = round(min(np - 1, i + 0.5 * FilterNormalSamples))

        # Make sure we account for the start and end sections (navg is usually FilterNormalSamples, except near the borders)
        n_avg = id_avg_to - id_avg_from
        normal_i = [0, 0, 0]
        for j in range(id_avg_from, id_avg_to):
            ni = points[j][3:6]
            normal_i = add3(normal_i, ni)

        # Normalize
        normal_i = normalize3(normal_i)
        #this would not be correct: normal_i = mult3(normal_i, 1.0/n_avg)

        # Add the new normal to the list
        new_normals.append(normal_i)

    # Combine the normals with the list of points
    for i in range(np):
        points[i][3:6] = new_normals[i][0:3]

    # Filter points, if desired
    if FilterPointDistance > 0:
        lastp = None
        points_filtered = []
        points_filtered.append(points[0])
        lastp = points[0]

        for i in range(1, np):
            if distance(lastp, points[i]) > FilterPointDistance:
                points_filtered.append(points[i])
                lastp = points[i]

        points = points_filtered

    # For the first curve: create a new object, rename it and place it in the same location of the original object
    if obj_filtered is None:
        obj_filtered = RDK.AddCurve(points, 0, False, PROJECTION_NONE)
        obj_filtered.setName(obj.Name() + " Filtered")
        obj_filtered.setParent(obj.Parent())
        obj_filtered.setGeometryPose(obj_filtered.GeometryPose())

    else:
        # After the first curve has been added, add following curves to the same object
        RDK.AddCurve(points, obj_filtered, True, PROJECTION_NONE)

# Set the curve display width
obj_filtered.setValue('DISPLAY', 'LINEW=2')
# Set the curve color as RGBA values [0-1.0]
obj_filtered.setColorCurve([0.0, 0.5, 1.0, 0.8])

#This Script changes the normals of the curve to point in +Z direction by changing the i,j,k vectors to (0,0,1)

#----------------------------------------
# Start the RoboDK API
from robodk.robolink import *  # RoboDK API

RDK = Robolink()

# Ask the user to select the object
obj = RDK.ItemUserPick("Select the object or the tool to filter curves")  # we can optionally filter by ITEM_TYPE_OBJECT or ITEM_TYPE_TOOL (not both)
# Exit if the user selects cancel
if not obj.Valid():
    quit()

curve_list = []
i = 0

# Iterate through all object curves, extract the curve points and retrieve the curves in a list
while True:
    curve_points, name_feature = obj.GetPoints(FEATURE_CURVE, i)
    if len(curve_points) == 0:
        break
    i = i + 1
    curve_list.append(curve_points)
# Retrieved list contains each points as [x,y,z,i,j,k]
# Change the i,j,k vectors to 0,0,1
for curve in curve_list:
    for idx in range(len(curve)):
        i, j, k = curve[idx][3:6]
        curve[idx][3:6] = [0, 0, 1]
    #print(new_curve)

#Add all maniplated curves as a new object
obj_new = None
for curve in curve_list:
    obj_new = RDK.AddCurve(curve, obj_new, True, PROJECTION_NONE)

# Set the new object name and parent
obj_new.setName(obj.Name() + " New")
obj_new.setParent(obj.Parent())
obj_new.setGeometryPose(obj_new.GeometryPose())

# Set the curve display width
obj_new.setValue('Display', 'LINEW=4')

# Set the curve color as RGBA values [0-1.0]
obj_new.setColorCurve([0.0, 0.5, 1.0, 0.8])

#Delete all curves on the object first retrieved
obj.setParam("DeleteCurves")

# This script allows you to attach an object to a robot link

from robodk.robolink import *  # API to communicate with RoboDK
from robodk.robodialogs import *

RDK = Robolink()  # Initialize the RoboDK API

# Select the robot
# Add the shape to a robot link
robot = RDK.ItemUserPick('Select a robot to add an object', ITEM_TYPE_ROBOT)
if not robot.Valid():
    raise Exception('No robot selected. Add a robot to attach this geometry to a link')

while True:
    value = mbox('Enter the joint to attach the object to:\n' + robot.Name() + '\nExample: 0 is the base, 3 is joint 3', entry='3')
    if not value:
        quit(0)

    link_id = int(value)
    robotlink = robot.ObjectLink(link_id)
    if robotlink.Valid():
        break

    RDK.ShowMessage("Invalid link. Select a valid robot link")

object = RDK.ItemUserPick('Select the object to add', ITEM_TYPE_OBJECT)
if not object.Valid():
    raise Exception('Object not selected')

# List of objects to merge (first one must be the robot link)
merge = [robotlink, object]

# Merge the object
RDK.MergeItems(merge)

# Set original object invisible (or delete)
object.setVisible(False)
# object.Delete()

# This macro allows moving a robot using the keyboard
# More information about the RoboDK API here:
# https://robodk.com/doc/en/RoboDK-API.html
# Type help("robodk.robolink") or help("robodk.robomath") 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 robodk.robolink import *  # API to communicate with RoboDK
from robodk.robomath import *  # basic matrix operations

RDK = Robolink()

# Arrow keys program example

# get a robot
robot = RDK.Item('', ITEM_TYPE_ROBOT)
if not robot.Valid():
    print("No robot in the station. Load a robot first, then run this program.")
    pause(5)
    raise Exception("No robot in the station!")

print('Using robot: %s' % robot.Name())
print('Use the arrows (left, right, up, down), Q and A keys to move the robot')
print('Note: This works on console mode only, you must run the PY file separately')

# define the move increment
move_speed = 10

from msvcrt import getch
while True:
    key = (ord(getch()))
    move_direction = [0, 0, 0]
    # print(key)
    if key == 75:
        print('arrow left (Y-)')
        move_direction = [0, -1, 0]
    elif key == 77:
        print('arrow right (Y+)')
        move_direction = [0, 1, 0]
    elif key == 72:
        print('arrow up (X-)')
        move_direction = [-1, 0, 0]
    elif key == 80:
        print('arrow down (X+)')
        move_direction = [1, 0, 0]
    elif key == 113:
        print('Q (Z+)')
        move_direction = [0, 0, 1]
    elif key == 97:
        print('A (Z-)')
        move_direction = [0, 0, -1]

    # make sure that a movement direction is specified
    if norm(move_direction) <= 0:
        continue

    # calculate the movement in mm according to the movement speed
    xyz_move = mult3(move_direction, move_speed)

    # get the robot joints
    robot_joints = robot.Joints()

    # get the robot position from the joints (calculate forward kinematics)
    robot_position = robot.SolveFK(robot_joints)

    # get the robot configuration (robot joint state)
    robot_config = robot.JointsConfig(robot_joints)

    # calculate the new robot position
    new_robot_position = transl(xyz_move) * robot_position

    # calculate the new robot joints
    new_robot_joints = robot.SolveIK(new_robot_position)
    if len(new_robot_joints.tolist()) < 6:
        print("No robot solution!! The new position is too far, out of reach or close to a singularity")
        continue

    # calculate the robot configuration for the new joints
    new_robot_config = robot.JointsConfig(new_robot_joints)

    if robot_config[0] != new_robot_config[0] or robot_config[1] != new_robot_config[1] or robot_config[2] != new_robot_config[2]:
        print("Warning!! Robot configuration changed!! This will lead to unextected movements!")
        print(robot_config)
        print(new_robot_config)
    else:

        # move the robot joints to the new position
        robot.MoveJ(new_robot_joints)
        #robot.MoveL(new_robot_joints)

# Type help("robodk.robolink") or help("robodk.robomath") for more information
# Press F5 to run the script
# More information about the RoboDK API here:
# https://robodk.com/doc/en/RoboDK-API.html
# Note: you do not need to keep a copy of this file, your python script is saved with the station
from robodk.robolink import *  # API to communicate with RoboDK

# Start RoboDK API
RDK = Robolink()

# gather all robots as item objects
robots = RDK.ItemList(ITEM_TYPE_ROBOT, False)

# loop through all the robots and connect to the robot
errors = ''
count = 0
for robot in robots:
    count = count + 1

    # force disconnect from all robots by simulating a double click
    #if count == 0:
    #    robot.Disconnect()
    #    robot.Disconnect()
    #    pause(1)

    # Important, each robot needs a new API connection to allow moving them separately in different threads (if required)
    rdk = Robolink()
    robot.link = rdk

    # Force simulation mode in case we are already connected to the robot.
    # Then, gather the joint position of the robots.
    # This will gather the position of the simulated robot instead of the real robot.
    rdk.setRunMode(RUNMODE_SIMULATE)
    jnts = robot.Joints()

    # connect to the robot:
    # rdk.setRunMode(RUNMODE_RUN_ROBOT) # not needed because connect will automatically do it
    # state = robot.ConnectSafe()
    state = robot.Connect()
    print(state)

    # Check the connection status and message
    state, msg = robot.ConnectedState()
    print(state)
    print(msg)
    if state != ROBOTCOM_READY:
        errors = errors + 'Problems connecting: ' + robot.Name() + ': ' + msg + '\n'
    else:
        # move to the joint position in the simulator:
        robot.MoveJ(jnts, False)

# Display connection errors, if any
if len(errors) > 0:
    print(errors)
    raise Exception(errors)
else:
    quit(0)

# This macro will record the robot joints (in degrees) in a CSV file (including a time stamp in seconds). This file avoids recording the same joints twice.
# Tip: Use the script JointsPlayback.py to move along the recorded joints

from robodk.robolink import *  # API to communicate with RoboDK
from robodk.robomath import *  # basic matrix operations
from time import gmtime, strftime

RDK = Robolink()

# Ask the user to select a robot arm (mechanisms are ignored)
robot = RDK.ItemUserPick('Select a robot', ITEM_TYPE_ROBOT_ARM)
if not robot.Valid():
    raise Exception("Robot is not available")

# Generate a file in the same folder where we have the RDK file
file_path = RDK.getParam('PATH_OPENSTATION') + '/joints-' + strftime("%Y-%m-%d %H-%M-%S", gmtime()) + '.csv'


def joints_changed(j1, j2, tolerance=0.0001):
    """Returns True if joints 1 and joints 2 are different"""
    if j1 is None or j2 is None:
        return True

    for j1, j2 in zip(j1, j2):
        if abs(j1 - j2) > tolerance:
            return True

    return False


# Infinite loop to record robot joints
joints_last = None
print("Recording robot joints to file: " + file_path)
with open(file_path, 'a') as fid:
    tic()
    while True:
        time = toc()
        joints = robot.Joints().list()
        if joints_changed(joints, joints_last):
            print('Time (s): ' + str(time))
            fid.write(str(joints)[1:-1] + (", %.3f" % time) + '\n')
            joints_last = joints
            pause(0.01)

# This script creates a thread to monitor the position and other variables from a real UR robot
# With this script active, RoboDK will create a new target when the robot is moved a certain tolerance
#
# More information about the RoboDK API here:
# https://robodk.com/doc/en/RoboDK-API.html
# Press F5 to run the script
# Or visit: https://robodk.com/doc/en/PythonAPI/index.html
from robodk.robolink import *  # API to communicate with RoboDK
from robodk.robomath import *  # Robot toolbox
import threading
import socket
import struct
import os
import time

# Refresh the screen every time the robot position changes
TOLERANCE_JOINTS_REFRESH = 0.1
RETRIEVE_JOINTS_ONCE = False  # If True, the current robot position will be retrieved once only

# Create targets given a tolerance in degrees
CREATE_TARGETS = False
TOLERANCE_JOINTS_NEWTARGET = 10  # in degrees

REFRESH_RATE = 0.01

# Make current robot joints accessible in case we run it on a separate thread
global ROBOT_JOINTS


# Procedure to check if robot joint positions are different according to a certain tolerance
def Robot_Joints_Check(jA, jB, tolerance_deg=1):
    if jA is None:
        return True

    for i in range(6):
        if abs(jA[i] - jB[i]) > tolerance_deg * pi / 180:
            return True
    return False


#########################################################################
# Byte shifts to point to the right byte data inside a packet
UR_GET_TIME = 1
UR_GET_JOINT_POSITIONS = 252
UR_GET_JOINT_SPEEDS = 300
UR_GET_JOINT_CURRENTS = 348
UR_GET_TCP_FORCES = 540


# Get packet size according to the byte array
def packet_size(buf):
    if len(buf) < 4:
        return 0
    return struct.unpack_from("!i", buf, 0)[0]


# Check if a packet is complete
def packet_check(buf):
    msg_sz = packet_size(buf)
    if len(buf) < msg_sz:
        print("Incorrect packet size %i vs %i" % (msg_sz, len(buf)))
        return False

    return True


# Get specific information from a packet
def packet_value(buf, offset, nval=6):
    if len(buf) < offset + nval:
        print("Not available offset (maybe older Polyscope version?): %i - %i" % (len(buf), offset))
        return None
    format = '!'
    for i in range(nval):
        format += 'd'
    return list(struct.unpack_from(format, buf, offset))  #return list(struct.unpack_from("!dddddd", buf, offset))


# Action to take when a new packet arrives
def on_packet(packet):
    global ROBOT_JOINTS
    # Retrieve desired information from a packet
    rob_joints_RAD = packet_value(packet, UR_GET_JOINT_POSITIONS)
    ROBOT_JOINTS = [ji * 180.0 / pi for ji in rob_joints_RAD]
    #ROBOT_SPEED = packet_value(packet, UR_GET_JOINT_SPEEDS)
    #ROBOT_CURRENT = packet_value(packet, UR_GET_JOINT_CURRENTS)
    #print(ROBOT_JOINTS)


# Monitor thread to retrieve information from the robot
def UR_Monitor():
    while True:
        rt_socket = socket.create_connection((ROBOT_IP, ROBOT_PORT))
        buf = b''
        packet_count = 0
        packet_time_last = time.time()
        while True:
            more = rt_socket.recv(4096)
            if more:
                buf = buf + more
                if packet_check(buf):
                    packet_len = packet_size(buf)
                    packet, buf = buf[:packet_len], buf[packet_len:]
                    on_packet(packet)
                    packet_count += 1
                    if packet_count % 125 == 0:
                        t_now = time.time()
                        print("Monitoring at %.1f packets per second" % (packet_count / (t_now - packet_time_last)))
                        packet_count = 0
                        packet_time_last = t_now

        rt_socket.close()


#########################################################################

# Enter RoboDK IP and Port
ROBOT_IP = None  #'192.168.2.31'
ROBOT_PORT = 30003

# Start RoboDK API
RDK = Robolink()

# Retrieve a robot
robot = RDK.ItemUserPick('Select a UR robot to retrieve current position', ITEM_TYPE_ROBOT)
if not robot.Valid():
    quit()

# Retrieve Robot's IP:
if ROBOT_IP is None:
    ip, port, path, ftpuser, ftppass = robot.ConnectionParams()
    ROBOT_IP = ip

ROBOT_JOINTS = None
last_joints_target = None
last_joints_refresh = None

# Start the Robot Monitor thread
#q = queue.Queue()
t = threading.Thread(target=UR_Monitor)
t.daemon = True
t.start()
#UR_Monitor()

# Start the main loop to refresh RoboDK and create targets/programs automatically
target_count = 0
while True:
    # Wait for a valid robot joints reading
    if ROBOT_JOINTS is None:
        continue

    # Set the robot to that position
    if Robot_Joints_Check(last_joints_refresh, ROBOT_JOINTS, TOLERANCE_JOINTS_REFRESH):
        last_joints_refresh = ROBOT_JOINTS
        robot.setJoints(ROBOT_JOINTS)

    # Stop here if we need only the current position
    if RETRIEVE_JOINTS_ONCE:
        quit(0)

    # Check if the robot has moved enough to create a new target
    if CREATE_TARGETS and Robot_Joints_Check(last_joints_target, ROBOT_JOINTS, TOLERANCE_JOINTS_NEWTARGET):
        last_joints_target = ROBOT_JOINTS
        target_count = target_count + 1
        newtarget = RDK.AddTarget('T %i' % target_count, 0, robot)

    # Take a short break
    pause(REFRESH_RATE)