5. Examples

These examples are included in the /RoboDK/Library/Macros folder and some of them are also used in sample RDK files provided in the /RoboDK/Library folder.

These examples were made using Python 3 and might require slight adjustments to work on Python 2.

5.1. Offline Programming

This example shows how to generate a hexagonal path given one target. This macro draws a polygon of radius R and n_sides vertices using the RoboDK API for Python.

More information here:
Offline programming - Weld test
# This macro shows an example to draw a polygon of radius R and n_sides vertices using the RoboDK API for Python
from robolink import *    # API to communicate with RoboDK for simulation and offline/online programming
from robodk import *      # Robotics toolbox for industrial robots

# 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.setZoneData(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')

5.2. Offline Programming (GUI)

This example is an improved version of the previous example. It prompts the user for some input before simulating or generating a program. This example shows how RoboDK and the Python GUI tkinter can display graphical user interface to customize program generation according to certain parameters.

More information here:
Offline programming with GUI
# This example shows how RoboDK and the Python GUI tkinter can display graphical user interface to customize program generation according to certain parameters
from robolink import *    # API to communicate with RoboDK for simulation and offline/online programming
from robodk import *      # Robotics toolbox for industrial robots

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

5.3. Online Programming

This example is a modified version of the previous two examples which supports running the program on the robot directly from the script. This example will run a Python program on the robot from the Python API (online programming). If a robot is connected to the PC, the simulation and the real robot will move at the same time as the Python program is executed. The same program can also be used for simulation or offline programming.

More information here:
Online programming
# 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)
#
from robolink import *    # API to communicate with RoboDK for simulation and offline/online programming
from robodk import *      # Robotics toolbox for industrial robots

# 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.setZoneData(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") 

5.4. Move Through Points

This example shows different ways of moving a robot between two points (a line) or a list of points.

Move robot through points
# 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 robolink import *    # API to communicate with RoboDK for simulation and offline/online programming
from robodk 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_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()


5.5. Connect to Robots

This example shows how to connect to all robots available in the RoboDK station using robot drivers and move the robot to the positions set in RoboDK. This example shows how to communicate with more than one robot at the same time.

More information here:
Move robots online
from robolink import *    # API to communicate with RoboDK for simulation and offline/online programming
from robodk import *      # Robotics toolbox for industrial robots

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



5.6. Monitor Joints

This example shows how to save the simulated position of a robot to a text or CSV file.

Monitor simulated joints
# This macro will save a time stamp and robot joints each 50 ms
from robolink import *    # API to communicate with RoboDK for simulation and offline/online programming
from robodk import *      # Robotics toolbox for industrial robots
RDK = Robolink()

robot = RDK.Item('',ITEM_TYPE_ROBOT)

if not robot.Valid():
    raise Exception("Robot is not available")

file_path = RDK.getParam('PATH_OPENSTATION') + '/joints.txt'

fid = open(file_path,'w')
tic()
while True:
    time = toc()
    print('Current time (s):' + str(time))
    joints = str(robot.Joints().tolist())
    fid.write(str(time) + ', ' + joints[1:-1] + '\n')
    pause(0.05)

fid.close()
            

5.7. Monitor a Real UR robot

This example shows how to monitor a Universal Robot connected to a PC. Among other things, the position of the robot, speed and acceleration can be monitored at 125 Hz.

More information here:
Monitor a UR robot
from robolink import *    # API to communicate with RoboDK for simulation and offline/online programming
from robodk import *      # Robotics toolbox for industrial robots
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 = True
TOLERANCE_JOINTS_NEWTARGET = 10 # in degrees

REFRESH_RATE = 0.1

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

5.8. Pick and Place

This example shows an advanced pick and place application using a Fanuc M-710iC/50 robot (Example 2 from the RoboDK library).

In this example all the robot movements of the Fanuc robot are managed by the Python program. Using the Python API it is possible to create, move, modify and delete any object, reference frame, robot or other items in the station.

Example 02-1 - Pick and place with python
from robolink import *    # API to communicate with RoboDK for simulation and offline/online programming
from robodk import *      # Robotics toolbox for industrial robots

# Setup global parameters
BALL_DIAMETER = 100 # diameter of one ball
APPROACH = 100      # approach distance to grab each part, in mm
nTCPs = 6           # number of TCP's in the tool

#----------------------------------------------
# Function definitions

