2.
TRANSFORMATION MATRICES
INDUSTRIAL ROBOT MODELS
DESIGNING AND ANALYSIS WITH
APPLICATION OF MATLAB SOFTWARE
Ivan MILIĆEVIĆ
Radomir SLAVKOVIĆ
Dragan GOLUBOVIĆ
Abstract: The research presented in this paper present
the generalization in solving mechanics (kinematics and
dynamics) of manipulators with application of a PC. In
that order 4×4 homogenous transformation matrices are
used to describe problems of rotation and translation of
manipulator links, and parts of MATLAB software –
Simulink and Robotics toolbox are used for simulation.
Key words: industrial robots, design, analyze, MATLAB
1. INTRODUCTION
These papers describe industrial robot models designing
and analysis with application of parts of MATLAB
software, Simulink and Robotics toolbox [5].
Simulink is the block diagram editing and simulation
environment for Matlab, and it’s his standard part.
Robotics Toolbox is Matlab toolbox specialized for
analysis and simulation of industrial robots. This Toolbox
provides many functions that are useful in robotics
including such things as kinematics, dynamics, and
trajectory generation. It is useful for simulation as well as
analyzing results from experiments with real robots. The
Toolbox is based on a very general method of
representing the kinematics and dynamics of serial-link
manipulators. Robot objects can be created by the user for
any serial-link manipulator. The Toolbox also provides
functions for manipulating and converting between
datatypes such as vectors, homogeneous transformations
and unit-quaternions which are necessary to represent 3-
dimensional position and orientation. It use standard and
modified Denavit-Hartenberg [2] representation for serial-
link manipulator kinematics. Using the Toolbox with
Simulink, includes a library of blocks for use in
constructing robot kinematic and dynamic models.
Further text show application of this software on concrete
examples for industral robot Puma 560.
In the field of robotics there are many possible ways of
representing positions and orientations of robot’s end
effector, but the homogeneous transformation is well
matched to MATLABs powerful tools for matrix
manipulation. Homogeneous transformations describe the
relationships between Cartesian coordinate frames in
terms of translation and orientation.
The homogeneous transformation matrix is a 4×4 matrix
which maps a position vector expressed in homogeneous
coordinates from one coordinate system to another
coordinate system. A homogeneous transformation matrix
can be considered to consist of four submatrices [3]:
⎡ rotation | position ⎤
⎡ R 3×3 | p3×1 ⎤ ⎢ matrix vector ⎥
⎢ ⎥ ⎢ ⎥
T=⎢ ⎯ | ⎯⎥=⎢ ⎯ | ⎯ ⎥
⎣⎢ f1×3 | 1× 1⎦⎥ ⎢ perspective ⎥
⎢⎣ transformation | scaling ⎥⎦
The upper left 3×3 submatrix represents the rotation
matrix and it can be defined as a transformation matrix
which operates on a position vector in a three-dimensional
euclidean space and maps its coordinates expressed in a
rotated coordinate system Ouvw (body-attached frame) to
a reference coordinate system Oxyz. The upper right 3×1
submatrix represents the position vector of the origin of
the rotated coordinate system with respect to the reference
system. The lower left 1×3 submatrix represents
perspective transformation. The fourth diagonal element
is the global scaling factor. The homogeneous
transformation matrix can be used to explain the
geometric relationship between the body-attached frame
Ouvw and the reference coordinate system Oxyz.
In industrial robots application, usual form of
homogeneous transformation matrix is:
⎡ R p⎤
T=⎢
1 ⎥⎦
⎣ 0 0 0
It describe the relationships between Cartesian coordinate
frames in terms of a Cartesian translation, by vector p,
and orientation expressed as an 3×3 orthonormal rotation
matrix, R.
For example, in Matlab’s Robotics toolbox, a
homogeneous transformation representing a pure
translation of 0.25m in the x direction can be created by:
>> T= transl(0.25, 0.0, 0.0)
As result, in Matlab Command Window will show 4×4
homogenous transformation matrix T:
T=
1.0000 0 0 0.2500
0 1.0000 0 0
0 0 1.0000 0
0 0 0 1.0000
And a rotation of 90° about the y axis:
>> T= roty(pi/2)
As result, in Matlab Command Window will show 4×4
homogenous transformation matrix T:
71
T=
0.0000 0 1.0000 0 shown in Fig. 1:
0 1.0000 0 0
-1.0000 0 0.0000 0
0 0 0 1.0000
These transformations can be connected by
multiplication:
>> T= transl(0.25, 0.0, 0.0) * roty(pi/2) * rotz(-pi/2)
And result is:
T=
0.0000 0.0000 1.0000 0.2500
-1.0000 0.0000 0 0
0.0000 -1.0000 0.0000 0
0 0 0 1.0000
3. KINEMATICS
A mechanical manipulator can be modeled as an open-
loop articulated chain with several rigid bodies (links)
connected in series by either revolute or prismatic joints
driven by actuators. One end of the chain is attached to a
supporting base while the other end is free and attached
with a tool (the end-effector) to manipulate objects or
perform assembly tasks. The relative motion of the joints
results in the motion of the links that positions the hand in
a desired orientation.
Robot arm kinematics deals with the analytical study of
the geometry of motion of a robot arm with respect to a
fixed reference coordinate system as a function of time
without regard to the forces/moments that cause the
motion. Thus, it deals with the analytical description of
the spatial displacement of the robot as a function of time,
in particular the relations between the joint-variable space
and the position and orientation of the end-effector of a
robot arm.
There are two fundamental questions of both theoretical
and practical interest in robot arm kinematics:
1. For a given manipulator, given the joint angle vector
q(t) = [q1(t), q2(t), ..., qn(t)]T and the geometric link
parameters, where n is the number of degrees of freedom,
what is the position and orientation of the end-effector of
the manipulator with respect to a reference coordinate
system?
2. Given a desired position and orientation of the end-
effector of the manipulator and the geometric link
parameters with respect to a reference coordinate system,
can the manipulator reach the desired prescribed
manipulator hand position and orientation? And if it can,
how many different manipulator configurations will
satisfy the same condition?
The first question is usually referred to as the direct (or
forward) kinematics problem, while the second question
is the inverse kinematics (or arm solution) problem.
Since the independent variables in a robot arm are the
joint variables and a task is usually stated in terms of the
reference coordinate frame, the inverse kinematics
problem is used more frequently. A simple block diagram
72
indicating the relationship between these two problems is
Link parameters
Position and
Joint angles Direct
orientation of
q1(t), q2(t),..., qn(t) kinematics
the end-effector
Link parameters
Joint angles Inverse
kinematics
q1(t), q2(t),..., qn(t)
Fig. 1: The direct and inverse kinematics problem
3.1. D-H convention
Denavit-Hartenberg [2] in 1955 developed a notation for
assigning orthonormal coordinate frames to a pair of
adjacent links in an open kinematic chain. The procedure
involves finding the link coordinates and using them to
find the 4×4 homogeneous transformation matrix
composed of four separate submatrices to perform
transformations from one coordinate frame to its adjacent
coordinate frame. D-H notation is valuable to the area of
robotics in which robot manipulators can be modeled as
links of rigid bodies. The ability to control a robot end-
effector in three-dimensional space requires the
knowledge of a relationship between the robot’s joints
and the position and orientation of the end-effector. D-H
notation is a method of assigning coordinate frames to the
different joints of a robotic manipulator. Every coordinate
frame is determined and established on the basis of three
rules:
1. The zi-1 axis lies along the axis of motion of the i-th
joint.
2. The xi axis is normal to the zi-1 axis, and pointing away
from it.
3. The yi axis completes the right-handed coordinate
system as required.
The method involves determining four parameters to
build a complete homogeneous transformation matrix:
link length aithe offset distance between the
zi−1 and zi axes along the xi axis
link twist angle αi the angle from the zi−1 axis to
the zi axis about the xi axis
link offset di the distance from the origin of
frame i−1 to the xi axis along
the zi−1 axis
joint angle θi the angle between the xi−1 and xi
axes about the zi−1 axis
Parameters ai and αi are based on the geometry of the
manipulator and are constant values based on the
manipulator geometry, while parameters di and θi can be
variable. For a revolute axis θi is the joint variable and di
is constant, while for a prismatic joint di is variable, and θi
is constant. In many of the formulations that follow it is
used generalized coordinates, qi, where:
 ⎧θ for a revolute joints qz for all zero joint angles, qr for the 'READY' position
