Proceedings of the 2011 IEEE

International Conference on Mechatronics

April 13-15, 2011, Istanbul, Turkey

Dynamic Modeling and Kinematic Simulation of

Staubli TX40 Robot Using MATLAB/ADAMS

Co-simulation
Farzad Cheraghpour, IEEE Membel1, Masoud Vaezi#2, Resam Eddin Shoori Jazeh#3, S. Ali A. Moosavian#4
#

Islamic Azad University, Pardis Branch, Department ofMechanical Engineering, Iran

#
K.N Toosi University of Technology, Department ofMechanical Engineering, Iran
#
K.N Toosi University of Technology, Department ofElectrical Engineering, Iran
#
K.N Toosi University of Technology, Department ofMechanical Engineering, Iran
lsamavati@pardisiau.ac.ir
2masoud.vaezi@gmail.com
3hesam.shoori@gmail.com
4moosavian@kntu.ac.ir

Abstract- This paper presents dynamic modeling and simulation

of the industrial robot, Stiiubli TX40, and proposes a precise
simulator to develop approaches for experimental simulation in
kinematics, dynamics and control analysis. The robot model has
been developed as accurate as the real one by implementing
dynamic model of robot in ADAMS, dynamic modeling software,
and also linking with MA TLAB for motion studies.

simulator

is

verified

by

tracking

predefined

Finally the
pose

with

minimum error.

Keywords---

completely, the model is exported to ADAMS dynamic

modeling software. This software is capable of simulating
kinematics and kinetics behavior of any mechanical system.
As result dynamic behavior of systems with comparison to the
real models can be simulated with an acceptable error.
For obtaining an exact modeling, inertial and geometric
parameters are accurately measured and recorded in the
software database. Also for simulating control algorithms, co
simulation between ADAMS and MATLAB is performed.

Robot, Dynamic Modeling, Simulation, Inverse and

II.

Forward Kinematics, Genetic Algorithm

I.

INTRODUCTION

In recent decades, robot manipulators are widely used in

industrial applications. Of the most important ones, welding
and painting robots in car plants [1], electronic board
assembly, repairing nuclear installations [2], and etc can be
mentioned.
Besides, academic and research Labs try to develop new
methods and algorithms so as to release the results for
industrial uses after validation. The researches are applied to
different areas such as motion planning, manipulation
planning [3,4,5], grasp planning [6,7,8], and control
algorithms like position control [9], force control [10],
Impedance control [11,12], and etc. Real robots are
inaccessible due to high prices therefore creating precise
simulated models are extensively demanded by research Labs.
Prior to this, simulation and analysis of PUMA 560 as one
of the most renowned industrial robots have been performed
[13]. With the advent of new models of robot manipulators by
manufacturers, still there is a need for exact simulated models.

This paper introduces simulated models of 6-DOF Stiiubli

TX40 robot. All procedures presented in this paper are in way
that reader can model specific robot in software environments.
The main advantage of this paper is to use multi-software
environments interactively. After a 3D modeling of robot
manipulator in CATIA software, as it will be mentioned

978-1-61284-985-0/11/$26.00 2011

MODELING

In modeling using CATIA, CAD software, robot model is

divided into seven parts. Each individual part is designed in
"Part Design" separately. The sketches are obtained from the
manufacturer company [14]. Afterwards these parts are
reproduced in "Assembly" environment and assembled with
constraint commands. The process of defining constraints
starts with fixing the base part using "Fix" command and ends
up with reproducing the other parts and applying "Contact"
and "Coincidence" to each couple of them. To validate the
motion of 6-DOF robot, "manipulation" command is used.
The assembled model of robot and the real one are shown in
Figure 1.

IEEE
386

Fig. 1 A. created model in CATIA B. the real Staubli

TX40 robot

III. KINEMATIC S ANALYSIS

A. ADAMS Dynamic Modeling Software

To import the designed model to ADAMS, the initial step is

to transfer each of the seven parts of robot from "Assembly"
to "SolidWork" software. Then each part is saved in
"parasolid" format, since this format is better recognized in
ADAMS. The next step is to import the model into ADAMS.
It should be noted that since the parts are assembled in
CATIA and converted into a defInite format, the parts are put
together with pre-determined positions in ADAMS.
After transferring model, inertial parameters of the parts
such as density, are recorded in software database. The
imported model to ADAMS is without mass and inertia in
default. Determination of exact density of the robot parts is
done by measuring mass and density of the second link lid
(Fig. 2). Thus ADAMS is able to calculate the mass and
moments of inertia of each part automatically. The values are
illustrated in Table 1. The computed parameters are verifIed
by making a comparison between the total weights of
simulated robot with the data given in the corresponding
catalogue. The result shows that the cumulative error is less
than %2.

