Mechanics of Robot Manipulators course project The manipulator's model (graphical and mathematical) was implemented via self-written Matlab scripts. -Analytic model of the robot -Direct and inverse kinematics solution -Direct and inverse dynamics solution -Trajectory planning An additional Simulink model was implemented to solve the inverse dynamics problem.

1. Corso di Laurea Magistrale in Ingegneria Meccanica
Mechanics of Robot Manipulators - Dionisio Del Vescovo
REPORT:
“KINEMATICS, TRAJECTORY PLANNING AND DYNAMICS
OF A PUMA 560”
Alessandro Mazzali: 1744396
Andrea Patrizi: 1742937
1

2. Contents
1 ANALYTICAL MODEL FOR POSITION 4
1.1 FRAMES ASSIGNMENT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.2 DENAVIT-HARTENBERG PARAMETERS . . . . . . . . . . . . . . . . . . . 7
1.3 HOMOGENEOUS TRANSFORMATIONS . . . . . . . . . . . . . . . . . . . . 8
1.4 POSITION FORWARD KINEMATICS . . . . . . . . . . . . . . . . . . . . . . 9
1.5 POSITION INVERSE KINEMATICS . . . . . . . . . . . . . . . . . . . . . . . 10
1.6 GRIPPER POSE REPRESENTATION:
DIRECT AND INVERSE PROBLEMS . . . . . . . . . . . . . . . . . . . . . . 17
1.7 COMPLETE KINEMATIC PROBLEM FOR POSITION:
RECAPITULATION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
2 ANALYTICAL MODEL FOR VELOCITY
AND ACCELERATION 21
2.1 FORWARD ANALYSIS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.2 INVERSE ANALYSIS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3 TRAJECTORY PLANNING 31
3.1 POLYNOMIAL PEISEKAH TRAJECTORY . . . . . . . . . . . . . . . . . . . 33
3.2 GUTMAN TRAJECTORY . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.3 PIECEWISE CONSTANT ACCELERATION FOR SPECIFIC
POWER MINIMIZATION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
3.4 NUMERICAL EXAMPLE: TRAJECTORIES COMPARISION . . . . . . . . . 39
4 DYNAMICS 44
4.1 INVERSE DYNAMICS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.2 FORWARD DYNAMICS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
4.3 NUMERICAL EXAMPLES AND COMPARISONS . . . . . . . . . . . . . . . 52
4.3.1 INVERSE DYNAMICS . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
4.3.2 FORWARD DYNAMICS . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5 APPENDIX 61
5.1 MATLAB LIVE SCRIPT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
5.2 EXPLICIT EXPRESSION OF M06 . . . . . . . . . . . . . . . . . . . . . . . . 62
5.3 INVERSE DYNAMICS SIMULINK MODEL . . . . . . . . . . . . . . . . . . . 63
2

3. List of Figures
1.1 PUMA 560 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.2 Rest position and reference frames . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.3 Relative motion: frame (1) wrt (0) and frame (2) wrt (1) . . . . . . . . . . . . 5
1.4 Relative motion: frame (3) wrt (2) . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.5 Relative motion: frame (4) wrt (3) and frame (5) wrt (4) . . . . . . . . . . . . 6
1.6 Relative motion of frame (6) wrt (5) and position of tool frame wrt (6) . . . . . 6
1.7 Inverse kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.8 Wrist lock . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.9 Elbow lock . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
1.10 Shoulder lock . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
1.11 WCP locus of reachable points . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
1.12 Flow charts for the direct (left) and inverse position (right) problem . . . . . . 20
2.1 Flow charts for the direct velocity (left) and acceleration (right) problems . . . 24
2.2 Flow charts for the inverse velocity (left) and acceleration (right) problems . . 27
2.3 TCP position (above), velocity (center) and acceleration (below) . . . . . . . . 28
2.4 Inverse kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
2.5 Joint positions (above), joint speeds (below-right) and joint accelerations (below-
left) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.1 Flow chart describing the trajectory planning procedure . . . . . . . . . . . . . 32
3.2 Trajectory planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
3.3 TCP trajectories comparison . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
3.4 Trajectory planning: joints positions . . . . . . . . . . . . . . . . . . . . . . . . 41
3.5 Trajectory planning: joints velocities . . . . . . . . . . . . . . . . . . . . . . . . 42
3.6 Trajectory planning: joints accelerations . . . . . . . . . . . . . . . . . . . . . . 43
4.1 Flow chart for solving the inverse dynamics problem . . . . . . . . . . . . . . . 47
4.2 Flow chart for solving the forward dynamics problem . . . . . . . . . . . . . . . 50
4.3 Flow chart describing the block diagram ”COMPUTATION OF M + F” . . . 51
4.4 Dynamics sample trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
4.5 Joints actions - dynamics sample trajectory . . . . . . . . . . . . . . . . . . . . 54
4.6 Link kinetic energies - dynamics sample trajectory . . . . . . . . . . . . . . . . 55
4.7 Equivalent joint inertia - dynamics sample trajectory . . . . . . . . . . . . . . . 56
4.8 Piecewise constant acceleration: original vs numeric . . . . . . . . . . . . . . . 57
4.9 Gutman trajectory: original vs numeric . . . . . . . . . . . . . . . . . . . . . . 58
4.10 Peisekah trajectory: original vs numeric . . . . . . . . . . . . . . . . . . . . . . 59
4.11 MaxSpPow with increased precision . . . . . . . . . . . . . . . . . . . . . . . . 60
5.1 Simulink block diagrams . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
5.2 Trajectory and joint actions (Simulink model) . . . . . . . . . . . . . . . . . . . 64
5.3 Simulink: arm 3D trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
3

4. 1. ANALYTICAL MODEL FOR POSITION
The first part of the report will be dedicated to the development of an analytical model for the
position of the robot of interest: the PUMA 560.
The PUMA 560 is a stationary robot with three major axes and a spherical wrist, for a total
of six revolute joints.
All the computations and the vast majority of the inserted images were entirely made and
generated via completely self-written Matlab scripts. Moreover, section 5.1 of the Appendix
contains a demonstration of the execution of a Matlab Live Script for the interactive motion of
the arm. It proved to be particularly useful in visualizing the 3D motion of the robotic arm.
Figure 1.1: PUMA 560
4

5. 1.1. FRAMES ASSIGNMENT
The frames are assigned by applying the Denavit-Hartenberg convention:
Figure 1.2: Rest position and reference frames
The following figures show each consecutive couple of reference frames and their relative motion
about the joint axes, highlighting all the six joint variables qi, as well as all the characteristic
dimensions.
x0
y0
z0
x1
z1
y1
h
q1
x1
z1
y1
x2
z2
y2
q2
l2
Figure 1.3: Relative motion of frame (1) with respect to (0) [left] and of
frame (2) with respect to (1) [right]
5

6. x2
z2
y2
x3
z3
y3
l3
q3
b3
Figure 1.4: Relative motion of frame (3) with respect to (2)
x4
y4
z4
x3
y3
z3
l4
q4
x4
y4
z4≡y5
x5
z5
q5
Figure 1.5: Relative motion of frame (4) with respect to (3) [left] and of
frame (5) with respect to (4) [right]
x5
y5
z5≡z6
x6
y6
q6
x6
y6
z6
yt
xt
zt
dwt
Figure 1.6: Relative motion of frame (6) with respect to (5) [left] and
position of the tool frame with respect to frame (6) [right]
Note that, as shown in figure 1.2, all frames from (4) to (6) have a common origin, which in
literature is frequently named WCP (“wrist center point”).
Similarly, the origin of the tool frame acquires the name TCP (“tool center point”).
6

7. 1.2. DENAVIT-HARTENBERG PARAMETERS
The following table contains all the Denavit- Hartenberg parameters resulting from the previ-
ous choice of the reference frames:
D - H parameters
i type ri ti d0i θ0i ai αi
1 R 1 0 d1 π 0 π/2
2 R 1 0 0 0 a2 0
3 R 1 0 d3 0 a3 π/2
4 R 1 0 d4 0 0 π/2
5 R 1 0 0 π 0 π/2
6 R 1 0 0 0 0 0
t n.d. n.d. n.d. dt π/2 0 0
In particular, expliciting the value of the characteristic dimensions for d0i and ai (if different
from 0):
• d1 → h
• d3 → l3
• d4 → l4
• dt → dwt
• a2 → - l2
• a3 → - b3
Moreover, being all joints of R type, the relationships describing the rotations θi and the offsets
di are:
θi = qi + θ0i
di = d0i
7

8. 1.3. HOMOGENEOUS TRANSFORMATIONS
By means of the D-H parameters, the general expression of the homogeneous transformation
matrix between frames (i − 1) and (i), calculated by combining four successive screw displace-
ments, takes the form:
Mi−1, i =








cos θi − sin θi cos αi sin θi sin αi ai cos θi
sin θi cos θi cos αi − cos θi sin αi ai sin θi
0 sin αi cos αi di
0 0 0 1








For the sake of simplicity, from now on sin () and cos () will be substituted, respectively, by
s () and c (). Using this notation, Mi−1,i becomes:
Mi−1,i =








c(θi) −s(θi) c(αi) s(θi) s(αi) ai c(θi)
s(θi) c(θi) c(αi) −c(θi) s(αi) ai s(θi)
0 s(αi) c(αi) di
0 0 0 1








By substituing all parameters into the general form of the matrix, for i = 1, 2, ..., 6 :
M01 =








−c (q1) 0 −s (q1) 0
−s (q1) 0 c (q1) 0
0 1 0 d1
0 0 0 1








M12 =








c (q2) −s (q2) 0 a2 c (q2)
s (q2) c (q2) 0 a2 s (q2)
0 0 1 0
0 0 0 1








M23 =








c (q3) 0 s (q3) a3 c (q3)
s (q3) 0 −c (q3) a3 s (q3)
0 1 0 d3
0 0 0 1








M34 =








c (q4) 0 s (q4) 0
s (q4) 0 −c (q4) 0
0 1 0 d4
0 0 0 1








M45 =








−c (q5) 0 −s (q5) 0
−s (q5) 0 c (q5) 0
0 1 0 0
0 0 0 1








M56 =








c (q6) −s (q6) 0 0
s (q6) c (q6) 0 0
0 0 1 0
0 0 0 1








Since the relative homogeneous transformations between successive frames are now known, the
absolute ones from frame (i) to frame (0) are easily obtained through M0i =
Qi
n=1 {Mn−1,n},
with i = 1, 2, ..., 6 :
M01
M02 = M01 M12
M03 = M02 M23
M04 = M03 M34
M05 = M04 M45
M06 = M05 M56
For the explicit expression of M06 refer to section 5.2 of the Appendix.
8

9. 1.4. POSITION FORWARD KINEMATICS
The position forward kinematics consists in determining the pose of the tool frame, provided
that all joint variables qi, for i = 1, 2, ..., 6, are known. However, since M6t is a constant matrix,
it is also possible to solve the problem exclusively for the end-effector, by using M06.
M06 (q1, q2, q3, q4, q5, q6) =




m11 m12 m13 m14
m21 m22 m23 m24
m31 m32 m33 m34
0 0 0 1



 =




xW CP
R06 yW CP
zW CP
0 0 0 1




