See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/220774479

Closed-form dynamic model of PUMA 560 robot arm

Conference Paper · January 2009

DOI: 10.1109/ICARA.2000.4803940 · Source: DBLP

CITATIONS READS

6 766

1 author:

Alireza Izadbakhsh
Islamic Azad University, Garmsar Branch
11 PUBLICATIONS   75 CITATIONS   

SEE PROFILE

All content following this page was uploaded by Alireza Izadbakhsh on 17 March 2016.

The user has requested enhancement of the downloaded file.

 Proceedings of the 4th International Conference on Autonomous Robots and Agents, Feb 10-12, 2009, Wellington, New Zealand
Closed-Form Dynamic Model of
PUMA 560 Robot Arm
Alireza. Izadbakhsh
Department of Electrical Engineering
Islamic Azad University - Garmsar Branch, Garmsar, Iran
Izadbakhsh_alireza@hotmail.com
Abstract—This paper investigates an explicit dynamic model of
the PUMA 560 robot manipulators, based on standard Denavit-
Hartenberg approach and without any mathematical
simplifications. The presented model obviates the existing
shortcomings in reference model in MATLAB robotic toolbox
and it can be an appropriate substitution for robotic toolbox. A
numerical comparison, employing different inputs, is utilized to
illustrate the accuracy of the mentioned model.
Keywords-Dynamic modeling; Cristoffel symbol; MATLAB
robotic toolbox; Standard Denavit- Hartenberg
I. INTRODUCTION
It is well known that, the dynamic equation of the
manipulator motion describes the system response and
dynamic behavior of the manipulator such as the mechanical
energy, mechanical power, torque, position, velocity and
acceleration. Also, the system stability is one of the main
active fields of research, which is analyzed based on dynamic
equations. In addition, many fast and high-accuracy motions
of robots, such as arc welding, laser cutting of thin films and
glue dispensing, are attainable only with dynamics-based
control techniques that require complete knowledge of the
dominant dynamics of the manipulator links and drive system
[1-2]. The main reason for this sensitivity refers to dynamic
reflection of the robot manipulator and difficulties resulting
from using model-free controllers [3]. On the other hand, such
equations of motion are useful for evaluation of the kinematic
design and structure of the robot arms; design, analyze and
develop of suitable control methods for robot manipulators
and test of its results; off-line programming and visualization
of manipulator by simulator tools. However, obtaining an
exact, simple and flexible dynamic model of the manipulator
that is representative of the real system, and it can also be
easily calculated in real time for implementation into the
control algorithm is very high expensive and involves very
high computing load. This work becomes hypersensitive, for
higher degrees of freedom. Because the computing load of
formulating algorithms increases as O(N3) in which, N is
degrees-of-freedom of mechanical system[4]. Therefore,
dynamic modeling or simulation problem by a model with
mentioned properties has been the subject of intense
researches interest over the past decades [5-8]. For instance,
we can mention to six degrees of freedom PUMA 560
industrial robots with all revolute joints. In the middle of
978-1-4244-2713-0/09/$25.00 ©2009 IEEE
obtained models for PUMA 560 arm, which addressed in
MATLAB software robotic Toolbox, only one operational
model with low calculations volume has been utilized. It must
be noted that, although the mentioned model have the
advantages of practicably and simplicity with acceptable
simulation results, but for this end many simplicities has been
done. For instance, [6] showed that by four main assumptions
can reduce the number of unique non-zero Cristoffel symbols
from 126 to 39. However, we show that using of these
simplifications and connivances of some of these terms has
been non-logical so that, it leads to inevitable results. This
article investigates a simple and accurate non-linear coupled
model of the PUMA 560 robot arm. The model developed is
simple enough and exact to be computed and used in real time
and dynamics-based control techniques. The presented model
is obtained without any parameters canceling and
simplification so that, it can lead to a very efficient
computation of the dynamics, without high computation
burden and loss in precision. Also, the presented model
obviates the existing shortcomings in reference model in
MATLAB software robotic toolbox. The organization of this
paper is as follows. Section 2 describes the representation of
manipulator dynamics and presents applied kinematics’
parameters for all links of PUMA 560 robot arm. Section 3
devoted to computer simulation results and compares two
models under equal conditions, and finally conclusions are
summarized in Section 4.
II. EXPLICIT DYNAMIC MODEL
The dynamic equation of an n-joint robot manipulator can
be described by a differential nonlinear matrix Lagrange-Euler
or Newton-Euler equation as follows [9]
A(q )q + B(q) [ qq
  ] + C (q) ⎡ q 2 ⎤ + g (q) = τ (1)
