POLYTECHNIC OF MILAN

Robot Mechanics University Project

Industrial Robot Analysis for

Cutting Cycle
Giorgio Fontana

11th July, 2013

Contents
Introduction

1 Direct Kinematics
1.1 Position . . . . . . . . . . . . . . . .
1.2 Velocity and Acceleration . . . . . .
1.2.1 Definition of the Jacobian . .
1.2.2 Resolution with Matrices . . .
1.3 Example of direct kinematics solution

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

6
6
9
9
9
12

2 Inverse Kinematics
14
2.1 Analytic Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.2 Iterative Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.3 Example of Inverse kinematics solution . . . . . . . . . . . . . . . . 17
3 Work Space

19

4 Singularity

21

5 Inverse Dynamics

22

6 Work Cycle
24
6.1 Motion between (P1 : P2 ) and (P3 : P1 ) . . . . . . . . . . . . . . . . 24
6.2 Motion between (P2 : Pi ) and (Pi : P3 ) . . . . . . . . . . . . . . . . 26
A Laws of Motion

32

iii

List of Figures
1
2
3
4
5

Example of serial and parallel robots . . . . . .

Robot schematic representation . . . . . . . . .
References adopted for the robot . . . . . . . .
Graphic simulation of the SimMechanics model
Work cycle to cut the extruded soap . . . . . .

.
.
.
.
.

2
2
3
3
5

1.1
1.2
1.3

Comparison between methods . . . . . . . . . . . . . . . . . . . . .

Movements in the joints space . . . . . . . . . . . . . . . . . . . . .
Movements in the work space . . . . . . . . . . . . . . . . . . . . .

12
13
13

2.1
2.2
2.3

Comparison between methods . . . . . . . . . . . . . . . . . . . . .

Movements in the work space . . . . . . . . . . . . . . . . . . . . .
Movements in the joints space . . . . . . . . . . . . . . . . . . . . .

17
18
18

3.1
3.2

Work area in the x-y plane . . . . . . . . . . . . . . . . . . . . . . .

Work space with fixed . . . . . . . . . . . . . . . . . . . . . . . .

20
20

4.1

Singularity points in the work space . . . . . . . . . . . . . . . . . .

21

6.1
6.2
6.3
6.4
6.5
6.6
6.7
6.8
6.9
6.10
6.11
6.12
6.13
6.14

(P1 , P2 ) and (P3 , P1 ) - Velocities in the joints space . . . .

(P1 , P2 ) and (P3 , P1 ) - Accelerations in the joints space . .
Work cycle - Position in the joints space . . . . . . . . . .
Work cycle - Velocities in the joints space . . . . . . . . . .
Work cycle - Accelerations in the joints space . . . . . . .
Work cycle - Position in the work space . . . . . . . . . . .
Work cycle - Robot dynamics . . . . . . . . . . . . . . . .
Work space - Robot dynamics with prismatic joint force . .
Work cycle - Point P1 . . . . . . . . . . . . . . . . . . . .
Work cycle - Point P2 . . . . . . . . . . . . . . . . . . . .
Work cycle - Point Pi . . . . . . . . . . . . . . . . . . . . .
Work cycle - Point P3 . . . . . . . . . . . . . . . . . . . .
Work cycle - Point P1 . . . . . . . . . . . . . . . . . . . .
Work cycle - scheme of the Simulink/SimMechanics model

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

25
25
27
27
28
28
29
29
30
30
30
31
31
31

A.1 Function three-steps motion law . . . . . . . . . . . . . . . . . . . .

A.2 Function cycloidal motion law . . . . . . . . . . . . . . . . . . . . .

32
33

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

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

.
.
.
.
.

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

.
.
.
.
.

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

.
.
.
.
.

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

List of Tables
1
2
3

Characteristics of the robot links . . . . . . . . . . . . . . . . . . .

Points coordinates for the work cycle . . . . . . . . . . . . . . . . .
Limits of the drivers . . . . . . . . . . . . . . . . . . . . . . . . . .

3
5
5

1.1
1.2

Initial and final joints space coordinates . . . . . . . . . . . . . . .

Initial and final work space coordinates . . . . . . . . . . . . . . . .

12
12

2.1
2.2

Initial and final work space coordinates . . . . . . . . . . . . . . . .

Initial and final joints space coordinates . . . . . . . . . . . . . . .

17
17

3.1

Joints limit switches . . . . . . . . . . . . . . . . . . . . . . . . . .

19

vii

Introduction
Serial and Parallel Robots1
In the automation technology field some basic characteristics separate robots from
generic automatic machines. Among all versatility, autonomy or ability to take
decisions, and reprogramming should be always part of a robot machine. Generally
in the industry sector the reference are serial robots. They are mechanical systems
distinguishably because similar to the human arm. The serial robots are made
of a certain number of parts and connected with joints, in order to realize an
open kinematics chain. The first part is fixed on the ground, while the last as a
human hand is able to manipulate objects and is called tool centre point (TCP)
or end-effector. The complete control system is a computer for elaboration and
the electronics to communicate with motors and transducers. Different is the case
of parallel robots, which are a less spread category but recently in strong develop.
They are considered complementary of the serial robots. In this case the kinematic
chain is close. They should be preferred for load operations because of the rigid
body. On the other hand, their usually have lower mobility. This means that the
space that can be reach from the end-effector is smaller, compromising the number
of task they can perform. If the motors are left on the ground the structure results
lighter, with directly increase speed and acceleration. Even from a mathematical
point of view serial and parallel robots show complementary characteristics.
Purpose of the Project
In this semester project a serial robot allocated in a production line has been extensively analyzed. The study covers the manipulator direct and inverse kinematics,
the inverse dynamics and all the main features. The equations obtained are implemented in Matlab and a model of the robot realized in Simulink/SimMechanics. In
this way simulations can be performed and the behaviour predicted. As shown in
Figure 2, the manipulator has the structure of an anthropomorphous robot, where
the entire body is considered as a chain of links connected with ideal constraints.
The sketch in Figure 3a refers to the robot 3 degree of freedom. The rotational
and the prismatic joints are responsible for the in-plane movements and , while
the rotational joint titled of 30 creates the out-of-plane rotation . Along the
kinematic chain, for all the joints reference systems are defined. In particular, the
last rotational joint requires a two-step definition. Moving from coordinates system
3 to system 4, a rotation of 30 around z3 -axis is actually made. In this way the
1

This section has been extracted from Robotica Industriale. Giovanni Legnani. Casa Editrice
Ambrosiana, (2003)

Introduction

(a) Serial robot SCARA

(b) Parallel Robot DELTA

Figure 1: Example of serial and parallel robots