qi = ⎨ i and qstretch for a fully extended arm horizontal pose. The
⎩α i for a prismatic joints forward kinematics may be computed, for example, for
The Denavit-Hartenberg representation results in a 4×4 the zero angle pose using fkine command:
homogeneous transformation matrix i-1Ai, known as the >> fkine(p560,qz)
D-H transformation matrix for adjacent coordinate
frames, i and i-1: As result, in Matlab Command Window will show 4×4
homogenous transformation matrix corresponding to the
⎡cos θi − sin θ i cos α i sin θ i sin α i ai cos θ i ⎤ last link of the manipulator:
⎢ sin θ cos θ i cos α i − cos θ i sin α i ai sin θ i ⎥⎥
i −1
Ai = ⎢ i
ans =
⎢ 0 sin α i cos α i di ⎥ 1.0000 0 0 0.4521
⎢ ⎥
⎣ 0 0 0 1 ⎦ 0 1.0000 0 -0.1500
Using the i-1Ai matrix, it can relate a point pi at rest in link 0 0 1.0000 0.4318
i, and expressed in homogeneous coordinates with respect 0 0 0 1.0000
to coordinate system i, to the coordinate system i-1
established at link i-1 by: Command fkine can also be used with a time sequence of
joint coordinates, or trajectory, which is generated by jtraj
pi-1 = i-1Ai pi
>> t = [0:.056:2] - generate a time vector
For an n-axis rigid-link manipulator: >> q = jtraj(qz, qr, t) - compute the joint coordinate
trajectory
Then, the homogeneous transform for each set of joint
coordinates is given by:
>> T = fkine(p560, q)
The results are 36 homogenous transformation matrices in
0
pn
Command Window which determines position and
orientation of the end-effector with respect to a base
coordinate system. One matrix is for each 56ms, starting
from beginning of motion (qz position), giving 36
matrices needed to get manipulator in vertical (qr)
position for 2 seconds. Cartesian coordinates x, y and z of
end-effector, may be plotted against time (Fig. 3) by:
Fig. 2: Transformation from the endeffector frame
to the base frame >> subplot(3,1,1)
plot(t, squeeze(T(1,4,:)))
Performing the composition from the n-th frame to the xlabel('Time (s)');
base frame (Fig. 2), we get: ylabel('X (m)')
n
⎡ 0R 0
pn ⎤ subplot(3,1,2)
T = 0 A n = 0 A1 ⋅ 1 A 2 ⋅K ⋅ n −1 A n = ∏ i −1 A i = ⎢ n ⎥ plot(t, squeeze(T(2,4,:)))
i =1 ⎣ 0 1 ⎦
xlabel('Time (s)');
Last equation determine the transformation from the robot ylabel('Y (m)')
end-effector to the base coordinate frame of the robot subplot(3,1,3)
arm. plot(t, squeeze(T(3,4,:)))
Within the Robotics toolbox the manipulator's kinematics xlabel('Time (s)');
are represented in a general way by a D-H matrix which is ylabel('Z (m)')
given as the first argument to Toolbox kinematic
functions.