⎣ ⎦
Where q , q , q are the vectors of joint angular position,
velocity and acceleration, respectively, τ is the vector of
applied joint torque, A(q) is the symmetric positive definite
inertia matrix, C(q), B(q) are the vectors characterizing
Coriolis and centripetal forces respectively, and G(q) is the
gravitational force vector. The symbols [ qq  ] and [ q 2 ] are the
vectors of velocity products and squared velocities,
675
 Proceedings of the 4th International Conference on Autonomous Robots and Agents, Feb 10-12, 2009, Wellington, New Zealand

respectively. The extended form of the above symbolic the specifications and limits of the system. It is noted that the
expressions is as below trajectory is started in where the joint angle is positioned. This
  = ⎡⎣q1q2
qq q1q3 ... q1q6 q2 q3 ... q4 q6
T
q5 q6 ⎤⎦ yields a zero initial tracking error which is a significant factor
to reduce the tracking error. An example of such trajectory to
T satisfy the above conditions, is as follows
q 2 = ⎡ q12 q2 2 ... ... ... q6 2 ⎤ π
⎣ ⎦
qd = −a.Cos ( t ) + a , t≥0 (2)
The standard D-H parameters required to computer T
simulation of PUMA 560 robot are presented in Table 1. Due As see, the mentioned trajectory starts at zero with a
to high volume and complexity of dynamic equations, they are velocity of zero and will finish at time T sec with a zero
listed in final appendix. It is noted that the achieved dynamic velocity. The manipulator moves 2a radian for operating time
equations do not include the actuators dynamics. Also, many of T sec that is a short time if we compare it with operating
details, especially on dynamic parameters, have been omitted range of industrial robots. Here, we set a=0.5rad, and T=1 Sec.
for lack of space. Please refer to [10] for a complete The simulation time is equal to 4sec and the simulation step is
description of them. 0.001sec. For comparison, the normalized and absolute error
vectors for all of joints obtained as Figs. 1~6. At first glance,
TABLE I. STANDARD D-H PARAMETERS OF PUMA 560 although the quantities of error vectors are apparently bounded,
however by computation the normalized model error, as
Remark: a2=0.4318m, a3= 0.0203m, d3=0.15005m, d4=0.4318m
depicted in Figs. 1~6, the effects of these connivances are
joint qi ai αi(0) di appearing, So that the quantity of this error is very big for joint
1 q1 0 90 0 5. This difference reaches to 70% in some of the times,
2 q2 a2 0 0 contrary to what introduced in [7].
3 q3 a3 -90 d3
4 q4 0 90 d4 x 10-14

5 q5 0 -90 0 2
6 q6 0 0 0
0
Error

III. SIMULATION RESULTS -2

The computer simulations to derive the dynamic model of

Absolute Error (N.m)
Nor malized Error(Pu)

the manipulator is composed of four the following procedures -4

0 1 2
Time(Sec)
3 4

[6]
Figure 1. Effect of dynamics simplification on joint 1
• Symbolic Generation of the kinetic energy matrix and
gravity vector elements by using homogeneous 0
coordinates together with the standard D-H four-
parameter representation of robot arm kinematics, and -0.01
Absolute Error (N.m)
Normalized Error(Pu)

finally performing the summations of Lagrange’s

formulation. -0.02
Error

• Combination of inertia constants for Simplification of

the kinetic energy matrix elements. -0.03

• Expression of the Coriolis and centrifugal matrix -0.04

0 1 2 3 4
elements in terms of partial derivatives of kinetic Time(Sec)

energy matrix elements. Figure 2. Effect of dynamics simplification on joint 2

• Calculation of mentioned partial derivatives, and

simplification of them by combining inertia constants 0
as 2.
All of these stages were carried out with writing code in -0.04
Error

MATLAB software in a manner similar to [6]. In this section,

we will present comparison of two models (exact and -0.08

simplified model) under equal condition. It must be noted that,

utilized trajectory for simulation test, is required to be smooth
Absolute Error (N.m)
Normalized Error(Pu)
-0.12 0 1 2 3 4
since the first and the second derivative of the trajectory are Time(Sec)

