RoboDK API examples

The following examples show some basic usage of RoboDK. The examples explained in this section are available with the default RoboDK download.

The station files for each of these examples can be accessed through File->Open, then, select the appropriate example in the default library folder.

Weld example

This example shows an advanced pick and place application. In this example, placing all the objects is automatically done through Python. We can place any object programmatically thanks to our Python API. Using the Python API we can create, modify or delete any objects. Additionally, we can also program the robot moves in the same Python script, this will allow us to automatically generate the robot program.

from robolink import *    # API to communicate with RoboDK
from robodk import *      # basic matrix operations

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

# get the robot item:
robot = RL.Item('KUKA KR 6 R900 sixx')

# get the home target and the welding targets:
home = RL.Item('Home')
target = RL.Item('Target 1')
# get the pose of the target (4x4 matrix):
poseref = target.Pose()

# move the robot to home, then to the center:
robot.MoveJ(home)
robot.MoveJ(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)
    robot.MoveL(posei)

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

Robot modeling example

 

#type help("robolink") or help("robodk") for more information
#(note: you do not need to keep a copy of this file, your python script is saved with the station)
from robolink import *    # API to communicate with robodk
from robodk import *      # basic matrix operations
RL = Robolink()


def FK_Robot(dh_table, joints):
    """Computes the forward kinematics of the robot.
    dh_table must be in mm and radians, the joints vector must be 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 to frameparent"""
    frames = []
    for i in range(nframes):
        newframe = frameparent.RL().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 cascaded to frameparent"""
    frames = []
    parent = frameparent
    for i in range(nframes):
        newframe = frameparent.RL().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
        
#-----------------------------------------------------
# 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 = RL.Item('ABB IRB 120-3/0.6')

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

# setup the parent frames for the test:
parent_frameabs = RL.AddFrame('Robot base (absolute frames)')
parent_framerel = RL.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)


last_joints = None

tic()
while True:
    # get the current robot joints
    joints = tr(robot.Joints())
    joints = joints.rows[0]

    # do not repaint if joints are the same
    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 after every Item call while we update all frames:
    RL.Render(False)
    # update all frames
    Set_Items_Pose(frames_abs, HabsList)
    Set_Items_Pose(frames_rel, HrelList)
    # render and turn on rendering
    RL.Render(True)

    last_joints = joints

    # display some information:
    toc()
    print('Current robot joints:')    
    print(joints)
    print('Pose of the robot (forward kinematics):')
    print(Hrobot)
    print('\n\n')

Pick and place

 

#type help("robolink") or help("robodk") for more information
#(note: you do not need to keep a copy of this file, your python script is saved with the station)
from robolink import *    # API to communicate with robodk
from robodk import *      # basic matrix operations


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


def box_calc(BALLS_SIDE=4, BALLS_MAX=None):
    """Calculates 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):
    """Calculates 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):
    """Deletes all child items whose name starts with \"ball\", from the provided list of parent items."""
    todelete = []
    for item in parentnodes:
        todelete.append(item.Childs())
    todelete = robottool.Childs() + frame1.Childs() + frame2.Childs()
    for item in todelete:
        if item.Name().startswith('ball'):
            item.Delete()

def TCP_On(toolitem, tcp_id):
    """Attaches 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.RL().RunMessage('Set air valve %i on' % (tcp_id+1))
    toolitem.RL().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.RL().RunMessage('Set air valve %i off' % (tcp_id+1))
    toolitem.RL().RunProgram('TCP_Off(%i)' % (tcp_id+1));


#----------------------------------------------------------
# the program starts here:

# Start the API with RoboDK
RL = Robolink()

# Turn off automatic rendering (faster)
RL.Render(False)
#RL.Set_Simulation_Speed(500); # controls the simulation speed

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

# Copy a ball
ballref = RL.Item('reference ball')
ballref.Copy()

# Run a station program to replace the two tables
prog_reset = RL.Item('Replace objects')
prog_reset.RunProgram()

# Call custom procedure to remove old objects
cleanup_balls([robottool, 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)

# Turn on automatic rendering
RL.Render(True)

# Calculate tool frames for the suction cup tool of 6 suction cups
TCPs = []
for i in range(nTCPs):
    TCPs = TCPs + [transl(0,0,100)*rotz((360/nTCPs)*i*pi/180)*transl(125,0,0)*roty(pi/2)]

# Move balls    
robot.setTool(robottool) # this is automatic if there is only one tool
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)
    robottool.setHtool(TCPs[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):
        Htool = TCPs[i]
        robottool.setHtool(Htool)
        # 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(robottool, i)
        robot.MoveL(target_app)
 
    # ------------------------------------------------------------------
    # second priority: unload the tool     
    # approach to frame 2 and place the tool balls into table 2
    robottool.setHtool(TCPs[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):
        Htool = TCPs[i]
        robottool.setHtool(Htool)
        if idLeave > nballs_frame2-1:
            raise Exception("No room left to place objects in Frame 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(robottool, 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])

Drawing with a robot

 

#type help("robolink") or help("robodk") for more information
#(note: you do not need to keep a copy of this file, your python script is saved with the station)
from robolink import *    # API to communicate with robodk
from robodk import *      # basic matrix operations

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."""
    RL.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)
            
    RL.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)
        RL.RunMessage('Drawing %s' % path.idname);
        RL.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