3.2. Direct kinematics

Forward kinematics is the problem of solving the
Cartesian position and orientation of the end-effector
given knowledge of the kinematic structure and the joint
coordinates. It require to finding a transformation matrix
that relates the body-attached coordinate frame to the
reference coordinate frame.
An example of solving forward kinematics in Matlab’s
Robotics toolbox will be shown on example of Puma 560
manipulator. The kinematics may be defined by the
puma560 command which creates a kinematic description
matrix p560 in the Matlab’s workspace using standard
Denavit-Hartenberg conventions. It also creates
workspace variables that define special joint angle poses: Fig. 3: Cartesian coordinates of wrist against time

73
Also, it can be plot X against Z (Fig. 4) to get some idea procedure can be slow, and the choice of starting value
of the Cartesian path followed by end-effector of the affects both the search time and the solution found, since
manipulator. in general a manipulator may have several poses which
result in the same transform for the last link. The starting
>> subplot(1,1,1)
point for the solution may be specified, or else it defaults
plot(squeeze(T(1,4,:)), squeeze(T(3,4,:)));
to zero (which is not a particularly good choice in this
xlabel('X (m)')
case), and provides limited control over the particular
ylabel('Z (m)')
solution that will be found.
grid
As result, in Command Window will show:
ans =
-0.0000
-0.7854
-0.7854
-0.0000
0.3927
0.0000
Compared with the original value q:
q=
0 -0.7854 -0.7854 0 0.3927 0
Note that a solution is not possible if the specified
transform describes a point out of reach of the
manipulator - in such a case the function will return with
an error.
Fig. 4: Cartesian path followed by end-effector Inverse kinematics may also be computed for a trajectory.
For example, a Cartesian straight line path can be taken.
3.3. Inverse kinematics >> t = [0:.056:2] - generate a time vector
Inverse kinematics is the problem of finding the robot >> T1 = transl(0.6, -0.5, 0.0) - define the start point of
joint coordinates, given a homogeneous transform the path with corresponding transformation matrix:
representing the pose of the end-effector with respect to a
base coordinate system. T1=
As an example of solving inverse kinematics in Matlab’s 1.0000 0 0 0.6000
Robotics toolbox, first generate the transform 0 1.0000 0 -0.5000
corresponding to a particular joint coordinate: 0 0 1.0000 0
>> q = [0 -pi/4 -pi/4 0 pi/8 0] 0 0 0 1.0000
Now it’s needed to generate a homogenous >> T2 = transl(0.4, 0.5, 0.2) - define the end point of the
transformation matrix which determines position and path with corresponding transformation matrix:
orientation of the end-effector with respect to a base
coordinate system, for above defined generalized T2=
coordinates: 1.0000 0 0 0.4000
>> T = fkine(p560, q) 0 1.0000 0 0.5000
0 0 1.0000 0.2000
Result is:
0 0 0 1.0000
T=
0.3827 0.0000 0.9239 0.7371 >> T = ctraj(T1, T2, length(t)) - compute a Cartesian path
-0.0000 1.0000 -0.0000 -0.1501 The results are 36 homogenous transformation matrices in
-0.9239 -0.0000 0.3827 -0.3256 Command Window which determines position and
0 0 0 1.0000 orientation of the end-effector with respect to a base
coordinate system for 36 points of Cartesian straight line
And then find the corresponding joint angles using path defined by time vector, start and end point.
command ikine: Now solve the inverse kinematics. When solving for a
trajectory, the starting joint coordinates for each point is
>> qi = ikine(p560, T) taken as the result of the previous inverse solution.
The inverse kinematic procedure for any specific robot >> tic - start timer
can be derived symbolically [4] and commonly an q = ikine(p560, T)
efficient closed-form solution can be obtained. However toc - stop timer
the Toolbox is given only a generalized description of the
manipulator in terms of kinematic parameters, so an In Command Window will show many transformation
iterative numerical solution must be used. Such a matrices which are calculated in iterative process.