Note that R06 columns represent the direction cosines of frame (6) with respect to frame
(0). Moreover, since the columns of R06 have to satisfy ortonormality relationships, only three
of the following equations deriving from R06 are independent:
m11 = c(q6){c(q5) [s(q1)s(q4) + c(q1)c(q4)c(q2 + q3)] + c(q1)s(q5)s(q2 + q3)}+
+ s(q6) [s(q1)c(q4) − c(q1)s(q4)c(q2 + q3)]
m21 = −c(q6){c(q5) [c(q1)s(q4) − s(q1)c(q4)c(q2 + q3)] − s(q1)s(q5)s(q2 + q3)}+
− s(q6) [c(q1)c(q4) + s(q1)s(q4)c(q2 + q3)]
m31 = c(q6) [s(q5)c(q2 + q3) − c(q4)c(q5)s(q2 + q3)] + s(q6)s(q4)s(q2 + q3)
m12 = −s(q6){c(q5) [s(q1)s(q4) + c(q1)c(q4)c(q2 + q3)] + c(q1)s(q5)s(q2 + q3)}+
+ c(q6) [s(q1)c(q4) − c(q1)s(q4)c(q2 + q3)]
m22 = s(q6){c(q5) [c(q1)s(q4) − s(q1)c(q4)c(q2 + q3)] − s(q1)s(q5)s(q2 + q3)}+
− c(q6) [c(q1)c(q4) + s(q1)s(q4)c(q2 + q3)]
m32 = −s(q6) [s(q5)c(q2 + q3) − c(q4)c(q5)s(q2 + q3)] + c(q6)s(q4)s(q2 + q3)
m13 = s(q5) [s(q1)s(q4) + c(q1)c(q4)c(q2 + q3)] − c(q1)c(q5)s(q2 + q3)}
m23 = −s(q5) [c(q1)s(q4) − s(q1)c(q4)c(q2 + q3)] − s(q1)c(q5)s(q2 + q3)}
m33 = −c(q5)c(q2 + q3) − c(q4)s(q5)s(q2 + q3)
Lastly:
m14 = xW CP = l2 c(q1) c(q2) − l3 s(q1) − l4 c(q1) s(q2 + q3) + b3 c(q1) c(q2 + q3)
m24 = yW CP = l2 s(q1) c(q2) + l3 c(q1) − l4 s(q1) s(q2 + q3) + b3 s(q1) c(q2 + q3)
m34 = zW CP = h − l4 c(q2 + q3) − l2 s(q2) − b3 s(q2 + q3)
where ( xW CP , yW CP , zW CP ) represents the position of frame (6) origin with respect to the
frame (0).
9

10. 1.5. POSITION INVERSE KINEMATICS
The position inverse kinematics consists in determining the joint variables qi which would
generate a chosen pose of the end-effector (or equivalently of the tool frame, by means of a
simple constant transformation). Obviously, to choose a required pose of the frame (6) is
equivalent to fix the matrix M06. In general, obtaining all qi is not an easy task: extracting
them from M06 involves the solution of a number of trascendental trigonometric equations.
Moreover, it is also not guaranteed that the solution exists. However, if n is the number of
degrees of freedom of the robot and m is the number of its degrees of mobility, a recent result
in kinematics shows that all systems with revolute and prismatic joints having m = 6 in a
single series chain are solvable.
In the majority of the cases an analytical solution is not achievable and, as a consequence,
numerical methods are usually implemented. It can be shown that a closed form analytical
solution exists if at least one of the following conditions is verified:
- Several joint axes are incident
- Many αi are 0 or ±π/2
Specifically, if the manipulator has 6 revolute joints, a sufficient condition is that 3 neighboring
joint axes intersect at a point. This is in fact the case of the PUMA 560.
Note that whenever an analytical solution exists, it has to be preferred to a numerical one,
which in general is more time-consuming and less accurate.
A possible procedure for the analytical solution of PUMA 560 inverse kinematics is the one
reported below.
As a first step towards the inverse problem solution, it is necessary to formulate a set of
equations in the joint variables qi:











































yW CP c (q1) − xW CP s (q1) = l3
l4 s (q3) − b3 c (q3) = K
[ l2 + b3 c (q3) − l4 s (q3) ] s (q2) + [ l4 c (q3) + b3 s (q3) ] c (q2) = h − zW CP
[ l2 + b3 c (q3) − l4 s (q3) ] c (q2) − [ l4 c (q3) + b3 s (q3) ] s (q2) = xp c (q1) + yp s (q1)
c (q5) = − [m13 c (q1) + m23 s (q1)] s (q2 + q3) − m33 c (q2 + q3)
s (q5) = ±
q
1 − c (q5)
2
c (q4) s (q5) = [ m13 c (q1) + m23 s (q1) ] c (q2 + q3) − m33 s (q2 + q3)
s (q4) s (q5) = m13 s (q1) − m23 c (q1)
c (q6) s (q5) = [ m11 c (q1) + m21 s (q1) ] s (q2 + q3) + m31 c (q2 + q3)
s (q6) s (q5) = − [m12 c (q1) + m22 s (q1)] s (q2 + q3) + m32 c (q2 + q3)
(1.1)
(1.2)
(1.3)
(1.4)
(1.5)
(1.6)
(1.7)
(1.8)
(1.9)
(1.10)
Where K := [ l2
2
+ l3
2
+ l4
2
+ b3
2
− xW CP
2
− yW CP
2
− (zW CP − h)
2
] / (2 l2) .
In particular:
- equation (1.1) is obtained from M06 by multiplying the second element of its fourth
column by c (q1), the first by s (q1) and subtracting them
- equation (1.2) is obtained from M06 by computing the squared sum of the first three
elements of its last column and then by separating the constant terms by the variable
ones
- equation (1.3) is simply obtained by extracting the third element of the fourth column
of M06 and then once again by separating the constant terms by the variable ones
- equation (1.4) is obtained from M06 by multiplying the first element of its last column
by c (q1), the second by s (q1) and adding them
10

11. The last three couples of equations are derived by using a graph cut. It is possible to write:
M34(q4) M45(q5) M56(q6) = M23
−1
(q3) M12
−1
(q2) M01
−1
(q1) M06(S)
where S sintetically represents the assigned pose of the end-effector.
By extracting only the rotational sub matrices:
R34(q4) R45(q5) R56(q6) = R23
−1
(q3) R12
−1
(q2) R01
−1
(q1) R06(S)
Since the right side of the matricial equation is known (once the system for q1, q2 and q3 is
solved), it provides a solvable set of trigonometric equations from which to derive the unknowns
q4, q5 and q6. In particular, from the elements (3, 3), (2, 3), (1, 3), (3, 2), (3, 1) it is possible to
obtain, respectively, equations (1.5), (1.7), (1.8), (1.9), (1.10). Lastly, equation (1.6) is trivially
derivable from the fundamental identity of trigonometry.
Moreover, a trigonometric equation of the form a sin(φ) + b cos(φ) = c has the following two
solutions:







φ1 = arcsin
c
√
a2 + b2
− atan2
b
a
φ2 = π − φ1 − 2 atan2
b
a
Since equations (1.1) and (1.2) have such a pattern, it is possible to straighforwardly obtain
(q11
, q12
) and ( q31
, q32
). Note that there exist four possible combinations between q1 and q3 and
that, in writing the previous solutions, the output of arcsin function has been conventionally
selected in the interval [−π/2, +π/2].
Looking at equations (1.3) and (1.4), it is clear how the previous four combinations will lead
to four different q2. In order to find them, an expression for c (q2) and s (q2) as a function of
q1 and q3 has to be derived. This can be achieved by solving the sistem formed by (1.3) and
(1.4) to isolate c (q2), s (q2):













c (q2) =
( xW CP c1 + yW CP s1 ) ( l2 − K ) + ( h − zW CP ) ( b3 s3 + l4 c3 )
x2
W CP + y2
W CP + (zW CP − h)
2
− l2
3
s (q2) =
( xW CP c1 + yW CP s1 ) K2
− l2
4 − b2
3
+ ( h − zW CP ) ( b3 s3 + l4 c3 ) ( l2 − K )
[ x2
W CP + y2
W CP + (zW CP − h)
2
− l2
3 ] ( b3 s3 + l4 c3 )
Once the expressions are determined, it is easy to obtain all the four different solutions as:
q2j
= atan2 s (q2j
), c (q2j
)
with j = 1, 2, 3, 4
Similarly, from (1.5) and (1.6) it is possible to obtain eight different q5 as:
q5k
= atan2 ( s (q5k
), c (q5k
) ) with k = 1, 2, ..., 8
Finally, by dividing (1.7), (1.8), (1.9), (1.10) by s (q5):











c (q4) =
[ m13 c (q1) + m23 s (q1) ] c (q2 + q3) − m33 s (q2 + q3)
s (q5)
s (q4) =
m13 s (q1) − m23 c (q1)
s (q5)











c (q6) =
[ m11 c (q1) + m21 s (q1) ] s (q2 + q3) + m31 c (q2 + q3)
s (q5)
s (q6) = −
[m12 c (q1) + m22 s (q1)] s (q2 + q3) + m32 c (q2 + q3)
s (q5)
Since all right-side members of the previous equations are now known, once again it is possible
to derive eight solutions for q4 and q6:
q4k
=atan2 ( s (q4k
), c (q4k
) ) with k = 1, 2, ..., 8
q6k
=atan2 ( s (q6k
), c (q6k
) ) with k = 1, 2, ..., 8
11

12. The following tree graph shows how all the different qi combine to generate a total of eight
nonsingular potentially valid configurations:
PUMA 560
END–EFFECTOR POSE
(M06)
q11 q12
q32
q31 q31 q32
q21
q22
q23
q24
q52
q51
q54
q53
q55
q56
q57
q58
q42
q41
q43
q44
q45
q46
q47
q48
q62
q61 q63 q64
q65
q66
q67
q68
(down)
(up) (up) (down)
(left) (right)
(flip)
(no flip) (flip)
(no flip) (no flip) (flip) (no flip) (flip)
The next series of pictures (generated by a entirely self-written Matlab script for the inverse
kinematics) show an example of a set of compatible configurations which lead to the same pose
of the end-effector ( flipped configurations are not shown, since they only differ by the relative
orientation of frames (4), (5) and (6) ).
(a) Right-up configuration (b) Right-down configuration
(c) Left-up configuration (d) Left-down configuration
Figure 1.7: Output of the self-written Matlab script,
with a randomly chosen pose of the end effector
12

13. As it can be inferred from the previous tree graph, for each nonsingular input pose in the
dexterous workspace there exist eight feasible configurations. However, it is possible for a
geometrically compatible configuration to be physically unacceptable (e.g. compenetrations of
links or robot parts).
Furthermore, the existence of 8 finite sets of multiple solutions is not always guaranteed. In
fact, there may exist special end-effector poses which lead to mathematical singularities of the
obtained equations.
In literature, singularities are generally divided into three main categories: wrist lock, elbow
lock and shoulder lock. In general, notwithstanding the distinction between different types, it
is possible for more than one singularity to be present at the same time.
A wrist lock happens whenever the axis of joints 4 and 6 are aligned (figure 1.8). This partic-
ular condition results into an infinite number of different possible couples q4, q6 which lead to
the same wrist orientation. From a mathematical point of view, such a condition arises when
s (q5) = 0, as shown by previous expressions for c (q4,5) and s (q4,5) and it causes a loss of one
rotational degree of freedom of the end-effector.
If, during the motion, the robot arm passes through such a position, then the CCU needs pre-
cise instructions to make a choice between all the infinite possible ones. One way to proceed
is to use q4c and q6c, the values of the nearest previous configuration. From the expression of
R36 computed for q5 = 0 and q5 = π,
R36 |q5=0,π =


∓c (q4 ± q6) s (q4 ± q6) 0
∓s (q4 ± q6) −c (q4 ± q6) 0
0 0 ±1