RL = Robolink()

# locate and import the svgpy module
path_stationfile = RL.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 = RL.Item('ABB IRB 4600-20/2.50')
framedraw = RL.Item('Frame draw')
tooldraw = RL.Item('Tool')

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

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

# make a drawing board base on the object reference "Blackboard 250mm"
board_1m = RL.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)

robodk.py - Matrix class for robotics

 

		
class Mat(builtins.object)
     |  A simple Python matrix class with basic
     |  operations and operator overloading.
     |  
     |  Methods defined here:
     |  
     |  Pos(self)
     |      Returns the position of a pose (assumes that a 4x4 homogeneous matrix is being used)
     |  
     |  catH(self, mat2)
     |      Concatenate with another matrix (horizontal concatenation)
     |  
     |  catV(self, mat2)
     |      Concatenate with another matrix (vertical concatenation)
     |  
     |  copy(self)
     |  
     |  eye(self, m=4)
     |      Make identity matrix of size (mxm)
     |  
     |  invH(self)
     |      Calculates the inverse of a homogeneous matrix
     |  
     |  isHomogeneous(self)
     |      returns 1 if it is a Homogeneous matrix
     |  
     |  save(self, strfile)
     |  
     |  setPos(self, newpos)
     |      Sets the XYZ position of a pose (assumes that a 4x4 homogeneous matrix is being used)
     |  
     |  size(self, dim=None)
     |      Returns the size of a matrix (m,n).
     |      Dim can be set to 1 to return m (rows) or 2 to return n (columns)
     |  
     |  tolist(self)
     |      Returns the first column vector of the matrix as a list
     |  
     |  tr(self)
     |      Returns a transpose of the matrix
     |  