used in the control system. In the case of existing jumps, Figure 3. Effect of dynamics simplification on joint 3
derivatives of variables may cause the infinity problem in
simulation. In addition, it causes responses that maybe out of

676
 Proceedings of the 4th International Conference on Autonomous Robots and Agents, Feb 10-12, 2009, Wellington, New Zealand

x 10-3
0.5 Absolute Error (N.m)
Normalized Error(Pu) Absolute Error (N.m)
3
0 Normalized Error(Pu)

Error
2
Error

-1

1
-2

-3 0 1 2 3 4
-0.5
0 1 2 3 4
Time(Sec) Time(Sec)

Figure 4. Effect of dynamics simplification on joint 4 Figure 9. Effect of dynamics simplification on joint 3

0 0.6

-0.2
0.4

Error
Error

-0.4
0.2

-0.6
Absolute Error (N.m) 0
Normalized Error(Pu)
-0.8 0 -0.1
1 2 3 4 0 1 2 3 4
Time(Sec) Time(Sec)

Figure 5. Effect of dynamics simplification on joint 5 Figure 10. Effect of dynamics simplification on joint 4

x 10-15
Absolute Error (N.m)
0.1
Normalized Error(Pu) Absolute Error (N.m)
12 0.08 Normalized Error(Pu)

8
0.04
Error
Error

4
0
0

-4 -0.04

0 1 2 3 4 0 1 2 3 4
Time(Sec) Time(Sec)

Figure 6. Effect of dynamics simplification on joint 6 Figure 11. Effect of dynamics simplification on joint 5

The simulations have been also carried out for the parabolic 1
x 10-13

and pulse inputs, as depicted in Figs. 7~12 and Figs.13~18. 0

x 10-13 -2
Absolute Error (N.m)
Error

Normalized Error(Pu)
6 Absolute Error (N.m)
Normalized Error(Pu)
-4
4
Error

2 -6

0 1 2 3 4
0 Time(Sec)

Figure 12. Effect of dynamics simplification on joint 6

-2

0 1 2
Time(Sec)
3 4
It can be seen from the results that the quantity of errors are
Figure 7. Effect of dynamics simplification on joint 1 considerable for joints 3, 4. Note that, these differences are
resulting from parametric simplification by Armstrong [6].
0.4
Absolute Error (N.m)
Normalized Error(Pu) Absolute Error (N.m)
0.04 Normalized Error(Pu)

0
Error
Error

-0.4

-0.04

-0.8

-0.080 1 2 3 4 -10 1 2 3 4
Time(Sec) Time(Sec)

Figure 8. Effect of dynamics simplification on joint 2 Figure 13. Effect of dynamics simplification on joint 1

677
 Proceedings of the 4th International Conference on Autonomous Robots and Agents, Feb 10-12, 2009, Wellington, New Zealand

0.005 model in MATLAB software robotic toolbox does not ensures

0 proper trajectory tracking.
Absolute Error (N.m)
Normalized Error(Pu)
-0.01 IV. CONCLUSIONS
Error

-0.02 In this article a simple and exact model of PUMA 560 robot
arm was presented. The model developed is simple enough
-0.03 and exact to be computed in real time and dynamic-based
-0.04
control techniques. The presented model is obtained without
0 1 2
Time(Sec)
3 4
any parameters canceling and simplification so that, it can lead
Figure 14. Effect of dynamics simplification on joint 2 to a very efficient computation of the dynamics, without high
computation burden and loss in precision. It is shown there is
10
x 10 -3
Absolute Error (N.m)
more than 1% error for special inputs which is introduced in
Normalized Error(Pu)
[6].
8

REFERENCES
Error

4 [1] W. Khalil, E. Dombre, Modeling identification and control of robots,

Hermes Penton Science, 2002.
2
[2] A. Vivas, V. Mosquera, “Predictive functional control of a PUMA
0 robot”, ACSE Conference, 19-21 December 2005, CICC, Cairo, Egypt.
[3] Y. Okada, E. Suzukki and K. Matsuda,” High-Speed Adaptive Control
-2
0 1 2
Time(Sec)
3 4 of a Robot Manipulator“, Journal of Robotics and Mechatronics, Vol.2,
No.1 pp. 22-27, 1990
Figure 15. Effect of dynamics simplification on joint 3 [4] C. Vibet”, Dynamics modeling of Lagrangian mechanisms matrix
elements from inertial matrix elements, Computer. Methods Appl. Mech.
2
x 10 -4 Engrg. 123 (1995) 317-326.
[5] R. P. Paul and H. Zhang, “Computationally efficient kinematics for
Absolute Error (N.m)
Normalized Error(Pu)
0
manipulators with spherical wrists,” Int. J. Robot. Res., vol. 5, no. 2, pp.
32–44, 1986.
[6] B. Armstrong, O. Khatib, and J. Burdick, “The Explicit Dynamic Model
Error

