This document summarizes a cylindrical robot simulation software developed for educational purposes. The software includes modules for direct and inverse kinematics, trajectory planning, and PID-computed torque control. It was created using Matlab and incorporates visual animations. Students found it helpful for understanding fundamental robot mechanics concepts.

1. Proceedings of ESDA2002:
6th Biennial Conference on Engineering Systems Design and Analysis
Istanbul, Turkey, July 8-11, 2002
ESDA2002/APM-131
A CYLINDRICAL ROBOT SIMULATION SOFTWARE FOR ROBOTICS EDUCATION
Cenk Yuksel Osman S. Turkay
Department of Mechanical Engineering Department of Mechanical Engineering
Bogazici University Bogazici University
Bebek 80815, Istanbul, TURKEY Bebek 80815, Istanbul, TURKEY
Email: cenky@hotmail.com Email: turkay@boun.edu.tr
ABSTRACT
2.1 DIRECT KINEMAT ICS
In this paper the simulation software of a cylindrical robot The aim of direct kinematics is to determine the end effector
arm developed for educational purpose that can be used in an position and orientation (outputs) as a function of the joint
academic environment, in seminars or for continuous education variables (inputs) (Sciavicco L. and Siciliano, B., 2000).
purpose is presented. The software developed in Matlab® A general manipulator may be thought of as a set of links
consists of direct kinematics, inverse kinematics, trajectory connected in a chain by joints. For the purpose of manipulator
planning and PID-computed torque control modules. Visual kinematics, a link is considered as rigid body, which defines the
animations have been incorporated into simulations for better relationship between two adjacent joint axes on the manipulator.
perception of the learners. PID-computed torque controller is The coordinate frames and the link parameters are shown in Fig.
developed using Matlab -Simulink block diagram approach. The 1. The parameters are summarized below:
impressions of the Robotics course students at Bogazici a i-1 = The normal distance from z i-1 to zi measured along xi-1
University have been positive in understanding the αi-1 = The angle between zi-1 and zi measured about xi-1
fundamental concepts of robot mechanics. d i = The distance from xi-1 to xi measured along zi
θ i = The angle between xi-1 to xi measured about zi
1 INTRODUCTION
These parameters are defined according to Denavit-
The main advantages of a virtual educational robot over the
Hartenberg convention [Craig, J.J., 1989) and are generally
physical one are that it is easier for demonstration, cheaper,
accepted as a standard notation. They are used to define the
portable and convenient for multi-users.
relative positions of two consecutive links so that the
The developed package having visual animation is very
mechanics of the whole is system consisting of a chain of links
suitable as an educational tool in an introductory Robotics
can be undertaken.
course in undergraduate or graduate level.
2 MODELING
Robot arm kinematics deals with the geometry of robot arm
motion with respect to a fixed-reference coordinate system as a
function of time without considering forces/moments that cause
the motion. Thus, it deals with the spatial configuration of the
robot as a function of time, in particular the relations between
the joint-variable space and the position and orientation of a
robot arm. The kinematics problem usually consist of two sub-
problems; the direct and inverse kinematics problems.
1 Copyright © 2002 by ASME

2. The link variables associated to the frames in Fig. 2, which
are known as the Denavit-Hartenberg parameters, are shown in
Table 1. (Yuksel, C., 2002).
Table 1. Denavit - Hartenberg parameters
Link i αi ai di θi
1 0 0 0 θ1
2 0 0 d2 0
3 π/ 2 100 d3 0
4 0 0 100 0
Fig. 1. Link relationships (Craig,J.J. 1989)
The corresponding homogeneous link transformations are
For a revolute joint, θ i is the joint variable and the other obtained by successively inserting the Denavit-Hartenberg
parameters from Table 1 into Eqn. (1). To describe the position
three are constants. For a prismatic joint, di is the joint variable
and orientation of the manipulator’s tip with respect to the base
and θi, a i-1 and αi-1 are constants, respectively. 0’th frame the following transformation chain is used:
Coordinates Transformations. It can be shown that the
coordinates of a vector between consecutive frames can be 0
T = 0 T 1T 2T 3T (2)
4 1 2 3 4
transformed using the following transformation matrix (Craig,
J.J., 1989):
The result is;
 c θi − sθi 0 α i −1  c θ1 0 sθ1 100cθ1 + (100 + d 3 )sθ1 