it possible to obtain, respectively:
q4 ± q6 = atan2(∓ R36(2, 1), −R36(2, 2) )
Note that the values of R36 elements are known once q1, q2 and q3 are too.
Now, the values for q4 and q6 are computed by solving the following systems:
(
q4 − q6 = q4c − q6c
q4 + q6 = atan2(− R36(2, 1), −R36(2, 2) )
(
q4 + q6 = q4c + q6c
q4 − q6 = atan2( R36(2, 1), −R36(2, 2) )
where the first one is relative to q5 = 0, while the second to q5 = π.
Figure 1.8: Planar view of a wrist singularity
13

14. Note that the chosen procedure for fixing the values of q4 and q6 corresponds to distributing
equally the rotation among them. Moreover, since geometrical compenetration is not physi-
cally acceptable, in the case of PUMA 560 a wrist singularity may only exist if q5 = 0.
An elbow lock happens if the WCP lies in the plane formed by the joint axes 2 and 3 (fig-
ure 1.9). In such a case the up and down configurations collapse into a single one, with only
two possible combinations of ( q1, q2, q3 ), leading to a total of 4 distinct configurations. This
happens if the term ( b3 s3 + l4 c3 ) at s (q2) denominator becomes 0. Under this condition,
s3 = −(l4c3)/b3; by substituing this expression into (1.2), (1.3) and (1.4), it is possible to
obtain:











s3 =
K l4
l4
2
+ b3
2
c3 = −
K b3
l4
2
+ b3
2









s2 =
h − zp
l2 − K
c2 =
xp c1 + yp s1
l2 − K
In particular, (1.1) and the previous systems lead to two different solutions for q1 and q2 and
only one solution for q3. Consequently, there will be only two distinct configurations for the
first three joint variables, once again resulting in a loss of one degree of freedom of the end-
effector.
Figure 1.9: Planar view of an elbow lock
14

15. Lastly, a shoulder lock happens whenever the WCP lies in the plane defined by joint axes 1
and 2. In this case, xp = −l3 s (q1) and yp = l3 c (q1), with equation (1.1) resulting in the iden-
tity l3 = l3. This condition means that the WCP belongs to the cylindrical surface centered at
the origin of frame (0), with radius l3 (figure 1.10). Moreover, note that such a surface has a
maximum height of h + l2 +
p
b3
2
+ l4
2
, reachable by the WCP only if a shoulder singularity
combines with an elbow one.
If xp and yp are both different from 0, it is possible to find a unique solution for q1. Conse-
quently, if no other singularity occurs, there are 4 different possible configurations. On the
contrary, if both xp and yp are null (only possible if l3 = 0), the WCP belongs to joint axis
1 and there exists an infinite number of values for q1 which yield to the same pose of the
end-effector (in this case the cylindrical surface collapses on axis 1). In addition to this, it
is trivial to note that the internal volume of the cylindrical surface is not reacheable by the
WCP: it represents a so called dead zone for the end-effector.
Figure 1.10: Planar and 3D view of a shoulder-locked configuration
15

16. Note that c (q2 ) and s (q2 ) denominators can become 0 also if the WCP belongs to the
spherical surface of radius l3 centered in (0, 0, h). Since this surface is enclosed by the cylindrical
one, the only case in which the denominator can become null is when the WCP lies on the
circumference defined by the intersection of the two surfaces. For every possible q1, it is
geometrically trivial to note that this can only happen if both the following conditions are
satisfied:
- the WCP lies in the plane formed by axes 2 and 3 (or equivalently if an elbow singularity
occurs)
- l2 =
p
b3
2
+ l4
2
, which ensures that the WCP can reach the axis of joint 1
However, since it is not physically possible for link 1 and the end-effector to compenetrate,
such a singularity of c (q2 ) and s (q2 ) denominators will never appear.
For sake of completeness, figure 1.11 shows the locus of reachable points for the WCP: it
is a pseudo-spherical domain, internally delimited by the cylindrical dead zone and externally
by the spherical surface associated to the robot maximum reach.
In a real context, actually, the reachable volume should also account for joint limits and any
possible mechanical interference deriving by the actual robot geometry.
Figure 1.11: Locus of reachable points for the WCP (blue shaded volume)
16

17. 1.6. GRIPPER POSE REPRESENTATION:
DIRECT AND INVERSE PROBLEMS
Throughout all sections the analytical model was developed taking into account the end-effector
only. It is clear that such a procedure does not result in any limitation whatsoever. This sec-
tion completes the previous ones by introducing also the pose of the gripper. By means of such
a step, the analytical model for the position of PUMA 560 is fully defined.
In particular, it is necessary to compute the homogeneous transformation M6t, which is a ma-
trix of constant values:
M6t =




0 −1 0 0
1 0 0 0
0 0 1 dwt
0 0 0 1




With the knowledge of this matrix it is also easy to compute the absolute homogeneous trans-
formation M0t:
M0t = M06 M6t
Where,
M0t =




r11 r12 r13 xt
r21 r22 r23 yt
r11 r12 r13 zt
0 0 0 1



 =




xT CP
R0t yT CP
zT CP
0 0 0 1




Choosing a ZYZ Euler angle representation and defining the basic rotation matrices
R1z =


c (θ1) −s (θ1) 0
s (θ1) c (θ1) 0
0 0 1

 R2y =


c (θ2) 0 s (θ2)
0 1 0
−s (θ2) 0 c (θ2)


R3z =


c (θ3) −s (θ3) 0
s (θ3) c (θ3) 0
0 0 1


the rotational part of M0t takes the form:
R0t (θ1, θ2, θ3) = R1z (θ1) R2y (θ2) R3z (θ3) =
=






c(θ1) c(θ2) c(θ3) − s(θ1) s(θ3) −c(θ1) c(θ2) s(θ3) − c(θ3) s(θ1) c(θ1) s(θ2)
s(θ1) c(θ2) c(θ3) + c(θ1) s(θ3) −s(θ1) c(θ2) s(θ3) + c(θ1) c(θ3) s(θ1) s(θ2)
−c(θ3) s(θ2) s(θ2) s(θ3) c(θ2)






17

18. The knowledge of R0t expression allows to solve both the direct and inverse problem for
the pose of the gripper. In particular, the direct one consists in determining the values of θ1, θ2
and θ3 and the absolute position of the TCP, assuming M0t is known. The TCP is readily
available by extracting the first three elements of the fourth column of M0t, while to obtain all
θi it is necessary to solve a set of trigonometric equations concerning the rotary part of M0t.
Specifically, it is possible to obtain the following relationships:
c (θ2) = r33 → s (θ2) =
q
1 − c (θ2)
2
c (θ1) =
r13
s (θ2)
s (θ1) =
r23
s (θ2)
c (θ3) = −
r31
s (θ2)
s (θ3) =
r32
s (θ2)
Hence, from the previous equations θ1, θ2 and θ3 can be trivially obtained as:
θ1 = atan2 (s (θ1), c (θ1))
θ2 = atan2 (s (θ2), c (θ2))
θ3 = atan2 (s (θ3), c (θ3))
Note that for s (θ2) it has been conventionally selected the positive value and s (θ2) = 0 is a
degenerate solution and it represents a rotation of θ1 + θ3 around the axis z0.
In particular, from the expression of R0t computed for θ2 = 0 and θ2 = π,
R0t |θ2=0,π =


±c (θ1 ± θ3) −s (θ1 ± θ3) 0
±s (θ1 ± θ3) c (θ1 ± θ3) 0
0 0 ±1


it possible to obtain, respectively:
θ1 ± θ3 = atan2(± R0t(2, 1), R0t(2, 2) )
Now, the values for θ1 and θ3 are computed by solving the following systems:
(
θ1 − θ3 = θ1c − θ3c
θ1 + θ3 = atan2( R0t(2, 1), R0t(2, 2) )
(
θ1 + θ3 = θ1c + θ3c
θ1 − θ3 = atan2(− R0t(2, 1), R0t(2, 2) )
where the first one is relative to θ2 = 0, while the second to θ2 = π and θ1c, θ3c are the current
values of the Euler angles.
Once the direct problem is solved, the absolute pose of the tool frame is fully known.
18

19. The inverse problem, on the contrary, consists in determining the value of M0t, once the
required absolute orientation and position of the tool frame are assigned. The orientation can
be easily computed by calculating R0t for the desired values of θ1, θ2 and θ3, while the trans-
lational part of M0t is directly available when the position of the TCP is chosen.
r11 = c(θ1) c(θ2) c(θ3) − s(θ1) s(θ3)
r12 = −c(θ1) c(θ2) s(θ3) − c(θ3) s(θ1)
r13 = c(θ1) s(θ2)
r21 = s(θ1) c(θ2) c(θ3) + c(θ1) s(θ3)
r22 = −s(θ1) c(θ2) s(θ3) + c(θ1) c(θ3)
r23 = s(θ1) s(θ2)
r31 = −c(θ3) s(θ2)
r32 = s(θ2) s(θ3)
r33 = c(θ2)
Once the analytical model for the gripper pose is defined, it is possible to solve the complete
direct and inverse problems for position of the PUMA 560.
19

20. 1.7. COMPLETE KINEMATIC PROBLEM FOR POSITION:
RECAPITULATION
The following flow charts describe all the logical steps involving both the complete direct prob-
lem for position (left) and the inverse one (right):
COMPLETE DIRECT PROBLEM
FOR POSITION (PUMA 560)
COMPLETE INVERSE PROBLEM
FOR POSITION (PUMA 560)
INPUTS
M06 (q1, q2, q3, q4, q5, q6)
M0t = M06 M6t
R0t [ xT CP , yT CP , zT CP ]
[ θ1, θ2, θ3 ]
TCP POSE
INPUTS R0t (θ1, θ2, θ3)
[ θ1, θ2, θ3 ]
M0t
M06 = M0t M6t
−1
WCP
singular
pose?
Finite set of {qi}
(max. 8 distinct
geometrically compatible
configurations)
- No solution
- ∞ solutions
- Finite set (6= 8)
Set of {qi},
for i = 1, 2, ..., 6
[ xT CP , yT CP , zT CP ]
(No) (Yes)
Figure 1.12: Flow charts for the direct (left) and inverse position (right) problem
The algorithms for the solution of the direct and inverse position kinematics are syntheti-
cally described by the above flow charts.
Those algorithms are implemented in a series of Matlab scripts, attached to the report.
In particular, for what concerns the inverse part, a thorough analysis of the singular positions
was carried out: the scripts are robust with respect to any possible singular configuration the
robotic arm may assume.
20

21. 2. ANALYTICAL MODEL FOR VELOCITY
AND ACCELERATION
The second part of the report will be dedicated to the development of the analytical model for
velocity and acceleration kinematics.
2.1. FORWARD ANALYSIS
The forward analysis for velocity and acceleration consists in determining, respectively, the
TCP velocity and acceleration, once a trajectory for the joint variables is chosen.
From now on, the following notation will be used:
Q = (q1, q2, q3, q4, q5, q6)
T
Q̇ = ( ˙
q1, ˙
q2, ˙
q3, ˙
q4, ˙
q5, ˙
q6)
T
Q̈ = ( ¨
q1, ¨
q2, ¨
q3, ¨
q4, ¨
q5, ¨
q6)
T
For what concerns the direct velocity problem, the first step is the computation of the relative
screw axis matrices Li−1,i between adjacent links. Thanks to the choice of the frames in
compliance with the Denavit-Hartenberg convention, the screw axis of frame i has always the
same orientation with respect to the local frame i − 1.
As a consequence, being all the joint of rotoidal type, the relative screw matrices take the form:
L
(i−1)
i−1,i =




0 −1 0 0
1 0 0 0
0 0 0 0
0 0 0 0




To compute the relative velocity matrices of all joints with respect to frame (0), it is also
necessary to express the relative screw matrices with respect to frame (0):
L
(0)
i−1,i ( q1, ..., qi−1 ) = M0,i−1 ( q1, ..., qi−1 ) L
(i−1)
i−1,i Mi−1,0 ( q1, ..., qi−1 )
The relative velocity matrix of frame i with respect to frame i − 1, written in the absolute
frame (0), thus becomes:
W
(0)
i−1,i = L
(0)
i−1,i ˙
qi
The relationship between the position of a point P belonging the rigid link j and the position
of the same point with respect to the link i, written in an arbitrary frame (k) for motion
description, is:
P
(k)
k = Mk,i Mi,j P
(j)
j
21

22. Differentiation with respect to time of the previous expression and the introduction of some
useful identity matrices in the form ( Mh,l Ml,h ) lead to:
Ṗ
(k)
k = Ṁk,i Mi,j + Mk,i Ṁi,j
P
(j)
j =
= { Ṁk,i (Mi,k Mk,i) Mi,j + Mk,i Ṁi,j [Mj,i (Mi,k Mk,i) Mi,j] } P
(j)
j =
=
Ṁk,i Mi,k + Mk,i Ṁi,j Mj,i
Mi,k
Mk,i Mi,j P
(j)
j =
=
W
(k)
k,i + Mk,i W
(i)
i,j Mi,k
P
(k)
k =
=
W
(k)
k,i + W
(k)
i,j
P
(k)
k
The comparison with Ṗ
(k)
k = W
(k)
k,j P
(k)
k implies:
W
(k)
k,j = W
(k)
k,i + W
(k)
i,j
By exploiting the previous result, it is possible to write the absolute velocity matrix of the nth
link as follows:
W
(0)
0n =
n
X
i=1
W
(0)
i−1,i =




ω 3
V
0 0 0 0




(0)
0n
=




0 −ωz ωy V0x
ωz 0 −ωx V0y
−ωy ωx 0 V0z
0 0 0 0




(0)
0n
However, instead of writing all the matrices in the frame (0), a convenient choice is to use
an auxiliary frame (a), with the same orientation of (0) and origin in the TCP. This way, the
angular velocities of all links remain the same and the velocity of the TCP is readily obtainable
by looking at V
(a)
0x , V
(a)
0y and V
(a)
0z in W
(a)
06 .
In particular:
W
(a)
0n = Ma0 W
(0)
0n M0a =




0 −ωz ωy V0x
ωz 0 −ωx V0y
−ωy ωx 0 V0z
0 0 0 0




(a)
0n
Where
M0a =




1 0 0 xT CP ( Q )
0 1 0 yT CP ( Q )
0 0 1 zT CP ( Q )
0 0 0 0




represents the homogeneous transformation from frame (a) to (0).
The well known expression of the acceleration matrix between two generic frames (h) and (l),
written in the coordinate frame (k), has the following form:
H
(k)
h,l =




ω̇ + ω2 3
A
0 0 0 0




(k)
h,l
By double differentiating P
(k)
k = Mk,i Mi,j P
(j)
j with respect to time and, once again, by in-
serting some useful identity matrices:
22

23. P̈
(k)
k = M̈k,i Mi,j + 2Ṁk,iṀi,j + Mk,i M̈i,j
P
(j)
j =
=
M̈k,i (Mi,kMk,i) Mi,j + 2Ṁk,i (Mi,kMk,i) Ṁi,j (Mj,iMi,j) + Mk,iM̈i,j (Mj,iMi,j)
P
(j)
j =
=
M̈k,iMi,kMk,i + 2Ṁk,iMi,kMk,iṀi,jMj,i (Mi,kMk,i) +
+ Mk,iM̈i,jMj,i (Mi,kMk,i)
Mi,jP
(j)
j =
=
M̈k,iMi,k + 2 Ṁk,iMi,k
Mk,i Ṁi,jMj,i
Mi,k + Mk,i M̈i,jMj,i
Mi,k
Mk,iMi,jP
(j)
j =
=
H
(k)
k,i + 2W
(k)
k,i Mk,iW
(i)
i,j Mi,k + Mk,iH
(i)
i,j Mi,k
P
(k)
k =
=
H
(k)
k,i + 2W
(k)
k,i W
(k)
i,j + H
(k)
i,j
P
(k)
k
The comparison with P̈
(k)
k = H
(k)
k,j P
(k)
k leads to:
H
(k)
k,j = H
(k)
k,i + 2W
(k)
k,i W
(k)
i,j + H
(k)
i,j
By exploiting this relationship and by considering the auxiliary frame (a) it is possible to obtain:
H
(a)
0n =
n
X
i=1
H
(a)
i−1,i + 2
n
X
h=2
h−1
X
k=1
W
(a)
k−1,k W
(a)
h−1,h =
=
n
X
i=1
L
(a)
i−1,i q̈i + L
(a) 2
i−1,i ˙
qi
2
+ 2
n
X
h=2
h−1
X
k=1
L
(a)
k−1,k L
(a)
h−1,h ˙
qk ˙
qh
In particular, H
(a)
0n has the following form:
H
(a)
0n =




A0x
ω̇ + ω2
A0y
A0z
0 0 0 0




(a)
0n
In an analogous way with respect to the case of velocity matrices, by choosing the frame (a),
the angular accelerations of all links are invariant and the acceleration of the TCP is readily
obtainable by looking at A
(a)
0x , A
(a)
0y and A
(a)
0z in H
(a)
06 .
Summing up:




V0x
ω V0y
V0z
0 0 0 0




(a)
06
= W
(a)
06 Q, Q̇
Since the right-hand side of the previous equation is known, it is possible to obtain the ro-
tational and translational components of the velocity of the TCP by simply looking at the
elements of W
(a)
06 .
The translational part of the acceleration is also directly obtainable by looking at the first
three elements of the fourth column of H
(a)
06 :




A0x
ω̇ + ω2
A0y
A0z
0 0 0 0




(a)
06
= H
(a)
06 Q, Q̇, Q̈
Since now ω is known, from the previous equation, ω̇ can be computed in the following way:
ω̇ = H
(a)
06 (1 : 3, 1 : 3) − ω2
23

24. In conclusion, the TCP velocity and acceleration in world coordinates have been computed
and it is now possible to write:
Ṡ = [V0x, V0y, V0z, ωx, ωy, ωz]
S̈ = [A0x, A0y, A0z, ω̇x, ω̇y, ω̇z]
The following flow charts describe the logical steps involving both the direct problem for ve-
locity (left) and acceleration (right):
DIRECT PROBLEM
FOR VELOCITY
DIRECT PROBLEM
FOR ACCELERATION
M0,i−1
Q
L
(0)
i−1,i = M0,i−1 L
(i−1)
i−1,i Mi−1,0
L
(i−1)
i−1,i
W
(0)
i−1,i = L
(0)
i−1,i q̇i
Q̇
W
(0)
06 =
P6
i=1 W
(0)
i−1,i
W
(a)
06 = Ma0 W
(0)
06 M0a M0a
Ṡ = [V0x, V0y, V0z, ωx, ωy, ωz]
M0a
L
(a)
i−1,i W
(a)
i−1,i
L
(0)
i−1,i W
(0)
i−1,i
H
(a)
06 =
P6
i=1
L
(a)
i−1,i q̈i + L
(a) 2
i−1,i ˙
qi
2
+
+2
P6
h=2
Ph−1
k=1 W
(a)
k−1,k W
(a)
h−1,h
Q, Q̇
ω̇ = H
(a)
06 (1 : 3, 1 : 3) − ω2
ω
[A0x, A0y, A0z]
H
(a)
06 =
P6
i=1
L
(a)
i−1,i q̈i + L
(a) 2
i−1,i ˙
qi
2
+
+2
P6
h=2
Ph−1
k=1 W
(a)
k−1,k W
(a)
h−1,h
S̈ = [A0x, A0y, A0z, ω̇x, ω̇y, ω̇z]
Figure 2.1: Flow charts for the direct velocity (left) and acceleration (right) problems
Note that some of the inputs of the direct problem for acceleration are computed during the
solution of the velocity one.
24

25. 2.2. INVERSE ANALYSIS
The inverse analysis for velocity and acceleration consists in determining compatible Q̇ and Q̈
with respect to assigned TCP velocity and acceleration. For determining the joint velocities
and accelerations, the first step requires the solution of the inverse position problem, for the
assigned pose of the gripper. The solution of the kinematic problem for velocity and acceleration
will have to be carried out for each set of compatible joint configurations Q.
Once Q is known, it is necessary to compute the absolute homogeneous transformations M0i
of each link, which are then used to compute L
(a)
i−1,i in the following way:
L
(a)
i−1,i ( Q ) = Ma0 ( Q ) M0,i−1 ( q1, ..., qi−1 ) L
(i−1)
i−1,i Mi−1,0 ( q1, ..., qi−1 ) M0a ( Q )
By exploiting the theorem of relative motions, it is possible to obtain:
6
X
i=1
L
(a)
i−1,i(Q) q̇i = W
(a)
06 (S, Ṡ)
where the right-hand side is fully known.
In particular, the equation has the following explicit expression:




0 −uz1 uy1 tx1
uz1 0 −ux1 ty1
−uy1 ux1 0 tz1
0 0 0 0




(a)
q̇1 + . . . . +




0 −uz6 uy6 tx6
uz6 0 −ux6 ty6
−uy6 ux6 0 tz6
0 0 0 0




(a)
q̇6 =
=




0 −ωz ωy V0x
ωz 0 −ωx V0y
−ωy ωx 0 V0z
0 0 0 0




(a)
06
It represents a linear system in the unknowns Q̇, rearrangable in the form:








tx1 tx2 tx3 tx4 tx5 tx6
ty1 ty2 ty3 ty4 ty5 ty6
tz1 tz2 tz3 tz4 tz5 tz6
ux1 ux2 ux3 ux4 ux5 ux6
uy1 uy2 uy3 uy4 uy5 uy6
uz1 uz2 uz3 uz4 uz5 uz6








(a)







q̇1
q̇2
q̇3
q̇4
q̇5
q̇6








=








V0x
V0y
V0z
ωx
ωy
ωz








The previous expression can be synthetically rewritten by introducing the Jacobian matrix J(a)
:
J(a)
(Q) Q̇ = V (a)
(S, Ṡ)
Therefore, if J(a)
is nonsingular, it is possible to obtain Q̇ as:
Q̇ = J(a)
(Q)−1
V (a)
(S, Ṡ)
25

26. Once Q̇ is known, it is possible to proceed with the solution of the inverse problem for the
acceleration.
In particular, by exploiting the previously found relationship for the absolute acceleration ma-
trix H
(a)
06 , it is possible to write:
H
(a)
06 (Q, Q̇, Q̈) =
6
X
i=1
L
(a)
i−1,i q̈i + L
(a) 2
i−1,i ˙
qi
2
+ 2
6
X
h=2
h−1
X
k=1
L
(a)
k−1,k L
(a)
h−1,h ˙
qk ˙
qh
It follows that:
6
X
i=1
L
(a)
i−1,i q̈i = H
(a)
06 (S, Ṡ, S̈)−
6
X
i=1
L
(a) 2
i−1,i ˙
qi
2
−2
6
X
h=2
h−1
X
k=1
L
(a)
k−1,k L
(a)
h−1,h ˙
qk ˙
qh := H̃
(a)
06 (Q, Q̇, S, Ṡ, S̈)
6
X
i=1
L
(a)
i−1,i q̈i = H̃
(a)
06 (Q, Q̇, S, Ṡ, S̈)
where H̃
(a)
06 is completely known, once the inverse analysis for velocity is carried out.
Once again the equation represents a linear system, this time in Q̈, and can be rewritten using
the same Jacobian computed before:
J(a)
(Q) Q̈ = Ã(a)
(S, Ṡ, S̈)
With
Ã(a)
:=
H̃(a)
(1, 4), H̃(a)
(2, 4), H̃(a)
(3, 4), H̃(a)
(3, 2), H̃(a)
(1, 3), H̃(a)
(2, 1)
T
If J(a)
is nonsingular, the joint accelerations can be obtained as:
Q̈ = J(a)
(Q)−1
Ã(a)
(S, Ṡ, S̈)
Now the inverse kinematic problem for both velocity and acceleration is solved.
Recall that the inverse problems for both velocity and acceleration are solvable if and only if
the Jacobian matrix is nonsingular.
In particular, the expression of J(a)
determinant is the following:
| J(a)
| = −
l2 sin (q5)
2
b3
2
sin (q2 + 2 q3) − l4
2
sin (q2) − b3
2
sin (q2) − l4
2
sin (q2 + 2 q3)
+ 2 b3 l4 cos (q2 + 2 q3) + l2 l4 cos (q2 − q3) − b3 l2 sin (q2 − q3) +
+ l2 l4 cos (q2 + q3) + b3 l2 sin (q2 + q3) ]
By looking at the values for which the determinant becomes null it is easy to note that those
conditons are the same for which a wrist or an elbow singularity occurs.
In fact, the condition sin (q5) = 0 simply corresponds to a wrist lock.
Moreover, by imposing an elbow lock (corresponding to q3 = atan2(−l4, b3) ), the determinant
becomes:
| J(a)
| = −
l2 sin (q5)
2
(
cos (q2)
l4
3
l4
2
b3
2 + 1
1
b3
− b3 l4 +
b3 l4
l4
2
b3
2 + 1
!
+
+ sin (q2)
b3
2
l4
2
b3
2 + 1
− b3
2
+
l4
2
l4
2
b3
2 + 1
! )
The terms multiplying cos (q2) and sin (q2) are clearly null and, as a consequence, | J(a)
| is also
null. Consequently, it is possible to conclude that both the wrist and elbow locks are not only
position singularities, but also velocity and acceleration ones.
In particular, during the motion, velocities and accelerations tend to assume very high values
in proximity of a singular position. As a consequence it is crucial, from a trajectory planning
point of view, to avoid such conditions.
26

27. The following flow charts describe the logical steps involving both the inverse problem for
velocity (left) and acceleration (right):
INVERSE PROBLEM
FOR VELOCITY
INVERSE PROBLEM
FOR ACCELERATION
Q
M0,i−1 M0a
L
(i−1)
i−1,i
L
(a)
i−1,i = Ma0 M0,i−1 L
(i−1)
i−1,i Mi−1,0 M0a
P6
i=1 L
(a)
i−1,i q̇i = W
(a)
06 (S, Ṡ)
S, Ṡ
J(a)
(Q) Q̇ = V (a)
(S, Ṡ)
Q̇ = J(a)
(Q)−1
V (a)
(S, Ṡ)
L
(a)
i−1,i
S, Ṡ, S̈ Q̇
H̃
(a)
06 = H
(a)
06 (S, Ṡ, S̈) −
P6
i=1 L
(a) 2
i−1,i ˙
qi
2
+
−2
P6
h=2
Ph−1
k=1 L
(a)
k−1,k L
(a)
h−1,h ˙
qk ˙
qh
P6
i=1 L
(a)
i−1,i q̈i = H̃
(a)
06
J(a)
(Q) Q̈ = Ã(a)
Q̈ = J(a)−1
Ã(a)
Figure 2.2: Flow charts for the inverse velocity (left) and acceleration (right) problems
Note that, once again, some of the inputs of the inverse problem for acceleration are computed
during the solution of the velocity one.
27

28. The algorithms above described were implemented and tested using dedicated Matlab func-
tions: the main test function plots all compatible linear trajectories for given initial and final
TCP pose. In particular, the joint trajectories are computed imposing piecewise constant ac-
celerations of the TCP (resulting in trapezoidal velocities and polynomial positions):
Figure 2.3: TCP position (above), velocity (center) and acceleration (below)
Note that the above graphs show the comparison between task and test values (useful to vali-
date the results): the task is completely assigned for each point of the trajectory, while the test
is computed by applying the forward analysis to the values obtained by the inverse analysis.
28

29. In particular, the selected initial and final pose for the test are:
[ xT CP , yT CP , zT CP , θ1, θ2, θ3 ]initial
= [0.57, 0, 0.41, 90◦
, 90◦
, 0◦
]
[ xT CP , yT CP , zT CP , θ1, θ2, θ3 ]final
= [0.67, 0.30, 1.25, 90◦
, 100◦
, 0◦
]
The following figures show the arm initial and final pose, as well as the trajectory of the
TCP (which is linear by constraint):
Figure 2.4: Initial pose (left) and final pose (right)
29

30. The following figures show the joint positions, velocities and accelerations computed with
the inverse analysis. Once again, these graphs show the comparison between reference (+
markers) and test values (continuous lines): the references are computed by making numerical
derivatives of joint trajectories, while the tests are the ones obtained by solving the inverse
problem.
Figure 2.5: Joint positions (above), joint speeds (below-right) and joint accelerations (below-left)
By looking at the graphs, for instance at the third joint, the coherence between position, veloc-
ity and acceleration is evident. As an example, the almost linear behaviour of velocity in the
range [0.3, 0.6]s results in a plateau for the acceleration in the same interval. Analogously, the
big excursion of velocity in the same range reflects a consistent change in related joint position.
Note, moreover, that in proximity of the final positions high accelerations are needed to make
the joint velocity null, because of the assignment of a rest-to-rest trajectory.
As a final remark, it is important to highlight that the procedures carried out to test the
algorithms for both the direct and inverse problems do not constitute a proper example of
trajectory planning, which will be the topic of the following section.
30

31. 3. TRAJECTORY PLANNING
The third part of the report will be dedicated to the trajectory planning: it is the set of
procedures and techniques which have to be implemented in order to make the robot perform
a specific task.
There are several possible kinds of motion which can be found in practice; they can all be
subdivided, however, into two main categories:
- Point to point motion
- Motion with assigned trajectory
For what concerns the first, it only requires information about the initial position, the final
position and the execution time.
In the second, on the contrary, the path and/or the motion equations are specified. In the
following discussion only the first type of motion will be taken into consideration.
In particular, the point to point trajectory planning is made of several steps.
First of all, the inverse kinematics for position is solved for the initial and final poses of the
TCP. Then, the difference between the final and initial position of each joint is computed for
each compatible joint configuration. At this point, a criterion for selecting one of the compat-
ible configurations is adopted: a possible choice, for example, is to select the configuration for
which
P6
1 | ∆qi | is minimum.
Once the equations of motion for each joint are established, the minimal execution time Ti
for each joint is computed. Note that Ti is evaluated on the basis of the selected equations of
motion, also taking into account all the actuators limitations. The maximum between all Ti
is then selected as the robot execution time Te. Since it is convenient to make the joints start
and stop simultaneously to avoid useless motor overload, the joint trajectories are then scaled
with respect to time.
In the upcoming sections three different types of rest-to-rest trajectories will be discussed:
- Polynomial Peisekah trajectory
- Gutman trajectory (for reducing vibrations)
- Piecewise constant acceleration for minimizing the maximum specific power
31

32. The following flow chart synthetizes the above described procedure:
TRAJECTORY PLANNING
PROCEDURE
(point to point)
INVERSE KINEMATICS
FOR POSITION
Initial TCP pose
∆Q for each
compatible configuration
Final TCP pose
SELECTING CRITERION
Selected ∆Q
Ti
Equation/s of motion
Te = max( Ti )
Scaled trajectory
for each joint
Figure 3.1: Flow chart describing the trajectory planning procedure
32

33. 3.1. POLYNOMIAL PEISEKAH TRAJECTORY
The Peisekah polydyne function taken into account is of 9th
order and has the following form:
q( t ) = qin + ( qf − qin )
126
t
T
5
− 420
t
T
6
+ 540
t
T
7
− 315
t
T
8
+ 70
t
T
9
#
where (qf − qin) is the difference between the joint final position and the initial one.
The order and the coefficients of the function are chosen in order to meet a series of boundary
conditions, which become evident by studying the shape of the derivatives. In particular, by
deriving the trajectory equation four times with respect to λ : = t/T, it is easy to obtain:
d q(λ)
d λ
= 630 (qf − qin) (λ − 1)
4
λ4
d2
qi(λ)
d λ2
= 2520 (qf − qin) (λ − 1)
3
λ3
(2 λ − 1)
d3
q(λ)
d λ3
= 2520 (qf − qin) (λ − 1)
2
λ2
14 λ2
− 14 λ + 3
d4
q(λ)
d λ4
= 15120 (qf − qin) λ 14 λ4
− 35 λ3
+ 30 λ2
− 10 λ + 1
(3.1)
(3.2)
(3.3)
(3.4)
By looking at the previous equations, it is clear how the velocity, acceleration, jerk and snap
are all null in t = 0 and t = T, that is at the start and end of the trajectory. This is a useful
property, which avoids exiting vibrations [2].
Moreover, from (3.1) and (3.2) it is easy to see that the velocity as a unique maximum (or
minimum if (qf − qin) 0) for λ = 1/2.
By cosidering only absolute values and imposing:
q̇(t) |t=T/2 =
d q(λ)
d λ
d λ
d t

37. λ=1/2
=
1
Tv, min
d q(λ)
d λ

41. λ=1/2
= Vmax
where Vmax is the maximum reachable velocity, it is possible to write:
Tv, min =
315 | qf − qin |
128 Vmax
Note that Tv, min is the execution time for which the maximum allowed joint velocity is reached
during the trajectory and it has to be computed for each joint.
For what concerns the acceleration, it has one maximum and one minimum, antisimmetric
with respect to λ = 1/2, with equal amplitude (in modulus) and located at λ = (7 ±
√
7)/14.
Again, by cosidering only absolute values and imposing
q̈(t) |t=
(7+
√
7) T
14
=
1
T 2
a, min
d2
q(λ)
d λ2

45. λ=
(7+
√
7)
14
= Amax
where Amax is the maximum reachable acceleration, it is possible to write:
Ta, min =
s
8505 | qf − qin |
341
√
7 Amax
Analogously, Ta, min is the execution time for which the maximum allowed acceleration is
reached during the trajectory and it has to be computed for each joint.
Since both the maximum velocity and the maximum joint acceleration must not be exceeded,
each joint minimum execution time Ti is selected as the maximum value between i
Tv, min and
i
Ta, min.
33

46. Lastly, the robot execution time Te is selected as:
Te = max( Ti )
The trajectory of the ith
joint, for i = 1, 2, .., 6, takes the form:
qi( t ) = qin,i+( qf,i−qin,i )
126
t
Te
5
− 420
t
Te
6
+ 540
t
Te
7
− 315
t
Te
8
+ 70
t
Te
9
#
˙
qi( t ) =
630 ( qf,i − qin,i )
Te
t
Te
4
− 4
t
Te
5
+ 6
t
Te
6
− 4
t
Te
7
+
t
Te
8
#
¨
qi( t ) =
2520 ( qf,i − qin,i )
T 2
e
t
Te
3
− 5
t
Te
4
+ 9
t
Te
5
− 7
t
Te
6
+ 2
t
Te
7
#
34

47. 3.2. GUTMAN TRAJECTORY
The Gutman trajectory has the following form:
q( t ) = qin + ( qf − qin )
t
T
−
15
32π
sin
2π
t
T
−
1
96π
sin
6π
t
T
where (qf − qin) is the difference between the joint final position and the initial one.
This trajectory is obtained as a Fourier series expansion of the parabolic profile, by taking
into account only the first two terms [1]. Also for this kind of trajectory, the order and the
coefficients of the function are chosen in order to meet a series of boundary conditions. In
particular, by deriving the trajectory equation four times with respect to λ : = t/T, it is easy
to obtain:
d q(λ)
d λ
=
(qf − qin)
16
[16 − 15 cos (2πλ) − cos (6πλ) ]
d2
q(λ)
d λ2
=
3π (qf − qin)
8
[ 5 sin (2πλ) + sin (6πλ) ]
d3
q(λ)
d λ3
=
3π2
(qf − qin)
4
[ 5 cos (2πλ) + 3 cos (6πλ) ]
(3.5)
(3.6)
(3.7)
(3.8)
By looking at previous equations, it is clear how the velocity and acceleration are both null in
t = 0 and t = T.
Moreover, from (3.5) and (3.6) it can be deduced that the velocity has once again a unique
maximum (or minimum if (qf − qin) 0) for λ = 1/2.
By cosidering only absolute values and imposing:
q̇(t) |t=T/2 =
d q(λ)
d λ
d λ
d t

51. λ=1/2
=
1
Tv, min
d q(λ)
d λ

55. λ=1/2
= Vmax
it is possible to write:
Tv, min =
2 | qf − qin |
Vmax
For what concerns the acceleration, it has two maximums and two minimums , antisymmetric
with respect to λ = 1/2, with equal amplitude (in modulus) and located at
λ =
atan
q
2 ±
√
3
/π; λ = 1 −
atan
q
2 ±
√
3
/π
.
Again, by cosidering only absolute values and imposing
q̈(t) |
t=
atan(
√
2+
√
3)T
π
=
1
T 2
a, min
d2
q(λ)
d λ2

59. λ=
atan(
√
2+
√
3)
π
= Amax
where Amax is the maximum reachable acceleration, it is possible to write:
Ta, min =
s
2 π | qf − qin |
Amax
r
2
3
Since both the maximum velocity and the maximum joint acceleration must not be exceeded,
each joint minimum execution time Ti is selected as the maximum value between i
Tv, min and
i
Ta, min.
Lastly, the robot execution time Te is selected as:
Te = max( Ti )
35

60. The trajectory of the ith
joint, for i = 1, 2, , .., 6, takes the form:
qi( t ) = qin,i + ( qf,i − qin,i )
t
Te
−
15
32π
sin
2π
t
Te
−
1
96π
sin
6π
t
Te
˙
qi( t ) =
(qf,i − qin,i)
16 Te
16 − 15 cos
2π
t
Te
− cos
6π
t
Te
¨
qi( t ) =
3π (qf − qin)
8 T 2
e
5 sin
2π
t
Te
+ sin
6π
t
Te
36

61. 3.3. PIECEWISE CONSTANT ACCELERATION FOR SPECIFIC
POWER MINIMIZATION
The problem of determining a piecewise constant acceleration trajectory simply reduces to the
computation of the coefficients of three polynomial functions, defined on the intervals ∆t1, ∆t2
and ∆t3.
In particular, the joint position q( t ) has the form:
q( t ) =























A
t
T
2
+ B
t
T
+ C + qin for 0 ≤ t t1
D
t − t1
T
+ E + qin for t1 ≤ t t2
F
t − t2
T
2
+ G
t − t2
T
+ H + qin for t2 ≤ t ≤ T
The robot execution time is T = 2 ∆t1 + ∆t2, where ∆t1 = ∆t3 was imposed.
Note that such a trajectory shape results into a trapezoidal joint velocity profile.
Defining Λ : = ∆t1/T, it is possible to write the relationships between the maximum joint
velocity Vm, the maximum joint acceleration Am and the rest of the parameters.
In particular (see [3]) :
Vm =
qf − qin
T (1 − Λ)
; Am =
qf − qin
T 2 Λ (1 − Λ)
The maximum required specific power Pm, in the case of dominant inertial forces, can be sim-
ply computed as:
Pm = Am Vm =
(qf − qin)
2
T 3 Λ(1 − Λ)
2
The minimum of the previous expression is located at Λ = 1/3, which implies ∆t1 = ∆t2 = ∆t3.
Finally, the coefficients of the piecewise polynomial trajectory can be simply computed by im-
posing to q( t ) proper boundary and continuity conditions.
Specifically, those conditions are:
q( 0 ) = qin; q̇( 0 ) = 0
q( T ) = qf ; q̇( T ) = 0
q( t−
1 ) = q( t+
1 ); q̇( t−
1 ) = q̇( t+
1 )
q( t−
2 ) = q( t+
2 ); q̇( t−
2 ) = q̇( t+
2 )
By imposing the previous constraints, it is easy to obtain:
A = 9/4 (qf − qin) ; B = 0; C = 0;
D = 3/2 (qf − qin) ; E = 1/4 (qf − qin)
F = −9/4 (qf − qin) ; G = 3/2 (qf − qin) ; H = 3/4 (qf − qin)
By an analogous reasoning with respect to the one adopted in the previous sections, the Tv,min
and Ta,min are computed as:
Tv, min =
3 | qf − qin |
2 Vmax
Ta, min = 3
s
| qf − qin |
2 Amax
37

62. where Vmax and Amax are the maximum allowed joint velocity and acceleration.
Subsequently, each joint minimum execution time Ti is selected as the maximum value between
Tv, min and Ta, min.
Finally, the robot execution time Te is again selected as:
Te = max( Ti )
. The trajectory of the ith
joint, for i = 1, 2, , .., 6, takes the form:
qi( t ) =

























qin,i +
9
4
(qf,i − qin,i)
t
Te
2
for 0 ≤ t t1
qin,i + (qf,i − qin.i)
3
2
t − t1
Te
+
1
4
for t1 ≤ t t2
qin,i + (qf,i − qin,i)
−
9
4
t − t2
Te
2
+
3
2
t − t2
Te
+
3
4
#
for t2 ≤ t ≤ Te
q̇i( t ) =





















9
2
(qf,i − qin,i)
Te
2 t for 0 ≤ t t1
3 (qf,i − qin.i)
2 Te
for t1 ≤ t t2
(qf,i − qin,i)
−
9
2 Te
2 ( t − t2 ) +
3
2 Te
for t2 ≤ t ≤ Te
q̈i( t ) =



















9
2
(qf,i − qin,i)
Te
2 for 0 ≤ t t1
0 for t1 ≤ t t2
−
9
2
(qf,i − qin,i)
Te
2 for t2 ≤ t ≤ Te
38

63. 3.4. NUMERICAL EXAMPLE: TRAJECTORIES COMPARISION
The algorithm described by figure 3.1 was implemented in a dedicated Matlab script. In
particular, the script takes as inputs the initial joints configuration Qin and the desired final
TCP pose ( xT CP , yT CP , zT CP , θ1, θ2, θ3 ), as well as the kind of trajectory function to be
used and the number of discretization points nf , and plots the selected trajectory. Specifically,
the selecting criterion used for choosing the best trajectory, among the compatible ones, is the
minimization of
P6
1 | ∆qi |.
For the following example, the inputs were set as:
- nf = 100
- Qin = ( −35, −85, 0, 0, 0, 0 )
- TCPfinal = ( −10, 10, 5, 0, 90, 0 )
where all the angles are provided in degrees.
The results of the simulations are here provided:
Figure 3.2: Sample trajectory
39

64. Figure 3.3: TCP trajectories comparison
The above figure shows a comparison between the TCP position of all the three types
of trajectories. The initial and final pose are clearly identifiable and are obviously equal for
all trajectories. Moreover, the task execution times are also evident from the graphs: the
Peisekah trajectory is the slowest, while the piecewise constant acceleration one (indicated as
MaxSpPow) is the fastest.
Aside from the different task execution times, all the TCP trajectories have similar shape and
behaviour.
Looking at figure 3.4, it is possible to appreciate how the imposed boundary conditions are
verified: each joint moves from the related initial configuration to the final one.
Moreover, figure 3.5 confirms the conditions of zero joint velocities at the start and at the
end of the trajectory. Also, by looking at figure 3.5 and at 3.6, it is clear how the Gutman
and Peisekah trajectories have zero acceleration at the extremes and in the middle of the time
axis; the piecewise trajectory, on the contrary, has zero acceleration only for the central time
interval. Note that in all the following figures, joint 5 is not distinguishable from joint 6. It is
not an error: for the chosen trajectory, in fact, they move with almost the same joint trajectory.
More importantly, by looking again at figure 3.6, some interesting considerations about the
jerk can be made:
- For the piecewise trajectory, the jerk is represented by two impulses centered on the two
acceleration discontinuities.
- In the Gutman trajectory, the jerk is small for the majority of the task execution (in
fact the acceleration is approximately constant on two intervals). It is an example of
jerk-limited trajectory.
- The Peisekah trajectory has null jerk at the start and at the end of the trajectory (the
snap is null as well). Also this represents a case of jerk-limited trajectory.
High jerk values imply large variations of the inertial forces, which in turn enhance vibrational
phenomena. For this reason, properties like the ones of the Gutman and Peisekah trajectories
are particularly suited for robotics applications in which the minimization of vibrations is
important.
40

65. Figure 3.4: Joints positions
41

66. Figure 3.5: Joints velocities
42

67. Figure 3.6: Joints accelerations
43

68. 4. DYNAMICS
4.1. INVERSE DYNAMICS
The inverse dynamics constists in determining the driving actuator forces needed to generate
an assigned robot motion.
By using the homogenous notation, it is possible to write the relationship between the total
action matrix of the rigid link i, Φi,T OT , and its absolute acceleration matrix, H0i, by means
of the skew operator and the inertia matrix Ii (for the expression of the matrices see [3]):
skew{H
(h)
0i I
(h)
i } = Φ
(h)
i,T OT
where all the matrices are written w.r.t. the same reference frame (h). As a remark, note that
the previous equation is equivalent to both the linear and angular momentum conservation
principle and the left-hand side represents the inertial terms.
If a change of reference frame is necessary, it is possible to use the following formulae to operate
it (see [3]):
I
(k)
i = Mkh I
(h)
i Mkh
T
Φ
(k)
i = Mkh Φi
(h)
Mkh
T
From now on, for simplicity of notation, the indication of the reference frame will be omitted
if not strictly necessary.
The matrix Φi,T OT contains all the actions exerted on (and by) the link. By choosing as posi-
tive actions the ones acting on the link (inward actions, see [3]), the above mentioned matrix
can thus be expressed as:
Φ
(h)
i,T OT = Φi,ext + Φ∗
i+1 − Φ∗
i
where Φi,ext are the external actions acting on the link (e.g weight forces), Φ∗
i+1 is the action
matrix of the link i + 1 on i (constraint and driving forces) and Φ∗
i is the action matrix of the
link i on i − 1.
In particular, the external action matrix can be expressed by separating the gravitational con-
tribution from the other ones in the following way:
Φi,ext = skew{Hg Ii} + Φ̃i
The equilibrium equation thus takes the form:
skew{H0i Ii} = skew{Hg Ii} + Φ̃i + Φ∗
i+1 − Φ∗
i
where
H(0)
g =




0 0 0 0
0 0 0 0
0 0 0 −g
0 0 0 0




44

69. The unknowns of the inverse problem are both Φ∗
i and Φ∗
i+1.
They can be determined by isolating the Φ∗
i in the previous equation:
Φ∗
i = skew{(Hg − H0i) Ii} + Φ̃i + Φ∗
i+1
Indeed, starting from the last link n, since the term Φ∗
n+1 is null (open chain manipulator),
the unknown action matrices can be determined iteratively solving:
Φ∗
n = skew{(Hg − H0n) In} + Φ̃n
And then,
Φ∗
i = skew{(Hg − H0i) Ii} + Φ̃i + Φ∗
i+1 for i= n-1 ,..., 2, 1
The result of the procedure is:
Φ∗
i =
n
X
j=i
skew{(Hg − H0j) Ij} +
n
X
j=i
Φ̃j for i= n ,..., 2, 1
The joint driving torques can be obtained by applying the virtual work principle for the joint
virtual displacement δqi:
Li = φi δqi + Φ∗
i ◦ Li−1,i δqi = 0
By isolating the φi in the previous equation:
φi = −Φ∗
i ◦ Li−1,i
where ◦ represents a pseudo-scalar product. This expression can be higly simplified by choos-
ing to write the Φ∗
i in the local frame (i − 1). In fact, in that case, it simply reduces to the
extraction of one element of Φ∗
i ; taking into account the consequences of having applied the DH
convention and the antisymmetric shape of the matrices, the driving torques can be computed
as:
φi = −Φ
∗ (i−1)
i (2, 1)
If necessary, it is possible to compute the kinetic energy of the single link i as:
Ec,i =
1
2
trace{ W
(h)
0i I
(h)
i W
(h) T
0i }
Note that φi and Ec,i are invariant w.r.t. any change of reference frame.
Another useful quantity is the so called “equivalent joint inertia”. It is the cumulative axial
inertia opposing to each joint actuator and depends on the current configuration of joints from
i to n, thus providing information on the instantaneous geometrical arrangement of the manip-
ulator. In particular, for revolute joints, it has the dimensions of a moment of inertia (Kg m2
)
and is computed as:
J eq
i =
n
X
j=i
I
(i−1)
xx,j + I
(i−1)
yy,j
= ˆ
I
(i−1)
xx,i + ˆ
I
(i−1)
yy,i
Where I
(i−1)
xx,j and I
(i−1)
yy,j are, if the DH convention is applied, the elements I
(i−1)
j (1, 1) and
I
(i−1)
j (2, 2), while ˆ
Ii :=
Pn
j=i Ij.
Note that, for the computation of the previous expression, it is necessary to apply to the
inertia matrices a series of reference changes.
If the inertia tensors J are provided with respect to the principal axes of inertia (i.e. J is
diagonal), it is necessary to write their equivalent with respect to the local frames i.
45

70. Supposing the principal axes are parallel to the local frame ones (it is an approximation,
often legitimate), if the coordinates of the center of mass of the joint with respect to the local
frame and the joint mass are known, the inertia tensor of the link i with respect to the local
frame i takes the form:
J
(i)
i =







J
(p)
xx,i + mi yG,i
2
+ zG,i
2
−mi xG,i yG,i −mi xG,i zG,i
−mi xG,i yG,i J
(p)
yy,i + mi xG,i
2
+ zG,i
2
−mi yG,i zG,i
−mi xG,i zG,i −mi yG,i zG,i J
(p)
zz,i + mi xG,i
2
+ yG,i
2







where the superscript (p) refers to the frame with origin on the center of mass of the link
i, with coordinate axis parallel w.r.t. the local frame i.
Once the J
(i)
i is known, it is possible to obtain the inertia matrices I
(i)
i in the following way:
I
(i)
i =














−Jxx,i + Jyy,i + Jzz,i
2
−Jxy,i −Jxz,i mi xG,i
−Jxy,i
Jxx,i − Jyy,i + Jzz,i
2
−Jyz,i mi yG,i
−Jxz,i −Jyz,i
Jxx,i + Jyy,i − Jzz,i
2
mi yG,i
mi xG,i mi yG,i mi zG,i mi














(i)
46

71. The following figure shows a flow chart describing a possible algorithm for the solution of
the inverse dynamic problem (note that it has to be executed for each trajectory point) :
INVERSE DYNAMICS
PROBLEM
INPUTS
Q, Q̇, Q̈
Φ̃i M0i H
(0)
g
L
(i−1)
i
Φ
∗ (0)
i = skew{
H
(0)
g − H
(0)
0i
I
(0)
i } + Φ̃
(0)
i + Φ
∗ (0)
i+1
ˆ
I
(0)
i = ˆ
I
(0)
i+1 + I
(0)
i
J eq
i = ˆ
I
(i−1)
xx,i + ˆ
I
(i−1)
yy,i
Ec,i = 1
2 trace{ W
(0)
0i I
(0)
i W
(0) T
0i }
Φ
∗ (i−1)
i = Mi−1,0 Φ
∗ (0)
i MT
i−1,0
Φ
∗ (0)
i+1 , ˆ
I
(0)
i+1
Φ
∗ (0)
6+1 = 0
ˆ
I
(0)
6+1 = 0
I
(i)
i
VIRTUAL WORK
PRINCIPLE
STOP
i
?
= 1
φi = −Φ
∗ (i−1)
i ◦ L
(i−1)
i =
= −Φ∗
i (2, 1)
LINK
KINETIC ENERGY
JOINT EQUIVALENT
INERTIA
i = 6
yes
no
i = i − 1
Figure 4.1: Flow chart for solving the inverse dynamics problem
47

72. 4.2. FORWARD DYNAMICS
The forward dynamics constists in determining the motion of the robot (i.e. the joint trajec-
tories), once the driving actuator forces are assigned.
The previously obtained expression for the joint action matrices is reported here:
Φ∗
i =
n
X
j=i
skew{Hg Ij} −
n
X
j=i
skew {H0j Ij} +
n
X
j=i
Φ̃j for i= n ,..., 2, 1
where the inertial and the gravitational terms were separated (and the indication of the refer-
ence frame was omitted).
Recalling the expression of H0j:
H0j =
j
X
h=1
Lh q̈h +
j
X
h=1
L2
h ˙
qh
2
+ 2
j
X
r=2
r−1
X
s=1
Ls Lr ˙
qs ˙
qr
and isolating the term depending only on the known joint velocities and positions:
H∗
j :=
j
X
h=1
L2
h ˙
qh
2
+ 2
j
X
r=2
r−1
X
s=1
Ls Lr ˙
qs ˙
qr
the term skew{H0j Ij} thus takes the form:
skew{H0j Ij} = skew
( j
X
h=1
Lh q̈h + H∗
j
#
Ij
)
=
j
X
h=1
skew{Lh Ij} q̈h + skew{H∗
j Ij}
Consequently, the explicit expression of the joint action matrices becomes:
Φ∗
i = −
n
X
j=i
j
X
h=1
skew{Lh q̈h Ij} −
n
X
j=i
skew{H∗
j Ij} +
n
X
j=i
skew{Hg Ij} +
n
X
j=i
Φ̃j
One way to obtain the joint equations of motion of the robot is applying the virtual work
principle to each joint:
Li = φi δqi + Φ∗
i ◦ Li δqi = 0
This leads to:
φi + Φ∗
i ◦ Li = 0
The previous expression represents a system of differential equations in the joint variables qi.
Such a system can be rewritten, by applying the properties of the skew operator (see [3]), as:
M Q̈ = F
which can be solved by a numerical integration.
The elements of the inertial matrix M (which is symmetrical) are:
M = [ mij ] = trace{Li
ˆ
Ih LT
j } with h = max (i, j)
While the elements of the forcing vector F are:
F = [ fi ] = φi +


n
X
j=i
Φ̃j

 ◦ Li + trace{Li
ˆ
Ii H T
g } −
n
X
j=i
trace{Li Ij H∗ T
j }
with
ˆ
Ii :=
n
X
j=i
Ij
48

73. In the case of the PUMA 560 (n = 6), the inertial matrix has the form:
M =











trace{ L1
ˆ
I1 LT
1 } trace{ L1
ˆ
I2 LT
2 } trace{ L1
ˆ
I3 LT
3 } trace{ L1
ˆ
I4 LT
4 } trace{ L1
ˆ
I5 LT
5 } trace{ L1 I6 LT
6 }
// trace{ L2
ˆ
I2 LT
2 } trace{ L2
ˆ
I3 LT
3 } trace{ L2
ˆ
I4 LT
4 } trace{ L2
ˆ
I5 LT
5 } trace{ L2 I6 LT
6 }
// // trace{ L3
ˆ
I3 LT
3 } trace{ L3
ˆ
I4 LT
4 } trace{ L3
ˆ
I5 LT
5 } trace{ L3 I6 LT
6 }
// // // trace{ L4
ˆ
I4 LT
4 } trace{ L4
ˆ
I5 LT
5 } trace{ L4 I6 LT
6 }
// // // // trace{ L5
ˆ
I5 LT
5 } trace{ L5 I6 LT
6 }
// // // // // trace{ L6 I6 LT
6 }