-4
and Inertial Parameters of the PUMA 560 arm”, IEEE, 1986.
[7] M.B. Leahy, JR and G.N. Saridis, “Compensation of Industrial
-8 Manipulator Dynamics”, the international Journal of Robotics Research,
pp. 73-84, 1989.
-12
[8] C-J. Li, A. Hemami and T.S. Sankar, “A New Computational Method
0 1 2
Time(Sec)
3 4 for Linearized Dynamic Models for Robot Manipulators”, the
international Journal of Robotics Research, 1990, pp. 134-144.
Figure 16. Effect of dynamics simplification on joint 4 [9] A. Lesewed, J. Kurek, “ Calculation of Robot Parameters Based on
Neural Nets”, Fifth International Workshop on Robot Motion and
2
x 10 -4
Absolute Error (N.m)
Control, 2005, PP.117-122
0
Normalized Error(Pu) [10] P. Corke and B. Armstrong-H´elouvry, “A search for consensus among
model parameters reported for the PUMA 560 robot,”IEEE Int. Conf.
Robotics and Automation, 1994, pp. 1608–1613.
Error

-4
APPENDIX
-8 In the following tables, the extended form of A, B, and C
matrices and the g gravity vector are presented. These matrices
-12 are made in terms of summation and product of a) the constants
0 1 2 3 4
Time(Sec) which have units of inertia or torque, and b) trigonometric
Figure 17. Effect of dynamics simplification on joint 5 terms that are functions of the joint angles. The abbreviated
form of trigonometric functions is given by writing S2 to mean
4
x 10 -16 Sin (q2), C2 to mean Cos (q5), S232 to mean Sin (2q2+q3) and
Absolute Error (N.m)
Normalized Error(Pu)
C2323 to mean Cos (2q2+2q3). Also when a product or several
trigonometric operations on the same joint variable appears we
0 write CC2 to mean Cos (q2) × Cos (q2), SC2 to mean Sin (q2) ×
Error

Cos (q2) [6]. In dynamic equations that will be appear in below,

-4
Izzi and mi refer to the second moment of link i about the z axis
of frame i and the mass of link i respectively. The rxi, ai and di
are the offsets to the center of gravity of link i in the ith
-8 0 1 2 3 4 coordinate frame and the D-H parameters, respectively. It must
Time(Sec)
be noted that, due to symmetry properties of the A(q) matrix,
Figure 18. Effect of dynamics simplification on joint 6 only equations for elements on and above the matrix diagonal
are presented. The Coriolis terms have been presented in the
Simulation tests have shown that, the quality of both models form of three dimensional arrays, whit a convention for the
is comparable. So that the present PUMA 560 mathematical indices that matches that of the Cristoffel symbols. Element bijk

678
 Proceedings of the 4th International Conference on Autonomous Robots and Agents, Feb 10-12, 2009, Wellington, New Zealand

Multiples qi and q j to give a contribution to the torque at joint
k. the Coriolis matrix may also be written as 6×15 array,
where the 15 columns correspond to the 15 possible
combinations of joint velocities.
Part I. Inertia Matrix Elements
C 441 = -I 4 S4.S5.S23-I5 S4.SC5.S23+I 7 S5.S4.C2+I10 S4.S5.C23
+I12 C4.S5.C23;
C551 =-I 4 S4.S5.S23+I7 S4.S5.C2+I10 S5.S4.C23+I12 C5.S23
+I12 C4.S5.C23;
C112 = -0.5 × I 2 CC4.S2323+I3 C22-I 4 C4.S5.C2323-I 4 C5.S2323

a11 = I1 +I 2CC4.SS23-2I3CS2+2I 4C5SS23+2I 4C4.SC23.S5+I5CC4.CC5.SS23 -I5 C4.SC5.C2323-0.5 × I5 .S2323.CC5.(1+CC4)+I 6 C232