def box_calc(BALLS_SIDE=4, BALLS_MAX=None):
    """Calculate a list of points (ball center) as if the balls were stored in a box"""
    if BALLS_MAX is None: BALLS_MAX = BALLS_SIDE**3
    xyz_list = []
    for h in range(BALLS_SIDE):
        for i in range(BALLS_SIDE):
            for j in range(BALLS_SIDE):
                xyz_list = xyz_list + [[(i+0.5)*BALL_DIAMETER, (j+0.5)*BALL_DIAMETER, (h+0.5)*BALL_DIAMETER]]
                if len(xyz_list) >= BALLS_MAX:
                    return xyz_list
    return xyz_list

def pyramid_calc(BALLS_SIDE=4):
    """Calculate a list of points (ball center) as if the balls were place in a pyramid"""
    #the number of balls can be calculated as: int(BALLS_SIDE*(BALLS_SIDE+1)*(2*BALLS_SIDE+1)/6)
    BALL_DIAMETER = 100
    xyz_list = []
    sqrt2 = 2**(0.5)
    for h in range(BALLS_SIDE):
        for i in range(BALLS_SIDE-h):
            for j in range(BALLS_SIDE-h):
                height = h*BALL_DIAMETER/sqrt2 + BALL_DIAMETER/2
                xyz_list = xyz_list + [[i*BALL_DIAMETER + (h+1)*BALL_DIAMETER*0.5, j*BALL_DIAMETER + (h+1)*BALL_DIAMETER*0.5, height]]
    return xyz_list

def balls_setup(frame, positions):
    """Place a list of balls in a reference frame. The reference object (ball) must have been previously copied to the clipboard."""
    nballs = len(positions)
    step = 1.0/(nballs - 1)
    for i in range(nballs):
        newball = frame.Paste()
        newball.setName('ball ' + str(i)) #set item name
        newball.setPose(transl(positions[i])) #set item position with respect to parent
        newball.setVisible(True, False) #make item visible but hide the reference frame
        newball.Recolor([1-step*i, step*i, 0.2, 1]) #set RGBA color

def cleanup_balls(parentnodes):
    """Delete all child items whose name starts with \"ball\", from the provided list of parent items."""
    todelete = []
    for item in parentnodes:
        todelete = todelete + item.Childs()

    for item in todelete:
        if item.Name().startswith('ball'):
            item.Delete()

def TCP_On(toolitem, tcp_id):
    """Attach the closest object to the toolitem tool pose,
    furthermore, it will output appropriate function calls on the generated robot program (call to TCP_On)"""
    toolitem.AttachClosest()
    toolitem.RDK().RunMessage('Set air valve %i on' % (tcp_id+1))
    toolitem.RDK().RunProgram('TCP_On(%i)' % (tcp_id+1));
        
def TCP_Off(toolitem, tcp_id, itemleave=0):
    """Detaches the closest object attached to the toolitem tool pose,
    furthermore, it will output appropriate function calls on the generated robot program (call to TCP_Off)"""
    toolitem.DetachAll(itemleave)
    toolitem.RDK().RunMessage('Set air valve %i off' % (tcp_id+1))
    toolitem.RDK().RunProgram('TCP_Off(%i)' % (tcp_id+1));


#----------------------------------------------------------
# The program starts here:

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

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

#RDK.Set_Simulation_Speed(500); # set the simulation speed

# Gather required items from the station tree
robot = RDK.Item('Fanuc M-710iC/50')
robot_tools = robot.Childs()
#robottool = RDK.Item('MainTool')
frame1 = RDK.Item('Table 1')
frame2 = RDK.Item('Table 2')

# Copy a ball as an object (same as CTRL+C)
ballref = RDK.Item('reference ball')
ballref.Copy()

# Run a pre-defined station program (in RoboDK) to replace the two tables
prog_reset = RDK.Item('Replace objects')
prog_reset.RunProgram()

# Call custom procedure to remove old objects
cleanup_balls([frame1, frame2])

# Make a list of positions to place the objects
frame1_list = pyramid_calc(4)
frame2_list = pyramid_calc(4)

# Programmatically place the objects with a custom-made procedure
balls_setup(frame1, frame1_list)

# Delete previously generated tools
for tool in robot_tools:
    if tool.Name().startswith('TCP'):
        tool.Delete()
        