It has proven to be particularly useful to encode the forcing term containing the H∗
j in an
upper triangular matrix with the following form:
F
M
aux := −













trace{ L1 I1 H∗ T
1 } trace{ L1 I2 H∗ T
2 } trace{ L1 I3 H∗ T
3 } trace{ L1 I4 H∗ T
4 } trace{ L1 I5 H∗ T
5 } trace{ L1 I6 H∗ T
6 }
0 trace{ L2 I2 H∗ T
2 } trace{ L2 I3 H∗ T
3 } trace{ L2 I4 H∗ T
4 } trace{ L2 I5 H∗ T
5 } trace{ L2 I6 H∗ T
6 }
0 0 trace{ L3 I3 H∗ T
3 } trace{ L3 I4 H∗ T
4 } trace{ L3 I5 H∗ T
5 } trace{ L3 I6 H∗ T
6 }
0 0 0 trace{ L4 I4 H∗ T
4 } trace{ L4 I5 H∗ T
5 } trace{ L4 I6 H∗ T
6 }
0 0 0 0 trace{ L5 I5 H∗ T
5 } trace{ L5 I6 H∗ T
6 }
0 0 0 0 0 trace{ L6 I6 H∗ T
6 }













With this notation, if the contribution of the term Φ̃j is neglected, the vector F can be
expressed as:
F (Q, Q̇, HT
g , Ii, L
(i−1)
i , t, φi) = Fact + Fg + Faux
where
Faux =



