sθ cα cθ i cα i −1 − sα i −1 − sαi −1d i   
sθ1 0 cθ1 100sθ1 − (100 + d3 ) cθ1  (3)
 i i −1 
iT =
4T =
i −1 0
 0 
sθi sα i −1 sθ i sα i −1 cα i −1 c αi −1 d i  
1 0 d2

   0 0 0 1 
 0 0 0 1 
(1) 2.2 INVERSE KINEMATICS
The inverse kinematics problem consists of the
where s and c denote sine and cosine, respectively. determination of the joint variables necessary to bring the end-
Figure 2 shows the three degrees of freedom of cylindrical effector to a desired posture.
manipulator structure and the coordinate frame attachments The inverse kinematics computation can be carried out step
used in this work where the frames and parameters are assigned by step as follows (Riley, D.L., 1986),
as described previously. 100
z2 0
4 T = 0 T 1T 2T 3T
1 2 3 4
(4)
x2
O2
y 3 y4
x3 x4
0
1T −1 0T = 1T 2T 3T
4 2 3 4
(5)
100 z4,3 − cθ1 nx + sθ1 n y cθ1 ox + s θ1 o y cθ1 a x + s θ1a y cθ1 px + sθ1 p y 
mm d3 O32 O4 − sθ n + cθ n − s θ1o x + cθ1 oy − s θ1 a x+ + cθ1 a y − sθ1 p x + cθ1 p y 
 1 x 1 y 
 nz oz az pz 
 
d2  0 0 0 1 
z0,1 x0
1 0 0 100 
O0, O1 0 0 − 1 − 100 − d3  (6)
x1 = 
0 1 0 d2 
θ1  
0 0 0 1 
Fig. 2. Coordinate frame attachment of cylindrical manipulator It is seen from Eqn. (6) that the position vector column, i.e.,
the fourth column can be used to solve for θ 1, d 2 and d 3.
2 Copyright © 2002 by ASME

3. ± p 2 + p 2 − (100) 2   J + m2 d 3
2
0 0  θ  2 d 3m2 θd 3  
&& && 0   n1 
θ1 = arctan 2 
x y
 + arctan 2 p y 
 
(7)   &&       
 100   px   0 ( m1 + m2 ) 0  d 2  +  0  + ( m1 + m2 ) g  =  f 2 
  
 0 0 m2  d 3   − m2 d 3θ 2  
 &&  
&
  0  f3
  
d 2 = pz (8)
(14)
d 3 = −100 + p 2 + p 2 − ( 100) 2
x y
(9)
2.4 TRAJECTORY PLANNING
The goal of trajectory planning is to generate the reference
2.3 DYNAMICS inputs to the motion control system that ensures the
The goal of this section is to obtain the dynamic model of manipulator to execute the planned trajectories. The user
the cylindrical manipulator that describes the relationship typically specifies a number of parameters to describe the
between the joint actuator torques and the motion of the desired trajectory. Planning consists of generating a time
structure. The Lagrange-Euler formulation is used for this sequence of values obtained by polynomial function
derivation. The Lagrangian is defined as: interpolating the desired trajectory (Sciavicco L. and B.
Siciliano, 2000).
L=K– P (10) In this study trapezoidal velocity profile generation
technique will be used.
where K and P are respectively the total kinetic energy and The case examined is one where only the initial and final
potential energy of the system. points on the path and the traveling time are specified. This
The Lagrange’s equations of motion for a conservative method is called as point-to-point motion case. The manipulator
system are obtained by: has to move from an initial to a final joint configuration in a
given time tf. In this case, the actual path is of no concern. The
d ∂L ∂L (11) algorithm should generate a trajectory, which, in respect to the
− =τ
dt ∂q ∂q
& above general requirements, is also capable to optimize joint
variables and their velocities and accelerations when the joint is
The dynamic model of cylindrical manipulator is illustrated moved from one position to another.
in Figure 3. This model shows link masses, inertial moments and In the case of a trapezoidal velocity profile, the assigned
motion capabilities of the cylindrical manipulator. trajectory imposes a constant acceleration in the start phase, a
cruise velocity, and a constant deceleration in the arrival phase.
d3 Th following equations are the mathematical descriptions of
m1 m2 trapezoidal velocity profile, and are used in the simulation code.
 1
 qi + q c t 2