# Calculate tool frames for the suction cup tool of 6 suction cups
TCP_list = []
for i in range(nTCPs):
    TCPi_pose = transl(0,0,100)*rotz((360/nTCPs)*i*pi/180)*transl(125,0,0)*roty(pi/2)
    TCPi = robot.AddTool(TCPi_pose, 'TCP %i' % (i+1))
    TCP_list.append(TCPi)

TCP_0 = TCP_list[0]

# Turn on automatic rendering
RDK.Render(True)

# Move balls    
robot.setPoseTool(TCP_list[0])
nballs_frame1 = len(frame1_list)
nballs_frame2 = len(frame2_list)
idTake = nballs_frame1 - 1
idLeave = 0
idTCP = 0
target_app_frame = transl(2*BALL_DIAMETER, 2*BALL_DIAMETER, 4*BALL_DIAMETER)*roty(pi)*transl(0,0,-APPROACH)

while idTake >= 0:
    # ------------------------------------------------------------------
    # first priority: grab as many balls as possible
    # the tool is empty at this point, so take as many balls as possible (up to a maximum of 6 -> nTCPs)
    ntake = min(nTCPs, idTake + 1)

    # approach to frame 1
    robot.setPoseFrame(frame1)
    robot.setPoseTool(TCP_0)
    robot.MoveJ([0,0,0,0,10,-200])
    robot.MoveJ(target_app_frame)

    # grab ntake balls from frame 1
    for i in range(ntake):
        TCPi = TCP_list[i]
        robot.setPoseTool(TCPi)
        # calculate target wrt frame1: rotation about Y is needed since Z and X axis are inverted
        target = transl(frame1_list[idTake])*roty(pi)*rotx(30*pi/180)
        target_app = target*transl(0,0,-APPROACH)
        idTake = idTake - 1        
        robot.MoveL(target_app)
        robot.MoveL(target)
        TCP_On(TCPi, i)
        robot.MoveL(target_app)
 
    # ------------------------------------------------------------------
    # second priority: unload the tool     
    # approach to frame 2 and place the tool balls into table 2
    robot.setPoseTool(TCP_0)
    robot.MoveJ(target_app_frame)
    robot.MoveJ([0,0,0,0,10,-200])
    robot.setPoseFrame(frame2)    
    robot.MoveJ(target_app_frame)
    for i in range(ntake):
        TCPi = TCP_list[i]
        robot.setPoseTool(TCPi)
        if idLeave > nballs_frame2-1:
            raise Exception("No room left to place objects in Table 2")
        
        # calculate target wrt frame1: rotation of 180 about Y is needed since Z and X axis are inverted
        target = transl(frame2_list[idLeave])*roty(pi)*rotx(30*pi/180)
        target_app = target*transl(0,0,-APPROACH)
        idLeave = idLeave + 1        
        robot.MoveL(target_app)
        robot.MoveL(target)
        TCP_Off(TCPi, i, frame2)
        robot.MoveL(target_app)

    robot.MoveJ(target_app_frame)

# Move home when the robot finishes
robot.MoveJ([0,0,0,0,10,-200])

5.9. Drawing an SVG image

A robot is programmed given an SVG image to simulate a drawing application. An ABB IRB 4600-20/2.50 is used in this example.

3D HTML simulation of this example: https://www.robodk.com/simulations/Robot-Drawing.html

Example 03 - Drawing with a robot
from robolink import *    # API to communicate with RoboDK for simulation and offline/online programming
from robodk import *      # Robotics toolbox for industrial robots

import sys 
import os
import re
   
#--------------------------------------------------------------------------------
# function definitions:

def point2D_2_pose(point, tangent):
    """Converts a 2D point to a 3D pose in the XY plane (4x4 homogeneous matrix)"""
    return transl(point.x, point.y, 0)*rotz(tangent.angle())

def svg_draw_quick(svg_img, board, pix_ref):
    """Quickly shows the image result without checking the robot movements."""
    RDK.Render(False)
    count = 0
    for path in svg_img:
        count = count + 1
        # use the pixel reference to set the path color, set pixel width and copy as a reference
        pix_ref.Recolor(path.fill_color)
        np = path.nPoints()
        print('drawing path %i/%i' % (count, len(svg_img)))
        for i in range(np):
            p_i = path.getPoint(i)
            v_i = path.getVector(i)
            pt_pose = point2D_2_pose(p_i, v_i)
            
            # add the pixel geometry to the drawing board object, at the calculated pixel pose
            board.AddGeometry(pix_ref, pt_pose)
            
    RDK.Render(True)