P6
k=1 FM
aux (1, k)
P6
k=1 FM
aux (2, k)
P6
k=1 FM
aux (3, k)
P6
k=1 FM
aux (4, k)
P6
k=1 FM
aux (5, k)
P6
k=1 FM
aux (6, k)



























Fg =

















trace{ L1
ˆ
I1 HT
g }
trace{ L2
ˆ
I2 HT
g }
trace{ L3
ˆ
I3 HT
g }
trace{ L4
ˆ
I4 HT
g }
trace{ L5
ˆ
I5 HT
g }
trace{ L6
ˆ
I6 HT
g }

















Fact =















φ1
φ2
φ3
φ4
φ5
φ6















As a final remark, it is important to underline that the F and M do not depend on the ref-
erence frame: in fact, the trace operator is invariant w.r.t any change of coordinates. It is
therefore possible to choose the most convenient representation. In particular, expressing all
the matrices with respect to the frame (0) proved to be the best alternative.
49

74. The following figure shows a flow chart describing a possible algorithm for the solution of
the direct dynamic problem:
FORWARD DYNAMICS
PROBLEM
INPUTS
(at time t)
Φi, Φ̃i, φi M0i H
(0)
g
Q̇
L
(i−1)
i
Φ∗
i = skew{(Hg − H0i) Ii} + Φ̃i + Φ∗
i+1
Φ∗
i = −
Pn
j=i
Pj
h=1 skew{Lh q̈h Ij} −
Pn
j=i skew{H∗
j Ij} +
Pn
j=i skew{Hg Ij} +
Pn
j=i Φ̃j
VIRTUAL WORK
PRINCIPLE
ˆ
I
(0)
i = ˆ
I
(0)
i+1 + I
(0)
i
COMPUTATION
OF M AND F
ˆ
I
(0)
i+1
ˆ
I
(0)
6+1 = 0
I
(i)
i
Q̈t = M−1
F
STOP
i
?
= 1
NUMERICAL INTEGRATION
Q̇t+1, Qt+1
i = 6
yes
no
i = i − 1
Figure 4.2: Flow chart for solving the forward dynamics problem
50