+2I5C4.SC23.SC5-I5CC23.CC5+2I12S4S5-2I6C2.S23-2I7 C2.C5.S23 +I 7 C5.C232-I 7 S5.C4.S232+I8 S232+I9 C2323+I10 C5.C2323
+I13CC2-2I7S5.C2.C4.C23+2I8C2C23-2I9SC23-2I10SC23.C5
-2I10S5CC23.C4-I11CC23; -I10 S5.S2323.C4-0.5 × I11 .S2323+0.5 × I13S22;
C 442 = -I 4 C4.S5-I5SC5.C4+I 7 S5.C4.S3;
a12 =a21 = -I 2 SC4.S23-I 4S4.S5.C23-I 5SC4.S23.CC5-I 5S4.C23.SC5-I 7 S2.S4.S5
C552 = -I 4 C4.S5-I10 C5+I 7 S5.C4.S3-I 7 C5.C3; C662 =0;
-I10 S5.S4.S23-I12 C23C5+I12 S23.C4.S5-I14 C23-I15 S23-I16 S2-I17 C2;
C113 = −0.5 × I 2 CC4.S2323 − I 4 C5.S2323 − I 4 C4.S5.C2323 − I5 C4.SC5.C2323
a13 = a31 =-I 4 S4.S5.C23-I 5S4.C23.SC5-I 5SC4.S23.CC5-I10 S5.S4.S23-I12 C23.C5
+I12 C4.S5.S23-I15S23-I14 C23-I 2 SC4.S23; −0.5 × I5 S2323.CC5.(1 + CC4) + I 6 C2.C23 + I 7 C2.C23.C5 + I8 C2.S23
− I 7 C2.S5.S23.C4 + I9 C2323 + I10 C5.C2323 − I10 S5.S2323.C4
a14 = a41 =I 4 C4.S23.S5+I5 C4.S23.SC5-I5 C23.CC5-I 7 C2.C4.S5+I 20 C23
−0.5 × I11S2323;
-I10 C23.C4.S5+I12 S5.S4.C23;
C 223 = I 7 (C5.C3-C4.S5.S3)+I8S3+I 6 C3; C333 =0;
a15 = a51 = I 21S4.S23+I 4 S4.S23.C5-I10 S4.C5.C23-I 7 C2.S4.C5+I12 S23.S5
C 443 = -I 4 C4.S5-I5SC5.C4; C553 = -I 4 C4.S5-I10 C5; C663 =0;
-I12 C5.C23.C4;
C114 =I 2 SC4.SS23+I5 SC4.CC5.SS23-I10 S5.CC23.S4-I12 S5.C4
a16 =a61 =I19 (-S23.C4.S5+C23.C5);
+0.5 × I 4 S4.S5.S2323+0.5 × I 5S4.S2323.SC5-I 7 S5.C23.S4.C2
a22 = 2I 4 C5+I5 CC5.SS4-2I6 S3-2I 7 C5.S3-2I 7 C4.S5.C3+2I8 C3 + I 22
C 224 =-I5SC4.CC5-I7 S4.S5.C3-I10 S4.S5-I 2 SC4;
-2I10 C4.S5-I 2 CC4;
C334 =-I5SC4.CC5-I10S4.S5-I 2 SC4; C 444 =C554 =C 664 =0;
a23 = a32 = 2I 4 C5+I5 CC5.SS4-I 6 S3-I 7 C5.S3-I 7 C4.S5.C3+I8 C3-I 2 CC4
C115 =-I 4 S5.CC23-0.5 × I 4 C4.C5.S2323+I 4 S5-0.5 × I 5 C4.S2323.C55
-2I10 C4.S5 + I18 ;
+I 5 SC5.CC4.SS23-I5 CC23.SC5-I 7 C2.S23.S5+I 7 C2.C5.C23.C4
a24 = a42 = -I 4 S4.S5-I5SC5.S4+I 7 S3.S4.S5;
+I10 C5.CC23.C4-0.5 × I10 S2323.S5-I12 C5.S4;
a25 = a52 = I 21C4+I 4 C4.C5-I10 S5-I 7 S5.C3-I 7 C5.C4.S3;
C 225 =I 4 S5+I5 SC5.SS4-I 7 S5.S3+I 7 C4.C5.C3+I10 C4.C5;
a26 = a62 = I19 S4.S5;
C335 =I 4 S5+I5SC5.SS4+I10 C4.C5; C 445 =-I5SC5;
a33 = 2I 4 C5+I5 CC5-I5 CC4.CC5-2I10 C4.S5-I 2 CC4+I18 ;
C555 =C665 =C116 =C 226 =C336 =C 446 =C556 =C 666 =0;
a34 = a43 = -I 4 S5.S4-I5 SC5.S4;
a35 = a53 = I 21C4+I 4 C4.C5-I10 S5; Part IV. Coriolis Matrix Elements
a36 = a63 = I19 .S5.S4; a44 = I 20 -I5 CC5; a45 = a54 = 0; b121 =I 2 CC4.S2323-2I3 C22+2I 4 C5.S2323+2I 4 S5.C4.C2323+I5 S2323.CC5
a46 = a64 = I19 .C5; a55 = I 21 ; a56 = a65 = 0; a66 = I19 +2I5 C4.S5.C5.C2323+I 5S2323.CC5.CC4-2I 6 C232-2I 7 C5.C232
+2I 7 .S5.C4.S232-2I8S232-2I9 C2323-2I10 C5.C2323+2I10 S5.C4.S2323
Part II. Gravity terms (Newton-meters)
+I11S2323-I13 S22;