&& 0 ≤ t ≤ tc
g  2 (15)
d2 J q (t ) =  q i + q c tc (t − tc / 2 )
&& tc < t ≤ t f − t c
 1 t f − tc < t ≤ t f
 q f − q c (t f − t )
2
&&
θ  2
2.5 COMPUTED TORQUE CONTROL METHOD
Figure 3. Dynamic model The problem of manipulator control is to find the time
behavior of the forces and torques to be delivered by the joint
Using this model, we obtain the following Lagrangian actuators so as to ensure the execution of the reference
equality: trajectories. PID-computed torque control, which is a well-
known control scheme, is considered for the simulation (Lewis,
 Jθ + 2d 3m2θ&& 3 + m2 d 3 θ 
&& d 2 &&
F. L., et. al., 1993).
d ∂L ∂ L  && &&  (12)
− = m d + m1 d 2 + (m1 + m2 ) g  The general robot arm dynamics is given by the Eqn. (13).
dt ∂ q ∂q  2 2
&


&& &
m2 d 3 − m 2d 3θ 2 

The PID-computed torque scheme could be derived as,
τ = M ( q)( q d + Kv e + K p e + K i ε ) + N ( q , q
&& & &) (16)
Equation (12) can be arranged in matrix form as,
M (q )q + V ( q, q) + G(q ) = τ
&& & (13) The block diagram representation is presented in Fig.4.
3 Copyright © 2002 by ASME

4. Figure 4. PID Computed Torque Control
3 SIMULATION SOFTWARE
The simulation and visual animation software are
developed using the mathematical models of the previous
section and the Matlab software version 5.1. Figure 5. Main GUI (Graphical User Interface)
3.1 SOFTWARE STRUCTURE The desired module can be selected for the execution.
The software is composed of four main modules and three There is also a mini information window at the bottom of this
supplementary functions. The main modules are: figure, which informs the user about the executed module.
· Direct Kinematics
· Inverse Kinematics 3.2 MAIN MODULES
· Trajectory Planning In this section, the structures of the main modules are
· PID Computed Torque Control described.
The supplementary modules are developed for an effective Direct Kinematics module is based on the mathematical
usage. These are Reset, Workspace Bounds, Close Function derivations obtained in Section 2.1. In the GUI of this module,
functions. the desired input joint variables (theta1, d2 and d3) are
Software is run with the execution of quot;robotedu.mquot; code and requested in order to simulate and animate the resulting motion.
then it executes quot;main.mquot; file, which is the main GUI (Graphical This module processes the given data and then passes the
User Interface). The main GUI interface is shown in the Fig. 5. results to trajectory matrix generation module.
The trajectory generation file quot;TP.mquot; constructs a separate
vector for all three joint variables. These vectors show the
changes of the joint variables until the motion ends. While
constructing these vectors, the software code calculates the
position values according to trapezoidal velocity profile
equations explained in Section 2.4. The computed joint variables
vectors and the transformation matrices of Section 2.1 are used
in the animation code to visualize the motion of the manipulator
in 3-D space. During this animation, path of the end-effector
path is traced and displayed on the screen. Also, the user can
see the initial and final end-effector positions by means of the
coordinate frame axes attached to the links. At the end of the
animation, the user can change the view angle at any
perspective view that enables the user to investigate the
realized motion, path and manipulator structure within t e h
kinematics context.
With the execution of the “Inverse Kinematics” module, the
related GUI requests the desired end-effector position
coordinates px, py and pz , and then it calculates the necessary
4 Copyright © 2002 by ASME