Before transferring the model into MATLAB software,

forward kinematics equations are considered briefly. SpecifIed
coordinates and ModifIed Denavit-Hartenberg (MDH)
parameters are depicted in Fig. 3 and Table 2.
TABLE II
DENAVIT -HARTENBERG PARAMETERS

lyyX Iff]

luX Iff]

[kg]

Ikg.m2]

Ikg.m2]

Ikg.m2]

base

5.35

26

22

19

8.61

48

47

23

l'

TX40ROBOT ARMS

Ixxx Iff]

Link

4.85

47

45

4.55

12

11

4.09

15

14

0.32

0.23

0.2

0.084

0.02

0.0058

0.003

0.003

ai_l

di

9i

01

-900

O2

0.225

0.035

03

0.225

04

90 0
-900

Os

90 0

0.065

06

.?f z,

Finally in order to place a motor for each Revolute joint of

robot in ADAMS, "motion" command should be applied to
each joint.

Fig. 3 The attached coordinates of robot joints

The transformation matrices are computed in the following

[15]:

T=

2T_

[CO'
[ CO,0

[CO'
00
[C0O'

2
3T =

Fig. 2 A. The designed model of the second lid B. The real lid

1Ij-1

z,

TABLE I
lNERTIAL PARAMETERS OF THE STAUBU

3
4

T-

387

S l

- 82

S83

S 4

00
00 01 !l
0 001 l
0 00 !

0
00 01 0.0135l
0 001 l
0 0

-S81
C81

-S82

-C82

-S83
C83

-S84
C84

(1)

[0 0 0
00 ]

0
[ 0 0 0 -Of65
g
0 00 ]
CO,

sT - - Os -COs

CO' -S06

6T- S
6
_

82 =Atan2 [(-a3-a2C3)P z+(Clpx+S1Py)(a2S3-d4)

(a2C3- d4)pz+(a3+a2C3)(CIP x+S1Py)]- 83

-SOs

C06

83=Atan2(a3,d4)-

Atan2(a3c3 - d4S3,.Ja + d + (a3c3 - d4s3)

84=Atan2(-T13s1+T23Cl , -T13CIC23-T23S1C23+T33S23)

-1

85 = Atan2 (Ss,Cs)

86=Atan2(s6,c6)

In which C8j , S8j are representative for Cos(8j) and Sin(8j),

respectively.
The following relation specifies the end effector position
and orientation with respect to the base.

(2)
2T is a 4x4 matrix whose elements are shown in Appendix.
The position of joints through the process of transferring
into ADAMS is considered as zero state in default.
Consequently, when assembling model in CATIA, zero state
of the joints should be the same as zero state of specified
coordinates. Fig. 4 shows zero state of Stiiubli robot joints. To
adapt the direction of joint motion with the real robot, the
positive direction of rotation should be compatible with right
handed direction of Z axis.

Due to the nonlinearity of equations, their closed solution

might be difficult to attain, so the Genetic Algorithm as a
numerical method has been considered for calculating 8j ,[16].
MATLAB software aims at achieving joint space variables for
a given end effector position and orientation (EqA) based on
GA. The comparison between GA results and analytical
solution is made in Table 3. It should be stated that angular
limits are assigned to each motor that lead to unique solution
[17]. Fig. 5 illustrates the error between numerical solution of
the inverse kinematics and analytical solution that is about
ll
3.06e- mm
.

Px = -0.26 mm
cp=O.OO rad

Pz=0.22 mm
\jI=O.OO rad

Py = -0.18 mm
8=0.00 rad

(4)

TABLE III
COMPARISON OF NUMERICAL SOLUTION WITH ANALYTICAL ONE

Joint
angles

Closed

Genetic

Error

equations

Algorithm

percentage

[rad]

[rad]

[%]

91

0.7165

0.7165

0.000

92

-2.0048

-2.0052

0.019

93

0.2143

0.2145

0.093

94

0.000

95

1.7912

1.7908

0.022

96

-0.7165

-0.7165

0.000

Best:

3.060ge')11

Mean:

3.4471e-008

Best fitness
Mean 1nness

Fig. 4 Zero state of joint angles in ADAMS

0

A. Inverse Kinematics
g

The inverse kinematics problem consists of the

determination of joint variables corresponding to a given end
effector position and orientation. The solution to this problem
is of fundamental importance in order to transform the motion
specifications, assigned to the end effector in operational
space, into the corresponding joint space motions that allow
execution of desired motion.
Inverse kinematics of 6-DOF Stiiubli manipulator consists
of establishing closed analytical equations of the joint space
using algebraic and trigonometric methods and the final
results are specified in equations 3.

81=Atan2(Py,px)-Atan2(d3,

r- - - - - -p + P - d D

(3)

"

u:

.
.

500"--
10 00--
15 00--
--25
2000
00
--
3000

0"

PQuee

Generation

Fig. 5 Numerical solution error using Genetic Algorithm

As seen in Table 3, consequences of numerical solution are

highly acceptable and reliable.

388

B. MATLAB and ADAMS Co-simulation:

MATLAB is a powerful tool for applying control

commands to the robot so for having an efficient simulator
package, the co-simulation between ADAMS an MATLAB is
noticeable. In order to execute a co-simulation between
ADAMS and MATLAB, defmition of an acceptable format
for the inputs and outputs of each program is required. The
objective of co-simulation is to make a connection so that any
change in one of the programs affects the other one [18].
To provide a simulation that enables ADMAS to recognize
the exported output from MATLAB, there is a need for
activating "control" plug-in in "Plug-in Manager" of ADAMS
and defining the robot as a plant. After activation, a new
window appears for the determination of plant and its inputs
and outputs. Afterwards, ADAMS saves dynamic model of
robot motion as a complex of seven matrices that is capable of
sending to MATLAB. The first four are A,B,C,D matrices of
state-space representation. The fifth and sixth matrices are
associated with predefmed inputs and outputs. And the last
one includes information about state variables of the plant
[19,20]. Trajectory planning of each robot joint (9(t)), as
ADAMS inputs, is stated as a five-degree polynomials for
solving forward kinematics of robot, [21].
This function is suitable for each joint in the view point of
dynamic considerations, [22].
(5)
The coefficients of this function regarding to initial and
fmal position, velocity, acceleration values of each joint are
assigned based on equations in [15]. ADAMS outputs are
components of end effector position (x,y,z) with respect to the
frame attached to the base.
To call the generated plant in MATLAB, the plant name
should be entered in the "command window" so that the input
and output information will appear. By executing "Adams_sys"
command, the blocks containing information about dynamic
model of robot in SIMULINK environment, are 10aded.(Fig. 6)

S-Function

State Spa ce

generated in "Embedded MATLAB Function". Outputs of this

block represent the plant inputs of robot in ADAMS. By
starting the simulation in SIMULINK, the signals of function
coefficients for 9(t) will enter the plant and run ADAMS
simultaneously which continues to a defmite fmal state.
Generated trajectory functions for each motor yields the end
effector to track the desired position and orientation. Fig. 7
demonstrates the block diagram of forward kinematics.

ffiG
[0
G
[0

1---1'=

1::'

H---'.

8
8-

'

8
[0

.
.

ciJ8
G
ffi

..

ciJ

g;;J

8
88
8
8
8G
8
8
8G
8
8

..

I:

'-

'
-

...

1----1=

Fig. 7 block diagram of forward kinematics in SIMULINK

In this simulation initial values of position, velocity and

acceleration and also fmal velocity and acceleration of each
joint are presumed to be zero. The fmal position of each joint,
9r, in period of 5 seconds is assumed as equation 6.
8f= [0.7165 -2 0.2145 0 1.79 -0.7165]

(6)

With these 6 values of initial and fmal position, velocity,

acceleration all 6 coefficients of the equation 5 can be
determined so that trajectory function of the joints are
generated. The fmal functions are as equation 7.
9 1 (t) = 0.2654t3-O.1327t4+O.0176t5
92 (t) = -0.7427t3+0.3714t4-0.049t5
93(t) = 0.079t3_0.039t4+0.005t5
94(t) = 0
95(t) = 0.663t3_0.331t4+0.044t5
96(t) = -0.265t3+O.1327t4-O.017t5

(7)

Based on the analytical viewpoint and forward kinematics

solution, each joint approaching its given fmal position, 9r, the
end effector must track the position and orientation according
to equation 8. Note that cp, 9, 'I' are roll, pitch, yaw angles
respectively.

1-------lD

Px = -0.26 mm
cp=O.OO rad

adams.:-.,sub

Fig. 6 model plant and state space blocks

By having "Adams-sub" block in SIMULINK environment,

the robot model is suitable for control and motion simulation
as a defmed system in MATLAB.
Regarding the forward kinematics of robot, the fmal
transformation matrix and trajectory planning block are

py= -0.18 mm
9=0.00 rad

pz = 0 .22 mm
'1'=0.00 rad

(8)

By the end of the simulation, the end effector position in

work space is depicted in scope of MATLAB/SIMULINK Fig.
8. By comparing the results with the forward kinematics
solution, it can be inferred that the simulation satisfies our
desired accuracy.

389

 r . ...

02 ..........,
';" 0.1
.
c
D ...........:............
o

.. ........... . . . ....... 1. ,

...

.1

<.

.. ..
.

............ ..

.... I

..... ....................................

05

.. .

....... .

.. . . ...

--

. ......

. .... . .

15

--

--

--

....'__ .

35

Analysis for forward kinematics

--

--

..... I ......... I .. . .....l . ......

.....

-0.1

. ...

. ... ..

,
,
. ....... . . .... . . . . .. . .... . , . .

. .

..... .. ......... ...

.

. .. . .

="-' ...... . ....... '

OA ....

OJ ..

g, Oli
O

. ..

. . .. . ... . \.. . .
,
.

. . ..

......... ........ 1 . . .

.. .

. : ........
.

..

... . ,1 . ....

. .

. .

. . .. . . ... . . ..... .. . . ........

,
,
,
.

.. 1.... . ......

Determination of inputs, outputs and

.. .

'" Oli ....

... .

. . j .

. ....

--

. . . .. . .. ..!.. . .........

15

..... ...'.......... I ......_ _ . I . __.... . l ......

. . . . ..., ..

..

... ..

--

I . .

.... ..

------

.....

15

0 2 r---r-

]0.1

:.:.:.:..

,..

. . .I. . .

.)..

plant in ADAMS

:...... . .. .. : ............ .
,
.

,
.

1 ..
,

I ...

,
.

Creating M-File of end effector

......

transformation matrices

..

.... . . . ....
t
.

..

..

...

Jj
lOL-----:O-':-j-----'----1:-:cj------:-------:':
1 :----'----'':---L-----:':----Ij
-:'
-....,(s)
Fig. 8 The end effector position showing in scope

IV. CONCLUSIONS

Fig. 9 The flow-chart showing the procedure of co-simulation

In this paper modeling and simulation of 6-DOF Stiiubli

robot have been performed using CATIA, ADAMS,
MATLAB programs. The Denavit-Hartenberg and inertia
parameters and also kinematic analysis of robot were
exploited. Then the procedure of establishing a co-simulation
between ADAMS and MATLAB has been clearly expressed
and the flowchart in Fig. 9 shows the whole process.
Consequences of this simulation clarify the correct guidance
of robot in the software modeling. With the help of created
model, design and simulation of closed loop control systems
and applying optimized design parameters are simply
executable.

REFERENCES
[I]

[2]

[3]

[4]

[5]

[6]

[7]

[8]

[9]

K. T. Park, Y. J. Shin and C. H. Park Y. S. Mo, D. C. Jeong,Y. S.

Shin,"Robot Application for Assembly Process of Engine Part",
International Conference on Control, Automation and Systems in
CDE}{, Korea, pp. 1-4, 2008
DD Ray, M. Singh, "Development of a Force Reflecting Tele-robot for
Remote Handling in Nuclear Installations", 1st International
Conference on Applied Robotics for the Power Industry Delta Centre
Ville Montreal, Canada, p.p 1-6, 2010
F. Cheraghpour., S.A.A Moosavian, and A. Nahvi, "Multiple Aspect
Grasp performance index for cooperative object manipulation tasks",
IEEE/ASME Int. Conf. on Advanced Intelligent Mechatronics, pp 386391, 2009
M. Eslamy, S.A.A Moosavian, "Dynamics and Cooperative object
Manipulation Control of Suspended Mobile Manipulators", J Intell
Robot Syst Springer Science+Business Media B.V, pp. 1-19, 2010
K. Harada, K. Kaneko, F. Kanehiro, "Fast Grasp Planning for
Hand/Arms Systems Based on Convex Model", IEEE International
Conference onRobotics and Automation, pp. 1-7, 2008
F. Cheraghpour., S.A.A Moosavian, and A. Nahvi, "Robotic grasp
planning by multiple aspects grasp index for object manipulation tasks",
18th Iranian Conf. on Electrical Engineering (ICEE), pp 635-640, 2010
A. Agovic, N. Papanikolopoulos, "Grasp Planning by Alignment of
Pairwise Shape Descriptorss", IEEEIRSJ International Conference on
Intelligent Robots and Systems, pp. 1-8, 2009
M. Strandberg, B. Wahlberg, "A Method for Grasp Evaluation Based
on Disturbance Force Rejection", IEEE transactions on robotics, pp. 19, 2006
M. Karimi, S.A.A. Moosavian, "Modified Transpose Effective
Jacobian Law for Control of Underactuated Manipulators",
Koninklijke Brill
, Leiden and The Robotics Society of Japan, pp.
605-626, 2009
G. Zeng, A. Hemami, "An overview of robot force control", Robotica,
Cambridge University Press, volume 15 , pp. 473 - 482, 1997
S.A.A. Moosavian, and E. Papadopoulos, "Cooperative Object
Manipulation with Contact Impact Using Multiple Impedance Control,"
NY

[10]
[11]

390

[12)

[13)

[14)
[15)
[16)

[17)

[18)

[19)

[20)

[21)

[22)

International Journal of Control, Automation, and Systems, pp 314327., 2010

S. Dehghani, H.D. Taghirad, M Darainy, "Self-tuning Dynamic
Impedance Control for Human Arm Motion", Biomedical Engineering
(ICBME), 17th Iranian Conference, pp. 1-6, 2010
B. Armstrong, O. Khatib And J. Burdick, "The Explicit Dynamic
Model and Inertial Parameters of the PUMA 560 Arm, " Robotics And
Automation IEEE International Conf. pp 510-518 , 1986

Staub Ii
TX40 product leaflet, 2002, AT http://www.staubli.com.
J.J.Craig, Introduction to Robotics, Addison-Wesley, pp 81-86 , 2005
P. shi, y. cui,"Dynamic path planning for mobile robot based on
genetic algorithm in unknown environment", control and decision
conference (CCDC), china, p.p 4325 - 4329, 2010
G. pengfei w. xuezhi, h. yingshi, "The enhanced genetic algorithms
for the optimization design", Biomedical Engineering and Informatics
(BMEI), 2010 3rd International Conference on, p.p 2990-2994, 2010.
Z. Yi, X. Min-min, Q. Jin-yi , Z. Hu , "Research on co-simulation
using ADAMS and MATLAB for automobile active suspension
system" , Computer Application and System Modeling (ICCASM),
International Conference, p.p V14-366 - VI4-370 , 2010.
Z. Affi, L. Romdhane, "ADAMS/SlMULINK interface for Dynamic
Modeling and Control of Closed Loop Mechanisms", Proceedings of
the 7th WSEAS International Conference on automatic control,
modeling and simulation, Prague, Czech Republic,pp. 353-356,2006
Z.Z. Naing, M.P. AI-Mamun, "Integrated ADAMS+MATLAB
environment for design of an autonomous single wheel robot, "
Industrial Electronics,.IECON 35th Annual Conference of IEEE, p.p
2253-2258, 2009
T. Fraichard , INRIA Rhone Alpes, Grenoble ,"Dynamic trajectory
planning with dynamic constraints: A 'state-time space' approach",
Intelligent Robots and Systems '93, IROS '93. Proceedings of the 1993
IEEEIRSJ International Conference , p.p 1393 - 1400 vo1.2, 2002.
Y. Guan, K. Yokoi, , O. Stasse, , A. Kheddar, ,"On robotic trajectory
planning using polynomial interpolations", Robotics and Biomimetics
(ROBIO). IEEE International Conference , p.p 111 - 116, 2006.
APPENDIX

81 = tl
84 =t4
CI =Cos81

82 =t2
85 = t5
Sl =Sin 81

Px = (9*cos(tl)*cos(t2))/40-(7*sin(tl))/200(13*sin(t5)*(sin(tl)*sin(t4)+ cos(t4)*(cos(tl)*sin(t2)*sin(t3)
cos(t1)*cos(t2)*cos(t3))))/200+
(13*cos(t5)*(cos(tl)*cos(t2)*sin(t3)+
cos(tl)*cos(t3)*sin(t2)))/200+ (9*cos(tl)*cos(t2)*sin(t3))/40+
(9*cos(t1)*cos(t3)*sin(t2))/40+(0.26);

'[21 = sin(t6)*(cos(tl)*cos(t4)+
sin(t4)*(sin(tl)*sin(t2)*sin(t3)-cos(t2)*cos(t3)*sin(tl)))+
cos(t6)*(cos(t5)*(cos(tl)*sin(t4)cos(t4)*(sin(t1)*sin(t2)*sin(t3)-cos(t2)*cos(t3)*sin(t1)))
sin(t5)*(cos(t2)*sin(tl)*sin(t3)+cos(t3)*sin(tl)*sin(t2)));
'[22 = cos(t6)*(cos(tl)*cos(t4)+
sin(t4)*(sin(tl)*sin(t2)*sin(t3)-cos(t2)*cos(t3)*sin(tl)))
sin(t6)*(cos(t5)*(cos(tl)*sin(t4)cos(t4)*(sin(tl)*sin(t2)*sin(t3)-cos(t2)*cos(t3)*sin(tl))) sin(t5)*(cos(t2)*sin(t1)*sin(t3)+cos(t3)*sin(tl)*sin(t2)))-(1);
'[23 = sin(t5)*(cos(t1)*sin(t4)- cos(t4)*(sin(tl)*sin(t2)*sin(t3)
cos(t2)*cos(t3)*sin(t1)))+
cos(t5)*(cos(t2)*sin(tl)*sin(t3)+cos(t3)*sin(tl)*sin(t2));
Py

= (7*cos(tl))/200 + (9*cos(t2)*sin(tl))/40+
(13*sin(t5)*(cos(tl)*sin(t4)- cos(t4)*(sin(tl)*sin(t2)*sin(t3)
cos(t2)*cos(t3)*sin(t1))))/200+
(13*cos(t5)*(cos(t2)*sin(tl)*sin(t3)+
cos(t3)*sin(tl)*sin(t2)))/200+(9*cos(t2)*sin(tl)*sin(t3))/40 +
(9*cos(t3)*sin(tl)*sin(t2))/40+(0.18);
'[31 = sin(t4)*sin(t6)*(cos(t2)*sin(t3)+cos(t3)*sin(t2))
cos(t6)*(sin(t5)*(cos(t2)*cos(t3)-sin(t2)*sin(t3)) +
cos(t4)*cos(t5)*(cos(t2)*sin(t3)+cos(t3)*sin(t2)));
'[32 = sin(t6)*(sin(t5)*(cos(t2)*cos(t3)-sin(t2)*sin(t3))+
cos(t4)*cos(t5)*(cos(t2)*sin(t3)+cos(t3)*sin(t2)))+
cos(t6)*sin(t4)*(cos(t2)*sin(t3)+cos(t3)*sin(t2));
'[33 = cos(t5)*(cos(t2)*cos(t3)-sin(t2)*sin(t3))
cos(t4)*sin(t5)*(cos(t2)*sin(t3)+cos(t3)*sin(t2))-(1);

'[11 = - sin(t6)*(cos(t4)*sin(tl)sin(t4)*(cos(tl)*sin(t2)*sin(t3)-cos(tl)*cos(t2)*cos(t3)))
cos(t6)*(cos(t5)*(sin(tl)*sin(t4)+
cos(t4)*(cos(tl)*sin(t2)*sin(t3)-cos(t1)*cos(t2)*cos(t3))) +
sin(t5)*(cos(t1)*cos(t2)*sin(t3)+ cos(tl)*cos(t3)*sin(t2)))-(l);

Pz = (9*cos(t2)*cos(t3))/40-(9*sin(t2))/40(9*sin(t2)*sin(t3))/40+(13*cos(t5)*(cos(t2)*cos(t3)
sin(t2)*sin(t3)))/200-(13*cos(t4)*sin(t5)*(cos(t2)*sin(t3)+
cos(t3)*sin(t2)))/200-(0.22);

'[12=Sin(t6)*(cos(t5)*(sin(tl)*sin(t4)+
cos(t4)*(cos(t1)*sin(t2)*sin(t3)-cos(tl)*cos(t2)*cos(t3))) +
sin(t5)*(cos(tl)*cos(t2)*sin(t3)+ cos(tl)*cos(t3)*sin(t2))) cos(t6)*(cos(t4)*sin(tl)- sin(t4)*(cos(tl)*sin(t2)*sin(t3)
cos(tl)*cos(t2)*cos(t3)));
'[13=cos(t5)*(cos(tl)*cos(t2)*sin(t3)+
cos(t1)*cos(t3)*sin(t2))-sin(t5)*(sin(tl)*sin(t4)+
cos(t4)*(cos(tl)*sin(t2)*sin(t3)- cos(tl)*cos(t2)*cos(t3)));

391