75. The following flow chart describes the operations which are represented by the block di-
agram “COMPUTATION OF M + F” inserted in figure 4.2. It describes accurately the
algorithm delineated in the analytical discussion of this section.
COMPUTATION
OF M AND F
(Fg + Fact)i = trace{ L
(0)
i
ˆ
I
(0)
i H
(0)
g } + φi
M (j, i) = trace{ L
(0)
i
ˆ
I
(0)
i L
(0) T
i } = M (i, j)
FM
aux (j, i) = trace{ L
(0)
i I
(0)
i (H
∗(0)
i )T
}
j
?
= 1
STOP
i
?
= 1
M (j, i) = trace{ L
(0)
i
ˆ
I
(0)
i L
(0) T
i } = M (i, j)
FM
aux (j, i) = trace{ L
(0)
i I
(0)
i (H
∗(0)
i )T
}
Fg + Fact
M FM
aux
Faux
Faux,i =
P6
k=1 FM
aux (i, k)
+
F
OUTPUT
i = 6
j = i
yes
yes
no
i = 1 − 1
no
j = j − 1
Figure 4.3: Flow chart describing the block diagram ”COMPUTATION OF M + F”
51

76. 4.3. NUMERICAL EXAMPLES AND COMPARISONS
This section is dedicated to the analysis of a numerical example of application of the algorithms
previously described for the inverse and forward dynamics.
The numerical examples are generated using some Matlab scripts in which all the previously
described algorithms are implemented. In particular, a sample trajectory is given as input.
The first step is the application of the inverse dynamics algorithm, which computes the joint
actions, the kinetic energy of the links and the equivalent inertia opposing to the actuators.
As a second step, the obtained actions are given as input to the forward algorithms, which
computes the joint trajectories.
In the appendix, section 5.3, it is briefly reported a simulink example which solves the
inverse dynamics problem for the PUMA 560.
4.3.1. INVERSE DYNAMICS
The inverse dynamics algorithm uses the output trajectories of the trajectory generation algo-
rithm, with inputs:
- Qin = ( −35, −85, 0, 0, 0, 0 )
- TCPfinal = ( −0.15, 0.4, 0.1, 0, 90, 0 )
where TCPfinal = ( xT CP , yT CP , zT CP , θ1, θ2, θ3 ) and all angles are provided in degrees.
In particular, three different types of trajectories will be compared: the piecewise constant
acceleration trajectory for maximum specific power minimization (indicated as MaxSpPow),
the Gutman trajectory and the Peisekah one.
Figure 4.4: Dynamics sample trajectory
52