def svg_draw_robot(svg_img, board, pix_ref, item_frame, item_tool, robot):
    """Draws the image with the robot. It is slower that svg_draw_quick but it makes sure that the image can be drawn with the robot."""

    APPROACH = 100  # approach distance in MM for each path    
    home_joints = [0,0,0,0,90,0] # home joints, in deg
    
    robot.setPoseFrame(item_frame)
    robot.setPoseTool(item_tool)
    robot.MoveJ(home_joints)
    
    # get the target orientation depending on the tool orientation at home position
    orient_frame2tool = invH(item_frame.Pose())*robot.SolveFK(home_joints)*item_tool.Pose()
    orient_frame2tool[0:3,3] = Mat([0,0,0])
    # alternative: orient_frame2tool = roty(pi)

    for path in svg_img:
        # use the pixel reference to set the path color, set pixel width and copy as a reference
        print('Drawing %s, RGB color = [%.3f,%.3f,%.3f]'%(path.idname, path.fill_color[0], path.fill_color[1], path.fill_color[2]))
        pix_ref.Recolor(path.fill_color) 
        np = path.nPoints()

        # robot movement: approach to the first target
        p_0 = path.getPoint(0)
        target0 = transl(p_0.x, p_0.y, 0)*orient_frame2tool
        target0_app = target0*transl(0,0,-APPROACH)
        robot.MoveL(target0_app)
        RDK.RunMessage('Drawing %s' % path.idname);
        RDK.RunProgram('SetColorRGB(%.3f,%.3f,%.3f)' % (path.fill_color[0], path.fill_color[1], path.fill_color[2]))
        for i in range(np):
            p_i = path.getPoint(i)
            v_i = path.getVector(i)
            pt_pose = point2D_2_pose(p_i, v_i)            
        
            # robot movement:
            target = transl(p_i.x, p_i.y, 0)*orient_frame2tool #keep the tool orientation constant
            #target = pt_pose*orient_frame2tool #moving the tool along the path (axis 6 may reach its limits)
            robot.MoveL(target)

            # create a new pixel object with the calculated pixel pose
            board.AddGeometry(pix_ref, pt_pose)

        target_app = target*transl(0,0,-APPROACH)
        robot.MoveL(target_app)
        
    robot.MoveL(home_joints)

#--------------------------------------------------------------------------------
# Program start
RDK = Robolink()

# locate and import the svgpy module
path_stationfile = RDK.getParam('PATH_OPENSTATION')
sys.path.append(os.path.abspath(path_stationfile)) # temporary add path to import station modules
from svgpy.svg import *

# select the file to draw
svgfile = path_stationfile + '/World map.svg'
#svgfile = path_stationfile + '/RoboDK text.svg'

# import the SVG file
svgdata = svg_load(svgfile)

IMAGE_SIZE = Point(1000,2000)   # size of the image in MM
MM_X_PIXEL = 10                 # in mm the path will be cut is depending on the pixel size
svgdata.calc_polygon_fit(IMAGE_SIZE, MM_X_PIXEL)
size_img = svgdata.size_poly()  # returns the size of the current polygon

# get the robot, frame and tool objects
robot = RDK.Item('ABB IRB 4600-20/2.50')
framedraw = RDK.Item('Frame draw')
tooldraw = RDK.Item('Tool')

# get the pixel reference to draw
pixel_ref = RDK.Item('pixel')

# delete previous image if any
image = RDK.Item('Board & image')
if image.Valid() and image.Type() == ITEM_TYPE_OBJECT: image.Delete()

# make a drawing board base on the object reference "Blackboard 250mm"
board_1m = RDK.Item('Blackboard 250mm')
board_1m.Copy()
board_draw = framedraw.Paste()
board_draw.setName('Board & image')
board_draw.Scale([size_img.x/250, size_img.y/250, 1]) # adjust the board size to the image size (scale)

# quickly show the final result without checking the robot movements:
#svg_draw_quick(svgdata, board_draw, pixel_ref)

# draw the image with the robot:
svg_draw_robot(svgdata, board_draw, pixel_ref, framedraw, tooldraw, robot)