5. joint variables using the inverse kinematics equations of Section position, velocity and acceleration matrices for all three joint
2.2. The code works in a similar way to direct kinematics module variables as shown in Fig. 6. Then the code combines these data
and the visual animation occurs in the same way. The 3 -D into the position, velocit y and acceleration matrices of qd, qdp,
animation code developed in this project is based on the simple qdpp that include 4 columns and 209 rows (see Fig.4). The first
version of the 3-D code of reference (Piner, Z. and Hansel, A., columns of the time values are the same for all of the three
1998). matrices. After these matrices are constructed, the code of
When the third trajectory -planning module is executed, the quot;contdata.mquot; defines the mass and inertia values for the robot
related GUI requests the desired joint variables as in direct arm and also creates Kp, Ki and Kv diagonal PID control
kinematics module and also asks for the acceleration and motion matrices. Then the simulation model quot;cont.mdlquot; starts to operate
time to describe the trapezoidal velocity profile. The difference and uses these data as the main input for Simulink simulation
of this module from the previous ones is simply the request of execution.
acceleration and time as interactive input variables whereas; in Figure 7 is a sample error gra ph between the reference
the direct and inverse kinematics animation modules a fixed positions and the actual positions of the three links obtained for
acceleration value of 5 mm/sec2 and a fixed motion time of 20 the PID computed-torque controller values of:
seconds are used. Figure 6 shows a user described sample K v = diag {0 .1} , K p = diag{ } , K i = diag{1}
10
trajectory curve obtained by the execution of trajectory module
for an end-effector speed of 5 mm/sec 2 and duration of 20
seconds together with θ 1=180o, d2=200mm and d3=200mm joint
variable inputs. The resulting curves of the joint variable
positions and their first and second derivatives as a function of
time are seen in this figure.
q : θ [ degree ] q : d [ mm ] q : d [ mm ]
1 1 2 2 3 3
200 200 200
150 150 150
100 100 100
50 50 50
0 0 0
0 5 10 15 20 0 5 10 15 20 0 5 10 15 20
q′ q′ q′
1 2 3
6 6 6
4 4 4
2 2 2 Figure 7. Sample error graph
0 0 0
0 5
q′′
10 15 20 0 5
q′′
10 15 20 0 5
q′′
10 15 20 CONCLUSIONS
1 2 3
5 5 5
The direct and inverse kinematics equations, the trajectory
planning technique and the PID computed-torque control
method of a cylindrical type three-link manipulator has been
0 0 0 presented. A special interactive software has been developed in
Matlab environment to simulate and animate the robot for
-5 -5 -5
education purpose. The software has been used in teaching a
0 5 10 15 20 0 5 10 15 20 0 5 10 15 20 graduate level robot mechanics course. The feedback of the
students has been encouraging.
Figure 6. Sample trajectory module graphics output This software is open to development for other manipulator
Upon the execution of the PID-computed control module types. Including the gripper kinematics and animation can
from the main GUI in Figure 5, quot;contstart.mquot; code runs and ameliorate the kinematics module. The dynamics and control
loads the trajectory information into Matlab workspace. The module can be improved by including the motor dynamics and
user can change the default trajectory data using the M file friction effects. Also the motion of the links can be scaled to
editor before executing this mo dule. The default trajectory data match the motion of an actual robot. The virtual robot needs
includes an end-effector speed of 5 mm/sec2 and duration of 20 some more visual elaboration concerning shape and dimension
seconds together with θ1=180o, d2=200mm and d3=200mm joint of links and the attached coordinate frames.
variable inputs. The trajectory matrix code creates the desired
5 Copyright © 2002 by ASME

6. The use of a virtual robot has proved to be a valuable tool
in robotics education for its cost effectiveness, parametric
investigation, portability and multi-end user facility.
REFERENCES
Craig, J. J., “Introduction to Robotics: Mechanics and
Control”, Addison-Wesley, Mass, 1989.
Lewis, F. L., Abdallah, C. T. and Dawson, D. M., “Control of
Robot Manipulators”, MacMillan Pub. Co., New York, 1993.
Piner, Z. and Hansel, A., “Robot Animation of Linear
Trajectory with Real Time Error Plot”,
http://www.mathworks.com/matlabcentral/fileexchange/Files.jspf
ileId=156.
Riley, D. L., “Design Modeling, Trajectory Calculation and
General Calibration of a Three Degrees of Freedom Cylindrical
Coordinate System Robot”, M.S. Thesis, Washington State
University, 1986.
Sciavicco L. and Siciliano, B., “Modeling and Control of
Robot Manipulators”, Springer, New York, 2000.
Yuksel, C., “Development of a Simulation Software for
Robotics Education”, M.S. Thesis, Bogazici University, 2002.
6 Copyright © 2002 by ASME