77. Figure 4.5 shows the joint actions of the different types of trajectories. It is possible to note
that, moving from the MaxSpPow toward the Peisekah trajectory, the actions have on average
decreasing maximum values. Consequently, the actuators load is less severe for the Peisekah
trajectory. For what concerns joint 1 actuator, it starts with a non-null torque only for the
MaxSpPow trajectory: in fact, this is the only trajectory which has non-null initial joint 1
acceleration. The other joint actuators, even for the other trajectories, posses a non-null ini-
tial torque simply because they have to counteract the gravitational acceleration (for instance,
joint 3 actuator has a negative initial torque because gravity would make it rotate towards the
positive orientation of the joint). This is not the case of the first joint, which has a rotation
axis parallel w.r.t. the gravitational acceleration and is therefore not affected by gravity.
Figure 4.6 shows the link kinetic energies for the different types of trajectories. In this case, it
is possible to note, on average, an higher kinetic energy for the MaxSpPow (this is consistent
with the observation that it is the fastest trajectory). The other two trajectories do not exhibit
particularly relevant differences.
Moreover, looking at the plots, the peak values are reached approximately at the middle of
the execution time. In fact, even if the joint velocity profiles exhibit a peak in the middle of
the joint trajectory, the kinetic energies do not assume their maximum value exactly at the
middle execution time because they also depend on the absolute velocity of the link. The
pseudo-plateau in the MaxSpPow graph is due to the particular shape of the joint trajectory,
which mantains a cruise maximum joint velocity for a finite amount of time.
Figure 4.7 shows the equivalent joint inertia opposing to the actuators, for the different types
of trajectories.
Aside from the different execution times (which produces a stretch of the curves), the equiv-
alent inertia have substantially the same behaviour between the different trajectories. This is
not unexpected, considering that the joint positions have all very similar behaviour (between
different trajectories) and that the equivalent inertia depends on the manipulator instanta-
neous geometrical configuration.
The first joint has a maximum approximately located in the middle of the trajectory; this is
reasonable since, at that time, links 2 and 3 are almost orthogonal with respect to the rotation
axis of link 1. Similarly, the equivalent inertia of link 2 reaches its maximum at the end of
the trajectory because link 3 becomes almost collinear w.r.t link 2. The small decrease in link
3 equivalent inertia can be explained by considering that link 5 starts by an almost collinear
orientation w.r.t. link 3 and ends the trajectory in an approximately orthogonal orientation
w.r.t. the same link. The same reasoning can be applied to the remaining joints.
53