G11 =0; b131 =I 2 CC4.S2323+2I 4 C5.S2323+2I 4 C4.S5.C2323+I5 S2323.CC5-2I 6 C2.C23

G 21 = -g1S2+g 2 C2+g 3 C23-g 4 S23-g 5 C23.C4.S5-g 5 S23.C5; +2I5 C4.SC5.C2323+I5 CC4.S2323.CC5-2I 7 C2.C23.C5+2I 7 C2.S5.S23.C4
-2I8S23.C2-2I9 C2323-2I10 C5.C2323+2I10 S5.C4.S2323+I11S2323
G 31 = g 3 C23-g 4 S23-g 5 C4.S5.C23-g 5 C5.S23;
b141 = −2I 2 SC4.SS23 − I 4 S4.S5.S2323 − I 5S4.S2323.SC5 − 2I 5 CC5.SC4.SS23
(
G 41 =g 5 .S23.S4.S5; G 51 = -g 5 S23.C4.C5+C23.S5 ; )
+2I 7 S5.C23.S4.C2 + 2I10 S5.CC23.S4 + 2I12 S5.C4
G 61 =0;
b151 =-2I 4 S5.SS23+I 4 C4.C5.S2323+I5 C4.S2323.C55+2I5 CC23.SC5
Part III. Centrifugal Matrix Elements -2I5 SC5.CC4.SS23+2I 7 C2.S23.S5-2I 7 C2.C5.C23.C4
+I10 S2323.S5-2I10 C5.CC23.C4+2I12 C5.S4;
C111 =0; C661 =0; C 222 =0; C332 =-I 6 C3-I8S3-I7 C5.C3+I 7 S5.C4.S3;
C 221 =-I 2 SC4.C23+I 4 S4.S5.S23+I 5S4.SC5.S23-I 5 SC4.C23.CC5-I 7 C2.S4.S5 b161 =0;

-I10 S4.S5.C23+I12 S23.C5+I12 C23.C4.S5+I14 S23-I15 C 23+I17 S2-I16 C2; b 231 = -2I 2 SC4.C23+2I 4 S4.S5.S23+2I5 S4.C5(S23.S5-C4.C23.C5)

C331 =I 4 S4.S5.S23-I 5SC4.C23.CC5+I5S4.SC5.S23-I10 S5.S4.C23 -2I10 S5.S4.C23+2I12 S23.C5+2I12 C23.C4.S5+2I14 S23-2I15 C23;

+I12 C23.C4.S5+I12 S23.C5-I15 C23-I 2 SC4.C23+I14 S23; b 241 =-I 2 S23.C44+2I 5 .S23.CC5.SS4-2I12 S5.S4.S23-I 20 S23;
b 251 =-I 5 S4.C23.C55+2I5 S23.SC5.SC4+2I12 C23.S5+2I12 C5.S23.C4
+I 21S4.C23;

679
 Proceedings of the 4th International Conference on Autonomous Robots and Agents, Feb 10-12, 2009, Wellington, New Zealand