5.10. Synchronize 3 Robots

This example shows to synchronize multiple robots at the same time. The robots can be synchronized together given keypoints and using Python threads. This example is similar to Offline Programming but updated to support moving multiple robots at the same time. The robots can be connected to the computer using appropriate robot drivers and switch from the simulation to moving the real robots.

Offline programming - 3 robots simultaneously
from robolink import *    # API to communicate with RoboDK for offline/online programming
from robodk import *      # Robotics toolbox for industrial robots

import threading
import queue

#----------------------------------------------
# Function definitions and global variable declarations

# Global variables used to synchronize the robot movements
# These variables are managed by SyncSet() and SynchWait()

SYNC_COUNT = 0
SYNC_TOTAL = 0
SYNC_ID = 0
lock = threading.Lock()

def SyncSet(total_sync):
    """SyncSet will set the number of total robot programs (threads) that must be synchronized togeter.
    Every time SyncSet is called SYNC_ID is increased by one."""
    global SYNC_COUNT
    global SYNC_TOTAL
    global SYNC_ID    
    with lock:
        SYNC_COUNT = 0
        SYNC_TOTAL = total_sync
        SYNC_ID = SYNC_ID + 1
        #print('SyncSet')

def SyncWait():
    """SyncWait will block the robot movements for a robot when necessary, synchronizing the movements sequentially.
    Use SyncSet(nrobots) to define how many robots must be synchronized together."""
    global SYNC_COUNT
    # Save a local variable with the sync event id
    sync_id = SYNC_ID
    with lock:
        # Increase the number of threads that are synchronized
        SYNC_COUNT += 1

    # Move to the next sync event if all threads reached the SyncWait (SYNC_COUNT = SYNC_TOTAL)
    if SYNC_COUNT >= SYNC_TOTAL:
        SyncSet(SYNC_TOTAL)
        return

    # Wait for a SynchSet to move forward
    while sync_id >= SYNC_ID:
        time.sleep(0.0001)


# Main procedure to move each robot   
def DoWeld(q, robotname):
    # Any interaction with RoboDK must be done through Robolink()
    # Each robot movement requires a new Robolink() object (new link of communication).
    # Two robots can't be moved by the same communication link.
    
    rdk = Robolink()

    # get the robot item:
    robot = rdk.Item(robotname)

    # get the home joints target
    home = robot.JointsHome()

    # get the reference welding target:
    target = rdk.Item('Target')

    # get the reference frame and set it to the robot
    reference = target.Parent()
    robot.setPoseFrame(reference)

    # get the pose of the target (4x4 matrix):
    poseref = target.Pose()
    pose_approach = poseref*transl(0,0,-100)

    # move the robot to home, then to the center:
    robot.MoveJ(home)
    robot.MoveJ(pose_approach)
    SyncWait()
    robot.MoveL(target)

    # make an hexagon around the center:
    for i in range(7):
        ang = i*2*pi/6 #angle: 0, 60, 120, ...
        posei = poseref*rotz(ang)*transl(200,0,0)*rotz(-ang)
        SyncWait()
        robot.MoveL(posei)

    # move back to the center, then home:
    SyncWait()
    robot.MoveL(target)
    robot.MoveL(pose_approach)
    robot.MoveJ(home)
    q.put('Robot %s finished' % robotname)

#----------------------------------------
# Python program start 
    
# retrieve all available robots in the RoboDK station (as a list of names)
RDK = Robolink()
robots = RDK.ItemList(ITEM_TYPE_ROBOT)
print(robots)

# retrieve the number of robots to synchronize together
nrobots = len(robots)
SyncSet(nrobots)

# the queue allows sharing messages between threads
q = queue.Queue()

# Start the DoWeld program for all robots. Each robot will run on a separate thread.
threads = []
for i in range(nrobots):
    robotname = robots[i]
    t = threading.Thread(target=DoWeld, args = (q, robotname))
    t.daemon = True
    t.start()
    threads.append(t)

# wait for every thead to finish
for x in threads:
    x.join()
    print(q.get())

print('Main program finished')

5.11. Robot Model (DH)

This example models the forward and inverse kinematics of an ABB IRB 120 robot using the RoboDK API for Python. Reference frames are placed according to an existing robot in the station.

Robot Model - Mirror test
from robolink import *    # API to communicate with RoboDK for simulation and offline/online programming
from robodk import *      # Robotics toolbox for industrial robots