FUNCTIONS
    Motoman_2_Pose(xyzwpr)
        Converts a Motoman target into a pose.
    
    Pose_2_Motoman(H)
        Converts a pose into a Motoman target.
    
    acos(value)
        Returns the arc cosinus in radians
    
    add3(a, b)
        Adds two 3D vectors c=a+b
    
    angle3(a, b)
        Returns the angle in radians of two 3D vectors
    
    angles_2_joints(jin, type)
        Converts the angles between links into the robot joint space depending on the type of the robot.
    
    asin(value)
        Returns the arc sinus in radians
    
    atan2(y, x)
        Returns angle of a 2D coordinate in the XY plane
    
    catH(mat1, mat2)
        Concatenate 2 matrix (horizontal concatenation)
    
    catV(mat1, mat2)
        Concatenate 2 matrix (vertical concatenation)
    
    cos(value)
        Returns the cosinus of an angle in radians
    
    cross(a, b)
        Returns the cross product of two 3D vectors
    
    dh(rz, tx=None, tz=None, rx=None)
        Returns the Denavit-Hartenberg 4x4 matrix for a robot link.
        calling dh(rz,tx,tz,rx) is the same as using rotz(rz)*transl(tx,0,tx)*rotx(rx)
        calling dh(rz,tx,tz,rx) is the same as calling dh([rz,tx,tz,rx])
    
    dhm(rx, tx=None, tz=None, rz=None)
        Returns the Denavit-Hartenberg Modified 4x4 matrix for a robot link (Craig 1986).
        calling dhm(rx,tx,tz,rz) is the same as using rotx(rx)*transl(tx,0,tx)*rotz(rz)
        calling dhm(rx,tx,tz,rz) is the same as calling dhm([rx,tx,tz,rz])
    
    dot(a, b)
        Returns the dot product of two 3D vectors
    
    eye(size=4)
        Return the identity matrix
                |  1   0   0   0 |
        eye() = |  0   1   0   0 |
                |  0   0   1   0 |
                |  0   0   0   1 |
    
    fitPlane(points)
    
    getOpenFile()
    
    getSaveFile(strdir='C:\\', strfile='file.txt', strtitle='Save file as ...')
    
    intersect_line_2_plane(pline, vline, pplane, vplane)
    
    invH(matrix)
        Calculates the inverse of a homogeneous matrix
    
    joints_2_angles(jin, type)
        Converts the robot joints into angles between links depending on the type of the robot.
    
    mbox(msg, b1='OK', b2='Cancel', frame=True, t=False, entry=None)
        Create an instance of MessageBox, and get data back from the user.
        msg = string to be displayed
        b1 = text for left button, or a tuple (, )
        b2 = text for right button, or a tuple (, )
        frame = include a standard outerframe: True or False
        t = time in seconds (int or float) until the msgbox automatically closes
        entry = include an entry widget that will have its contents returned: 'default entry'
        Examples:
          mbox('Enter your name', entry=True)
          mbox('Enter your name', entry='default')
          mbox('Male or female?', ('male', 'm'), ('female', 'f'))
          mbox('Process dones')
    
    mult3(v, d)
        Multiplies a 3D vector to a scalar
    
    norm(p)
        Returns the norm of a 3D vector
    
    normalize3(a)
        Returns the unitary vector
    
    pause(seconds)
        Pause in seconds
    
    pose_2_quaternion(Ti)
    
    pose_2_xyzrpw(H)
        Calculates the equivalent position and euler angles ([x,y,z,r,p,w] vector) of the given pose 
        Note: transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180)
        See also: xyzrpw_2_pose()
    
    pose_angle(pose)
        Returns the angle in radians of a 4x4 matrix pose
    
    pose_angle_between(pose1, pose2)
        Returns the angle in radians between two poses (4x4 matrix pose)
    
    print_pose_ABB(pose)
    
    proj_pt_2_line(point, paxe, vaxe)
    
    proj_pt_2_plane(point, planepoint, planeABC)
    
    quaternion_2_pose(qin)
    
    rotx(rx)
        Return a X-axis rotation matrix
                  |  1  0       0       0 |
        rotx(A) = |  0  cos(A) -sin(A)  0 |
                  |  0  sin(A)  cos(A)  0 |
                  |  0  0       0       1 |
    
    roty(ry)
        Return a Y-axis rotation matrix
                  |  cos(A)  0   sin(A)  0 |
        roty(A) = |  0       1   0       0 |
                  | -sin(A)  0   cos(A)  0 |
                  |  0       0   0       1 |
    
    rotz(rz)
        Return a Z-axis rotation matrix
                  |  cos(A)  -sin(A)   0   0 |
        rotz(A) = |  sin(A)   cos(A)   0   0 |
                  |  0        0        1   0 |
                  |  0        0        0   1 |
    
    sin(value)
        Returns the sinus of an angle in radians
    
    size(matrix, dim=None)
        Returns the size of a matrix (m,n).
        Dim can be set to 1 to return m (rows) or 2 to return n (columns)
    
    sqrt(value)
        Returns the square root of a value
    
    subs3(a, b)
        Subtracts two 3D vectors c=a-b
    
    tic()
        Start a stopwatch timer
    
    toc()
        Read the stopwatch timer
    
    tr(matrix)
        Calculates the transpose of the matrix
    
    transl(x, y=None, z=None)
        Return a translation matrix
                        |  1   0   0   X |
        transl(X,Y,Z) = |  0   1   0   Y |
                        |  0   0   1   Z |
                        |  0   0   0   1 |
    
    xyzrpw_2_pose(xyzrpw)
        Calculates the pose from the position and euler angles ([x,y,z,r,p,w] vector)
        The result is the same as calling: H = transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180)
        See also: pose_2_xyzrpw()

DATA
    pi = 3.141592653589793