b 261 =b361 =-b162 =b126 = -b163 =b136 = -I19 (S23.C5+C23.S5.C4); I 2 =(-I yy5 +I zz5 +I zz6 -I xx6 -m 6 rz6 );
2
I3 =m 2 ry2 a 2 +rx2 ; ( )
b341 = 2I5 S23.CC5.SS4-2I12 S4.S5.S23-I 2 S23.C44-I 20 S23; 2
I5 =(m 6 rz6 -I zz6 +I yy6 );
I 4 =d 4 m 6 rz6 ;
b351 =-I5 S4.C23.C55+2I5 S23.SC5.SC4+2I12 C4.C5.S23+2I12 S5.C23
I6 =a 2 (d 4 m 6+ d 4 m 5 +m 4 ry4 +m 4 d 4 +m 3 rz3 ); I7 =a 2 m 6 rz6 ;
+I 22 S4.C23;
I8 =a 2 (a 3 m 5 +a 3 m 6 +m 3 rx3 +a 3 m 4 +a 3 m 3 );
b 451 =2I 4 C4.C5.S23+2I5 SC5.C23+I5 C4.S23.C55-2I 7 C2.C4.C5+I 21C4.S23
I9 =(a 3 d 4 m 6 +a 3 d 4 m 5 +a 3 m 4 ry4 +a 3 m 4 d 4 +m3 a 3 rz3 +rx3 m3 rz3 );
-2I10 C4.C5.C23+2I12 C5.S4.C23;
I10 =a 3 m 6 rz6 ;
b 461 =I19 S23.S4.S5; (
b561 =-I19 S5.C23+C4.C5.S23 ; ) (
I11 =-2a 3 m 3 rx3 -I xx5 +I yy5 +I xx3 +I zz4 -I yy4 -I zz3 + m 4 +m 5 +m 6 d 4 ) 2

b122 =b132 =b123 =0; 2 2 2

(
+m 4 ry4 +m 3 rz3 -m 3 rx3 - m 3 +m 6 +m 4 +m5 a 3 +2m 4 ry4 d 4 ; ) 2

b142 =-b124 =-I 2 S23.C44-2I 4 S5.C4.C23-2I5 S5.C4.C23.C5

I12 =d 3 m 6 rz6 ;
-2I5 S23.CC5.CC4-2I 7 S5.C4.S2-2I10 S5.C4.S23+I 20 S23;
b152 =-b125 =-2I 4 C5.C23.S4-I5 C23.C55.S4+2I5 .SC4.SC5.S23
(
I13 =I yy2 -I xx2 + m 2 +m 3 +m 4 +m 5 +m 6 a 2 +m 2 rx2 -m 2 ry2 ) 2 2 2

+2m 2 rx2 a 2 ;
-2I 7 C5.S2.S4-2I10 C5.S23.S4-I 21C23.S4;
I14 =(d 3 d 4 m5 +d 3 d 4 m 6 +d 3 m 4 d 4 +d 3 m 4 ry4 +d 3 m3 rz3 -m3 ry3 rz3 );
b 232 = -2I 7 C3.C5+2I7 C4.S5.S3-2I 6 C3-2I8 S 3;
b 242 =2I 2SC4+2I5 SC4.CC5+2I 7 S4.S5.C3+2I10 S4.S5; ( )
I15 =d 3 m 3 rx3 + m 3 +m 4 +m 5 +m 6 d 3 a 3 -m 3 ry3 rx3 -a 3 m 3 ry ;
3

b 252 = -2I 4 S5-2I5SC5.SS4+2I 7 S3.S5-2I 7 C3.C4.C5-2I10 C4.C5; (

I16 =rz2 m 2 a 2 +m 2 rz2 rx2 + m 3 +m 4 +m 5 +m 6 d 3 a 2 -a 2 m 3 ry3 ; )
b 262 =0; b342 =2I 2 SC4+2I5 SC4.CC5+2I 7 S4.S5.C3+2I10 S5.S4; I17 =m 2 rz2 ry2 ;
b352 = -2I 4 S5-2I5SC5.SS4-2I 7 C3.C5.C4+2I 7 S3.S5-2I10 C4.C5 ( )
I18 =2a 3 m3 rx3 +2m 4 ry4 d 4 + m 4 +m 5 +m 6 d 4 +m3 rx3 +m 4 ry4 +m 3 rz3
2 2 2 2

b362 =0; b 452 = -2I 4 S4.C5-I5 S4.C55+2I7 C5.S3.S4-I 21S4;