x4 -axis and the joint rotational axis are coincident, reducing the complexity of the
equations. The tern in 0 will be also called as absolute reference system. Table 1
provides all the information about link properties. In particular, in Figure 3b the
link l0 is exactly 45 when l2 is at the initial position of 0 . The robot is supposed
made in aluminium 6010. As first step of the study, effects of the electrical motors
are neglected. In addition, to simplify the problem the link l0 is also neglected,
and the relative coordinate translated in a rotational dof of the first joint.

Figure 2: Robot schematic representation

Introduction

x5
y5

z5

x4
y3

y1
x1

l4

x3
z3 = z4
y2
y4

x2
z2

z1

l3

l2

3-4

l0

l1

y0
x0

z0
(a) Joints system of coordinates

(b) Definition of links length

Figure 3: References adopted for the robot

Figure 4: Graphic simulation of the SimMechanics model

Link

Length
[m]

Ext. Diameter
[m]

Int. Diameter
[m]

Weight
[kg]

1
2
3
4
0

1,00
1,00
0,25
0,25
0,71

0,250
0,125
0,125
-

0,230
0,105
0,105
-

20,36
4,88
4,88
-

Table 1: Characteristics of the robot links

Introduction

Here next are listed matrices of the inertial moments [kgm2 ] respect to the links
centre of mass gi . To obtain the matrices the following equations has been applied
1
2
2
+ r2i
)
Jx2(g2) = m2 (r2e
2

(1)

1
2
2
m2 [3(r2e
+ r2i
) + h22 ] + m2 d22
(2)
12
where di is the distance between the centre of gravity of the i-th link and the i-th
reference system, providing the momentum of transport.

0.293660
0
0

0
1.843290
0
J2(g2) =
(3)
0
0
1.843290

0.016248
0
0

0
0.109735
0
J3(g3) =
(4)
0
0
0.109735

0.016248
0
0

0
0.109735
0
J4(g4) =
(5)
0
0
0.109735
Jy2 (g2) =

The matrices of the inertial moments [kgm2 ] respect to the reference systems are
basically the same except that there is not momentum of transport.

0.293660
0
0

0
6.932670
0
J2(1) =
(6)
0
0
6.932670

0.016248
0
0

0
0.414567
0
J3(2) =
(7)
0
0
0.414567

0.016248
0
0

0
0.414567
0
J4(4) =
(8)
0
0
0.414567
Case of Study
After be able to correctly describe the manipulator, the next step is to assign the
instructions for the task the robot has been chosen. The robot is integrated in the
production line of soap. In particular the task assigned is a cut operation for the
extruded product. The robot will move with an orthogonal cut, considering the
direction of progress of the line. In order to follow this request, the relative speed
between the cutting utensil and the material must be zero. The figure shows the
geometry trajectory required for the TCP where the motion laws are the following
Motion law with minimum time from waiting position P1 to position P2
Vertical cut operation reaching point P3 with only straight lines

Introduction

Return to P1 with minimum time

It is possible to identify an intermediate point Pi at z = 0 where the TCP changes
the direction of motion, and the couple (P2 , P3 ) is specular respect to this point.
The speed of progress for the extruded soap ve is 0, 1 ms . The work cycle will be
performed respecting limits of engine speed, acceleration and torque. As in Table
3, maximum velocities and accelerations raise from the first to the last electric
drive because masses and inertias are lower toward the kinematic chain end.

P1

P2

P3

ve

Pi

Figure 5: Work cycle to cut the extruded soap

Position x

[m]
P1
P2
Pi
P3

0,5
1,0
1,0
1,0

y
[m]

z
[m]

0,50
0,6
0,0
0,6

0,0
0,1
0,0
-0,1

Table 2: Points coordinates for the work cycle

dof vmax

[ ms ]

0,5
4,0
5,0

+amax
[ sm2 ]

amax
[ sm2 ]

2,0
20
30

2,0
20
30

Table 3: Limits of the drivers

Chapter 1
Direct Kinematics
The direct kinematics problem means to completely describe the TCP kinematics (position, velocity and acceleration) through only information from the joints.
In other words, to translate the assignments for the drivers into the consequent
movement for the hand. Because this is a serial robot, it is possible to obtain the
analytic expression. The linear movement of the prismatic joint is converted in
the angular dof of the free rotational joint.

  l 2
0
1
(1.1)
= a sin 2
l1
Therefore, the coordinates in the joints space are summarized in the vector
Q = [, , ]

(1.2)

while the vector for the coordinates in the work space is

S = [x, y, z]

(1.3)

In the convention adopted and are anticlockwise rotations around the z1 and z2
axes, while is the anticlockwise rotation around the x4 axis. Next in this chapter
the position is found using position matrices, because it reduces the complexity to
describe the movement out-of-plane. Velocity and acceleration instead are obtained
with both Jacobian method and matrices resolution.

1.1

Position

The position of points in the space can be completely described using 3 coordinates.
Nevertheless, to easily solve the geometric problem the homogeneous coordinates
are introduced.
P = [x, y, z, 1]T
(1.4)
With this formulation it is possible to convert point coordinates from a tern 0 to
a tern 1 thanks to the M01 4x4 matrix built in blocks

R01
T(0)
P1(1)
P0(0) = M01 P1(1) =

0 0 0 1
6

(1.5)

1.1. Position

where R01 is the rotation matrix and T(0) is the translation matrix. They represent the angular and relative positions of the two terns evaluated. Extending this
process among all the coordinate systems from the base to the hand, the transformation matrices are defined.

1 0 0 0
0 1 0 l1

M01 =
(1.6)
0 0 1 0
0 0 0 1

cos sin 0 l2 cos

sin cos 0 l2 sin

M12 =
0
0
1
0
0
0
0
1

cos sin 0 l3 cos

sin cos 0 l3 sin

M23 =
0
0
1
0
0
0
0
1

cos sin 0 0
sin cos 0 0

M34 =
0
0
1 0
0
0
0 1

1
0
0
l4 cos
0 cos sin l4 sin cos

M45 =
0 sin cos l4 sin sin
0
0
0
1

(1.7)

(1.8)

(1.9)

(1.10)

It is possible to have multiple change in the following way

Mik = Mij Mjk

(1.11)

Therefore, through the serial kinematic chain it is possible to obtain the transformation matrix from the end to the base.
M05 =

5
Y

Mi1,i

(1.12)

i=1

The T translation vector in M05 is the projection of the reference system 5 (centre
of the TCP) along the directions of the absolute reference system 0. In the next
step this matrix will be analytically derived.

M02 = M01 M12

cos sin
sin cos
=
0
0
0
0

0
l2 cos
0 l1 + l2 sin

1
0
0
1

(1.13)

Chapter 1. Direct Kinematics

M03 = M02 M23

C C S S C S S C
S C + C S S S + C C
=

0
0
0
0

0
l2 C + l3 (C C S S )
0 l1 + l2 S + l3 (S C + C S )

1
0
0
1
(1.14)

Taking use of the Werner formulas

M03

M04 = M03 M34

C+ S+
S+ C+
=
0
0
0
0

0
l2 C + l3 C+
0 l1 + l2 S + l3 S+

1
0
0
1

C+ C S+ S C+ S S+ C
S+ C + C+ S S+ S + C+ C
=

0
0
0
0

(1.15)

0
l2 C + l3 C+
0 l1 + l2 S + l3 S+

1
0
0
1
(1.16)

Again using the Werner formulas

M04

M05

M05

C++ S++
S++ C++
=
0
0
0
0

0
l2 C + l3 C+
0 l1 + l2 S + l3 S+

1
0
0
1

(1.17)

C++ S++ C S++ S

l2 C + l3 C+ + l4 C C++ + l4 S C S++
S++ C++ C C++ S l1 + l2 S + l3 S+ + l4 C S++ l4 S C C++

= M04 M45 =

0
S
C
l4 S S
0
0
0
1
(1.18)
Rearranging the equations, the matrix became the following






C++ S++ C S++ S

l2 C + l3 C+ + l24 C++2 1 C
+ C+ 1 + C

S++ C++ C C++ S l1 + l2 S + l3 S+ + l4 S++2 1 C + S+ S C

2
=

0
S
C
l4 S S
0
0
0
1
(1.19)
The last column is actually the system of equations which describe the work space
as function of joints space.
S = F (Q)
(1.20)






l4

1 C + C+ 1 + C
x = l2 C + l3 C+ + 2 C++2




y = l1 + l2 S + l3 S+ + l24 S++2 1 C + S+ S C

z = l4 S S

(1.21)

1.2. Velocity and Acceleration

1.2

Velocity and Acceleration

The first method here presented requires to write a Jacobian and its derivative.
Especially for accelerations it can be a long process and far from be without any
mistake. Instead a matrix-based script is well suited to be implemented in Matlab,
where only the initial matrices are defined and left to the software the task to
calculate all the rest.

1.2.1

Definition of the Jacobian

One of the possible way to proceed is to calculate end-effector speed and acceleration deriving the expression of S.
S = J(Q)Q

(1.22)

S = J(Q)
Q + J(Q)Q

(1.23)

Therefore, it is necessary to define Jacobian matrix and its derivative.

J(Q) =

dx
d
dy
d
dz
d

dx
d
dy
d
dz
d

dx
d
dy
d
dz
d

dJij
dJij dJij
Jij =
+
+

d
d
d

(1.24)

(1.25)

The analytic equations are not reported because of the huge space required. Anyway, they have been written as Matlab functions.

1.2.2

Resolution with Matrices

Always working with homogeneous coordinates W is a 4x4 matrix built in blocks

V0
P(i)
P(i) = W(i) P(i) =

0 0 0 0

(1.26)

where is the angular speeds matrix and V0 the speed vector of a point moving
in-built with P , and passing through the reference system origin. In vector P(i) the
first three positions are the velocity components respect the i-th reference system.
The last element is zero. Because in this case there is only one dof between two
joints, the W matrices are simple.

0 0 0 0
0 0 0
0 0 0 0
0 0 0

W01(0) =
W
=
(1.27)
12(1)
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0

10

Chapter 1. Direct Kinematics

0
0
=
0 0
0 0

W23(2)

0
0
0
0

W45(4)

0
0

0
0

0
0
=
0
0

0
0

0
0
W34(3) =
0
0

0 0
0

0 0
0 0

0
0
0
0

0
0
0
0

0
0

0
0

(1.28)

(1.29)

As for the position, even in this case the matrices will be reported to the absolute
reference system.
W(i) = Mij W(j) Mji
(1.30)
where the matrix M has the following structure

T
Rij

Mji = Mij1 =

T
Rij
Tij

0
1

(1.31)

To obtain the matrix of speeds for the TCP in the reference system 0, the last step
is to sum all the matrices calculated in the absolute reference. The same procedure
is suggested from the theorem of velocities composition or the Theorem of Rivals
Wik(r) = Wij (r) + Wjk (r)

(1.32)

For the case under study the formulation is the next

X
5
W05(0) =
i=1 Wi1,i (0)

(1.33)

Therefore, if P(0) is the position vector of the end effector respect to the tern 0,
the velocity vector of the same can be calculated in this way
P(0) = W05(0) P(0)

(1.34)

With the same approach is possible to proceed for the accelerations. Now H is a
4x4 matrix built in blocks

+ 2
A0
P(i)
P(i) = H(i) P(i) =

0
0
0 0
where and 2 are the tangential and the normal accelerations.

2
0 0 0 0

0
2
0 0 0 0

0
H01(0) =
H
=
12(1)
0 0 0 0
0
0 0
0 0 0 0
0
0 0

(1.35)

0
0

0
0

(1.36)

1.2. Velocity and Acceleration

H23(2)

2

2
=
0
0
0
0

H45(4)

0 0
0

0
0 0
H34(3) =

0
0 0
0
0 0

0 0
0 0
0 2 0

=
0 2 0
0 0
0 0

11
0
0
0
0

0
0
0
0

0
0

0
0

(1.37)

(1.38)

It could be possible to have the same result using only W matrices already defined
(k) + W 2
H(k) = W
k

(1.39)

Obviously even the H matrices will be redefined in the absolute coordinate system,
exactly as already seen for the speed.
H(i) = Mij H(j) Mji

(1.40)

To obtain the matrix of acceleration for the TCP in the reference system 0, the
last step is to sum the matrices calculated in the absolute reference with the same
procedure suggested from the theorem of accelerations composition or the Theorem
of Coriolis
Hik(r) = Hij (r) + Hjk (r) + 2Wij (r) Wjk (r)
(1.41)
For the case under study the formulation is the next
X
X j1
X
5
5
H
+
H05(0) =
j=2
i=1 i1,i (0)
k=1 2Wk1,k (0) Wj1,j (0)

(1.42)

Therefore, if P(0) is the position vector of the end effector respect to the tern 0,
the acceleration vector of the same can be calculated in this way
P(0) = H05(0) P(0)

(1.43)

12

Chapter 1. Direct Kinematics

Position

[m]

[ ]

[ ]

[ ]

start
end

0,00
0,15

0
50

0
40

0
40

Table 1.1: Initial and final joints space coordinates

Position

x
[m]

y
[m]

z
[m]

Pstart
Pend

1,5
0,605

1,0
2,258

0,0
-0,080

Table 1.2: Initial and final work space coordinates

1.3

Example of direct kinematics solution

In Table 1.1 and in Figure 1.2 the set of graphs shows initial and final joints
position. The movement is described with a 3-steps motion law. They are inputs
for the direct kinematics problem. The solution is in Table 1.2 and in Figure 1.3. In
particular, every acceleration has the tipical step shape. Because of the differrence
in scale it is not possible to recognize the trapezoid for the speeds. Comparing
results from the two methods except for numerical approximations the difference
is always zero, as shown in Figure 1.1. Therefore for the direct kinematics both
the tecniques described represt a valid alternative.
CONFRONTO FRA PROCEDIMENTI - CINEMATICA DIRETTA
16

x 10

x
y
z

diff [m]

4
3
2
1
0

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

15

1.2

x 10

xp
yp
zp

diff [m/s]

0.8
0.6
0.4
0.2
0

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

15

x 10

xpp
ypp
zpp

diff [m/s2]

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

Figure 1.1: Comparison between methods

CONFRONTO FRA PROCEDIMENTI - CINEMATICA INVERSA
4000
alpha
theta
phi

diff [rad]

3000

2000

1000

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

1.3. Example of direct kinematics solution

13

ESEMPIO SOLUZIONE CINEMATICA DIRETTA

MOVIMENTO NELLO SPAZIO DEI GIUNTI
0.2

rho [m]

0.15
0.1

ESEMPIO SOLUZIONE CINEMATICA DIRETTA

0.05
0

0.1

0.2

0.3

0.4
0.6
MOVIMENTO
NELLOt0.5[s]SPAZIO DEI
GIUNTI0.7

0.8

0.9

0
00
0

0.1
0.1

0.2
0.2

0.3
0.3

0.4
0.4

0.5
t0.5
[s]
t [s]

0.6
0.6

0.7
0.7

0.8
0.8

0.9
0.9

1
1

0.1
0.1

0.2
0.2

0.3
0.3

0.4
0.4

0.5
t0.5
[s]
t [s]

0.6
0.6

0.7
0.7

0.8
0.8

0.9
0.9

1
1

0.1
0.1

0.2
0.2

0.3
0.3

0.4
0.4

0.5
t0.5
[s]
t [s]

0.6
0.6

0.7
0.7

0.8
0.8

0.9
0.9

1
1

rho [m]
alpha []

50
0.2
40
0.15
30
0.1
20

10
0.05

40
50
alpha
theta[][]

30
40
30
20
20
10
10
0
00
0
40
40

theta
phi[]
[]

30
30
20
20
10
10
0
00
0
40

NELLO SPAZIO
DI LAVORO
FigureMOVIMENTO
1.2: Movements
in the
joints space

phi []

30

10
20
x[m]
xp[m/s]
xpp[m/s2]

10

50

0.1

0.2

0.3

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

MOVIMENTO NELLO SPAZIO DI LAVORO

5
10 0

0.1

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

1
x[m]
xp[m/s]
xpp[m/s2]

15
5
y[m]
yp[m/s]
ypp[m/s2]

10
0
5
0
5
0
5

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

10
15 0

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

1
y[m]
yp[m/s]
ypp[m/s2]

10
0.6
5
0.4

z[m]
zp[m/s]
zpp[m/s2]

0
0.2
0
5
0.2
10
0.40

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

0.5
t [s]

0.6

0.6
0.8
0.6 0

1
z[m]
zp[m/s]
zpp[m/s2]

0.4
0.2
0
0.2
0.4
0.6
0.8

0.1

0.2

0.3

0.4

0.7

Figure 1.3: Movements in the work space

0.8

0.9

Chapter 2
Inverse Kinematics
The target of the inverse kinematics analysis is to calculate the motion law for the
electric drivers in order to perform the TCP expected movement. The nonlinear
shape of the F function in the relation S = F (Q) brings some consequences. Analytically derive the expression of Q from S could be difficult if the kinematic chain
became complex, and the solution of the inverse problem more than one. The
procedure to obtain the solution is not standard and it requires the analysis of the
geometry. Otherwise approximation-based numerical methods can be explored.

2.1

Analytic Solution

For the case under study the analytic expression of the inverse kinematic has been
found. Exactly as for the direct kinematics, it is preferred to work with the dof
and only at the end convert it to . This is possible because the equivalent
expression is simple and easily implementable. The first step is to find the dof ,
because only function of z.
z = l4 sin sin
(2.1)
From this relation one of the variables is already defined
 z 

 z 
, 180 +
1,2 = arcsin
l4 sin
l4 sin

(2.2)

Therefore, only the last joint is responsible for rotations out-of-plane and so for
variations in z. The dof can move the end effector in two specular positions
z. To find the dof the idea is to combine coordinates in the work and in the
joints space. Following this approach, a distance d which ideally link the points
1 and 5 can be expressed at the same time through variables in the only work
space and with variables in the only joints space. But because d would be written
without , the triangle generated is rotated of degrees. In this way only is
unknown.

d2 = x2 +(yl1 )2 = [l2 +l3 C +l4 C C+ +l4 S C S+ ]2 +[l3 S +l4 C S+ l4 S C C+ ]2

(2.3)
14

2.1. Analytic Solution

15

Passing across several simplifications the equation has the following shape
a sin + b cos = c

(2.4)

where

a = l2 l4 (1 C )S2
b = 2l2 l3 + l2 l4 (1 C )C2 + l2 l4 (1 + C )

c = x2 + y 2 + l12 2yl1 l22 l32 (l4 C )2 (l4 S C )2 2l3 l4 (C2 + S2 C )

(2.5)
The next step is to operate a change of variables, where t = tan 2
2at + b(1 t2 ) = c(1 + t2 )
(
2t
sin = 1+t
2
1t2
cos = 1+t2

(2.6)
(2.7)

From the resolution of the second degree equation, two values for t can be found

a a2 + b 2 c 2
t=
(2.8)
b+c
and from the direct relation the second dof in the joints space is
= 2 arctan (t) + 2k

(2.9)

Now the only variable to define is and it can be easily derived from one of the
equations for the direct kinematics. For example, using x
x = l2 C + l3 C+ + l4 C C++ + l4 S C S++

(2.10)

It it always possible to write this formulation

d sin + e cos = f
where

d = l3 S l4 C S+ + l4 S C C+
e = l2 + l3 C + l4 C C+ + l4 S C S+

f =x

(2.11)

(2.12)

The same change of variables is now t = tan 2

(
2t
sin = 1+t
2
1t2
cos = 1+t2

(2.13)

From the resolution of the second degree equation, two values for t can be found
p
d d2 + e2 f 2
t=
(2.14)
e+f

16

Chapter 2. Inverse Kinematics

and the last dof in the joints space is

= 2 arctan (t) + 2k

(2.15)

The inverse kinematics problem for the position is now completely defined and it
leads to more possible combinations. Each one the results 1,2 carry the 2 solutions
1,2 and so 4 couples of values (,). Therefore, it is possible to reach the same
position for the TCP through 2 different but symmetric configurations. The others
2 solutions move the end effector in symmetric position, where the symmetry axis
is directed as x and it starts at y = 1m. This because of the positive angles from
the x-axis and the quote of 1m for the origin of the triangle sketched. Finally,
the 2 solutions 1,2 raise to 8 the possible combinations. In the choice of the right
couple it is necessary to check the elbow links creates and that the final position
is the one expected.
Thanks to the Jacobian already defined for the direct kinematics, velocity and
acceleration can be easily derived as

2.2

Q = J 1 S

(2.16)

= J 1 (S JQ)

(2.17)

Iterative Method

Among the techniques available in literature, the Newton-Raphson algorithm has

been implemented in this study. It is based on a system linearization around one
point Q which should be closed to the solution. The final approximation desired is
set. Because the direct problem is already solved, the relation S = F (Q) is known.
This is approximated with the first terms of the Taylor Series.
F (Q) F (Q0 ) + J(Q0 )(Q Q0 )

(2.18)

Defining Sf as the TCP position, a sequence of values should converges at the root
Sf = F (Q) searched.
Qn+1 = Qn + Jn1 (Sf F (Qn ))

(2.19)

It is an iterative process under the condition

kSf F (Qn )k

(2.20)

2.3. Example of Inverse kinematics solution

Position x

[m]
Pstart
Pend

1,4
1,0

17

y
[m]

z
[m]

1,2
1,6

0,0
0,1

CONFRONTO FRA PROCEDIMENTI - CINEMATICA DIRETTA

16

x 10

Table 2.1: Initial and final work space coordinates

x
y
z

diff [m]

Position

3
2
1
0

0.1

0.2

15

1.2

x 10

start
end
0.3

[m]
0,00
0,15

0.4

[ ]

[ ]

[ ]

-5,39 41,41
0
7,07 76,81 -53,13
0.5
t [s]

0.6

0.7

0.8

0.9

xp
yp
zp

Table 2.2: Initial and final joints space coordinates

diff [m/s]

0.8

0.6
0.4

2.3
0.2
0

Example of Inverse kinematics solution

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

In Table 2.1 and in Figure 2.2 the set of graphs shows initial and final TCP position.
Even in this case the movement is described with a 3-steps motion law. They
are inputs for the inverse kinematics problem. The solution in Table 2.2 and
in Figure 2.3 is obtained with both the analytic set of equations and using a
numerical method. As shown in Figure 2.1, approaching a singularity the iterative
algorithm became highly unstable and the difference from the analytic approach
unrestrained. In addition, the solution is influenced from initial conditions. It
guarantee satisfying
results only for initial angles close to the result.
CONFRONTO FRA PROCEDIMENTI - CINEMATICA INVERSA
15

x 10

xpp
ypp
zpp

diff [m/s2]

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

4000
alpha
theta
phi

diff [rad]

3000

2000

1000

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

2
alphap
thetap
phip

diff [rad/s]

1.5

0.5

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

1.4
alphapp
thetapp
phipp

1.2
diff [rad/s2]

1
0.8
0.6
0.4
0.2
0

'

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

Figure 2.1: Comparison between methods

0.8

0.9

'

18

Chapter 2. Inverse Kinematics

ESEMPIO SOLUZIONE CINEMATICA INVERSA
MOVIMENTO NELLO SPAZIO DI LAVORO

1.4

ESEMPIO SOLUZIONE CINEMATICA INVERSA

x[m]

1.3

MOVIMENTO NELLO SPAZIO DI LAVORO

1.2

1.4
x[m]

1.1

1.3
1
0
1.2

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

1.1
1.6
y[m]
1.5
1

0.1

0.2

0.3

0.4

1.4

0.5
t [s]

0.6

0.7

0.8

0.9

1.3
1.6

y[m]
1.2
1.5
1.1
1.40

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

1.3
0.1
1.2
0.08
1.1

z[m]

0.1

0.2

0.3

0.4

0.06

0.5
t [s]

0.6

0.7

0.8

0.9

0.04
0.1

z[m]
0.02
0.08
0
0.060

0.1

0.2

0.04
0.02
0

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

' in the work space

Figure 2.2: Movements
'
0.3

0.4

0.5

0.6

0.7

0.8

0.8

0.9

'

0.9

MOVIMENTO NELLOt [s]SPAZIO DEI GIUNTI

'
'

0.2
0
0.2
0.4
0.2
0.6
0
0

'

rho[m]
rhop[m/s]
rhopp[m/s2]

MOVIMENTO NELLO SPAZIO DEI GIUNTI

0.1

0.2

0.3

0.4

0.5
t [s]

0.6

0.7

0.8

0.9

rho[m]
rhop[m/s] 1
rhopp[m/s2]

0.2
10
alpha[]
alphap[rad/s]
alphapp[rad/s2]

0.4
5
0.6
0

5
10
10
0
5

0.1

0.2

0.3

0.1

0.2

0.3

0.4

0.4

0.5
t [s]

0.5
t [s]

0.6

0.6

0.7

0.7

0.8

0.8

0.9

0.9

alpha[]
alphap[rad/s] 1
alphapp[rad/s2]

800
theta[]
thetap[rad/s]
thetapp[rad/s2]

60
5
40
10
20 0

0.1

0.2

0.3

0
80
20
600

0.1

0.2

0.3

0.4

0.4

0.5
t [s]

0.5
t [s]

0.6

0.6

0.7

0.7

0.8

0.8

0.9

0.9

40
20
20

theta[]
thetap[rad/s] 1
thetapp[rad/s2]
phi[]
phip[rad/s]
phipp[rad/s2]

0
0
20
20

0.1

0.2

0.3

0.1

0.2

0.3

0.1

0.2

0.3

0.4

40

20
60
0
0

0.4

0.5
t [s]

0.5
t [s]

0.6

0.6

0.7

0.7

0.8

0.8

0.9

0.9

phi[]
phip[rad/s] 1
phipp[rad/s2]

'

20
40
60

0.4

0.5
t [s]

0.6

0.7

Figure 2.3: Movements in the joints space

0.8

0.9

'

Chapter 3
Work Space
Whichever is the trajectory assigned, the robot must not be able to hit itself.
Because of that limit switches for all the joints are set. They are reported in Table
3.1. The limit switches for the prismatic joint is the maximum possible extension
of link l0 , starting from the initial configuration of 45

2
) (2l l 2)
(3.1)
= (l
2
With all the information available the complete work space can be sketched and
some conclusions summarized.
The end efector cannot reach points outside the circumference of radius l2 +
l3 + l4 and center (x0 , y0 + l1 )
Because they are singularity, points on circumference border are reached with
only one link configuration
The TCP is able of positioning in points inside the circumference with two
link configurations
Points where joints are close to the limit switches are reached with only one
link configuration

dof

Min
[ ]

Max
[ ]

-45
-135
-180

+90
+135
+180

Table 3.1: Joints limit switches

19

- I punti al di fuori dellarco di cfr di raggio l2+l3+l4 e centro (x0, y0+l1) non sono raggiungibili
- 20
I punti che giacciono su tale cfr sono raggiungibili in una sola configurazione
Chapter(singolarit)
3. Work Space
- I punti interni alla cfr sono raggiungibili per due possibili configurazioni dei links

The picture in Figure 3.1 shows the work space in x-y plane: the thin line refers
to link l2 while the thick line to the robot extremity.

- In prossimit dei fine corsa le soluzioni possibili diventano una sola

2.5

y [m]

1.5

0.5

X: 0
Y: 0

0.5
0.5

0.5
x [m]

1.5

Figure
3.1:di Work
in the
plane
Il grafico mostra gli estremi
dellarea
lavoro area
nel piano
x-y:x-y
la linea
pi sottile lestremit del
link 2 mentre quella pi spessa la posizione della pinza.
Questo grafico, ottenuto tenendo fisso a zero langolo e variando e , mette in luce le ellissi

The
next nel
3-dimensions
graphic
obtained fixing and with a sweep of and ,
generate
piano y-z dal giunto
ad asseissghembo.
it shows the ellipses in plane y-z generated from the oblique joint.

2.5

y [m]

1.5

0.5

0.5
0.2
0.15
0.1
0.05
0
0.05
0.1
0.15

z [m]

0.4

0.2

0.2

0.4

0.6

0.8

x [m]

Figure 3.2: Work space with fixed

1.2

1.4

1.6

Chapter 4
Singularity
5.''CONFIGURAZIONI'SINGOLARI'
delle configurazioni
di singolarit
pube
essere
condotta seguendo
differenti
strade:ways.
TheL'individuazione
study of singularity
configurations
can
conducted
through
different
Analisi della
soluzione
dellakinematics
cinematica inversa#
Analysis
of the
inverse
solutions. The singularity is the condition
Le configurazioni singolari possono essere viste come i casi in cui due o pi soluzioni della
of coincidence
of twoo quando
or more
solutions
of contenuta
the inverse
or if one
cinematica
inversa coincidono,
una funzione
in esse
non kinematics
definita. Condizione
of the functions
is not
There
is allineati).
a singularity when changing , the
di singolarit
per =0 e =(0,
180)defined.
al variare di
(bracci

links l2 , l3 are aligned and so = 0 and = 0 , 180 .

Analisi della matrice J

In corrispondenza di queste il determinante dello jacobiano si annulla: coprendo tutti i movimenti
Analysis of the J matrix. In case of singularity the Jacobian determinant is
concessi al robot con un delta dangolo sufficientemente piccolo possibile includere tutte le
annulled.singolari.
With a sweep dense enough all the singularity configurations are
configurazioni

covered.
Di seguito riportato il grafico 3D risultante, che coincide con quanto ipotizzato sopra. Le
singolari
con parte
degli estremi
di lavoro. are part of the work
As configurazioni
appears from
the coincidono
3-dimensions
graphics,
thedellarea
singularities

space limits.

2.5

y [m]

1.5

0.5

1.6
1.4
1.2

1
0.8
0.6
0.4

0.5
0.15

0.1

0.2
0.05

0
0.05

0.1

0.15

0.2
0.2

z [m]

Figure 4.1: Singularity points in the work space

'

21

x [m]

Chapter 5
Inverse Dynamics
Through the inverse dynamics the drivers action to realize the expected end-effector
movement is derived. The forces acting on the links and the actions in between
(joints reactions) are analyzed. To solve the inverse dynamics some matrices are
introduced.
Matrix of the actions : it represents a system of forces and couples respect
the reference system i. Because it is an emisimetric matrix, T(i) = (i) and
has the following shape

0
Cz Cy fx
Cz
0
Cx fy

(i) =
(5.1)
Cy Cx
0
fz
fx fy fz 0
Inertial pseudo-tensor J: it describes the mass distribution of a rigid body
around the reference system i with the following shape

Ixx Iyx Izx mxg

Ixy Iyy Izy myg

J(i) =
(5.2)
Ixz Iyz Izz mzg
mxg myg mzg m
Combining these matricies with M , W , H defined with the analysis of the direct
kinematics allows for a simple and systematic resolution of rigid bodies dynamics.
Precisely, to calculate inertial forces and couples respect the reference system i
T
(i) = H0k(i) Jk(i) Jk(i) H0k(i)

(5.3)

where H0k(i) is the matrix of absolute acceleration for the body k, calculated respect
an inertial system 0 (in this study also reported as the absolute reference system)
and projected on the tern i. In order to consider also the weight force, one matrix
in the absolute reference system will be defined

0 0 0
0
0 0 0 9, 81

Hg =
(5.4)
0 0 0
0
0 0 0
0
22

23
To derive the inverse dynamics the fisrt step is to define the inertial pseudo-tensor
of every link k, starting from the TCP toward the base. All the H matrices are
already known in the absolute reference. All the elements should be reported to
the same reference system, and so
Jk(0) = M0i Jk(i) M0iT

(5.5)

Summarizing, the matrix equation which covers all the possible forces acting on a
part
T
(i) = (H0k(i) Jk(i) Jk(i) H0k(i)
) + (Hg (i) Jk(i) Jk(i) HgT (i) ) + (i)

(5.6)

For every joint j the constraint reaction is derived with the use of dynamics equilibrium and considering the transmitted forces from joint i + 1, inertial terms, weight
and other external components
i = i+1 + i

(5.7)

Finally, to obtain the motor actions the Principle of Virtual Works suggests that
the virtual work of the i th motor equals the virtual work of actions at the i th
joint, which it depends from the sum of the previous links.
i qi + i Li qi = 0

(5.8)

From this consideration the force or couple the i th joint exercises is

i = i Li

(5.9)

Chapter 6
Work Cycle
The final goal of this study is to assign the cycle instructions described in the
introduction to the robot. Among the specifics, it is required that in the extrude
direction the relative speed between cutting edge and piece is zero. From an
external observer the TCP describes a V trajectory, while from an in-built point
the movement is only vertically.

6.1

Motion between (P1 : P2) and (P3 : P1)

The movements (P1 : P2 ) and (P3 : P1 ) are without any trajectory constraint
but only for initial and final points. They will be performed with the minimum
time, compatibly with the drivers limit. The first step is to calculate the joint
coordinates Q1 , Q2 and Q3 solving the inverse kinematics. With the definition of
the variations Q2,1 = Q2 Q1 and Q1,3 = Q1 Q3 , the minimum time for every
joint is achieved with the maximum acceleration. Depending from the space qi to
cover, the drive can effectively reach the maximum speed before the deceleration
phase or not. The two possible case are described as
s


amaxi + dmaxi
vmaxi vmaxi vmaxi
+
Tmini = 2kqi k
(6.1)
if kqi k
2
dmaxi amaxi
amaxi dmaxi


vmaxi vmaxi vmaxi
kqi k 1 vm (A + D)
if kqi k >
+
+
(6.2)
Tmini =
2
dmaxi amaxi
vm
2
AD
Generally the time for every joint is different, and the maximum among them is
the minimum time of the problem. Therefore, for the other joints there will be a
delay expressible as
Tmin
i =
(6.3)
Ti
where Tmin = 0.47s. Except for the drive with highest time, the others will enlarge
the time scale in order to require Tmin to reach the end. In this way overload in
the actuators is avoided. The shape of the motion law does not change, only the
maximum acceleration and speed. In this case the inertial forces decrease with
benefits for the drivers.
amaxi
vmaxi
dmaxi
ai =
vi =
di =
(6.4)
2
2
i
i
2i
24

6.1. Motion between (P1 : P2 ) and (P3 : P1 )

25

graphics
show velocity
acceleration
thegdl.
3 dof,
wherecome
the sia
second
IThe
grafici
riportanobelow
landamento
di velocitand
e accelerazione
perfor
i tre
Si osserva
il
secondo
motore the
a determinare
il tempo
drive decide
minimum
time.minimo. Quanto visto per questo primo tratto identico in
svolgimento e risultati anche per lultimo, fra P3 e P1.
I grafici riportano landamento di velocit e accelerazione per i tre gdl. Si osserva come sia il
secondo motore a determinare il tempo minimo. Quanto visto per questo primo tratto identico in
svolgimento e risultati anche per lultimo, fra P3 e P1.
rhop [m/s]

0.25

0.2

0.15
0.1

rhop [m/s]

0.25
0.05
0.2
0

0.05

0.1

0.15

0.05

0.1

0.15

0.05

0.1

0.15

0.05

0.1

0.15

3
0
0

0.05

0.1

0.15

0.2

0.15

0.25
t [s]

0.3

0.35

0.4

0.45

0.1
5

thetap [rad/s]

0.05
4
0

0.2

0.25
t [s]

0.3

0.35

0.4

0.45

2
5

thetap [rad/s]

1
4
0
30

0.2

0.25
t [s]

0.3

0.35

0.4

0.45

0.5

2
4
1

phip [rad/s]

phip [rad/s]

3
0

0.2

0.25
t [s]

0.3

0.35

0.4

0.45

0.5

4
1

0.2

0.3

0.35

0.4

0.45

0.5

Figure 6.1: (P1 , P2 ) and (P3 , P1 ) - Velocities in the joints space

0.25
t [s]

0.05

0.1

0.15

0.20

0.05

0.1

0.15

0.05

0.1

0.15

10
20
0

0.05

0.1

0.15

0.05

0.1

0.15

10
20
0

0.05

0.1

0.15

0.05

0.1

0.15

0.2

0.25
t [s]

0.3

0.35

0.4

0.45

0.5

0.6

rhopp [m/s2]

0.4
0.2
0
0.2

0.6
0.4

phipp [rad/s2]

phipp [rad/s2]

thetapp [rad/s2]

thetapp [rad/s2]

rhopp [m/s2]

0.4
0.6
0.2

0.25
t [s]

0.3

0.35

0.4

0.45

0.5

0.2
20
0.4
0.6
10
0.2

0.25
t [s]

0.3

0.35

0.4

0.45

0.5

20
10

0.2

0.25
t [s]

0.3

0.35

0.4

0.45

0.5

0
20
10
10
20

0.2

0.25
t [s]

0.3

0.35

0.4

0.45

0.5

20
10

0.2

0.25
t [s]

0.3

0.35

0.4

0.45

0.5

10

20

0.2

0.25
t [s]

0.3

0.35

0.4

0.45

Figure 6.2: (P1 , P2 ) and (P3 , P1 ) - Accelerations in the joints space

0.5

26

6.2

Chapter 6. Work Cycle

Motion between (P2 : Pi) and (Pi : P3)

For these couples of points the speed in z direction will be equal to ve and the
trajectory always made of straight lines. Considering now the descent phase (the
ascent is symmetric), the distance can be expressed as
dx = kP2x Pix k = 0, 0m

(6.5)

dy = kP2y Piy k = 0, 6m

(6.6)

dz = kP2z Piz k = 0, 1m

(6.7)

d=

q
d2x + d2y + d2z = 0, 6083m

(6.8)

The constrain in speed is translated in the time domain

dz
= 1s
t =
ve

(6.9)

To obtain a straight trajectory directional vectors are used

P (t) = P2 + u(t)

P (t) = u(t)

(6.10)

P (t) = u(t)

(6.12)

(6.11)

where u has the same orientation of the trajectory searched

u=

(Pi P2 )
kPi P2 k

(6.13)

and is a three-steps motion law. The goal here would be to move the cutting edge
with also constant vertical speed. This will be not completely true because of the
inversion of motion in Pi . Anyway, short dt for both acceleration and deceleration
leads to satisfying results. The alternative could be to change y coordinate for the
intermediate point Pi in order to leave the motion inversion under the extruded
product. Because this would be possible the conveyor belt must be soft and slightly
warps under utensil pressure.

6.2. Motion between (P2 : Pi ) and (Pi : P3 )

27

The set of graphics shows the entire cycle of work, which needs 2, 95s to terminate. Limitations and constraints are respected. First the 3 dof , , and their
derivatives are reported, while the next set are the x, y, z coordinates of the end
effector.
I grafici riportano il ciclo di lavoro in , e
0.02

rho [m]

0
0.02
0.04
0.06
0.08

0.5

1.5
t [s]

2.5

0.5

1.5
t [s]

2.5

0.5

1.5
t [s]

2.5

0.5

theta [rad]

1
1.5
2
2.5
3

phi [rad]

0.5

0.5

Figure 6.3: Work cycle - Position in the joints space

Infine
si riportano
gli andamenti
di velocit
dei giunti per lintero ciclo
I grafici
riportano iilgrafici
ciclo dicon
lavoro
svolto dallutensile
in ex,accelerazione
y e z.
di lavorazione, che ha una durata totale di 2.95s; garantito il rispetto dei limiti imposti.
1.2
x[m]

1.1
0.15
1
0.1
0.9
rhop [m/s]

0.05
0.8

0.7 0
0.05
0.6
0.1
0.5
0
0.15
0.2

0.5

0.5

0.8

1.5
t [s]

2.5

1.5
t [s]

2.5

y[m]
0.6

thetap [rad/s]

0.4 4
0.2

3
2

01

0.2 0
0
1
2

0.5

0.5

0.1

1.5
t [s]

2.5

1.5
t [s]

2.5

z[m]
0.05 4

phip [rad/s]

03

0.05 2

0.1 1
0
0

0.5

0.5

1.5
t [s]

2.5

1.5
t [s]

2.5

Figure 6.4: Work cycle - Velocities in the joints space

1.5

rhopp [m/s2]

1
0.5
0
0.5
1

20

0.5

1.5
t [s]

2.5

phi

0.5

1.5
t [s]

28

2.5

Chapter 6. Work Cycle

I grafici riportano il ciclo di lavoro in , e

1.5

rho [m]
rhopp [m/s2]

1
0.02
0.5
0

0
0.02
0.5
0.04
1
0.06 0

0.08

0
20

0.5

1.5
t [s]

0.5

1.5
t [s]

0.5

1.5
t [s]

0.5

1.5
t [s]

0.5

1.5
t [s]

0.5

1.5
t [s]

2.5

2.5

theta [rad]
thetapp [rad/s2]

10
0.5
0
1

10
1.5
20
2

30
2.5 0

phi [rad]
phipp [rad/s2]

0
20

2.5

2.5

10
1

0.50
10
0

20
0.5
0

2.5

2.5

Figure 6.5: Work cycle - Accelerations in the joints space

I grafici riportano il ciclo di lavoro svolto dallutensile in x, y e z.
1.2
x[m]

1.1
1
0.9
0.8
0.7
0.6
0.5

0.5

1.5
t [s]

2.5

0.8
y[m]
0.6
0.4
0.2
0
0.2

0.5

1.5
t [s]

2.5

0.1
z[m]
0.05

0.05

0.1

0.5

1.5
t [s]

2.5

Figure 6.6: Work cycle - Position in the work space

6.2. Motion between (P2 : Pi ) and (Pi : P3 )

29

The last group are the drivers. The first set considers the first rotational joint
and so the 3 actions C1 , C
, C3 , while
the real force is at the prismatic joint and
Infine sono riportate le azioni
dei2 motori:
prima considerando come motrice la coppia al primo
the second
set
is F1 , C2solo
, C3 .uno step di passaggio), poi la forza che agisce al manicotto.
giunto
rotoidale
(rappresenta
Infine sono riportate le azioni dei motori: prima considerando come motrice la coppia al primo
500
giunto
rotoidale (rappresenta solo uno step di passaggio), poi la forza che agisce al manicotto.

C1 [Nm]

C1 [Nm]

400
300
500
200
400
100
300
0
200 0

0.5

1.5
t [s]

2.5

0.5

1.5
t [s]

2.5

0.5

1.5
t [s]

2.5

0.5

1.5
t [s]

2.5

0.5

1.5
t [s]

2.5

0.5

2.5

100

C2 [Nm]

C2 [Nm]

40
0
0
20
0
40
20
20
40
0
60
20 0
40

10
60
0

C3 [Nm]

5
10
0

C3 [Nm]

5
5
0
10
5

10

Figure 6.7: Work cycle - Robot dynamics

1

1.5
t [s]

0.5

1.5
t [s]

2.5

0.5

1.5
t [s]

2.5

0.5

1.5
t [s]

2.5

0.5

1.5
t [s]

2.5

0.5

1.5
t [s]

2.5

0.5

1.5
t [s]

2.5

30

F1 [Nm]

F1 [Nm]

20
10
30
0
20
10
10
20
00
10

C2 [Nm]

C2 [Nm]

40
20
0
20
0
40
20
20
40
0
60
20 0
40

10
60
0

C3 [Nm]

5
10
0

C3 [Nm]

5
5
0
10
5

10

Figure 6.8: Work space - Robot dynamics with prismatic joint force

30

Chapter 6. Work Cycle

This sequence shows the entire work cycle simulated in Simulink/SimMechanics,

without the prismatic joint and providing , , as inputs. In Figure 6.14 is added
the complete robot scheme, with one more actuator in order to eventually simulate
the external force.

Figure 6.9: Work cycle - Point P1

Figure 6.10: Work cycle - Point P2

Figure 6.11: Work cycle - Point Pi

6.2. Motion between (P2 : Pi ) and (Pi : P3 )

Figure 6.12: Work cycle - Point P3

Figure 6.13: Work cycle - Point P1

Figure 6.14: Work cycle - scheme of the Simulink/SimMechanics model

31

Appendix A
Laws of Motion
Choosing between laws of motion available in literature, they should at least guarantee continuity in space and speed domains. Even the continuity in acceleration
would be good practise. The most common is the family of three-steps motion laws,
with a symmetric shape and where one of the derivatives has constant sections.
They are often chosen because simple and well suited to evaluate main behaviours
in the mechanism.
Three-steps motion law
Prof. Giovanni
Legnani,
Ing. Hermes
Giberti
Among all the possible cases
in this
project
the three-steps
constant acceleration
has been implemented. Therefore, the speed draws a symmetric trapezoid as in
Figure A.1.

Meccanica dei Rob

Figure A.1: Function three-steps motion law

32

Figura 2: Legge tre tratti e cicloidale.

Per il debug della cinematica si confrontino le velocit`a ed accelerazioni calcolate analitic

mente con quelle calcolate numericamente.
Ad esempio, definite le quantit`
a: s, v, a ovvero array di posizione, velocit`a ed accelerazio
calcolati analiticamente, tt array con tempi campionati e dT passo di campionamento,

33
Writing the kinematic relations under the assumption of t1 = t3 (always symmetric shape), the maximum speed and acceleration are
vmax =

s 1
T (1 )

(A.1)

amax =

1
s
2
T (1 )

(A.2)

where

t1
(A.3)
T
The variable is the degree of freedom to change the law shape. For example with
higher , because t1 grows also vmax increases while the amax decreases.
=

Cycloidal motion law

A different possibility is to implement a motion law described through trigonometric functions.

Figure A.2: Function cycloidal motion law

Figura 2: Legge tre tratti e cicloidale. 

t

 t 
1
sin 2
T
2
T


s
t  analiticamatica si confrontino le velocit`a ed accelerazioni
1 coscalcolate
2
x =
T
T
numericamente.

s
t
quantit`a: s, v, a ovvero array di posizione,
a
ed
accelerazione
x = velocit`
2
sin
2
T2
T
, tt array con tempi campionati e dT passo di campionamento, si
In this case
pondenti valori numericamente attraverso il seguente
s listato Matlab:
vmax = 2
T
velocit`
a ( calcolo per d e r i v a z i o n e nu mer ica )
s
( si perde un punto )
amax = 2 2
T a)
a c c e l e r a z i o n e ( calcolo per d e r i v a z i o n e nume ric
x = s

(A.4)
(A.5)
(A.6)

(A.7)
(A.8)

( si perde un punto )

, s_p ) % i grafici devono essere s o v r a p p o s t i

, s_pp ) % i grafici devono essere s o v r a p p o s t i