74
Also, generalized coordinates of joints for all 36 points of
trajectory will show. Elapsed time is 0.984000 seconds.
Clearly this approach is slow and not suitable for a real
robot controller where an inverse kinematic solution
would be required in a few milliseconds.
Let's examine the joint space trajectory that results in
straight line Cartesian motion. Generalized coordinates
for each joint of manipulator can be plotted against time
(Fig. 5) by:
>> subplot(3,1,1) subplot(3,1,1)
plot(t,q(:,1)) plot(t,q(:,4))
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('Joint 1 (rad)') ylabel('Joint 4 (rad)')
subplot(3,1,2) subplot(3,1,2)
plot(t,q(:,2)) plot(t,q(:,5))
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('Joint 2 (rad)') ylabel('Joint 5 (rad)')
subplot(3,1,3) subplot(3,1,3)
plot(t,q(:,3)) plot(t,q(:,6))
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('Joint 3 (rad)') ylabel('Joint 6 (rad)')

Fig. 5: Joint angles against time

Also, this joint space trajectory can be animated using

command:
>> plot(p560,q)
Several positions of manipulator derived from this
simulation are represented on Fig. 6: Fig. 6: Simulation of joint space trajectory

75
3.4. Trajectories subplot(2,1,2)
plot(t,qd(:,3))
As it’s shown before, homogeneous transformations can xlabel('Time (s)');
be used to represent position and orientation of a ylabel('Joint 3 vel (rad/s)')
coordinate frame in Cartesian space. In robotics it’s
frequently needed to deal with paths or trajectories, that
is, a sequence of Cartesian frames or joint angles.
Consider the example of a path which will move the
Puma robot from its zero angle pose to the upright (or
READY) pose. First create a time vector, completing the
motion in 2 seconds with a sample interval of 56 ms.
>> t = [0:.056:2]
And then compute a joint space trajectory between the 2
poses using command jtraj:
>> q = jtraj(qz, qr, t)
q is a matrix with one row per time step (in this case 36
rows), and 6 columns, for each of six generalized
coordinates. For this particular trajectory most of the
motion is done by joints 2 and 3, and this can be
conveniently plotted using standard Matlab operations:
>> subplot(2,1,1) Fig. 8: Joints velocity against time
plot(t,q(:,2)) And acceleration profile:
title('Theta')
xlabel('Time (s)'); >> subplot(2,1,1)
ylabel('Joint 2 (rad)') plot(t,qdd(:,2))
subplot(2,1,2) title('Acceleration')
plot(t,q(:,3)) xlabel('Time (s)');
xlabel('Time (s)'); ylabel('Joint 2 accel (rad/s2)')
ylabel('Joint 3 (rad)') subplot(2,1,2)
plot(t,qdd(:,3))
xlabel('Time (s)');
ylabel('Joint 3 accel (rad/s2)')

