5. Examples

5.1. Weld example

Basic example from the website (offline programming section) that shows how to generate a hexagonal path given one target. This example uses a KUKA KR 6 R900 sixx.

More information here:
Offline programming - Weld test
from robolink import *    # API to communicate with RoboDK
from robodk import *      # robodk robotics toolbox

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

# get the robot by name:
robot = RDK.Item('KUKA KR 6 R900 sixx', ITEM_TYPE_ROBOT)

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

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

# move the robot to home, then to the Target 1:
robot.MoveJ(home)
robot.MoveJ(target)

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

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

5.2. 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
from robodk import *      # robodk robotics toolbox

# 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/(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 Htool 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 Htool pose,
    furthermore, it will output appropriate function calls on the generated robot program (call to TCP_Off)"""
    toolitem.DetachClosest(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.setTool(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.setFrame(frame1)
    robot.setTool(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.setTool(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.setTool(TCP_0)
    robot.MoveJ(target_app_frame)
    robot.MoveJ([0,0,0,0,10,-200])
    robot.setFrame(frame2)    
    robot.MoveJ(target_app_frame)
    for i in range(ntake):
        TCPi = TCP_list[i]
        robot.setTool(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.3. Drawing an 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
from robodk import *      # robodk robotics toolbox

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.setFrame(item_frame)
    robot.setTool(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.4. Synch 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 Weld example 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
from robodk import *      # basic matrix operations
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.setFrame(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.5. Robot modeling

Example that 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
from robodk import *      # robodk robotics toolbox

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