#----------------------------------------------
# Function definitions

def FK_Robot(dh_table, joints):
    """Computes the forward kinematics of the robot.
    dh_table must be in mm and radians, the joints array must be given in degrees."""
    Habs = []
    Hrel = []    
    nlinks = len(dh_table)
    HiAbs = eye(4)
    for i in range(nlinks):
        [rz,tx,tz,rx] = dh_table[i]
        rz = rz + joints[i]*pi/180
        Hi = dh(rz,tx,tz,rx)
        HiAbs = HiAbs*Hi
        Hrel.append(Hi)
        Habs.append(HiAbs)

    return [HiAbs, Habs, Hrel]

def Frames_setup_absolute(frameparent, nframes):
    """Adds nframes reference frames to frameparent"""
    frames = []
    for i in range(nframes):
        newframe = frameparent.RDK().AddFrame('frame %i' % (i+1), frameparent)
        newframe.setPose(transl(0,0,100*i))
        frames.append(newframe)

    return frames

def Frames_setup_relative(frameparent, nframes):
    """Adds nframes reference frames cascaded to frameparent"""
    frames = []
    parent = frameparent
    for i in range(nframes):
        newframe = frameparent.RDK().AddFrame('frame %i' % (i+1), parent)
        parent = newframe
        newframe.setPose(transl(0,0,100))
        frames.append(newframe)

    return frames

def Set_Items_Pose(itemlist, poselist):
    """Sets the pose (3D position) of each item in itemlist"""
    for item, pose in zip(itemlist,poselist):
        item.setPose(pose)

def are_equal(j1, j2):
    """Returns True if j1 and j2 are equal, False otherwise"""
    if j1 is None or j2 is None:
        return False
    sum_diffs_abs = sum(abs(a - b) for a, b in zip(j1, j2))
    if sum_diffs_abs > 1e-3:
        return False
    return True
        
#----------------------------------------------------------
# The program starts here:
RDK = Robolink()        
        
#-----------------------------------------------------
# DH table of the robot: ABB IRB 120-3/0.6
DH_Table = []
#                 rZ (theta),   tX,   tZ,   rX (alpha)
DH_Table.append([          0,    0,  290,  -90*pi/180])
DH_Table.append([ -90*pi/180,  270,    0,           0])
DH_Table.append([          0,   70,    0,  -90*pi/180])
DH_Table.append([          0,    0,  302,   90*pi/180])
DH_Table.append([          0,    0,    0,  -90*pi/180])
DH_Table.append([ 180*pi/180,    0,   72,           0])

# degrees of freedom: (6 for ABB IRB 120-3/0.6)
DOFs = len(DH_Table)

# get the robot:
robot = RDK.Item('ABB IRB 120-3/0.6')

# cleanup of all items containing "Mirror tests" from previous tests
while True:
    todelete = RDK.Item('Robot base')
    # make sure an item was found
    if not todelete.Valid():
        break
    # delete only frames
    if todelete.Type() == ITEM_TYPE_FRAME:
        print('Deleting: ' + todelete.Name())
        todelete.Delete()

# setup the parent frames for the test:
parent_frameabs = RDK.AddFrame('Robot base (absolute frames)')
parent_framerel = RDK.AddFrame('Robot base (relative frames)')

# setup the child frames for the test:
frames_abs = Frames_setup_absolute(parent_frameabs, DOFs)
frames_rel = Frames_setup_relative(parent_framerel, DOFs)

# remember the last robot joints to update when necessary
last_joints = None

# infinite loop
while True:
    # get the current robot joints as a float array (list)
    joints = robot.Joints().tolist()

    # do not update if joints are the same as before
    if are_equal(joints, last_joints):
        continue

    # if joints changed, compute the forward kinematics for this position
    [Hrobot, HabsList, HrelList] = FK_Robot(DH_Table, joints)

    # turn off rendering while we update all frames:
    RDK.Render(False)
    # update all frames
    Set_Items_Pose(frames_abs, HabsList)
    Set_Items_Pose(frames_rel, HrelList)
    
    # render and turn on rendering
    RDK.Render(True)

    # remember the last robot joints
    last_joints = joints

    print('Current robot joints:')    
    print(joints)
    print('Pose of the robot (forward kinematics):')
    print(Hrobot)
    print('\n\n')