Fig. 7: Joint angles θ2 and θ3 against time

We can also get the velocity and acceleration profiles. We
could differentiate the angle trajectory using diff, but
more accurate results can be obtained by requesting that
jtraj return angular velocity and acceleration as follows:
>> [q,qd,qdd] = jtraj(qz, qr, t)
This can be plotted as before to give joints velocity profile
>> subplot(2,1,1)
plot(t,qd(:,2))
title('Velocity')
xlabel('Time (s)');
ylabel('Joint 2 vel (rad/s)')
Fig. 9: Joints acceleration against time
4. DYNAMICS
Robot arm dynamics deals with the mathematical
formulations of the equations of robot arm motion. The
dynamic equations of motion of a manipulator are a set of
mathematical equations describing the dynamic behaviour
of the manipulator, the way in which the manipulator
moves in response to torques applied by the actuators, or
external forces.

76
Such equations of motion are useful for computer space trajectory from the trajectory example above, (Fig.
simulation of the robot arm motion, the design of suitable 7, 8 and 9), the required joint torques can be computed for
control equations, and the evaluation of the kinematic each point of the trajectory by:
design and structure of a robot arm.
>> tau = rne(p560, q, qd, qdd)
There are two problems related to manipulator dynamics
that are important to solve: As with all Toolbox functions the result has one row per
• Inverse dynamics in which the manipulator’s equations time step and each row is a joint torque vector. The joint
of motion are solved for given motion to determine the torques can be plotted as a function of time by:
generalized forces, and >> subplot(6,1,1) subplot(6,1,4)
• Direct dynamics in which the equations of motion are plot(t, tau(:,1)) plot(t, tau(:,4))
integrated to determine the generalized coordinate xlabel('Time (s)'); xlabel('Time (s)');
response to applied generalized forces. ylabel('Joint torque 1') ylabel('Joint torque 4')
subplot(6,1,2) subplot(6,1,5)
The equations of motion for an n-axis manipulator are plot(t, tau(:,2)) plot(t, tau(:,5))
given by: xlabel('Time (s)'); xlabel('Time (s)');
Q = M (q)q
&& + C(q, q& )q& + F (q& ) + G (q) ylabel('Joint torque 2') ylabel('Joint torque 5')
subplot(6,1,3) subplot(6,1,6)
Where: plot(t, tau(:,3)) plot(t, tau(:,6))
q is the vector of generalized joint coordinates xlabel('Time (s)'); xlabel('Time (s)');
describing the pose of the manipulator ylabel('Joint torque 3') ylabel('Joint torque 6')
q& is the vector of joint velocities
&& is the vector of joint accelerations
q
M is the symmetric joint–space inertia matrix, or
manipulator inertia tensor
C describes Coriolis and centripetal effects – Centripetal
torques are proportional to q&i2 , while the Coriolis
torques are proportional to q&i q& j
F describes viscous and Coulomb friction and is not
generally considered part of the rigid body dynamics
G is the gravity loading
Q is the vector of generalized forces associated with the
generalized coordinates q
The equations may be derived via a number of techniques,
including Lagrange-Euler, Newton-Euler or d’Alembert
[1]. Within the Robotics Toolbox the manipulator's
kinematics and dynamics are represented in a general way
by a dyn matrix which is given as the first argument to
Toolbox dynamic functions.
4.1. Inverse dynamics
Inverse dynamics computes the joint torques required to
achieve the specified state of joint position, velocity and
acceleration. The recursive Newton-Euler formulation is
an efficient matrix oriented algorithm for computing the
inverse dynamics, and in Robotics Toolbox is
implemented in the function rne. Inverse dynamics
requires inertial and mass parameters of each link, as well
as the kinematic parameters. This is achieved by
augmenting the kinematic description matrix with
additional columns for the inertial and mass parameters
for each link.
For example, for a Puma 560 in the zero angle pose, with
all joint velocities of 5rad/s and accelerations of 1rad/s2,
the joint torques required can be computed with:
>> tau = rne(p560, qz, 5*ones(1,6), ones(1,6))
tau =
-79.4048 37.1694 13.5455 1.0728 0.9399 0.5119
As with other functions the inverse dynamics can be
computed for each point on a trajectory. Using the joint Fig. 10: Joints torques against time