( ) 2
+ m 3 +m 4 +m5 +m 6 a 3 +I zz6 +I yy3 +I zz5 +I xx4 ;
b 462 =I19 S5.C4; b562 =I19 C5.S4; b133 =0; 2 2
I19 =I zz6 ; I 20 =m 6 rz6 +I yy6 +I xx5 +I yy4 ; I 21 =I xx6 +I yy5 +m 6 rz6 ;
b143 =-b134 =-2I 4 S5.C4.C23-2I 5S23.CC4.CC5-2I 5 S5.C4.C23.C5
-2I10 S5.C4.S23-I 2 S23.C44+I 20 S23; (
I 22 =I xx4 +I zz5 +I yy3 +I zz2 +I zz6 +2m 4 ry4 d 4 + m3 +m 6 +m5 +m 4 a 3 ) 2

b153 =-b135 =-2I 4 C5.C23.S4+2I5 .SC4.SC5.S23-I5 C23.C55.S4 ( 2 2

)2
+2a 3 m3 rx3 + m 4 +m 5 +m 6 d 4 +m 4 ry4 +m3 rx3 +m 2 ry2
2

-2I10 C5.S23.S4-I 21C23.S4; 2

( )
+m3 rz3 + m 2 +m 3 +m 4 +m 5 +m 6 a 2 +m 2 rx2 +2m 2 rx2 a 2 ;
2 2

b 233 =0; b 243 =-b 234 =2I 2 SC4+2I5 SC4.CC5+2I10 S4.S5;

Part VI. Gravity Constants
b 253 =-b 235 =-2I 4 S5-2I5 SC5.SS4-2I10 C4.C5; b 263 =b 236 =0;
b343 = 2I 2 SC4+2I5 SC4.CC5+2I10 S4.S5; g1 =g.m 2 ry2 ;

b353 = -2I 4S5-2I5 SC5.SS4-2I10 C4.C5; b363 =0; (

b 453 = -2I 4 S4.C5-I 5 S4.C55-I 21S4;
b563 = I19 .C5.S4; b144 =0;
b154 =-b145 =I5 C4.S23.C55+2I5 SC5.C23-I 21C4.S23;
b164 =-b146 =-I19 .S23.S4.S5; b 244 =0;
b 264 =-b 246 =b 364 =-b 346 =-I19 .S5.C4;
b354 =-b345 =-I5 S4.C55 +I 21S4;
b564 =-I19 .S5; b155 =0;
b165 =-b156 =I19 .(S23.C4.C5+S5.C23);
b 265 =-b 256 =-I19 .C5.S4;
b 455 =0; b 465 =-b 456 =I19 .S5; b565 =0;
g 2 =g m 2 rx2 +m 2 a 2 +m 3 a 2 +m 4 a 2 +m 5 a 2 +m 6 a 2 ; )
b 463 =I19 .S5.C4; g 3 =g ( m 3 rx3 +m 3 a 3 +a 3 m 4 +a 3 m 5 +m 6 a 3 ) ;
(
g 4 =g m 3 rz3 +m 4 ry4 +m 4 d 4 +m 5 d 4 +m 6 d 4 ; )
g 5 =g.m 6 rz6 ;
b 254 =-b 245 =-I5 S4.C55+I 21S4;
b344 =0;
Alireza Izadbakhsh was born in Garmsar, Iran,
b 454 =2I5SC5; b 464 =0; in 1980. He received the B.S degree in electronic
Eng. from the Islamic Azad University, Garmsar
Branch and the M.S.c degree in control Eng.
b 255 =0; from the Shahrood University of Technology,
Shahrood, Iran, in 2003, 2007 respectively. His
b 355 =0; b365 =-b 356 =-I19 .C5.S4; current research is on robust control and power
electronic.

b166 =b 266 =b366 =b 466 =b 566 =0;

Part V. Inertia Constants

2 2 2 2
I1 =I zz4 +I xx2 +I xx3 +I xx6 +I yy1 +I yy5 +m 3 rz 3 +m 3 ry3 +m 2 ry2 +m 2 rz2

( ) 2
( ) 2
+ m 3 +m 4 +m5 +m 6 d 3 + m 5 +m 4 +m 6 d 4 +m 6 rz6 +2m 4 ry4 d 4
2

2
-2d 3 m 3 ry3 +m 4 ry4

680
View publication stats