78. Figure 4.5: Joints actions: piecewise constant acceleration (above), Gutman trajectory (center), Peisekah
trajectory (below)
54

79. Figure 4.6: Link kinetic energies: piecewise constant acceleration (above), Gutman trajectory (center), Peisekah
trajectory (below)
55

80. Figure 4.7: Equivalent joint inertia: piecewise constant acceleration (above), Gutman trajectory (center),
Peisekah trajectory (below)
56

81. 4.3.2. FORWARD DYNAMICS
The output joint actions are then used as input to the forward dynamics algorithm. The results
are compared with the original trajectory given as input to the inverse dynamics algorithm.
Figure 4.8: Piecewise constant acceleration: original vs. numeric
57

82. Figure 4.9: Gutman trajectory: original vs. numeric
58

83. Figure 4.10: Peisekah trajectory: original vs numeric
59

84. From the previous figures it is evident how the accuracy of the numerical method (Matlab’s
ODE45) is strongly influenced by the smoothness of the trajectory. In particular, if the integra-
tion step is the same, the piecewise constant trajectory is the one for which the errors are the
largest. The Gutman, which is smoother, is better approximated, while the Peisekah is by far
the most accurately represented. Two important parameters, which have to be set carefully in
order to obtain good results in the simulation, are the action number of discretization intervals
and the time step of the numerical integration. For the generation of the previous figures they
were set, respectively, to 221 and 0.001s.
By moving to a different numerical integrator (ODE 113, which allows smaller tolerances), by
increasing the actions discretization to 50000 and by diminishing the integration to 0.0001, the
obtained results are following (only the acceleration of MaxSpPow is reported, being the one
undergoing the highest distortions):
Figure 4.11: MaxSpPow with increased precision
The trajectory is clearly much less distorted than before.
60

85. 5. APPENDIX
5.1. MATLAB LIVE SCRIPT
The following picture shows the interface of a Matlab Live Script. Through six sliders it is
possible to interactively modify the value of each joint variable and to observe almost instan-
taneously the updated configuration of the robotic arm.
61

86. 5.2. EXPLICIT EXPRESSION OF M06
The elements of M06, grouped by columns, are:
M06 (:, 1) :








s(q6) [s(q1)c(q4) − c(q1)s(q4)c(q2 + q3)] + c(q6){c(q5) [s(q1)s(q4) + c(q1)c(q4)c(q2 + q3)] + c(q1)s(q5)s(q2 + q3)}
−s(q6) [c(q1)c(q4) + s(q1)s(q4)c(q2 + q3)] − c(q6){c(q5) [c(q1)s(q4) − s(q1)c(q4)c(q2 + q3)] − s(q1)s(q5)s(q2 + q3)}
c(q6) [s(q5)c(q2 + q3) − c(q4)c(q5)s(q2 + q3)] + s(q6)s(q4)s(q2 + q3)
0








M06 (:, 2) =








c(q6) [s(q1)c(q4) − c(q1)s(q4)c(q2 + q3)] − s(q6){c(q5) [s(q1)s(q4) + c(q1)c(q4)c(q2 + q3)] + c(q1)s(q5)s(q2 + q3)}
−c(q6) [c(q1)c(q4) + s(q1)s(q4)c(q2 + q3)] + s(q6){c(q5) [c(q1)s(q4) − s(q1)c(q4)c(q2 + q3)] − s(q1)s(q5)s(q2 + q3)}
−s(q6) [s(q5)c(q2 + q3) − c(q4)c(q5)s(q2 + q3)] + c(q6)s(q4)s(q2 + q3)
0








M06 (:, 3) =








s(q5) [s(q1)s(q4) + c(q1)c(q4)c(q2 + q3)] − c(q1)c(q5)s(q2 + q3)}
−s(q5) [c(q1)s(q4) − s(q1)c(q4)c(q2 + q3)] − s(q1)c(q5)s(q2 + q3)}
−c(q5)c(q2 + q3) − c(q4)s(q5)s(q2 + q3)
0








M06 (:, 4) =








l2c(q1)c(q2) − l3s(q1) − l4c(q1)s(q2 + q3) + b3c(q1)c(q2 + q3)
l2s(q1)c(q2) + l3c(q1) − l4s(q1)s(q2 + q3) + b3s(q1)c(q2 + q3)
h − l4c(q2 + q3) − l2s(q2) − b3s(q2 + q3)
1








62

87. 5.3. INVERSE DYNAMICS SIMULINK MODEL
The following figures show the Simulink block diagrams implementing the inverse dynamics.
In particular, the second reported block represents the physical model of the manipulator.
Each joint takes as input the sampled trajectory generated via a specific Matlab script.
Figure 5.1: Simulink global block diagram (above) and expanded PUMA560 block
Simulink, once the main physical parameters (e.g. density) and the geometry of the arm are
provided (e.g. STL files), automatically computes the joint actions necessary to generate the
input trajectory. The trajectory and the actions are then displayed by the inserted scope
blocks.
63

88. The following figures show a sample trajectory and the resulting actions, as well as three
frames from the Simulink graphical model of the PUMA 560.
Figure 5.2: Trajectory (above) and joint actions (below)
64

89. Figure 5.3: Trajectory: initial position, intermediate position and final position
By substituting the input trajectory with the joint actions, the Simulink model is also able
to solve the forward dynamics problem.
65

90. Bibliography
[1] L. Biagiotti and C. Melchiorri. Trajectory Planning for Automatic Machines and Robots.
Springer, 2008.
[2] Ivan Giorgio and Dionisio Del Vescovo. Non-linear lumped-parameter modeling of planar
multi-link manipulators with highly flexible arms. Robotics, 7:60, 09 2018.
[3] D. Del Vescovo. Mechanics of Robot Manipulators course notes.
66