77
As it’s shown before, for this particular trajectory most of 5. CONCLUSION
the motion is done by manipulator joints 2 and 3, and
much of the torque on this joints is due to gravity. That The dynamical model of robot which can be used with
component can be computed separately using gravload: computer software enables to create the simulation of
>> subplot(2,1,1) behavior, or verification of model which provides to do
taug = gravload(p560, q); the analysis of real robot behavior.
plot(t, taug(:,2)) This short paper has demonstrated, in tutorial form, the
xlabel('Time (s)'); principle features of the Robotics Toolbox for MATLAB.
ylabel('Gravity torque 2 (Nm)') The Toolbox provides many of the essential tools
subplot(2,1,2) necessary for robotic modeling and simulation, as well as
taug = gravload(p560, q); analyzing experimental results or teaching.
plot(t, taug(:,3))
xlabel('Time (s)'); REFERENCES
ylabel('Gravity torque 3 (Nm)')
Master thesis
[1] MILIĆEVIĆ, I., Transformation matrix approach to
kinematics and dynamics of robot manipulators,
Technical Faculty, Čačak, 2006.

Books
[2] DENAVIT, J., HARTENBERG, R. S., A Kinematic
Notation for Lower-Pair Mechanisms Based on
Matricies, Trans. of ASME J.App. Mech. vol. 77, pp.
215-221, June 1955.
[3] FU, K. S., GONZALEZ, R. C., LEE, C. S. G.,
Robotics: Control, Sensing, Vision, and Intelligence,
Mc Graw-Hill Book Company, ISBN 0-07-100421-1,
1987.
[4] Robotic and Automation Handbook, edited by Thomas
R. Kurfess, CRC Press, 2005.

Fig. 11: Gravity torques against time Journal articles

[5] CORKE, P. I., A Robotics Toolbox for MATLAB,
That can be plotted as a fraction of the total torque
IEEE Robotics and Automation Magazine, Vol.3,
required over the trajectory:
No.1, pp.24-32, March 1996.
>> subplot(2,1,1)
plot(t,[tau(:,2) taug(:,2)])
xlabel('Time (s)'); CORRESPONDENCE
ylabel('Torque on joint 2 (Nm)')
subplot(2,1,2)
Ivan MILIĆEVIĆ, M.Sc.
plot(t,[tau(:,3) taug(:,3)])
University of Kragujevac
xlabel('Time (s)');
Technical Faculty
ylabel('Torque on joint 3 (Nm)')
Svetog Save 65
32000 Čačak, Serbia
ivan_milicevic@beotel.yu

Radomir SLAVKOVIĆ, Ph.D.

University of Kragujevac
Technical Faculty
Svetog Save 65
32000 Čačak, Serbia
rstanka@eunet.yu

Dragan GOLUBOVIĆ, Ph.D.

University of Kragujevac
Technical Faculty
Svetog Save 65
32000 Čačak, Serbia
Fig. 12: Total torques and gravity components mehatron@ptt.yu

78