CHAPTERS 2-12 COVERED
SOLUTIONS MANUAL
,Luigi Villani, Giuseppe Oriolo, Bruno Siciliano
Solution Manual for
Robotics
Modelling, Planning and Control
February 6, 2009
Springer
,
,Preface
This manual presents the solutions to all the end-of-chapter problems con-
tained in the textbook Robotics: Modelling, Planning and Control
(ISBN 978-1-84628-641-4, e-ISBN 978-1-84628-642-1) by Bruno Siciliano,
Lorenzo Sciavicco, Luigi Villani and Giuseppe Oriolo, Springer-Verlag, Lon-
don, 2009.
Solutions to analytical problems are developed by emphasizing the crucial
steps towards the solution. Some problems may be solved in different ways; the
solution reported in the manual is believed to be the most straightforward. The
solutions to several problems contain useful analytical developments which are
complementary to the theoretical derivation in the textbook.
Solutions to programming problems are accompanied by results of com-
puter implementations in Matlab R
(version 7.4) with Simulink R 1
. The
code (downloadable from www.springer.com/978-1-84628-641-4) is avail-
able free of charge to those adopting this volume as a text for courses.
The software is not aimed at providing a complete toolbox, but only at
solving the end-of-chapter problems. Nonetheless, the code has been developed
in a modular fashion which should allow direct expansion to more complex
problems as well as ease of changing the problems data.
For the problems solved in Matlab, the solution is contained in a file with
.m extension, where the first letter is an s, followed by the problem number,
e.g., s4 1.m is the file to execute for solving Problem 4.1.
The problems requiring simulation of a dynamic system have been solved
in Simulink and the solution is contained in a file with .mdl extension, e.g.,
s3 21.mdl is the file to execute for solving Problem 3.21. Each problem of
this kind requires the initialization of certain variables before starting the
simulation. This is performed in a file where the first letter is an i, followed
by the problem number, e.g., i3 21.m is the initialization file for Problem
3.21.
1
Matlab and Simulink are registered trademarks of The MathWorks, Inc.
,vi Preface
For both Matlab- and Simulink-based problems, the output plots of
relevant variables are obtained by executing a file where the first letter is
a p, followed by the problem number, e.g., p3 21.m is the file for plotting the
output variables of Problem 3.21.
Variable initialization and plot can be activated by double clicking respec-
tively on the upper-left block and the lower-right block in the Simulink block
diagrams.
For problems requiring the simulation of two different systems, two files
have been created where letters a and b have been used to distinguish them,
e.g., s3 22a.mdl and s3 22b.mdl are the files for solving Problem 3.22 with
two different algorithms; accordingly, the files for plotting output variables
have been named p3 22a.m and p3 22b.m.
The above files are supplemented by other function and script files which
are needed to solve the programming problems.
All the files used to solve a given problem are collected into a folder with
the same label of the problem, e.g. Folder 3 21 contains the files of Problem
3.21.
Helpful comments are added to each file to describe its contents and func-
tions. A readme.txt file is also provided.
Finally, the authors wish to thank Luigi Freda for his contributions to the
software developed for the solution of some problems of Chapter 12.
Naples and Rome Luigi Villani
February 2009 Giuseppe Oriolo
Bruno Siciliano
,Contents
2 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
3 Differential Kinematics and Statics . . . . . . . . . . . . . . . . . . . . . . . . 19
4 Trajectory Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
5 Actuators and Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
6 Control Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
7 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
8 Motion Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
9 Force Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
10 Visual servoing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
11 Mobile Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
12 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143
,
,2
Kinematics
Solution to Problem 2.1
Composition of rotation matrices with respect to the current frame gives
R(φ) = Rz (ϕ)Rx′ (ϑ)Rz′′ (ψ).
Using the expressions of elementary rotation matrices in (2.6) and (2.8):
⎡ ⎤
cϕ −sϕ 0
Rz (ϕ) = ⎣ sϕ cϕ 0 ⎦
0 0 1
⎡ ⎤
1 0 0
Rx′ (ϑ) = ⎣ 0 cϑ −sϑ ⎦
0 sϑ cϑ
⎡ ⎤
cψ −sψ 0
Rz′′ (ϕ) = ⎣ sψ cψ 0 ⎦
0 0 1
and taking the products gives
⎡ ⎤
cϕ cψ − s ϕ cϑ s ψ −cϕ sψ − sϕ cϑ cψ sϕ sϑ
R(φ) = ⎣ sϕ cψ + cϕ cϑ sψ −sϕ sψ + cϕ cϑ cψ −cϕ sϑ ⎦ .
sϑ sψ sϑ cψ cϑ
As for the inverse problem, given a rotation matrix
⎡ ⎤
r11 r12 r13
R = ⎣ r21 r22 r23 ⎦ ,
r31 r32 r33
,2 2 Kinematics
the set of Euler angles ZXZ is given by
ϕ = Atan2(r13 , −r23 )
ϑ = Atan2 2 + r2 , r
r31 32 33
ψ = Atan2(r31 , r32 )
when ϑ ∈ (0, π). Otherwise, if ϑ ∈ (−π, 0) then the solution is
ϕ = Atan2(−r13 , r23 )
2 2
ϑ = Atan2 − r31 + r32 , r33
ψ = Atan2(−r31 , −r32 ).
Solution to Problem 2.2
In the case sϑ = 0, the rotation matrix in (2.18) becomes
⎡ ⎤
cϕ+ψ −sϕ+ψ 0
R(φ) = ⎣ sϕ+ψ cϕ+ψ 0⎦
0 0 1
when ϑ = 0. Otherwise, if ϑ = π, then the matrix is
⎡ ⎤
−cϕ−ψ −sϕ−ψ 0
R(φ) = ⎣ −sϕ−ψ cϕ−ψ 0 ⎦.
0 0 −1
From the elements [1, 2] and [2, 2] it is possible to compute only the sum or
difference of angles ϕ and ψ, i.e.,
ϕ ± ψ = Atan2(−r12 , r22 )
where the positive sign holds for ϑ = 0 and the negative sign holds for ϑ = π.
Solution to Problem 2.3
In the case cϑ = 0, the rotation matrix in (2.21) becomes
⎡ ⎤
0 sψ−ϕ cψ−ϕ
R(φ) = ⎣ 0 cψ−ϕ −sψ−ϕ ⎦
−1 0 0
when ϑ = π/2. Otherwise, if ϑ = −π/2, then the matrix is
⎡ ⎤
0 −sψ+ϕ −cψ+ϕ
R(φ) = ⎣ 0 cψ+ϕ −sψ+ϕ ⎦ .
1 0 0
, 2 Kinematics 3
From the elements [2, 2] and [2, 3] it is possible to compute only the sum or
difference of angles ψ and ϕ, i.e.,
ψ ± ϕ = Atan2(−r23 , r22 )
where the positive sign holds for ϑ = −π/2 and the negative sign holds for
ϑ = π/2.
Solution to Problem 2.4
The rotation matrix can be obtained as in (2.24)
R(ϑ, r) = Rz (α)Ry (β)Rz (ϑ)Ry (−β)Rz (−α),
where the elementary rotation matrices are given as in (2.6) and (2.7):
⎡ ⎤
cα −sα 0
Rz (α) = ⎣ sα cα 0 ⎦
0 0 1
⎡ ⎤
cβ 0 sβ
Ry (β) = ⎣ 0 1 0 ⎦
−sβ 0 cβ
⎡ ⎤
cϑ −sϑ 0
Rz (ϑ) = ⎣ sϑ cϑ 0 ⎦ .
0 0 1
Taking the first product gives
⎡ ⎤
cα cβ −sα cα sβ
Rz (α)Ry (β) = ⎣ sα cβ cα sα sβ ⎦ .
−sβ 0 cβ
The next product gives
⎡ ⎤
c α cβ cϑ − sα sϑ −cα cβ sϑ − sα cϑ cα sβ
Rz (α)Ry (β)Rz (ϑ) = ⎣ sα cβ cϑ + cα sϑ −sα cβ sϑ + cα cϑ sα sβ ⎦ .
−sβ cϑ sβ sϑ cβ
Then, by observing that
Ry (−β)Rz (−α) = (Rz (α)Ry (β))T ,
the overall rotation matrix is
⎡
(s2α + c2α c2β )cθ + c2α s2β sα cα s2β (1 − cϑ ) − cβ sϑ
R(ϑ, r) = ⎣ sα cα s2β (1 − cϑ ) + cβ sϑ (s2α c2β + c2α )cθ + s2α s2β
cα sβ cβ (1 − cϑ ) − sα sβ sϑ sα sβ cβ (1 − cϑ ) + cα sβ sϑ
⎤
cα sβ cβ (1 − cϑ ) + sα sβ sϑ
sα sβ cβ (1 − cϑ ) − cα sβ sϑ ⎦ .
s2β cϑ + c2β
SOLUTIONS MANUAL
,Luigi Villani, Giuseppe Oriolo, Bruno Siciliano
Solution Manual for
Robotics
Modelling, Planning and Control
February 6, 2009
Springer
,
,Preface
This manual presents the solutions to all the end-of-chapter problems con-
tained in the textbook Robotics: Modelling, Planning and Control
(ISBN 978-1-84628-641-4, e-ISBN 978-1-84628-642-1) by Bruno Siciliano,
Lorenzo Sciavicco, Luigi Villani and Giuseppe Oriolo, Springer-Verlag, Lon-
don, 2009.
Solutions to analytical problems are developed by emphasizing the crucial
steps towards the solution. Some problems may be solved in different ways; the
solution reported in the manual is believed to be the most straightforward. The
solutions to several problems contain useful analytical developments which are
complementary to the theoretical derivation in the textbook.
Solutions to programming problems are accompanied by results of com-
puter implementations in Matlab R
(version 7.4) with Simulink R 1
. The
code (downloadable from www.springer.com/978-1-84628-641-4) is avail-
able free of charge to those adopting this volume as a text for courses.
The software is not aimed at providing a complete toolbox, but only at
solving the end-of-chapter problems. Nonetheless, the code has been developed
in a modular fashion which should allow direct expansion to more complex
problems as well as ease of changing the problems data.
For the problems solved in Matlab, the solution is contained in a file with
.m extension, where the first letter is an s, followed by the problem number,
e.g., s4 1.m is the file to execute for solving Problem 4.1.
The problems requiring simulation of a dynamic system have been solved
in Simulink and the solution is contained in a file with .mdl extension, e.g.,
s3 21.mdl is the file to execute for solving Problem 3.21. Each problem of
this kind requires the initialization of certain variables before starting the
simulation. This is performed in a file where the first letter is an i, followed
by the problem number, e.g., i3 21.m is the initialization file for Problem
3.21.
1
Matlab and Simulink are registered trademarks of The MathWorks, Inc.
,vi Preface
For both Matlab- and Simulink-based problems, the output plots of
relevant variables are obtained by executing a file where the first letter is
a p, followed by the problem number, e.g., p3 21.m is the file for plotting the
output variables of Problem 3.21.
Variable initialization and plot can be activated by double clicking respec-
tively on the upper-left block and the lower-right block in the Simulink block
diagrams.
For problems requiring the simulation of two different systems, two files
have been created where letters a and b have been used to distinguish them,
e.g., s3 22a.mdl and s3 22b.mdl are the files for solving Problem 3.22 with
two different algorithms; accordingly, the files for plotting output variables
have been named p3 22a.m and p3 22b.m.
The above files are supplemented by other function and script files which
are needed to solve the programming problems.
All the files used to solve a given problem are collected into a folder with
the same label of the problem, e.g. Folder 3 21 contains the files of Problem
3.21.
Helpful comments are added to each file to describe its contents and func-
tions. A readme.txt file is also provided.
Finally, the authors wish to thank Luigi Freda for his contributions to the
software developed for the solution of some problems of Chapter 12.
Naples and Rome Luigi Villani
February 2009 Giuseppe Oriolo
Bruno Siciliano
,Contents
2 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
3 Differential Kinematics and Statics . . . . . . . . . . . . . . . . . . . . . . . . 19
4 Trajectory Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
5 Actuators and Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
6 Control Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
7 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
8 Motion Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
9 Force Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
10 Visual servoing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
11 Mobile Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
12 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143
,
,2
Kinematics
Solution to Problem 2.1
Composition of rotation matrices with respect to the current frame gives
R(φ) = Rz (ϕ)Rx′ (ϑ)Rz′′ (ψ).
Using the expressions of elementary rotation matrices in (2.6) and (2.8):
⎡ ⎤
cϕ −sϕ 0
Rz (ϕ) = ⎣ sϕ cϕ 0 ⎦
0 0 1
⎡ ⎤
1 0 0
Rx′ (ϑ) = ⎣ 0 cϑ −sϑ ⎦
0 sϑ cϑ
⎡ ⎤
cψ −sψ 0
Rz′′ (ϕ) = ⎣ sψ cψ 0 ⎦
0 0 1
and taking the products gives
⎡ ⎤
cϕ cψ − s ϕ cϑ s ψ −cϕ sψ − sϕ cϑ cψ sϕ sϑ
R(φ) = ⎣ sϕ cψ + cϕ cϑ sψ −sϕ sψ + cϕ cϑ cψ −cϕ sϑ ⎦ .
sϑ sψ sϑ cψ cϑ
As for the inverse problem, given a rotation matrix
⎡ ⎤
r11 r12 r13
R = ⎣ r21 r22 r23 ⎦ ,
r31 r32 r33
,2 2 Kinematics
the set of Euler angles ZXZ is given by
ϕ = Atan2(r13 , −r23 )
ϑ = Atan2 2 + r2 , r
r31 32 33
ψ = Atan2(r31 , r32 )
when ϑ ∈ (0, π). Otherwise, if ϑ ∈ (−π, 0) then the solution is
ϕ = Atan2(−r13 , r23 )
2 2
ϑ = Atan2 − r31 + r32 , r33
ψ = Atan2(−r31 , −r32 ).
Solution to Problem 2.2
In the case sϑ = 0, the rotation matrix in (2.18) becomes
⎡ ⎤
cϕ+ψ −sϕ+ψ 0
R(φ) = ⎣ sϕ+ψ cϕ+ψ 0⎦
0 0 1
when ϑ = 0. Otherwise, if ϑ = π, then the matrix is
⎡ ⎤
−cϕ−ψ −sϕ−ψ 0
R(φ) = ⎣ −sϕ−ψ cϕ−ψ 0 ⎦.
0 0 −1
From the elements [1, 2] and [2, 2] it is possible to compute only the sum or
difference of angles ϕ and ψ, i.e.,
ϕ ± ψ = Atan2(−r12 , r22 )
where the positive sign holds for ϑ = 0 and the negative sign holds for ϑ = π.
Solution to Problem 2.3
In the case cϑ = 0, the rotation matrix in (2.21) becomes
⎡ ⎤
0 sψ−ϕ cψ−ϕ
R(φ) = ⎣ 0 cψ−ϕ −sψ−ϕ ⎦
−1 0 0
when ϑ = π/2. Otherwise, if ϑ = −π/2, then the matrix is
⎡ ⎤
0 −sψ+ϕ −cψ+ϕ
R(φ) = ⎣ 0 cψ+ϕ −sψ+ϕ ⎦ .
1 0 0
, 2 Kinematics 3
From the elements [2, 2] and [2, 3] it is possible to compute only the sum or
difference of angles ψ and ϕ, i.e.,
ψ ± ϕ = Atan2(−r23 , r22 )
where the positive sign holds for ϑ = −π/2 and the negative sign holds for
ϑ = π/2.
Solution to Problem 2.4
The rotation matrix can be obtained as in (2.24)
R(ϑ, r) = Rz (α)Ry (β)Rz (ϑ)Ry (−β)Rz (−α),
where the elementary rotation matrices are given as in (2.6) and (2.7):
⎡ ⎤
cα −sα 0
Rz (α) = ⎣ sα cα 0 ⎦
0 0 1
⎡ ⎤
cβ 0 sβ
Ry (β) = ⎣ 0 1 0 ⎦
−sβ 0 cβ
⎡ ⎤
cϑ −sϑ 0
Rz (ϑ) = ⎣ sϑ cϑ 0 ⎦ .
0 0 1
Taking the first product gives
⎡ ⎤
cα cβ −sα cα sβ
Rz (α)Ry (β) = ⎣ sα cβ cα sα sβ ⎦ .
−sβ 0 cβ
The next product gives
⎡ ⎤
c α cβ cϑ − sα sϑ −cα cβ sϑ − sα cϑ cα sβ
Rz (α)Ry (β)Rz (ϑ) = ⎣ sα cβ cϑ + cα sϑ −sα cβ sϑ + cα cϑ sα sβ ⎦ .
−sβ cϑ sβ sϑ cβ
Then, by observing that
Ry (−β)Rz (−α) = (Rz (α)Ry (β))T ,
the overall rotation matrix is
⎡
(s2α + c2α c2β )cθ + c2α s2β sα cα s2β (1 − cϑ ) − cβ sϑ
R(ϑ, r) = ⎣ sα cα s2β (1 − cϑ ) + cβ sϑ (s2α c2β + c2α )cθ + s2α s2β
cα sβ cβ (1 − cϑ ) − sα sβ sϑ sα sβ cβ (1 − cϑ ) + cα sβ sϑ
⎤
cα sβ cβ (1 − cϑ ) + sα sβ sϑ
sα sβ cβ (1 − cϑ ) − cα sβ sϑ ⎦ .
s2β cϑ + c2β
