Solution Manual For Robotics Ling Planning And Control

User Manual:

Open the PDF directly: View PDF .
Page Count: 159

Download
Open PDF In Browser	View PDF

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 contained 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, London, 2009.
Solutions to analytical problems are developed by emphasizing the crucial
steps towards the solution. Some problems may be solved in diﬀerent 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 comR
R 1
(version 7.4) with Simulink
. The
puter implementations in Matlab
code (downloadable from www.springer.com/978-1-84628-641-4) is available 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 ﬁle with
.m extension, where the ﬁrst letter is an s, followed by the problem number,
e.g., s4 1.m is the ﬁle 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 ﬁle with .mdl extension, e.g.,
s3 21.mdl is the ﬁle 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 ﬁle where the ﬁrst letter is an i, followed
by the problem number, e.g., i3 21.m is the initialization ﬁle 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 ﬁle where the ﬁrst letter is
a p, followed by the problem number, e.g., p3 21.m is the ﬁle for plotting the
output variables of Problem 3.21.
Variable initialization and plot can be activated by double clicking respectively on the upper-left block and the lower-right block in the Simulink block
diagrams.
For problems requiring the simulation of two diﬀerent systems, two ﬁles
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 ﬁles for solving Problem 3.22 with
two diﬀerent algorithms; accordingly, the ﬁles for plotting output variables
have been named p3 22a.m and p3 22b.m.
The above ﬁles are supplemented by other function and script ﬁles which
are needed to solve the programming problems.
All the ﬁles 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 ﬁles of Problem
3.21.
Helpful comments are added to each ﬁle to describe its contents and functions. A readme.txt ﬁle 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
February 2009

Luigi Villani
Giuseppe Oriolo
Bruno Siciliano

Contents

2

Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1

3

Diﬀerential 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 ψ
R(φ) = ⎣ sϕ cψ + cϕ cϑ sψ
sϑ sψ

−cϕ sψ − sϕ cϑ cψ
−sϕ sψ + cϕ cϑ cψ
sϑ cψ

As for the inverse problem, given a rotation matrix
⎡
⎤
r11 r12 r13
R = ⎣ r21 r22 r23 ⎦ ,
r31 r32 r33

⎤
sϕ sϑ
−cϕ sϑ ⎦ .
cϑ

2

2 Kinematics

the set of Euler angles ZXZ is given by
ϕ = Atan2(r13 , −r23 )


2 + r2 , r
ϑ = Atan2
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)
⎡
cϕ+ψ −sϕ+ψ
R(φ) = ⎣ sϕ+ψ cϕ+ψ
0
0

becomes
⎤
0
0⎦
1

when ϑ = 0. Otherwise, if ϑ = π, then the matrix is
⎤
⎡
−cϕ−ψ −sϕ−ψ 0
0 ⎦.
R(φ) = ⎣ −sϕ−ψ cϕ−ψ
0
0
−1
From the elements [1, 2] and [2, 2] it is possible to compute only the sum or
diﬀerence 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
diﬀerence 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β
1 0 ⎦
Ry (β) = ⎣ 0
−sβ 0 cβ
⎡
⎤
cϑ −sϑ 0
Rz (ϑ) = ⎣ sϑ cϑ 0 ⎦ .
0
0
1
Taking the ﬁrst product gives

⎡

cα cβ
Rz (α)Ry (β) = ⎣ sα cβ
−sβ
The next product gives

⎡

cα cβ cϑ − sα sϑ
Rz (α)Ry (β)Rz (ϑ) = ⎣ sα cβ cϑ + cα sϑ
−sβ cϑ

−sα
cα
0

⎤
cα sβ
sα sβ ⎦ .
cβ

−cα cβ sϑ − sα cϑ
−sα cβ sϑ + cα cϑ
sβ sϑ

⎤
cα sβ
sα sβ ⎦ .
cβ

Then, by observing that
Ry (−β)Rz (−α) = (Rz (α)Ry (β))T ,
the overall rotation matrix is
⎡
(s2α + c2α c2β )cθ + c2α s2β
R(ϑ, r) = ⎣ sα cα s2β (1 − cϑ ) + cβ sϑ
cα sβ cβ (1 − cϑ ) − sα sβ sϑ

sα cα s2β (1 − cϑ ) − cβ sϑ

(s2α c2β + c2α )cθ + s2α s2β
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β

4

2 Kinematics

Recalling the relations
ry
sα = 
rx2 + ry2
sβ =


rx2 + ry2

rx
cα = 
rx2 + ry2
cβ = rz

rx2 + ry2 + rz2 = 1
and using the following identities:
s2α + c2α c2β = 1 − rx2
s2α c2β + c2α = 1 − ry2
leads to expression (2.25).
Solution to Problem 2.5
From the expression of R(ϑ, r) in (2.25), summing the elements on the diagonal gives
r11 + r22 + r33 = 1 + 2 cos ϑ
from which the angle is
ϑ = cos −1



r11 + r22 + r33 − 1
2


.

Then, taking suitable diﬀerences between the oﬀ-diagonal elements gives
r32 − r23 = 2rx sin ϑ
r13 − r31 = 2ry sin ϑ
r21 − r12 = 2rz sin ϑ
and thus

⎤
⎡
r − r23
1 ⎣ 32
r=
r13 − r31 ⎦ .
2 sin ϑ
r21 − r12

In the case sin ϑ = 0, if r11 + r22 + r33 = 3, then ϑ = 0; this means that no
rotation has occurred and r is arbitrary. Instead, if r11 + r22 + r33 = −1, then
ϑ = π and the expression of the rotation matrix becomes
⎤
⎡ 2
2rx rz
2rx − 1 2rx ry
R(π, r) = ⎣ 2rx ry 2ry2 − 1 2ry rz ⎦ .
2rx rz
2ry rz 2rz2 − 1

2 Kinematics

5

The three components of the unit vector can be computed by taking any row
or column; for instance, from the ﬁrst column it is
r11 + 1
2

rx = ±

r12
2rx
r13
rz =
.
2rx

ry =

However, if rx ≈ 0, then the computation of ry and rz is ill-conditioned. In
that case, it is better to use another column to compute either ry or rz , and
so forth.
Solution to Problem 2.6
With reference to (2.25), the quantities cϑ , ri sϑ and ri rj (1 − cϑ ) for i, j =
x, y, z can be respectively expressed as
cϑ = 2cos 2 (ϑ/2) − 1
= 2η 2 − 1
ri sϑ = 2ri sin (ϑ/2)cos (ϑ/2)
= 2η

i

ri rj (1 − cϑ ) = 2ri rj sin 2 (ϑ/2)
=2 i j
where (2.30) and (2.31) have been used. Hence, (2.33) follows.
Solution to Problem 2.7
Start observing that
R(ϑ, r)r = RT (ϑ, r)r = r
since r is the axis of the rotation described by R. Then, since  and r are
aligned, the result
R(η, ) = RT (η, ) = 
follows directly.
Solution to Problem 2.8
By taking the expressions of the diagonal elements of the matrix in (2.33),
the following equality holds:
1√
r11 + r22 + r33 + 1 = 6η 2 + 2(
2

2
x

+

2
y

+

2
z)

− 2,

6

2 Kinematics

and thus using the constraint in (2.32) gives (2.34).
By taking the expressions of the elements [2, 3] and [3, 2] of the matrix
in (2.33), together with the diagonal elements, the following equality holds:

√
1
1
sgn (r32 − r23 ) r11 − r22 − r33 + 1 = sgn (4η x ) 2( 2x − 2y − 2z − η 2 + 1),
2
2
and thus using again the constraint in (2.32) gives
√
1
sgn (r32 − r23 ) r11 − r22 − r33 + 1 = sgn (4η x )| x | =
2

x

where the assumption η ≥ 0 has been exploited. A similar argument can be
worked out to derive the expressions of y and z in (2.35).
Solution to Problem 2.9
Using the expression of the rotation matrix as a function of the unit quaternion
in (2.33), the product R1 R2 gives rise to a rotation matrix which is a function
of {η1 , 1 } and {η2 , 2 }. The diagonal elements of such matrix are
r11 = 2(η12 + 21x ) − 1 2(η22 + 22x ) − 1
+4( 1x 1y − η1 1z )( 2x 2y + η2 2z ) + 4(
r22 = 2(η12 +
+4(

1y 1z

r33 = 2(η12 +
+4(

2
1y )

− η1

2
1z )

1x 1z

−1

1x )(

−1

− η1

2(η22 +
2y 2z

2
2y )

1y )( 2x 2z

2
2z )

+ η1

1y )( 2x 2z

− η2

2y )

+ 4(

1x 1y

+ η1

1z )( 2x 2y

− η2

2z )

+ 4(

1y 1z

+ η1

1x )( 2y 2z

− η2

2x ).

−1

+ η2

2(η22 +

1x 1z

2x )

−1

+ η2

2y )

Then, compose these terms as follows:
r11 + r22 + r33 + 1 = 4

2 2
1x 2x

+2

+

2 2
1y 2y

1x 2x 1y

+ 21z 22z
2y + 2 1x 2x

1z 2z

+2

1y 2y 1z 2z )

−8η1 η2 (

1x 2x + 1y 2y + 1z 2z )
2
2
2
2
2x + 2y + 2z + (4η2
+12η12 η22 − 6η12 − 6η22 + 4.

+(4η12 − 2)

− 2)

2
1x

+

2
1y

+

2
1z

Using the constraint in (2.32) yields
1
(r11 + r22 + r33 + 1) = (η1 η2 − T1 2 )2
4
which, in view of (2.34), coincides with the square of the scalar part of the
quaternion product in (2.37).
A similar argument can be pursued to show the equivalence between the
vector part of the quaternion that can be extracted from the product R1 R2
using (2.33) and the vector part of the quaternion product in (2.37).

2 Kinematics

7

Fig. S2.1. Four-link closed-chain planar arm with frame assignment

Solution to Problem 2.10
The matrix

⎡
⎢
A01 = ⎣

⎤
R01

o01

0T

1

⎥
⎦

can be inverted as a block-partitioned matrix. In fact, by recalling that


−1  −1
A
−A−1 DB −1
A D
=
,
O
B −1
O B
expression (2.45) follows by observing that (R01 )−1 = R10 .
Solution to Problem 2.11
Joint 4 was selected as the cut joint. The link frames can be assigned as in
Fig. S2.1. With this choice, the Denavit-Hartenberg parameters are speciﬁed
in Table S2.1.
Table S2.1. DH parameters for the four-link closed-chain planar arm
Link
1
2
3
1
4

ai
a1
a2
0
a1
0

αi
0
0
π/2
−π/2
0

di
0
0
0
0
d4

ϑi
ϑ1
ϑ2
ϑ3
ϑ1
0

Notice that the parameters for Link 4 are all constant. For the ﬁrst two revolute joints, the homogeneous transformation matrix deﬁned in (2.52) has the

8

2 Kinematics

same structure as in (2.62), while for the third revolute joint, the homogeneous
transformation matrix is
⎤
⎡ 
c3 0 s3 0

⎢ s  0 −c3 0 ⎥
A23 (ϑ3 ) = ⎣ 3
⎦.
0 1
0
0
0 0
0
1
Therefore, the coordinate transformations for the two branches of the tree are
respectively:
⎡   
⎤
a1 c1 + a2 c1 2
c1 2 3 0 s1 2 3


⎢ s    0 −c1 2 3 a1 s1 + a2 s1 2 ⎥
A03 (q  ) = A01 A12 A23 = ⎣ 1 2 3
⎦
0
1
0
0
0
0
0
1
where q  = [ ϑ1

ϑ3 ]T , and
⎡ 
c1

s
⎢
A01 (q  ) = ⎣ 1
0
0

ϑ2

0 −s1
0
c1
−1
0
0
0

where q  = ϑ1 . To complete, the constant
the last link is
⎡
1 0 0

0 1 0
⎢
A34 = ⎣
0 0 1
0 0 0

⎤
a1 c1
a1 s1 ⎥
⎦
0
1

homogeneous transformation for
⎤
0
0 ⎥
⎦.
d4
1

With reference to (2.60), the orientation constraints are (ϑ3 1 = π)
z 03 (q  ) = z 01 (q  )

0

x0T
3 (q )x1 (q ) = −1

which give
s1 2 3 = −s1
c1 2 3 = −c1
and thus
ϑ2 + ϑ3 = π − ϑ1 + ϑ1 .

(S2.1)

On the other hand, the position constraints are
 
 0T  
0
x3 (q )
0

0

p3 (q ) − p1 (q ) =

0
(q
)
y 0T

3
which give
a1 cos (ϑ2 + ϑ3 ) + a2 cos ϑ3 − a1 cos (ϑ1 + ϑ2 + ϑ3 − ϑ1 ) = 0

2 Kinematics

9

Fig. S2.2. Cylindrical arm with frame assignment
Table S2.2. DH parameters for the cylindrical arm
Link
1
2
3

ai
0
0
0

αi
0
−π/2
0

di
0
d2
d3

ϑi
ϑ1
0
0

and, in view of (S2.1), it is
cos ϑ3 =

−a1 + a1 cos (ϑ1 − ϑ1 )
.
a2

(S2.2)

The solution to (S2.2) exists for any ϑ1 and ϑ1 provided that
a1 + a1 ≤ a2 .
Further, it is
s3 = ±


1 − c23

with c3 as in (S2.2). Therefore, it is
ϑ3 = Atan2(s3 , c3 )
ϑ2 = π − ϑ1 + ϑ1 − ϑ3 .
T

It follows that the vector of joint variables is q = [ ϑ1 ϑ1 ] . These joints
are natural candidates to be the actuated joints. The arm direct kinematics

can be computed as T 04 (q) = A03 (q)A34 where the expressions of ϑ2 and ϑ3
have to be substituted into the homogeneous transformation A03 .

10

2 Kinematics

Fig. S2.3. SCARA manipulator with frame assignment

Solution to Problem 2.12
The link frames can be assigned as in Fig. S2.2. With this choice, the DenavitHartenberg parameters are speciﬁed in Table S2.2.
The homogeneous transformation matrices (2.52) for the three joints are:
⎤
⎤
⎡
⎡
c1 −s1 0 0
1 0 0 0
c1 0 0 ⎥
⎢s
⎢0 0 1 0 ⎥
A12 (d2 ) = ⎣
A01 (ϑ1 ) = ⎣ 1
⎦
⎦
0
0
1 0
0 −1 0 d2
0
0
0 1
0 0 0 1
⎡
⎤
1 0 0 0
⎢0 1 0 0 ⎥
2
A3 (d3 ) = ⎣
⎦,
0 0 1 d3
0 0 0 1
and thus the arm direct kinematics is
⎡

c1
⎢ s1
0
0 1 2
T 3 (q) = A1 A2 A3 = ⎣
0
0

where q = [ ϑ1

d2

0 −s1
0
c1
−1
0
0
0

⎤
−d3 s1
d3 c1 ⎥
⎦
d2
1

d3 ]T .

Solution to Problem 2.13
The link frames can be assigned as in Fig. S2.3. Since the typical approach
to an object is from the top, it is reasonable to choose all the joint axes
pointing downwards. With this choice, the Denavit-Hartenberg parameters
are speciﬁed in Table S2.3.

2 Kinematics

11

Table S2.3. DH parameters for the SCARA manipulator
Link
1
2
3
4

ai
a1
a2
0
0

αi
0
0
0
0

di
0
0
d3
0

ϑi
ϑ1
ϑ2
0
ϑ4

The homogeneous transformation matrices (2.52) for the four joints are:
⎡

ci
⎢ si
i−1
Ai (ϑi ) = ⎣
0
0
⎡

1
⎢0
2
A3 (d3 ) = ⎣
0
0

0
1
0
0

−si
ci
0
0

⎤
0 0
0 0 ⎥
⎦
1 d3
0 1

0
0
1
0

⎤
a i ci
a i si ⎥
⎦
0
1

i = 1, 2
⎡

c4
⎢ s4
3
A4 (ϑ4 ) = ⎣
0
0

−s4
c4
0
0

0
0
1
0

⎤
0
0⎥
⎦,
0
1

and thus the manipulator direct kinematics is
⎡

c124
⎢ s124
0
0 1 2 3
T 4 (q) = A1 A2 A3 A4 = ⎣
0
0

−s124
c124
0
0

⎤
0 a1 c1 + a2 c12
0 a1 s1 + a2 s12 ⎥
⎦
1
d3
0
1

(S2.3)

where q = [ ϑ1 ϑ2 d3 ϑ4 ]T . It is worth noticing that the direct kinematics
of this structure can be conceptually derived from that of a three-link planar
arm in which a3 = 0, ϑ3 is replaced by ϑ4 , and the z coordinate is d3 .
Solution to Problem 2.14
The torso can be modelled as an anthropomorphic arm, corresponding to the
ﬁrst three DOFs. Therefore T 03 in (2.78) and (2.79) has the expression (2.66).
The constant matrices T tl and T tr can be computed in terms of the angle
β and of the lengths dl and dr of segments Ot Ol and Ot Or , being Ot , Ol and
Or the origins of frames t, l and r, respectively. In detail:
 t
 t


Rl ott,l
Rr ott,r
t
=
T tl =
T
,
r
0T
1
0T
1
⎡

where
ott,l

⎤
dl cβ
=⎣ 0 ⎦
dl sβ

⎡

ott,r

⎤
dr cβ
=⎣ 0 ⎦
−dr sβ

12

2 Kinematics

are the positions of the origins of frames l and r with respect to frame t and
⎤
⎤
⎡
⎡
0 −sβ cβ
0 s β cβ
Rtl = ⎣ 1
Rtr = ⎣ 1 0
0
0 ⎦
0 ⎦
0 cβ sβ
0 cβ −sβ
are the corresponding rotation matrices.
Finally, matrices T rrh = T llh can be computed using (2.72) with (2.77) in
place of (2.73).
Solution to Problem 2.15
Consider two sets of Euler angles ZYZ:
⎡
⎤
⎡
⎤
π/2
−π/2
φ1 = ⎣ π/2 ⎦
φ2 = ⎣ −π/2 ⎦ .
0
0
The corresponding rotation matrices are
⎡
⎡
⎤
0 −1 0
0
R(φ1 ) = ⎣ 0
0 1⎦
R(φ2 ) = ⎣ 0
−1 0 0
1

1
0
0

⎤
0
1⎦.
0

Composition of rotations with respect to the current frame gives
⎡
⎤
0 0 −1
0 ⎦
R(φ1 )R(φ2 ) = ⎣ 1 0
0 −1 0
to which the following two sets of Euler angles correspond as for (2.19) and
(2.20):
⎡
⎤
⎡
⎤
π
0
φb = ⎣ −π/2 ⎦ .
φa = ⎣ π/2 ⎦
−π/2
π/2
On the other hand, composition of rotations
gives
⎡
0
R(φ2 )R( φ1 ) = ⎣ −1
0

with respect to the ﬁxed frame
⎤
0 1
0 0⎦
−1 0

to which the following two sets of Euler angles correspond as for (2.19) and
(2.20):
⎡
⎤
⎡
⎤
0
π
φb = ⎣ −π/2 ⎦ .
φa = ⎣ π/2 ⎦
−π/2
π/2

2 Kinematics

13

It is evident that the two results diﬀer, and then it is not possible to commute
the order of rotations. Notice, also, that the rotation matrix resulting from
direct sum of the two given sets of angles is
R(φ1 + φ2 ) = I
to which the set of Euler angles φ = [ 0

0 0 ]T corresponds.

Solution to Problem 2.16
In view of the approximations cos (dφ) ≈ 1 and sin (dφ) ≈ dφ, the elementary
rotation matrices for inﬁnitesimal angles can be written as:
⎤ ⎡
⎤
⎡
1
0
0
1
0
0
Rx (dφx ) = ⎣ 0 cos (dφx ) −sin (dφx ) ⎦ ≈ ⎣ 0
1
−dφx ⎦
0 dφx
1
0 sin (dφx ) cos (dφx )
⎤ ⎡
⎤
⎡
1
0 dφy
cos (dφy ) 0 sin (dφy )
⎦≈⎣ 0
0
1
0
1
0 ⎦
Ry (dφy ) = ⎣
1
−sin (dφy ) 0 cos (dφy )
−dφy 0
⎡
⎤ ⎡
⎤
cos (dφz ) −sin (dφz ) 0
1
−dφz 0
1
0⎦.
Rz (dφz ) = ⎣ sin (dφz ) cos (dφz ) 0 ⎦ ≈ ⎣ dφz
0
0
1
0
0
1
Multiplying the ﬁrst two matrices gives
⎡

1
Rx (dφx )Ry (dφy ) ≈ ⎣ 0
−dφy

0
1
dφx

⎤
dφy
−dφx ⎦
1

where higher order terms have been neglected. Reversing the order of multiplication gives
⎡
⎤
1
0
dφy
1
−dφx ⎦ ,
Ry (dφy )Rx (dφx ) ≈ ⎣ 0
−dφy dφx
1
which shows that the rotation resulting from any two elementary rotations is
independent of the order of rotations when the angles of rotation are inﬁnitesimal.
Constructing the matrix of the three elementary rotations about coordinate axes for inﬁnitesimal angles gives
⎡
⎤
1
−dφz dφy
1
−dφx ⎦ .
R(dφx , dφy , dφz ) = ⎣ dφz
−dφy dφx
1

14

2 Kinematics

For another set of inﬁnitesimal angles, the rotation matrix can be formally
written as
⎡
⎤
1
−dφz dφy
R(dφx , dφy , dφz ) = ⎣ dφz
1
−dφx ⎦ .


−dφy dφx
1
Multiplying these two rotation matrices and neglecting higher order terms
gives
R(dφx , dφy , dφz )R(dφx , dφy , dφz ) =
⎡

⎤
dφy + dφy
1
−(dφz + dφz )
⎣ dφz + dφz
1
−(dφx + dφx ) ⎦


−(dφy + dφy )
dφx + dφx
1
= R(dφx + dφx , dφy + dφy , dφz + dφz ).

Solution to Problem 2.17
In order to ﬁnd the arm reachable workspace, it is worth considering all the
joint conﬁgurations giving a loss of mobility. These are the 23 = 8 conﬁgurations for the joint limits:
⎡
⎤
⎡
⎤
⎡
⎤
⎡
⎤
−π/3
−π/3
−π/3
−π/3
q A = ⎣ −2π/3 ⎦ q B = ⎣ −2π/3 ⎦ q C = ⎣ 2π/3 ⎦ q D = ⎣ 2π/3 ⎦
−π/2
π/2
−π/2
π/2
⎡
⎤
⎡
⎤
⎡
⎤
⎡
⎤
π/3
π/3
π/3
π/3
q E = ⎣ −2π/3 ⎦ q F = ⎣ −2π/3 ⎦ q G = ⎣ 2π/3 ⎦ q H = ⎣ 2π/3 ⎦ ,
−π/2
π/2
−π/2
π/2
the 4 conﬁgurations for ϑ2 = 0:
⎡
⎤
⎡
⎤
−π/3
−π/3
qI = ⎣ 0 ⎦ qJ = ⎣ 0 ⎦
−π/2
π/2
the 4 conﬁgurations for ϑ3 = 0:
⎡
⎡
⎤
⎤
−π/3
−π/3
q M = ⎣ −2π/3 ⎦ q N = ⎣ 2π/3 ⎦
0
0

qK

⎡

⎤
π/3
=⎣ 0 ⎦
−π/2

⎤
π/3
qL = ⎣ 0 ⎦ ,
π/2

⎡

⎡

⎤
π/3
q O = ⎣ −2π/3 ⎦
0

and the 2 conﬁgurations for both ϑ2 = 0 and ϑ3 = 0:
⎡
⎡
⎤
⎤
−π/3
π/3
qQ = ⎣ 0 ⎦ qR = ⎣ 0 ⎦ .
0
0

⎡

⎤
π/3
q P = ⎣ 2π/3 ⎦ ,
0

2 Kinematics
1

[m]

0.5

R

P
H

0
−0.5

15

A

E
D

M

−1
−0.5

Q

0

0.5
[m]

1

1.5

Fig. S2.4. Reachable workspace for a three-link planar arm

Starting from point A in Cartesian space corresponding to q A , it is necessary
to draw all the arcs connecting such point to all other points corresponding
to conﬁgurations that diﬀer only for one component, i.e., points B, C, E, I,
M . Repeating the procedure for each point leads to obtaining all the portions
of the requested workspace. It can be recognized that the external contour of
the area AM QRP HEDA delimits the reachable workspace, as illustrated in
Fig. S2.4.
Solution to Problem 2.18
When s3 = 0, the two cases c3 = 1 and c3 = −1 have to be considered.
In the case c3 = 1, the arm is outstretched with ϑ3 = 0. From (2.105)–
(2.108), it is



ϑ2,I = ϑ2,III = Atan2 pW z , p2W x + p2W y
(S2.4)



(S2.5)
ϑ2,II = ϑ2,IV = Atan2 pW z , − p2W x + p2W y .
Hence, if


p2W x + p2W y > 0, two diﬀerent solutions exist for ϑ2 . The corre-

sponding values of ϑ1 are those in (2.109) or (2.110). On the other hand, if

p2W x + p2W y = 0, i.e., pW x = pW y = 0, the admissibility of the solution requires that pW z = ±(a2 + a3 ). Hence, from (S2.4) and (S2.5) it is ϑ2 = ±π/2.
Moreover, in view of (2.109) and (2.110), an inﬁnity of solutions exists for ϑ1 .
In the case c3 = −1, the arm is retracted with ϑ3 = ±π. From (2.105)–
(2.108), it is



(S2.6)
ϑ2,I = ϑ2,III = Atan2 (a2 − a3 )pW z , (a2 − a3 ) p2W x + p2W y



ϑ2,II = ϑ2,IV = Atan2 (a2 − a3 )pW z , −(a2 − a3 ) p2W x + p2W y . (S2.7)
Hence, assuming that a2 = a3 , if


p2W x + p2W y > 0, two diﬀerent solutions

exist for ϑ2 . The corresponding values of ϑ1 are those in (2.109) or (2.110). On

16

2 Kinematics

the other hand, if


p2W x + p2W y = 0, i.e., pW x = pW y = 0, the admissibility

of the solution requires that pW z = ±|a2 − a3 |. Hence, from (S2.6) and (S2.7),
it is ϑ2 = ±π/2. As in previous case, in view of (2.109), (2.110), an inﬁnity
of solutions exists for ϑ1 . Notice that, if a2 = a3 , it is pW x = pW y = pW z = 0
necessarily. Therefore, an inﬁnity of solutions exists both for ϑ1 and ϑ2 .
Solution to Problem 2.19
From the direct kinematics as in Problem 2.12, the end-eﬀector position is
given by
px = −d3 s1
py = d3 c1
pz = d2 .
The ﬁrst joint variable can be computed from the ﬁrst two equations as
ϑ1 = Atan2(−px , py ).
Then, the third joint variable can be computed by squaring and summing
those same two equations leading to

d3 = p2x + p2y .
Finally, the second joint variables is
d2 = pz .

Solution to Problem 2.20
From the direct kinematics as in Problem 2.13, the end-eﬀector position is
given by
px = a1 c1 + a2 c12
py = a1 s1 + a2 s12
pz = d3 .
The ﬁrst two equations are the same as (2.91) and (2.92) for a three-link
planar arm. Then it is
p2x + p2y − a21 − a22
c2 =
2a1 a2
with −1 ≤ c2 ≤ 1 and
s2 = ±


1 − c22

2 Kinematics

17

where the positive sign corresponds to ϑ2 ∈ (0, π) and the negative sign corresponds to ϑ2 ∈ (−π, 0). The second joint variable is
ϑ2 = Atan2(s2 , c2 ).
The ﬁrst joint variable can be computed from
s1 =

(a1 + a2 c2 )py − a2 s2 px
p2x + p2y

c1 =

(a1 + a2 c2 )px + a2 s2 py
p2x + p2y

and thus
ϑ1 = Atan2(s1 , c1 ).
The end-eﬀector orientation can be expressed as
φ = ϑ1 + ϑ2 + ϑ4 ,
from which the fourth joint variable is
ϑ4 = φ − ϑ1 − ϑ2 .
Finally, the third joint variable is
d3 = pz .

3
Diﬀerential Kinematics and Statics

Solution to Problem 3.1
Let
R = [x

y

z]

T

where the unit vector triplet (x, y, z) forms a right-handed frame. Then, the
product RS(ω)RT can be written as
⎡ T⎤
x
RS(ω)RT = ⎣ y T ⎦ S(ω) [ x y z ]
zT
⎤
⎡ T
x S(ω)x xT S(ω)y xT S(ω)z
= ⎣ y T S(ω)x y T S(ω)y y T S(ω)z ⎦
z T S(ω)x z T S(ω)y z T S(ω)z
⎤
⎡ T
x (ω × x) xT (ω × y) xT (ω × z)
= ⎣ y T (ω × x) y T (ω × y) y T (ω × z) ⎦ .
z T (ω × x) z T (ω × y) z T (ω × z)
In view of the properties of scalar triple products, this matrix is skewsymmetric and then
⎤
⎡
ωy
0
−ωz
RS(ω)RT = S(ω ) = ⎣ ωz
(S3.1)
0
−ωx ⎦


−ωy ωx
0
where the elements of the previous matrix can be written as
ωx = z T (ω × y) = ωT (y × z) = ω T x
ωy = xT (ω × z) = ω T (z × x) = ω T y
ωz = y T (ω × x) = ω T (x × y) = ω T z.

20

3 Diﬀerential Kinematics and Statics

Hence, it is
ω  = [ ωx ωy ωz ]

T
= ωT [ x y z ]
T

= Rω.
Substituting ω in (S3.1) leads to conclude
RS(ω)RT = S(Rω).

Solution to Problem 3.2
For the cylindrical arm in Fig. 2.35, the Jacobian is


z 0 × (p − p0 ) z 1 z 2
J(q) =
z0
0 0
where the various vectors can be computed from arm direct kinematics
⎤
⎡ ⎤
⎡
0
−d3 s1
p0 = ⎣ 0 ⎦
p = ⎣ d3 c1 ⎦
d2
0
⎡ ⎤
⎡
⎤
0
−s1
z0 = z 1 = ⎣ 0 ⎦
z 2 = ⎣ c1 ⎦ .
0
1
Then, the expression of the geometric Jacobian is
⎡

−d3 c1
⎢ −d3 s1
⎢
⎢ 0
J =⎢
⎢ 0
⎣
0
1

0
0
1
0
0
0

⎤
−s1
c1 ⎥
⎥
0 ⎥
⎥
0 ⎥
⎦
0
0

(S3.2)

which reveals that it is inherently impossible to rotate about axes x and y.
Solution to Problem 3.3
For the SCARA manipulator in Fig. 2.36, the Jacobian is


z 0 × (p − p0 ) z 1 × (p − p1 ) z 2 z 3 × (p − p3 )
J (q) =
z1
0
z3
z0

3 Diﬀerential Kinematics and Statics

21

where the various vectors can be computed from manipulator direct kinematics
⎤
⎤
⎡
⎤
⎡ ⎤
⎡
⎡
a1 c1 + a2 c12
0
a 1 c1
a1 c1 + a2 c12
p0 = ⎣ 0 ⎦ p1 = ⎣ a1 s1 ⎦ p3 = ⎣ a1 s1 + a2 s12 ⎦ p = ⎣ a1 s1 + a2 s12 ⎦
0
0
d3
d3
⎡ ⎤
0
z 0 = z1 = z 2 = z 3 = ⎣ 0 ⎦ .
1
Then, the expression of the geometric Jacobian is
⎡
−a1 s1 − a2 s12 −a2 s12
a2 c12
⎢ a1 c1 + a2 c12
⎢
0
0
⎢
J =⎢
0
0
⎢
⎣
0
0
1
1

0
0
1
0
0
0

⎤
0
0⎥
⎥
0⎥
⎥
0⎥
⎦
0
1

which reveals that it is inherently impossible to rotate about axes x and y.
In view of this, if a four-dimensional operational space (r = 4) is of concern,
then the (4 × 4) analytical Jacobian can be extracted from J by eliminating
rows 4 and 5, i.e.,
⎡
⎤
−a1 s1 − a2 s12 −a2 s12 0 0
a2 c12 0 0 ⎥
⎢ a c + a2 c12
JA = ⎣ 1 1
(S3.3)
⎦.
0
0
1 0
1
1
0 1

Solution to Problem 3.4
From the expression of the geometric Jacobian in (3.35), it is possible to
extract the analytical Jacobian by eliminating the three null rows, i.e.,
⎡
⎤
−a1 s1 − a2 s12 − a3 s123 −a2 s12 − a3 s123 −a3 s123
a2 c12 + a3 c123
a3 c123 ⎦ .
J A = ⎣ a1 c1 + a2 c12 + a3 c123
1
1
1
In order to compute its determinant, it is worth subtracting the second column
from the ﬁrst one and the third column from the second one, respectively,
giving
⎤
⎡
−a1 s1 −a2 s12 −a3 s123
a2 c12
a3 c123 ⎦
J A = ⎣ a1 c1
0
0
1
whose determinant is

det(J A ) = a1 a2 s2 .

22

3 Diﬀerential Kinematics and Statics

It follows that, for a1 , a2 = 0, the determinant vanishes whenever
ϑ2 = 0

ϑ2 = π,

that are the same singularities of the two-link planar arm. It may be veriﬁed
that the rank of the Jacobian does not further decrease, and thus the arm has
no other singularities.
Solution to Problem 3.5
For the spherical arm in Fig. 2.22, the Jacobian is


z 0 × (p − p0 ) z 1 × (p − p1 ) z 2
J (q) =
z0
z1
0
where the various vectors can be computed from arm direct kinematics
⎤
⎡ ⎤
⎡
0
c1 s2 d3 − s1 d2
p0 = p1 = ⎣ 0 ⎦ p = ⎣ s1 s2 d3 + c1 d2 ⎦
c2 d3
0
⎤
⎤
⎡ ⎤
⎡
⎡
0
−s1
c1 s 2
z 0 = ⎣ 0 ⎦ z 1 = ⎣ c1 ⎦ z 2 = ⎣ s1 s2 ⎦ .
0
c2
1
Then, the expression of the geometric Jacobian is
⎤
⎡
−s1 s2 d3 − c1 d2 c1 c2 d3 c1 s2
⎢ c1 s2 d3 − s1 d2 s1 c2 d3 s1 s2 ⎥
⎥
⎢
0
−s2 d3
c2 ⎥
⎢
J =⎢
⎥.
0
−s1
0 ⎥
⎢
⎦
⎣
0
0
c1
1
0
0
With three DOFs, it is worth considering
corresponding to the ﬁrst three rows of the
⎡
−s1 s2 d3 − c1 d2
J P = ⎣ c1 s2 d3 − s1 d2
0

end-eﬀector linear velocity only,
Jacobian, i.e.,
⎤
c1 c2 d3 c1 s2
s1 c2 d3 s1 s2 ⎦
−s2 d3
c2

which could have been also obtained by diﬀerentiating the vector p above
with respect to the joint variable vector q (analytical Jacobian). In order to
determine singularities of J P , its determinant has to be computed, giving
det(J P ) = −d23 s2 .
This vanishes if s2 = 0 and/or d3 = 0. The ﬁrst situation occurs whenever
ϑ2 = 0

ϑ2 = π,

3 Diﬀerential Kinematics and Statics

23

whereas the second situation occurs whenever
d3 = 0,
i.e., when Frame 3 coincides with Frame 2 and thus Joint 2 velocity does not
contribute to end-eﬀector linear velocity. Finally, it is worth observing that
both types of singularities are internal singularities.
Solution to Problem 3.6
From the geometric Jacobian in (S3.2), the Jacobian relative to end-eﬀector
linear velocity can be extracted by considering only the ﬁrst three rows, i.e.,
⎤
⎡
−d3 c1 0 −s1
J P = ⎣ −d3 s1 0 c1 ⎦ .
0
1
0
Its determinant is
det(J P ) = d3
which vanishes at the singularity
d3 = 0.
This occurs when the end eﬀector is located along Joint 1 axis, and thus this
singularity is conceptually similar to the shoulder singularity of an anthropomorphic arm.
Solution to Problem 3.7
With reference to the analytical Jacobian in (S3.3), it is worth subtracting
the second column from the ﬁrst one. The modiﬁed Jacobian becomes
⎡
⎤
−a1 s1 −a2 s12 0 0
a2 c12 0 0 ⎥
⎢ a c
J A = ⎣ 1 1
⎦,
0
0
1 0
0
0
0 1
whose determinant is

det(J A ) = a1 a2 s2 .

For a1 , a2 = 0, the determinant vanishes whenever
ϑ2 = 0

ϑ2 = π

which are the same singularities of the two-link and three-link planar arms.
In other words, the addition of a prismatic joint to a three-link planar arm
—which makes it become a SCARA manipulator— does not introduce further
singularities.

24

3 Diﬀerential Kinematics and Statics

Solution to Problem 3.8
The singular values of J are deﬁned as the square roots of the eigenvalues of
the matrix JJ T . On the other hand, the determinant of a matrix is given by
the product of its eigenvalues, and thus the manipulability measure in (3.56)
is given by the product of the singular values of the Jacobian matrix.
Solution to Problem 3.9
Let R be the radius of the circular object. The expression of the minimum
distance of the arm from the obstacle can be written as
d = min(di − R)
di

i = 1, 2, 3,

(S3.4)

where di denotes the minimum distance of Link i from the centre o of the
obstacle. Let then pi = [ pix piy ]T denote the position vector of the point
along Link i which is closest to o. Link i lies along a line, whose equation can
be written in the parametric form
pix = xi−1 + si (xi − xi−1 )
piy = yi−1 + si (yi − yi−1 )

(S3.5)
(S3.6)

where (xi−1 , yi−1 ) and (xi , yi ) are the coordinates of the origins of Link i − 1
and Link i frames, respectively; obviously, the parameter si varies in the
range (0, 1). Therefore, computation of the minimum distance is given by the
solution to a minimization problem as a function of si , i.e.,
min (pix − xo )2 + (piy − yo )2
si

0 ≤ si ≤ 1

where (xo , yo ) are the coordinates of o. Using (S3.5) and (S3.6), and taking
the derivative with respect to si of the above function leads to the solution
si = −

(xi−1 − xo )(xi − xi−1 ) + (yi−1 − yo )(yi − yi−1 )
a2i

(S3.7)

where a2i = (xi − xi−1 )2 + (yi − yi−1 )2 . For the point pi to belong to Link i,
the parameter shall be subject to the constraint


(S3.8)
si = max 0, min{1, si } ,
which then allows computing pix and piy as in (S3.5), (S3.6). In sum, the
following operating procedure can be established.
Execute steps a and b for i = 1, 2, 3:
a. Compute si as in (S3.7) and si as in (S3.8).
b. Compute pi as in (S3.5), and

di = (pix − xo )2 + (piy − yo )2 .

3 Diﬀerential Kinematics and Statics

25

To complete, compute d as in (S3.4).
Solution to Problem 3.10
The problem is that to invert diﬀerential kinematics v e = J q̇ by tolerating a
ﬁnite error , i.e.,
v e − J q̇ = .
The solution can be obtained by minimizing the cost functional
g  (q̇, ) =

1 2 T
1
k q̇ q̇ + T .
2
2

To cast the problem as a classic constrained optimization problem, let
 
q̇
w=
A = [J I ].

With these positions, the above diﬀerential kinematics constraint can be
rewritten as
v e − Aw = 0,
while the cost functional becomes
g  (w) =


where
Q=

1 T
w Qw
2

k2 I
O

O
I



is a positive deﬁnite weighting matrix. By using the method of Lagrangian
multipliers, the modiﬁed cost functional can be written as
g  (w, λ) =

1 T
w Qw + λT (v e − Aw)
2

where λ is an (r × 1) vector. The optimal solution is conceptually similar
to (3.50), i.e.,
w = Q−1 AT (AQ−1 AT )−1 v e .
In view of the above expressions of w, A and Q, the solution can be formally
written after simple algebraic manipulation as
   T 
−1
q̇
J
T
2
+
k
I
ve.
J
J
=
k2 I

It follows that the joint velocity is given by

−1
q̇ = J T JJ T + k 2 I
ve

26

3 Diﬀerential Kinematics and Statics

where

J  = J T (J J T + k 2 I)−1

is the damped least-squares inverse of J , whereas the error is given by

−1
 = k2 J J T + k2 I
ve .
The damping factor k establishes the relative weight between the kinematic
constraint v e = J q̇ and the minimum norm joint velocity requirement. In
the neighbourhood of a singularity, k is to be chosen large enough so as to
render diﬀerential kinematics inversion well conditioned, whereas far from
singularities, k can be chosen small (even k = 0) so as to guarantee accurate
diﬀerential kinematics inversion.
Solution to Problem 3.11
From (3.6), it is
S(ω) = Ṙ(φ)RT (φ)
with R(φ) as in (2.18). Taking the required derivatives gives
S(ω) = Ṙz (ϕ)Ry (ϑ)Rz (ψ)RTz (ψ)RTy (ϑ)RTz (ϕ)
+Rz (ϕ)Ṙy (ϑ)Rz (ψ)RTz (ψ)RTy (ϑ)RTz (ϕ)
+Rz (ϕ)Ry (ϑ)Ṙz (ψ)RTz (ψ)RTy (ϑ)RTz (ϕ).
Then, in view of (2.4), (3.8), (3.11), the previous expression can be simpliﬁed
into
S(ω) = S(ϕ̇z)
+Rz (ϕ)S(ϑ̇y  )RTz (ϕ)
+Rz (ϕ)Ry (ϑ)S(ψ̇z  )RTy (ϑ)RTz (ϕ)
= S(ϕ̇z) + S(ϑ̇Rz (ϕ)y  ) + S(ψ̇Rz (ϕ)Ry (ϑ)z  )
and then
ω = [ z Rz (ϕ)y  Rz (ϕ)Ry (ϑ)z  ] φ̇
⎤
⎡
0 −sϕ cϕ sϑ
= ⎣ 0 cϕ sϕ sϑ ⎦ φ̇.
1
0
cϑ
Solution to Problem 3.12
From (2.21), the contributions to the angular velocity of the derivatives of the
three Roll–Pitch–Yaw angles can be written as:
⎡ ⎤
⎡ ⎤
⎡ ⎤
0
1
0


ϕ̇ = ϕ̇ ⎣ 0 ⎦
ψ̇ = ψ̇ ⎣ 0 ⎦ ,
ϑ̇ = ϑ̇ ⎣ 1 ⎦
0
0
1

3 Diﬀerential Kinematics and Statics

27

where the superscripts indicate that rotations occur about axes of current
frames. Then, expressing the last two vectors with respect to the reference
frame gives
⎤
⎡
−sϕ

ϑ̇ = Rz (ϕ)ϑ̇ = ϑ̇ ⎣ cϕ ⎦
0
⎤
⎡
cϕ cϑ

ψ̇ = Rz (ϕ)Ry (ϑ)ψ̇ = ψ̇ ⎣ sϕ cϑ ⎦ .
−sϑ
Summing the three contributions leads to
ω = ϕ̇ + ϑ̇ + ψ̇
⎡
⎤
0 −sϕ cϕ cϑ
= ⎣ 0 cϕ sϕ cϑ ⎦ φ̇,
1
0
−sϑ
⎡

from which it is

0 −sϕ
T (φ) = ⎣ 0 cϕ
1
0

⎤
cϕ cϑ
sϕ cϑ ⎦ .
−sϑ

The determinant of matrix T is −cϑ , which implies that the relationship
cannot be inverted for ϑ = −π/2, π/2. These are representation singularities of
φ; notice also that solutions (2.22) and (2.23) degenerate at such singularities.
Solution to Problem 3.13
The contributions to the angular velocity of the derivatives of the three Euler
angles can be written as:
ϕ̇ = ϕ̇eϕ



ϑ̇ = ϑ̇eϑ



ψ̇ = ψ̇eψ ,

where eϕ , eϑ and eψ are the constant unit vectors of the axes of the current
frames, which depend on the particular triplet of Euler angles. The superscripts indicate that vectors are expressed in the current frames. In the case
ϕ = ϑ = ψ = 0 the current frames coincide and the three contributions can
be added, i.e.,


ω = ϕ̇ + ϑ̇ + ψ̇ = T (0)φ̇
with T (0) = [ eϕ

eϑ

eψ ]. Therefore, with the choice
⎡ ⎤
⎡ ⎤
⎡ ⎤
1
0
0
eϕ = ⎣ 0 ⎦
eϑ = ⎣ 1 ⎦
eψ = ⎣ 0 ⎦ ,
0
0
1

i.e., for XYZ angles, it is T (0) = I.

28

3 Diﬀerential Kinematics and Statics

Fig. S3.1. Block scheme of the inverse kinematics algorithm for arm position

Solution to Problem 3.14
With reference to a 6-DOFs manipulator having a spherical wrist, let q P =
[ q1 q2 q3 ]T denote the vector of joint variables determining arm position
and q O = [ ϑ4 ϑ5 ϑ6 ]T the vector of joint variables determining wrist orientation. For such manipulator, it is possible to solve inverse kinematics in
two stages; namely, ﬁrst solve for q P , and then solve for q O .
Let pd (t) and Rd (t) = [ nd (t) sd (t) ad (t) ] be the desired time history
of end-eﬀector position and orientation. From (2.93), it is possible to compute
the desired time behaviour of wrist position as
pW d = pd − d6 ad .
Taking time derivative of both sides gives
ṗW d = ṗd − d6 ω d × ad
where the time derivative of the unit vector ad (constant norm) has been
expressed as ȧd = ω d × ad .
By using the block-partitioned form of the Jacobian in (3.42) with p =
pW , the diﬀerential kinematics equation for arm linear velocity is
ṗW = J P P (q P )q̇ P
where J P P formally coincides with J 11 evaluated for p = pW . In view of this,
the joint velocity solution of kind (3.70) can be written as
q̇ P = J −1
P P (q P )(ṗW d + K P eP W )
where eP W = pW d − pW and K P is a positive deﬁnite matrix. The resulting
block scheme is shown in Fig. S3.1.
Once the time behaviour of q P and q̇ P has been computed in the ﬁrst
stage, the diﬀerential kinematics equation for wrist angular velocity is
ω = J OP (q P )q̇ P + J OO (q P , q O )q̇ O

3 Diﬀerential Kinematics and Statics

29

Fig. S3.2. Block scheme of the inverse kinematics algorithm for wrist orientation

where J OP and J OO respectively denote the Jacobians J 21 and J 22 in (3.42)
evaluated for p = pW . In view of this, the joint velocity solution of kind (3.70)
can be written as
q̇ O = J −1
OO (q P , q O ) ω d − J OP (q P )q̇ P + K O eO
where eO is the orientation error in (3.85) and K O is a positive deﬁnite matrix.
The resulting block scheme for the second stage is shown in Fig. S3.2.
It is anticipated that the two-stage algorithm leads to a computational
savings with respect to the algorithm based on the inverse of the full Jacobian.
Solution to Problem 3.15
In the case ẋd = 0, the time derivative of the Lyapunov function is given as
in (3.77)
V̇ (e) = eT K ẋd − eT KJ A (q)J TA (q)Ke
where the sign of the ﬁrst term is indeﬁnite, and the second term is less than
or equal to zero. This function can be upper bounded as
V̇ (e) ≤ |eT K ẋd | − J TA Ke 2 .
In the worst case, it is necessary to take the largest value of the ﬁrst term and
the smallest value of the second term, leading to
V̇ (e) ≤ e λmax (K) ẋd

max

− σr2 (J A )λ2min (K) e 2 ,

where λmax (K) (λmin (K)) denotes the maximum (minimum) eigenvalue of K,
ẋd max denotes the maximum end-eﬀector velocity, and σr (J A ) denotes the
minimum singular value of J A . The quadratic term in the error prevails over
the linear term as long as
e ≥

ẋd max λmax (K)
σr2 (J A )λ2min (K)

30

3 Diﬀerential Kinematics and Statics

and V̇ (e) ≤ 0. It follows that an upper bound on the error is given by
e

max

=

ẋd max
kσr2 (J A )

where K has been conveniently chosen as a diagonal matrix K = kI.
Solution to Problem 3.16
The matrix product Rd RT gives
R RT =
⎡d
ndx nx + sdx sx + adx ax
⎣ ndy nx + sdy sx + ady ax
ndz nx + sdz sx + adz ax

ndx ny + sdx sy + adx ay
ndy ny + sdy sy + ady ay
ndz ny + sdz sy + adz ay

⎤
ndx nz + sdx sz + adx az
ndy nz + sdy sz + ady az ⎦ .
ndz nz + sdz sz + adz az

In view of (3.84), subtracting the element [1, 2] from the element [2, 1] and
equating the diﬀerence to 2rz sϑ as in (2.28) gives
2rz sϑ = (ndy nx − ndx ny ) + (sdy sx − sdx sy ) + (ady ax − adx ay ),
and thus

1
(n × nd )z + (s × sd )z + (a × ad )z
2
where the subscripts z denote the z-components of the relevant vectors. In a
similar way, subtracting the elements [3, 1] from the respective elements [1, 3]
and equating the diﬀerence to 2ry sϑ gives
rz sϑ =

ry sϑ =

1
(n × nd )y + (s × sd )y + (a × ad )y .
2

Yet, subtracting the elements [2, 3] from the respective elements [3, 2] and
equating the diﬀerence to 2rx sϑ gives
1
(n × nd )x + (s × sd )x + (a × ad )x .
2
In turn, grouping the previous three expressions leads to
rx sϑ =

r sin ϑ =

1
(n × nd + s × sd + a × ad )
2

and, in view of the deﬁnition of orientation error as in (3.83), the result in
(3.85) follows.
Solution to Problem 3.17
The expression of the orientation error from (3.85) is
eO =

1
(n × nd + s × sd + a × ad ) .
2

3 Diﬀerential Kinematics and Statics

31

Taking the time derivative of the ﬁrst term on the right-hand side gives
d
(n × nd ) = ṅ × nd + n × ṅd
dt
= −S(nd )ṅ + S(n)ṅd .
Expressing the time derivatives of the unit vectors as
ṅ = ω × n = −S(n)ω
ṅd = ω d × nd = −S(nd )ω d
leads to

d
(n × nd ) = S(nd )S(n)ω − S(n)S(nd )ω d .
dt
Proceeding in a similar way for the other two terms on the right-hand side of
(3.85) yields

d
(s × sd ) = S(sd )S(s)ω − S(s)S(sd )ω d
dt
d
(a × ad ) = S(ad )S(a)ω − S(a)S(ad )ω d .
dt
In turn, grouping the previous three contributions to the time derivative of
the orientation error gives
1
S(n)S(nd ) + S(s)S(sd ) + S(a)S(ad ) ωd
2
1
+ S(nd )S(n) + S(sd )S(s) + S(ad )S(a) ω
2
and, in view of the position (3.87), the result in (3.86) follows.
ėO = −

Solution to Problem 3.18
It is not diﬃcult to see that, by using the skew-symmetric operator S(·), (2.33)
can be rewritten in compact form as
R(η, ) = (2η 2 − 1)I + 2T + 2ηS().

(S3.9)

Computing the time derivative of (S3.9) gives
˙ T + 2˙T + 2η̇S() + 2ηS().
˙
Ṙ(η, ) = 4η̇ηI + 2

(S3.10)

Substituting (S3.9) and (S3.10) into (3.6) gives:
S(ω) = ṘRT
= 4η̇η(2η 2 − 1)I + 8η̇ηT − 8η̇η 2 S()
˙ T + 4
˙ T T − 4η 
˙ T S()
+2(2η 2 − 1)
+2(2η 2 − 1)˙T + 4˙T T − 4η˙T S()
+2η̇(2η 2 − 1)S() + 4η̇S()T − 4η̇ηS()S()
˙ + 4ηS()
˙ T − 4η 2 S()S().
˙
+2η̇(2η 2 − 1)S()

(S3.11)

32

3 Diﬀerential Kinematics and Statics
unconstrained
solution

time

constrained
solution
constraint
velocity
e_p

Jacobian pseudo-inverse
with constraint

[T,dp_d]
[T,p_d]

dq

dq_0

+
-

K_p

+
+

0.001z

dag(J_p(q))dp + P(q)dq_0

q

z-1

direct
kinematics

sinusoidal
functions

k_p(q)

cs(q)

plot

Fig. S3.3. Simulink block diagram of closed-loop Jacobian pseudo-inverse algorithm with constraint

Next, taking the time derivative of the constraint in (2.32) gives
T ˙ = ˙ T  = −η η̇.

(S3.12)

Then, using (2.32), (S3.12) and the property S(x)S(y) = yxT − xT yI yields
the following equalities:
˙ T
˙ T T = (1 − η 2 )
˙T T = −η̇ηT
S()S() = T − (1 − η 2 )I
˙
S()S()
= ˙ T + η̇ηI
T
˙
− η̇ηI)S()
˙ S() = (S()S()
˙ T = S()(S()S()
˙
S()
+ (1 − η 2 )I).
By virtue of the above equalities and observing that S() = 0, (S3.11) becomes
˙ − 2η̇S()
˙ T − ˙ T ) + 2ηS()
S(ω) = 2(
which, in view of the property S(S(x)y) = yxT − xy T , can be written in the
form
˙ + 2ηS()
˙ − 2η̇S().
S(ω) = 2S(S())
Finally, the angular velocity can be computed as
ω = 2S()˙ + 2η ˙ − 2η̇.

(S3.13)

3 Diﬀerential Kinematics and Statics
joint pos

joint vel

2
0

0

[rad/s]

[rad]

2

3
3

1

1

−0.2

−1
2

−2
0

0.5

1

1.5

2

1
−0.4
0

2.5

0.5

1

−6

pos error norm

2.5

2

2.5

distance

5

0.1

4

0.05

3

0

2

−0.05

1

−0.1

0
0

2

0.15

[m]

[m]

x 10

1.5
[s]

[s]
6

33

0.5

1

1.5

2

2.5

0

0.5

1

[s]

1.5
[s]

Fig. S3.4. Time history of the joint positions and velocities, of the norm of endeﬀector position error, and of the distance from the obstacle in the unconstrained
case

Solution to Problem 3.19
Multiplying both sides of (S3.13) by T and using (S3.12), (2.32) yields
T ω = 2ηT ˙ − 2η̇T  = −2η̇
and thus

1
η̇ = − T ω.
2
Multiplying both sides of (S3.13) by S() yields
˙
S()ω = 2S()S()˙ + 2ηS(),

which, by virtue of (S3.12), (2.32), (S3.13) and the property S(x)S(y) =
yxT − xT yI, can be rewritten in the form
S()ω = −2η̇η − 2(1 − η 2 )˙ + η(ω − 2η ˙ + 2η̇)
= −2˙ + ηω
and thus
˙ =

1
(ηI − S()) ω.
2

34

3 Diﬀerential Kinematics and Statics
joint pos

joint vel
5

1

1

0

3

[rad/s]

[rad]

2
1

2

0
3

−1

2

−2
0

0.5

1

1.5

2

−5
0

2.5

0.5

1

−6

x 10

pos error norm

2.5

2

2.5

distance

5

0.1

4

0.05

3

0

2

−0.05

1

−0.1

0
0

2

0.15

[m]

[m]

6

1.5
[s]

[s]

0.5

1

1.5

2

2.5

0

0.5

1

[s]

1.5
[s]

Fig. S3.5. Time history of the joint positions and velocities, of the norm of endeﬀector position error, and of the distance from the obstacle in the constrained case

Solution to Problem 3.20
Diﬀerentiating (3.96) with respect to time gives
V̇ = 2(ηd − η)(η̇d − η̇) + 2(d − )T (˙ d − ˙ )
= ηTd − ηd T − S(d ) (ω d − ω)
where the quaternion propagation in (3.94), (3.95), and the property S() =
0 have been exploited. The result in (3.97) follows by using (3.91) and (3.93).
Solution to Problem 3.21
From the given pd (0), the initial posture of the arm q(0) is computed
according to the formulæ in Sect. 2.12.1 (elbow-up) with an arbitrary value
of φ, e.g., φ = π/4. The desired trajectory regards the vertical component of
end-eﬀector position and is chosen as
pdy (t) = pdy (0) + 0.5 1 − cos (0.5πt) pdy (2) − pdy (0)

0≤t≤2

with pdy (0) = 0.2 and pdy (2) = −0.2. The desired velocity is found by diﬀerentiation of the above trajectory. As for the constraint, the minimum distance
of the arm from the obstacle (3.58) is computed according to the solution to

3 Diﬀerential Kinematics and Statics

35
time

variables
initialization

e

[T,dx_d]
[T,x_d]

dq

Jacobian
inverse
+

+
-

K

direct
kinematics

+

inv(J_a(q))dx

0.001z

q

z-1

sinusoidal
functions
cs(q)

k(q)

plot

Fig. S3.6. Simulink block diagram of closed-loop Jacobian inverse algorithm

Problem 3.11. Its gradient is found by numerical diﬀerentiation with a step of
10−6 .
The inverse kinematics algorithm based on (3.72) is used, with the matrix
gain K = diag{500, 500} and q̇ 0 as in (3.55) with k0 = 35. The resulting
Simulink block diagram is shown in Fig. S3.3, where both the unconstrained
case (k0 = 0) and the constrained case can be run.
The ﬁles with the solution can be found in Folder 3 21.
The resulting joint positions and velocities, as well as the norm of endeﬀector position error and the distance from the obstacle, in the two cases,
are shown in Figs. S3.4 and S3.5, respectively.
It can be recognized that the position error is small along the trajectory
and tends to zero at steady state in both cases. In the unconstrained case, the
arm is seen to collide with the obstacle (negative distance). On the other hand,
in the constrained case, large initial values of joint velocities occur which are
needed to move the arm away from the obstacle.
Solution to Problem 3.22
From the given xd (0), the initial posture of the manipulator q(0) is computed
according to the formulæ in the solution to Problem 2.20 (elbow-up). The
desired trajectory for the end-eﬀector position is chosen as
xd (t) = xd (0) + 0.5 1 − cos (0.5πt) xdy (2) − xdy (0)

0≤t≤2

36

3 Diﬀerential Kinematics and Statics
joint pos

joint vel
2

2

1

1

4

0

3

1

1

3
[rad/s]

[rad]

3

0

−1

−3
0

0.5

1

1.5

2

−2
0

2.5

4

2

−1

2

−2

0.5

1

−6

2

x 10

1.5

2

2.5

2

2.5

[s]

[s]
pos error norm

−5

1.5

x 10

orien error

1
1.5
[rad]

[m]

0.5
1

0
−0.5

0.5
−1
0
0

0.5

1

1.5
[s]

2

2.5

−1.5
0

0.5

1

1.5
[s]

Fig. S3.7. Time history of the joint positions and velocities, of the norm of endeﬀector position error, and of the end-eﬀector orientation error with closed-loop
Jacobian inverse algorithm

with xd (0) = [ 0.7 0 0 0 ]T and xd (2) = [ 0 0.8 0.5 π/2 ]T . The desired velocity is found by diﬀerentiation of the above trajectory.
First, the inverse kinematics algorithm based on (3.70) is used where the
analytical Jacobian is given in (S3.3). The matrix gain is chosen as K =
diag{500, 500, 500, 100}. The resulting Simulink block diagram is shown in
Fig. S3.6.
The ﬁles with the solution can be found in Folder 3 22.
The resulting joint positions and velocities, as well as the norm of endeﬀector position error and the end-eﬀector orientation error, are shown in
Fig. S3.7. The tracking performance is satisfactory and the errors vanish at
steady state.
Next, the inverse kinematics algorithm based on (3.76) is used with the
same matrix gain as above. The resulting Simulink block diagram is shown
in Fig. S3.8.
The resulting joint positions and velocities, as well as the norm of endeﬀector position error and the end-eﬀector orientation error, are shown in
Fig. S3.9. It can be recognized that tracking errors slightly increase, but
steady-state performance remains satisfactory.

3 Diﬀerential Kinematics and Statics

37

time
variables
initialization

e

dq

Jacobian
transpose
[T,x_d]

+
-

K

direct
kinematics

tr(J_a(q))Ke

0.001z

q

z-1

sinusoidal
functions
cs(q)

k(q)

plot

Fig. S3.8. Simulink block diagram of closed-loop Jacobian transpose algorithm

Solution to Problem 3.23
The core of the quadratic form deﬁning the velocity manipulability ellipsoid
in (3.123) is given by the inverse of the core of the quadratic form deﬁning
the force manipulability ellipsoid in (3.126). Let A = J J T denote such core.
The eigenvalues λi and the eigenvectors ui of A satisfy the equation
Aui = λi ui .
Premultiplying both sides by A−1 and dividing by λi gives
A−1 ui =

1
ui ,
λi

which demonstrates the notable result that the directions of the principal axes
(eigenvectors) of the force and velocity manipulability ellipsoids coincide while
their dimensions (eigenvalues) are in inverse proportion.

38

3 Diﬀerential Kinematics and Statics
joint pos

joint vel
2

2

1

1

4

0

3

1

1

3
[rad/s]

[rad]

3

0

−1

−3
0

0.5

1

1.5

2

−2
0

2.5

4

2

−1

2

−2

0.5

1

−3

6

0.008

4

2

2.5

x 10

2

2.5

orien error

2

0.006

[rad]

[m]

pos error norm
0.01

0.004

0
−2

0.002
0
0

1.5
[s]

[s]

−4
0.5

1

1.5
[s]

2

2.5

−6
0

0.5

1

1.5
[s]

Fig. S3.9. Time history of the joint positions and velocities, of the norm of endeﬀector position error, and of the end-eﬀector orientation error with closed-loop
Jacobian transpose algorithm

4
Trajectory Planning

Solution to Problem 4.1
In order to satisfy initial and ﬁnal constraints on position, velocity and acceleration, a ﬁfth-order polynomial is needed
q(t) = a5 t5 + a4 t4 + a3 t3 + a2 t2 + a1 t + a0 .
The coeﬃcients can be determined by solving the following system of linear
equations:
a0 = qi
a1 = q̇i
2a2 = q̈i
a5 t5f + a4 t4f + a3 t3f + a2 t2f + a1 tf + a0 = qf
5a5 t4f + 4a4 t3f + 3a3 t2f + 2a2 tf + a1 = q̇f
20a5 t3f + 12a4 t2f + 6a3 tf + 2a2 = q̈f .
With the given data, the ﬁles used to compute the coeﬃcients and to generate
the time behaviour of position, velocity and acceleration can be found in Folder
4 1.
The coeﬃcients are:
15
45
9
a4 = −
a5 =
a0 = 1
a1 = 0
a2 = 0
a3 =
4
16
16
and the resulting trajectory is illustrated in Fig. S4.1.
Solution to Problem 4.2
Imposing velocity constraints yields
q̇i = 0
q̇f = k(1 − cos (atf )).

40

4 Trajectory Planning
pos

[rad]

4
3
2
1
0

0.5

1
[s]

1.5

2

1.5

2

1.5

2

vel
2.5
[rad/s]

2
1.5
1
0.5
0
−0.5
0

0.5

1
[s]
acc

[rad/s^2]

5

0

−5
0

0.5

1
[s]

Fig. S4.1. Time history of position, velocity and acceleration with a ﬁfth-order
polynomial timing law

The second equation implies that
0≤

q̇f
≤2
k

which is satisﬁed for all k as long as q̇f = 0. In that case, it is a = 2π/tf .
Then, integrating the given velocity proﬁle gives



2πt
tf
sin
q(t) = k1 + k t −
,
2π
tf
and imposing position constraints yields the two coeﬃcients
k1 = qi

4 Trajectory Planning

41

pos

[rad]

3
2
1
0
0

0.5

1
[s]

1.5

2

1.5

2

1.5

2

vel

[rad/s]

3
2
1
0
0

0.5

1
[s]
acc

[rad/s^2]

5

0

−5
0

0.5

1
[s]

Fig. S4.2. Time history of position, velocity and acceleration with a raised cosine
timing law

k=

qf − qi
.
tf

With the given data, the ﬁles used to compute the two coeﬃcients and to
generate the time behaviour of position, velocity and acceleration can be found
in Folder 4 2.
The coeﬃcients are:
k = 1.5
k1 = 0
and the resulting trajectory is illustrated in Fig. S4.2.

42

4 Trajectory Planning
pos

[rad]

3
2
1
0
0

1

2
[s]

3

4

3

4

3

4

vel
2

[rad/s]

1.5
1
0.5
0
−0.5
0

1

2
[s]
acc

3

[rad/s^2]

2
1
0
−1
−2
−3
0

1

2
[s]

Fig. S4.3. Time history of position, velocity and acceleration with two ﬁfth-order
interpolating polynomials

Solution to Problem 4.3
It is required to satisfy initial and ﬁnal constraints on position, velocity and
acceleration (6 equations), intermediate position constraints (2 equations),
and continuity of velocity and acceleration at the intermediate point (2 equations). Therefore, with two ﬁfth-order interpolating polynomials it is possible
to specify also velocity and acceleration at the intermediate point.
Let qm denote the intermediate point at time tm . A viable choice is to
compute the mean velocities in the two intervals
q̇a =

qm − qi
tm − ti

q̇b =

qf − qm
,
tf − tm

4 Trajectory Planning

43

and then to choose velocity at the intermediate point as
q̇m =

q̇a + q̇b
.
2

Likewise, for acceleration it is
q̈a =

q̇m − q̇i
tm − ti

q̈b =

q̇f − q̇m
tf − tm

and then

q̈a + q̈b
.
2
With the given data and null initial and ﬁnal velocities and accelerations, the
ﬁles used to generate the time behaviour of position, velocity and acceleration
can be found in the Folder 4 3.
The resulting trajectory is illustrated in Fig. S4.3.
q̈m =

Solution to Problem 4.4
By using (4.13), (4.14), (4.23), (4.16), (4.25), the time derivative of Πk (t) in
(4.27) can be written as
Π̈k (tk )
Π̈k+1 (tk+1 )
(tk+1 − t)2 +
(t − tk )2
2Δtk
2Δtk

Δtk 
qk+1 − qk
Π̈k (tk ) − Π̈k+1 (tk+1 )
+
+
Δtk
6

Π̇k (t) = −

(S4.1)
k = 1, . . . , N + 1

where q2 and qN +1 are unknown. Then, using (4.15), (4.24) leads to
Δtk + Δtk+1
Δtk+1
Δtk
Π̈k (tk ) +
Π̈k+1 (tk+1 ) +
Π̈k+2 (tk+2 ) (S4.2)
6
3
6


1
qk
1
qk+2
=
−
+
k = 1, . . . , N
qk+1 +
Δtk
Δtk+1
Δtk
Δtk+1
where Π̈1 (t1 ) = q̈i as in (4.19) and Π̈N +2 (tN +2 ) = q̈f as in (4.22).
Evaluating (S4.1) for k = 1 at t = t1 and applying (4.17), (4.18) yields
q2 = qi + q̇i Δt1 + q̈i

Δt21
Δt2
+ Π̈2 (t2 ) 1 .
3
6

(S4.3)

Likewise, evaluating (S4.1) for k = N + 1 at t = tN +2 and applying (4.20),
(4.21) yields
qN +1 = qf − q̇f ΔtN +1 + q̈f

Δt2N +1
Δt2
+ Π̈N +1 (tN +1 ) N +1 .
3
6

(S4.4)

In the case N > 3, substituting (S4.3) into (S4.2) for k = 1, 2 and (S4.4) into
(S4.2) for k = N − 1, N leads to writing the linear system of N equations into
N unknowns given in (4.28).

44

4 Trajectory Planning

The coeﬃcient matrix takes on the tridiagonal band structure
⎡

a11
⎢ a21
⎢ .
A=⎢
⎢ ..
⎣ 0
0
where

a12
a22
..
.
0
0

...
...
..
.

0
0
..
.

0
0
..
.

. . . aN −1,N −1
...
aN,N −1

⎧
Δt2
Δt21
Δt1
⎪
⎪
a11 =
⎪
+
+
⎪
⎪
2
3
6Δt2
⎪
⎪
⎪
⎪
⎪
Δtk + Δtk+1
⎪
⎪
akk =
⎪
⎪
3
⎪
⎪
⎪
⎪
⎪
Δt2N +1
⎪
ΔtN +1
ΔtN
⎪
⎪
a
+
+
=
N
N
⎪
⎪
3
2
6ΔtN
⎪
⎪
⎪
⎨
2
Δt1
Δt2
−
a21 =
⎪
6
6Δt
⎪
2
⎪
⎪
⎪
⎪
Δt
⎪
k
⎪
⎪ ak,k−1 =
⎪
⎪
6
⎪
⎪
⎪
⎪
⎪
Δt
k+1
⎪a
⎪
k,k+1 =
⎪
⎪
6
⎪
⎪
⎪
⎪
⎪
Δt2N +1
ΔtN
⎪
⎪
⎩ aN −1,N =
−
6
6ΔtN

aN −1,N
aN N

⎤
⎥
⎥
⎥
⎥
⎦

k = 2, . . . , N − 1

k = 3, . . . , N
k = 1, . . . , N − 2

which depend only on the given time intervals Δtk , k = 1, . . . , N + 1.
Further, the components of the vector b of known terms in (4.28) are:
⎧



1
Δt21
Δt1
q3 − qi
1
⎪
⎪
b1 =
−
+
q̇i Δt1 + q̈i
− q̈i
⎪
⎪
⎪
Δt
Δt
Δt
3
6
2
2
1
⎪
⎪
⎪




⎪
⎪
⎪
1
Δt2
1
1
q4
⎪
⎪
b2 =
+
qi + q̇i Δt1 + q̈i 1 −
q3 +
⎪
⎪
Δt
3
Δt
Δt
Δt
⎪
2
3
2
3
⎪
⎪


⎪
⎨
1
qk
1
qk+2
bk =
−
+
k = 3, . . . , N − 2
qk+1 +
Δtk
Δtk+1
Δtk
Δtk+1
⎪
⎪
⎪
⎪




⎪
⎪
Δt2N +1
⎪
qN −1
1
1
1
⎪
⎪
b
=
−
+
+
−
q̇
Δt
+
q̈
q
q
N −1
N
f
f
N +1
f
⎪
⎪
ΔtN −1
ΔtN
ΔtN −1
ΔtN
3
⎪
⎪
⎪
⎪



⎪
2
⎪
Δt
1
ΔtN +1
qN − qf
1
⎪
⎪
⎩ bN =
−
+
−q̇f ΔtN +1 + q̈f N +1 − q̈f
ΔtN
ΔtN +1
ΔtN
3
6
where the values qk are given for k = 1, 3, . . . , N, N + 2.

4 Trajectory Planning

45

On the other hand, in the case N = 3, the same expressions for the a’s
can be used; for the b’s, instead, b1 and b3 can be computed as above and b2
takes on the expression


 
1
Δt2
1
1
b2 =
+
qi + q̇i Δt1 + q̈i 1 −
q3
Δt2
3
Δt3
Δt2


Δt2
1
+
qf − q̇f Δt4 + q̈f 4 .
Δt3
3
Solution to Problem 4.5
The time instants of the two virtual points are chosen as t1 = 1 and t4 =
3. With the given data, the ﬁles used to compute the matrix A and the
vector b in (4.28), and to generate the time behaviour of position, velocity
and acceleration can be found in the Folder 4 5.
The matrix of coeﬃcients is
⎡
⎤
⎡
⎤
1 1/6 0
2
A = ⎣ 0 2/3 0 ⎦
b = ⎣ −1 ⎦ ,
0 1/6 1
−1
and the resulting trajectory is illustrated in Fig. S4.4.
Solution to Problem 4.6
The interpolating trajectory given by a sequence of linear polynomials with
parabolic blends can be analytically described as
⎧
Δtk+1
Δtk
⎪
⎪
⎨ ak1 (t − tk ) + ak0
≤ t < tk+1 −
tk +
2
2
q(t) =


⎪
Δt
Δt
⎪
k
k
⎩ bk2 (t − tk )2 + bk1 (t − tk ) + bk0
≤ t < tk +
.
tk −
2
2
The velocity in the linear segments is
⎧
q̇i
⎪
⎪
⎨ qk − qk−1
q̇k−1,k =
Δtk−1
⎪
⎪
⎩
q̇f

k=1
k = 2, . . . , N
k = N + 1,

and then the coeﬃcients of the linear polynomials are
ak0 = qk

ak1 = q̇k,k+1

k = 1, . . . , N − 1.

Imposing continuity of velocity between the linear and parabolic polynomials
at time instants (tk −Δtk /2) and (tk +Δtk /2) allows computing the coeﬃcients
bk1 =

q̇k,k+1 + q̇k−1,k
2

bk2 =

q̇k,k+1 − q̇k−1,k
2Δtk

k = 1, . . . , N .

46

4 Trajectory Planning
pos

[rad]

3
2
1
0
0

1

2
[s]

3

4

3

4

3

4

vel
2

[rad/s]

1.5
1
0.5
0
−0.5
0

1

2
[s]
acc

3

[rad/s^2]

2
1
0
−1
−2
−3
0

1

2
[s]

Fig. S4.4. Time history of position, velocity and acceleration with a cubic spline
timing law

Then, imposing continuity of position at (tk + Δtk /2) yields
bk0 = qk + (q̇k,k+1 − q̇k−1,k )

Δtk
8

k = 1, . . . , N ;

it can be veriﬁed that this choice guarantees continuity of position also at
(tk − Δtk /2).
The duration of the blend times is chosen as Δtk = 0.2 for k = 1, 2, 3.
With the given data, the ﬁles used to compute the above coeﬃcients, and
to generate the time behaviour of position, velocity and acceleration can be
found in Folder 4 6.

4 Trajectory Planning

47

pos

[rad]

3
2
1
0
0

1

2
[s]

3

4

3

4

3

4

vel
2

[rad/s]

1.5
1
0.5
0
−0.5

0

1

2
[s]
acc

6

[rad/s^2]

4
2
0
−2
−4
−6

0

1

2
[s]

Fig. S4.5. Time history of position, velocity and acceleration with a timing law of
interpolating linear polynomials with parabolic blends

The coeﬃcients are:
a10 = 0

a11 = 1

b10 = 0.025

a20 = 2
b11 = 0.5

a21 = 0.5

b12 = 2.5

b20 = 1.9875

b21 = 0.75

b22 = −1.25

b30 = 2.9875

b31 = 0.25

b32 = −1.25,

and the resulting trajectory is illustrated in Fig. S4.5.

48

4 Trajectory Planning
x−pos

y−pos
0.6

1
0.4
0.8
[m]

[m]

0.2
0.6
0.4

0
−0.2

0.2
−0.4
0
0

0.5

1
[s]

1.5

2

0

0.5

x−vel

0.5
[m/s]

0.5
[m/s]

1

0
−0.5

1.5

2

1.5

2

−0.5

0.5

1
[s]

1.5

−1
0

2

0.5

x−acc

1
[s]
y−acc

2

1

1
[m/s^2]

[m/s^2]

2

0

2

0
−1
−2
0

1.5

y−vel

1

−1
0

1
[s]

0
−1

0.5

1
[s]

1.5

2

−2
0

0.5

1
[s]

Fig. S4.6. Time history of position, velocity and acceleration of x- and ycomponents along a straight path with a trapezoidal velocity proﬁle timing law

Solution to Problem 4.7
A trapezoidal velocity proﬁle for the path coordinate s is assigned according
to (4.8) with maximum velocity ṡc = 1. The timing law for p(t) and its
derivatives is generated via (4.34) and (4.42).
With the given data, the ﬁles used to generate the time behaviour of
position, velocity and acceleration can be found in Folder 4 7.
The resulting trajectories for the x- and y-components are illustrated in
Fig. S4.6.

4 Trajectory Planning
y−pos

49

z−pos

1

1.5

0.5
[m]

[m]

1
0

0.5
−0.5
−1
0

0.5

1
[s]

1.5

0
0

2

0.5

y−vel

0.5
[m/s]

0.5
[m/s]

1

0
−0.5

1.5

2

1.5

2

−0.5

0.5

1
[s]

1.5

−1
0

2

0.5

y−acc

1
[s]
z−acc

2

1

1
[m/s^2]

[m/s^2]

2

0

2

0
−1
−2
0

1.5

z−vel

1

−1
0

1
[s]

0
−1

0.5

1
[s]

1.5

2

−2
0

0.5

1
[s]

Fig. S4.7. Time history of position, velocity and acceleration of x- and ycomponents along a circular path with a trapezoidal velocity proﬁle timing law

Solution to Problem 4.8
The unit vectors of the axes of the reference frame for the circle are chosen as
x = y

y  = −z

z  = −x;

notice that the choice z  = −x ensures that the path is executed clockwise
about axis x. From (4.39), the path representation is
⎡
⎤
0
p(s) = ⎣ 0.5 cos (2s) ⎦ .
1 − 0.5 sin (2s)

50

4 Trajectory Planning

Since it is required to execute half a circle with tf = 2, the ﬁnal value of the
path coordinate is sf = s(2) = π/2. Then, a trapezoidal velocity proﬁle for
the path coordinate s is assigned according to (4.8) with maximum velocity
ṡc = 1. The timing law for p(t) and its derivatives is generated via (4.39),
(4.44).
With the given data, the ﬁles used to generate the time behaviour of
position, velocity and acceleration can be found in Folder 4 8.
The resulting trajectories for the y- and z-components are illustrated in
Fig. S4.7.

5
Actuators and Sensors

Solution to Problem 5.1
At steady state, (5.1)–(5.4) can be written in the time domain as

where

va = Ra ia + vg

(S5.1)

vg = kv ωm

(S5.2)

cm = Fm ωm + cr
cm = kt ia

(S5.3)
(S5.4)

va = Ci (0)Gv (vc − ki ia ).

(S5.5)

Combining (S5.1), (S5.2), (S5.5) with ki = 0 and K = Ci (0)Gv gives
Kvc = Ra ia + kv ωm
and yet, using (S5.3) and (S5.4) with cr = 0, it is


Fm Ra

Kvc =
+ kv ωm .
kt
If Fm

kv kt /Ra , then
ωm ≈

K 
v .
kv c

If instead ki = 0, on reduction of (S5.1)–(S5.5), it is


Ra + Kki

Kvc =
cm + kv ωm .
kt
If Kki

Ra , then
cm

kt
≈
ki



kv

vc − ωm .
K

52

5 Actuators and Sensors
time

variables
initialization

curr

G_v

C_i

+
-

T_v.s+1

V'_c

1
L_a.s

k_t

R_a

+

1

-

I_m.s

omega

F_m
k_v

plot

Fig. S5.1. Simulink block diagram of electric servomotor

From the block scheme in Fig. 5.3, the output can be computed via the closedloop transfer functions; namely, the input/output and disturbance/output
transfer functions, i.e.,

Ωm

If Fm

Kkt
1
Ra (sIm + Fm )
sIm + Fm

=
Vc −
Cr .
kv kt
kv kt
1+
1+
Ra (sIm + Fm )
Ra (sIm + Fm )

kv kt /Ra , then

Ωm

K
Ra
kv
kv kt

=
V −
C .
Ra Im c
Ra Im r
1+s
1+s
kv kt
kv kt

Finally, from the block scheme in Fig. 5.4, it is straightforward to compute
the output as
kt
1
ki Fm
Fm
Ωm =
V−
C .
Im c
Im r
1+s
1+s
Fm
Fm
Solution to Problem 5.2
With reference to the scheme in Fig. 5.2 (ki = 0 and Cr = 0) and the given
data, the resulting Simulink block diagram is shown in Fig. S5.1. The servo-

5 Actuators and Sensors
velocity

2.5

6

2

5

1.5

4
[rad/s]

[A]

current

1

3

0.5

2

0

1

−0.5
0

0.05

53

0.1

0.15

0
0

[s]

0.05

0.1

0.15

[s]

Fig. S5.2. Time history of current and velocity step response for an electric servomotor

motor is simulated as a continuous-time system using a variable-step integration method with a maximum step size of 1 ms. Notice that the ampliﬁer time
constant is too small to produce appreciable eﬀects at this sampling time.
The ﬁles with the solution can be found in Folder 5 2.
The resulting current and velocity responses to a unit step voltage input Vc
are shown in Fig. S5.2. It can be seen that the steady-state velocity slightly
deviates from the value of 5 rad/s as in (5.7), because of the presence of
mechanical friction (Fm ). This also implies that the steady-state current is
diﬀerent from zero.
Solution to Problem 5.3
By closing a current loop with large loop gain, the current response is expected
to become much faster than the velocity response. Velocity plays the role of a
disturbance for the voltage-to-current closed-loop system. Hence, it is worth
choosing a PI control structure for Ci (s) in order to obtain a null steady-state
error, i.e.,
1 + sTI
.
Ci (s) = KI
s
With the given data, by setting Ωm = 0, the closed-loop input/output transfer
function (Tv ≈ 0) is
Ia
=
Vc

1 + sTI
.
0.002
0.2 + KI TI
1+s
+ s2
KI
KI

Since the settling time within 1% is given by
ts =

4.6
,
ζωn

either the damping ratio or the natural frequency can be imposed. Choosing
ζ = 0.7 with ts = 2 ms yields
KI = 21592

TI = 0.000416.

54

5 Actuators and Sensors
current

time

velocity

k_i

+

C_i(s)

curr

G_v

+
-

T_v.s+1

V'_c

1

k_t

L_a.s

+

1

-

I_m.s

R_a

omega

F_m
k_v

plot

Fig. S5.3. Simulink block diagram of electric servomotor with current feedback
loop and PI control
current

velocity

2

25
20
[rad/s]

[A]

1.5
1
0.5
0
0

15
10
5

1

2

3
[s]

4

5
−3

x 10

0
0

0.2

0.4

0.6

0.8

1

[s]

Fig. S5.4. Time history of current and velocity step response for an electric servomotor with current feedback loop and PI control

The resulting Simulink block diagram is shown in Fig. S5.3. The servomotor
is simulated as a continuous-time system using a variable-step integration
method. Since the current dynamics is much faster than the velocity dynamics,
the maximum step size has been chosen much smaller than in Problem 5.2, i.e.,
0.05 ms. Further, in order to make a comparison between the velocity response
in this case and the case of Problem 5.2, the block diagram in Fig. S5.3 features
a separate execution with a longer ﬁnal time and a sampling time of 1 ms.
The ﬁles with the solution can be found in Folder 5 3.
The resulting current and velocity are shown in Fig. S5.4. The current
response goes as expected, while the velocity response is slowed down with
respect to the response in Fig. S5.2 because it is dominated by the time
constant Im /Tm .

5 Actuators and Sensors

55

Fig. S5.5. Reduction on the block scheme of a hydraulic motor with servovalve and
distributor

Solution to Problem 5.4
From this block scheme, the following relations can be found:
Θm =

1
Ωm
s

Ωm =

Cm − Cr
sIm + Fm

Cm = kt P
1
P =
skc + kl
=
X=



kx
kr




P
X−
− kq Ωm
kx

kx X − kr kq Ωm
1 + kr kl + skr kc
Gs
Vc .
1 + sTs

Substituting (S5.6) into (S5.6), and then (S5.6) into (S5.6), yields


kt kr kq
kt kx
1+
X
Ωm =
(sIm + Fm )(1 + kr kl + skr kc )
(1 + kr kl + skr kc )(sIm + Fm )
1
−
Cr .
sIm + Fm
Solving for Ωm , substituting it into (S5.6), and using (S5.6) leads to

Θm =



Fm
s(1 + sTs ) 1 +
kt kr kq

kx Gs
kr kq

 Vc


Im
1+s
(1 + kr kl + skr kc )
Fm

56

5 Actuators and Sensors

1 + kr kl + skr kc
kt kr kq
 Cr .
− 


Fm
Im
s 1+
1+s
(1 + kr kl + skr kc )
kt kr kq
Fm

Solution to Problem 5.5
Without loss of generality, the case of 4 bits is considered. The truth table of
the interconversion logic circuit is reported in Table S5.1.
Table S5.1. Truth table of the interconversion logic circuit from Gray-code to
binary code
#
0
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

Gray Code
x1 x2 x3 x4
0 0 0 0
0 0 0 1
0 0 1 1
0 0 1 0
0 1 1 0
0 1 1 1
0 1 0 1
0 1 0 0
1 1 0 0
1 1 0 1
1 1 1 1
1 1 1 0
1 0 1 0
1 0 1 1
1 0 0 1
1 0 0 0

Binary Code
y1 y2 y3 y4
0 0 0 0
0 0 0 1
0 0 1 0
0 0 1 1
0 1 0 0
0 1 0 1
0 1 1 0
0 1 1 1
1 0 0 0
1 0 0 1
1 0 1 0
1 0 1 1
1 1 0 0
1 1 0 1
1 1 1 0
1 1 1 1

The ﬁrst columns of the two codes are equal, and thus
y1 = x1 .
From the second column at the output, it can be recognized that y2 = 1 when
x1 = 0 and x2 = 1, or else when x1 = 1 and x2 = 0, and thus
y2 = x̄1 x2 + x1 x̄2 = x1 ⊕ x2 = y1 ⊕ x2
where “⊕” is the symbol for the logical operator XOR. From the third column
at the output, it can be recognized that y3 = 1 when y2 = 0 and x3 = 1, or
else when y2 = 1 and x3 = 0, and thus
y3 = y2 ⊕ x3 .

5 Actuators and Sensors

57

Fig. S5.6. Interconversion logic circuit from Gray code to binary code

Finally, from the fourth column it is
y4 = y3 ⊕ x4 .
The resulting interconversion logic circuit is illustrated in Fig. S5.6 where the
conventional symbol of XOR is used.
Solution to Problem 5.6
The skew-symmetric operator is

⎡

0
S(r ccs ) = ⎣ 0.2
0
and thus applying (5.32)
⎡
0
⎢
 c ⎢ 0
fc
⎢ 1
=⎢
μcc
⎢ 0
⎣
0.3
0

⎤
−0.2 0
0
0.3 ⎦ ,
−0.3 0

gives
0
1 0
−1 0 0
0
0 0
0.2 0 0
0 0.2 0
0.3 0 1

0
0
0
0
−1
0

⎤⎡ ⎤ ⎡ ⎤
0
20
0
0⎥⎢ 0 ⎥ ⎢ 0 ⎥
⎥⎢ ⎥ ⎢ ⎥
0 ⎥ ⎢ 0 ⎥ ⎢ 20 ⎥
⎥⎢ ⎥ = ⎢ ⎥,
1⎥⎢ 0 ⎥ ⎢ 0 ⎥
⎦⎣ ⎦ ⎣ ⎦
0
6
0
0
0
0

that is,
f cc = [ 0 0

20 ]T N

μcc = [ 0

0 0 ]T N·m.

As can be seen, there is no resulting moment in the contact frame.
Solution to Problem 5.7
Let the end-eﬀector frame coincide with Frame 4 in Fig. S2.3. Then, with
reference to (S2.3), the homogeneous transformation from the base frame to
the end-eﬀector frame at q = [ 0 π/4 0.1 0 ] is
⎡
⎤
0.707 −0.707 0 0.854
⎢ 0.707 0.707 0 0.354 ⎥
T bc = ⎣
⎦.
0
0
1 0.1
0
0
0
1

58

5 Actuators and Sensors

Using (5.35) gives

⎡

⎤
0.066
⎢ 0.141 ⎥
p̃c = ⎣
⎦
0.8
1

and, in view of (5.41), it is
⎡

⎤
633.6 0 250
Ω=⎣ 0
964 250 ⎦
0
0
1
and thus from (5.40)
xI = 241.82
yI = 335.92
λ = 0.8.
Hence
XI = 302.27

YI = 419.90.

6
Control Architecture

Solution to Problem 6.1
With reference to the hierarchical control architecture in Fig. 6.1, the solution
can be articulated at the task and action levels as follows.
At the task level, in order to decompose the task into a sequence of actions, the decision module shall consult the knowledge base available in the
modelling module concerning the manipulator and the environment. If the
environment is scarcely structured, the knowledge base shall be updated with
the information to be presented by the sensor module via the use of heteroceptive sensors, e.g., vision. Once the environment is structured, the modelling
module provides the relative location between the manipulator, the object
and the obstacle. Feasibility of the task shall be veriﬁed, and a possible sequence of actions is generated: approach position A, pick object, move object
while avoiding obstacle, approach position B, place object. The two approach
actions are opportune to ensure smooth pick-and-place of the object.
At the action level, the symbolic commands coming from the task level
are interpreted by the decision module which shall take the resolutions needed
to execute the given sequence of actions, that is, choice of reference frame,
motion in operational space, intermediate path or via points C, D, E, F
(see Fig. S6.1), duration of time intervals, path primitives oﬀ the obstacle,
and type of interpolating functions. By doing so, the decision module shall
consult the knowledge base available in the modelling module to verify path
feasibility. As for the approach actions, the decisions imparted to the primitive
level shall regard low values of velocities and vertical orientation of the end
eﬀector, whereas the knowledge base shall be updated with the information
available in the sensor module via the use of range or proximity sensors.

60

6 Control Architecture

Fig. S6.1. Choice of reference frame and intermediate positions for an object pickand-place task

Solution to Problem 6.2
With reference to the sequence of points illustrated in Fig. S6.1, the operational space motion primitives can be determined in the two cases of path
points and via points, respectively, as follows.
In the case of path points, the various points can be connected by segments. To ensure safe grasp of the object at point C and safe release of the
object at point F , it is advisable to choose null velocity and acceleration at
each of those points. As for the intermediate points D and E, it is suﬃcient
to impose null velocities. Hence, the timing law along the segments CD and
EF shall be polynomials of at least fourth order, whereas the timing law
along the segment DE shall be either a cubic polynomial or a trajectory with
trapezoidal velocity proﬁle.
In the case of via points, the actual path in the neighbourhood of D and
E is of no concern, as long as it is kept at a safety distance from the obstacle.
The passage in the proximity of the two via points can be obtained by using
the same interpolating functions as for the previous case, on condition that
the generation of the timing laws along the segments DE and EF are suitably
anticipated in time. As above, null initial and ﬁnal velocity and acceleration
are to be imposed.
Finally, the presence of a redundant DOFs relative to a bidimensional
position task can be exploited to keep end-eﬀector orientation constant along
−y.
Solution to Problem 6.3
Before executing the peg-in-hole task, the end eﬀector shall approach the hole.
It can be argued that it is diﬃcult to guarantee exact positioning of the peg
relative to the hole; this may render the insertion phase quite critical. It is
assumed that the main cause of uncertainty regards only position, that is the
axis of the peg can be considered to be aligned with the axis of the hole during

6 Control Architecture

61

Fig. S6.2. Flow chart for the execution of a peg-in-hole task

the approach to the hole; also, it is assumed that no jamming occurs during
insertion.
Choose a reference frame at the base of the arm, with axis x along the
horizontal direction (positive from the base to the hole) and axis y along the
positive vertical direction. A possible solution is to split the whole task into
three subtasks as follows.
At ﬁrst, a fast motion towards the position (xH , Δy) shall be executed,
where xH is the estimated x coordinate of the center of the hole and Δy is a
small distance from the estimated height of the hole (y = 0). This is meant
to avoid colliding the surface around the hole during the approach phase. A
pure motion control strategy is needed to accomplish this subtask.
Next, a slow motion along −y shall be executed, still with a pure motion control strategy. This goes on until y is below the given distance −Δy

62

6 Control Architecture

(when the peg is considered to have entered the hole), unless a contact force
fy is sensed above a threshold , due to a collision with the surface. In that
case, before proceeding with insertion, it is necessary to slide along the surface in either positive or negative horizontal direction until a force below the
threshold is sensed; if the x coordinate of end-eﬀector position goes too far
from xH (greater than a distance Δx), then the motion shall be inverted. A
force/position control strategy is needed to accomplish this subtask where position is controlled along x and force is controlled along y to a desired pushing
force. This goes on until the peg enters the hole.
Finally, the insertion along −y is accomplished by using a force/position
control strategy where position is controlled along y, force is controlled along
x, and moment is controlled about z; moment control is needed to keep the
peg in axis with the hole during insertion.
The ﬂow chart corresponding to the articulation of the whole task into
the above three subtasks is illustrated in Fig. S6.2.
Solution to Problem 6.4
The task can be divided into the following sequence of actions:
• command new object on conveyor,
• fast motion of end eﬀector to a location above object to pick,
• slow motion of end eﬀector towards object,
• pick object from conveyor,
• fast motion of object to a loading location above pallet,
• slow motion of object downwards,
• release object.
At the beginning, the end eﬀector is moved to a home location from which
the task starts. The above sequence is iterated until the pallet is ﬁlled, and
then the end eﬀector is taken back to the home location.
Below a PASCAL program is listed to execute the given task.
program FILL PALLET;
type point: array[1..6] of real;
const home: point=(------);
{end-effector home location}
object: point=(------);
{object location}
pallet: point=(------);
{lower-left pallet location}
lift: -.-;
{z-offset above object}
length: -.-;
{object x-dimension}
width: -.-;
{object y-dimension}
fast: -.-;
{fast velocity}
slow: -.-;
{slow velocity}
var
i,j: integer;
current: point;
{end-effector current location}
procedure MOVETO(var endpoint:point,velocity:real);
procedure GRASP;
procedure RELEASE;

6 Control Architecture

procedure NEW OBJECT;
begin
MOVETO(home,fast);
for i:=0 to 3 do
for j:= 0 to 3 do
begin
NEW OBJECT;
current:=object;
current[3]:=current[3]+lift;
MOVETO(current,fast);
MOVETO(object,slow);
GRASP;
MOVETO(current,slow);
current:=pallet;
current[1]:=current[1]+i*length;
current[2]:=current[2]+j*width;
current[3]:=current[3]+lift;
MOVETO(current,fast);
current[3]:=current[3]-lift;
MOVETO(current,slow);
RELEASE;
current[3]:=current[3]+lift;
MOVETO(current,slow);
end
MOVETO(home,fast);
end

63

7
Dynamics

Solution to Problem 7.1
Consider the Cartesian arm in Fig. S7.1, for which the vector of generalized
coordinates is q = [ d1 d2 ]T .
Let m 1 , m 2 be the masses of the two links and mm1 , mm2 the masses
of the rotors of the two joint motors. Let also Im1 , Im2 be the moments of
inertia about the axes of the two rotors. It is assumed that pmi = pi−1 and
z mi = z i−1 , for i = 1, 2, i.e., the motors are located on the joint axes with
centres of mass located at the origins of the respective frames.
√
√
With the choice of the frames in Fig. S7.1, it is z 1 = [ 1/ 2 0 1/ 2 ]T .
Hence, computation of the Jacobians in (7.16), (7.18) yields
√ ⎤
⎡
⎤
⎡
0 0
0 1/ 2
( 1)
(
)
0√ ⎦ .
JP 2 = ⎣ 0
JP = ⎣ 0 0 ⎦
1 1/ 2
1 0
(

)

(

)

Obviously it is J O 1 = J O 2 = O.
Computation of the Jacobians in (7.25), (7.26), (7.28), (7.29) respectively
yields
⎡
⎤
⎡
⎤
0 0
0 0
(m )
(m )
JP 1 = ⎣ 0 0 ⎦
JP 2 = ⎣ 0 0 ⎦
0 0
1 0
√ ⎤
⎡
⎡
⎤
0 0
0 kr2 / 2
(m )
(m )
JO 1 = ⎣ 0 0 ⎦
0√ ⎦
JO 2 = ⎣ 0
0 kr2 / 2
kr1 0
where kri is the gear reduction ratio of motor i.
From (7.32), the inertia matrix is
√


2
m 1 + mm2 + kr1
Im1 + m 2
m 2/ 2
√
B=
.
2
m 2 + kr2
Im2
m 2/ 2

66

7 Dynamics

Fig. S7.1. Two-link Cartesian arm where Joint 2 axis forms an angle of π/4 with
Joint 1 axis

Notice that B is constant, which implies that C = O. As for the gravitational
terms, with g 0 = [ 0 0 −g ]T , (7.39) gives:
√
g1 = (m 1 + mm2 + m 2 )g
g2 = m 2 g/ 2.
In the absence of friction and tip contact forces, the resulting equations of
motion are
(m

1

m
2
+ mm2 + kr1
Im1 + m 2 )d¨1 + √ 2 d¨2 + (m 1 + mm2 + m 2 )g = τ1
2
m2¨
m
2
√ d1 + (m 2 + kr2
Im2 )d¨2 + √ 2 g = τ2
2
2

where τ1 and τ2 denote the forces applied to the two joints. It is worth pointing
out that, diﬀerently from the arm in Fig. 7.3, the dynamic model is coupled
(nonnull oﬀ-diagonal terms in the inertia matrix) and gravity acts also on the
second joint.
Solution to Problem 7.2
A natural choice for the elements of matrix C satisfying (7.43) with hijk as
in (7.41) is

n 

∂bij
1 ∂bjk
q̇k .
−
cij =
∂qk
2 ∂qi
k=1

For the two-link planar arm of Fig. 7.4, they become
c11 =

2 

∂b11
k=1

1 ∂b1k
−
∂qk
2 ∂q1


q̇k = −2m 2 a1 2 s2 ϑ̇2

7 Dynamics

c12 =

c21 =

c22 =

2 

k=1
2 

k=1
2 

k=1

∂b12
1 ∂b2k
−
∂qk
2 ∂q1
∂b21
1 ∂b1k
−
∂qk
2 ∂q2
1 ∂b2k
∂b22
−
∂qk
2 ∂q2

Setting h = −m 2 a1 2 s2 leads to

C=

67


q̇k = −m 2 a1 2 s2 ϑ̇2


1
q̇k = m 2 a1 2 s2 ϑ̇1 − m 2 a1 2 s2 ϑ̇2
2


q̇k =

1
m a1 2 s2 ϑ̇1 .
2 2

2hϑ̇2
h
−hϑ̇1 + ϑ̇2
2

hϑ̇2
h
− ϑ̇1
2



while the time derivative of the inertia matrix can be written as


2hϑ̇2 hϑ̇2
Ḃ =
.
hϑ̇2
0
Therefore, the matrix N (q, q̇) = Ḃ − 2C becomes


−2hϑ̇2 −hϑ̇2
N (q, q̇) =
2hϑ̇1
hϑ̇1
which clearly is not skew-symmetric. Nevertheless, it is easy to show that the
product
q̇ T N (q, q̇)q̇ = 0,
conﬁrming that (7.49) holds even for a choice of C so that (7.48) does not
hold.
Solution to Problem 7.3
Consider the SCARA arm in Fig. S7.1, for which the vector of generalized coordinates is q = [ ϑ1 ϑ2 d3 ϑ4 ]T and link frames assigned as in Fig. S2.3.
For the ﬁrst two links, consider the same kinematics and dynamic parameters
of the two-link planar arm of Exmple 7.2. Moreover, links 3 and 4 can be
considered as a unique rigid body, referred as “last link”, which translates
along axis z2 of the prismatic Joint 3 and rotates about axis z3 of the revolute
Joint 4. Let m 3 denote the mass of the last link. For simplicity, it is assumed
that the centre of mass p 4 of the last link is located on axis z3 ; let I 4 denote
the element [3, 3] of the inertia tensor of the last link relative to the centre
of mass. Also, assume that the motors of Joints 3 and 4 have negligible mass
and inertia.
Due to the additivity property, the kinetic energy of the SCARA manipulator can be computed by adding the kinetic energy of the last link to the

68

7 Dynamics

kinetic energy of the two-link planar arm. Therefore, from (7.32), the inertia
matrix can be computed as
B(q) = B  (q) + B  (q)
where B  is the (4 × 4) matrix obtained completing the (2 × 2) inertial matrix
of the two-link planar arm of Sect. 7.3.2 with two null columns and two null
rows, while B  is the contribution of the last link, which can be computed as
B  (q) = m 3 J P 4 J P 4 + J O 4 R4 I i i RT4 J O 4 .
(

)T

(

The computation of the Jacobians in
⎡
−a1 s1 − a2 s12 −a2 s12
( 4)
J P = ⎣ a1 c1 + a2 c12
a2 c12
0
0

)

(

)T

(

)

(7.16), (7.17) yields
⎤
⎡
0 0
0
(
)
0 0⎦
J O4 = ⎣ 0
1 0
1

(S7.1)

0
0
1

⎤
0 0
0 0⎦
0 1

Hence, from (S7.1), the nonnull elements of matrix B  (q) are
b11 = m 3 (a21 + a22 + 2a1 a2 c2 ) + I 4
b12 = b21 = m 3 (a22 + a1 a2 c2 ) + I 4
b14 = b41 = I

4

b22 = m 3 a22 + I
b24 = b42 = I 4

4

b33 = m 3
b44 = I 4 .
Therefore, the nonnull elements of the inertia matrix of the SCARA manipulator are:
2
b11 = I 1 + m 1 21 + kr1
Im1 + I 2 + m 2 (a21 + 22 + 2a1 2 c2 )

b12

+Im2 + mm2 a21 + m 3 (a21 + a22 + 2a1 a2 c2 ) + I 4
= b21 = I 2 + m 2 (22 + a1 2 c2 ) + kr2 Im2 + m 3 (a22 + a1 a2 c2 ) + I

b14 = b41 = I 4
2
b22 = I 2 + m 2 22 + kr2
Im2 + m 3 a22 + I
b24 = b42 = I
b33 = m 3

4

4

4

b44 = I 4 .
The Christoﬀel symbols for the SCARA manipulator can be computed as
cijk = cijk + cijk

7 Dynamics

69

Fig. S7.2. Two-link planar arm with a prismatic joint and a revolute joint

where cijk are the Christoﬀel symbols of the two-link planar arm of Sect. 7.3.2,
while cijk are those computed from the elements bij of B  (q) as in (7.45).
The nonnull elements are:
c112 = −m 2 a1 2 s2 − m 3 a1 a2 s2 = k
c122 = k
c211 = −k,
leading to the matrix

⎡

k ϑ̇2
⎢ −k ϑ̇1
C(q, q̇) = ⎢
⎣ 0
0

k(ϑ̇1 + ϑ̇2 )
0
0
0

0
0
0
0

⎤
0
0⎥
⎥.
0⎦
0

As for the gravitational terms, it can be easily understood that gravity contributes only to Joint 3 force, which has the expression
g3 = m 3 g.
Solution to Problem 7.4
By using the expressions of the parameters in (7.83), the equations of motion
(7.82) can be rewritten as
2
π4 + (a21 + 2a1 a2 c2 + a22 )π5
τ1 = a21 π1 + 2a1 π2 + π3 + kr1
+(2a1 c2 + 2a2 )π6 + π7 + π8 )ϑ̈1
+ (a1 a2 c2 + a22 )π5 + (a1 c2 + 2a2 )π6 + π7 + kr2 π8 ϑ̈2

−(2a1 a2 s2 π5 + 2a1 s2 π6 )ϑ̇1 ϑ̇2 − (a1 a2 s2 π5 + a1 s2 π6 )ϑ̇22
+(a1 π1 + π2 + a1 π5 )gc1 + (a2 π5 + π6 )gc12
2
τ2 = (a22 π5 + 2a2 π6 + π7 + kr2
π8 )ϑ̈2
2
+ (a1 a2 c2 + a2 )π5 + (a1 c2 + 2a2 )π6 + π7 + kr2 π8 ϑ̈1

+(a1 a2 s2 π5 + a1 s2 π6 )ϑ̇21
+(a2 π5 + π6 )gc12 .

70

7 Dynamics

It is then possible to ﬁnd the following linear combinations between the parameters which bring the number thereof down to a minimum of 5:
π1 = a1 π1 + π2 + a1 π5 = a1 m1 + m1 C1 + a1 m2
2
2
π2 = a1 π2 + π3 + kr1
π4 = a1 m1 C1 + I¯1 + kr1
Im1
π3 = a2 π5 + π6 = a2 m2 + m2 C2
π4 = a2 π6 + π7 = a2 m2 C2 + I¯2

(S7.2)

π5 = π8 = Im2 .
Accordingly, the elements of the regressor become

y11
= a1 ϑ̈1 + gc1

y12
= ϑ̈1

y13 = (2a1 c2 + a2 )ϑ̈1 + (a1 c2 + a2 )ϑ̈2 − 2a1 s2 ϑ̇1 ϑ̇2 − a1 s2 ϑ̇22 + gc12

y14
= ϑ̈1 + ϑ̈2

y15
= ϑ̈1 + kr2 ϑ̈2

(S7.3)


y21
=0

y22
=0

y23 = a2 ϑ̈2 + (a1 c2 + a2 )ϑ̈1 + a1 s2 ϑ̇21 + gc12

y24
= ϑ̈2 + ϑ̈1

2
y25
= kr2
ϑ̈2 + kr2 ϑ̈1 .

Solution to Problem 7.5
Consider the planar arm in Fig. S7.2, for which the vector of generalized
coordinates is q = [ d1 ϑ2 ]T .
Let m 1 , m 2 be the masses of the two links, 1 , 2 the distances of the
centres of mass from the respective joint axes, and mm1 , mm2 the masses of
the rotors of the two joint motors. Let also I 2 be the inertia tensor of Link
2 about Joint 2 axis, and Im1 , Im2 the moments of inertia about the axes of
the two rotors. It is assumed that pmi = pi−1 and z mi = z i−1 , for i = 1, 2,
i.e., the motors are located on the joint axes with centres of mass located at
the origins of the respective frames.
With the choice of the frames in Fig. S7.2, it is z 1 = [ 0 1 0 ]T ,
p1 = pm2 = [ 0 0 d1 ]T , p 2 = [ −2 c2 0 d1 + 2 s2 ]T . Computation of
the Jacobians in (7.16), (7.18) yields
⎤
⎡
⎡
⎤
0 0
0  2 s2
( )
( )
JP 1 = ⎣ 0 0 ⎦
0 ⎦,
JP 2 = ⎣ 0
1  2 c2
1 0

7 Dynamics

whereas computation of the Jacobians in (7.17), (7.19) yields
⎡
⎤
⎡
⎤
0 0
0 0
( )
( )
J O1 = ⎣ 0 0 ⎦
J O2 = ⎣ 0 1 ⎦ .
0 0
0 0
Computation of the Jacobians in (7.25), (7.26) yields
⎡
⎤
⎡
⎤
0 0
0 0
(m )
(m )
JP 2 = ⎣ 0 0 ⎦ ,
JP 1 = ⎣ 0 0 ⎦
0 0
1 0
whereas computation of the Jacobians in (7.28), (7.29) yields
⎤
⎤
⎡
⎡
0 0
0 0
(m )
(m )
JO 1 = ⎣ 0 0 ⎦
J O 2 = ⎣ 0 kr2 ⎦
0 0
kr1 0
where kri is the gear reduction ratio of motor i.
From (7.32), the inertia matrix is


b11
b12 (ϑ2 )
B(q) =
b22
b12 (ϑ2 )
b11 = m

1

+ m m2 + m

2

2
+ kr1
Im1

b12 = m 2 2 c2
2
b22 = I 2 + m 2 22 + kr2
Im2
where I 2 = I 2 zz is the constant moment of inertia about Joint 2 axis.
Computation of Christoﬀel symbols as in (7.45) gives:
c111 = c112 = c121 = c211 = c212 = c221 = c222 = 0
∂b12
1 ∂b22
c122 =
−
= −m 2 2 s2
∂q2
2 ∂q1
leading to the matrix

C(q, q̇) =

0
0


−m 2 2 s2 ϑ̇2
.
0

Computing the matrix N in (7.47) gives
N (q, q̇) = Ḃ(q) − 2C(q, q̇)


0
m 2 2 s2 ϑ̇2
=
−m 2 2 s2 ϑ̇2
0
which is skew-symmetric.

71

72

7 Dynamics

As for the gravitational terms, with g 0 = [ −g

0 0 ]T , (7.39) gives:

g1 = 0
g2 = m 2 2 gs2 .
In the absence of friction and tip contact forces, the resulting equations of
motion are
(m

1

2
+ mm2 + m 2 + kr1
Im1 )d¨1 + m 2 2 c2 ϑ̈2 − m 2 2 s2 ϑ̇22 = τ1 (S7.4)
2
m 2 2 c2 d¨1 + (I 2 + m 2 22 + kr2
Im2 )ϑ̈2 + m 2 2 gs2 = τ2 (S7.5)

where τ1 and τ2 respectively denote the force applied to Joint 1 and the torque
applied to Joint 2.
The presence of a concentrated tip payload of mass mL alters Link 2
parameters as follows:
m 2 = m

2

2 =

+ mL

m 2  2 + m L a2
m 2

I 2 = I 2 + m 2 (2 − 2 )2 + mL (a2 − 2 )2
and the same formal dynamic model can be written as above in terms of the
new parameters.
A minimum parameterization of the dynamic model can be obtained in
terms of the following parameter vector:
π  = [ π1

π2

π3 ]T

2
π1 = m 1 + mm2 + m 2 + kr1
Im1

 
π2 = m 2 2
2
π3 = I 2 + m 2 2
2 + kr2 Im2 .

Accordingly, the elements of the regressor are

y11
= d¨1

y12
= c2 ϑ̈2 − s2 ϑ̇22

y13
=0

y21
=0

= c2 d¨1 + gs2
y22

y23
= ϑ̈2 .

Solution to Problem 7.6
Let x1 and x2 denote the absolute angles of the two joints. The relationship
between relative and absolute angles is
x1 = ϑ1

x2 = ϑ1 + ϑ2 .

7 Dynamics

73

This can be formally treated as a coordinate transformation from the joint
space to the operational space of absolute angles. The analytical Jacobian
characterizing such transformation at velocity level is then


1 0
JA =
,
1 1
which is always nonsingular, implying that the absolute angles constitute a
set of generalized coordinates for the system.
Therefore, the terms of the operational space dynamic model can be computed according to (7.134)–(7.136). From (7.134), the inertia matrix is


b
b
B A = A11 A12
bA12 bA22
where, using the expressions of bij in Sect. 7.3.2, the elements in the new
coordinates are
bA11 = b11 + b22 − 2b12
2
= I 1 + m 1 21 + kr1
Im1 + m 2 a21 + (1 − kr2 )2 Im2 + mm2 a21
bA12 = b12 − b22
= m 2 a1 2 cos (x2 − x1 ) + kr2 (1 − kr2 )Im2
bA22 = b22
2
= I 2 + m 2 22 + kr2
Im2 .

Notice that the diagonal elements are conﬁguration-invariant; also, if kr2 = 1
(direct drive), then the eﬀect of motor 2 inertia on both bA12 and bA22 vanishes.
Further, from (7.135), the velocity-dependent term is


c − c12 c21 − c22
C A ẋ = 11
c21
c22
which, using the expressions of cij in Sect. 4.3.2, in the new coordinates becomes


hẋ22
C A ẋ =
−hẋ21
where h = −m 2 a1 2 sin (x2 − x1 ). Interestingly enough, there is no Coriolis
eﬀect; this can be explained by observing that rotation of Link 2 is represented
with reference to Frame 0 and thus is independent of rotation of Link 1.
Finally, from (7.136), the gravity term is


g1 − g2
gA =
g2
which, using the expressions of gi in Sect. 4.3.2, in the new coordinates becomes


(m 1 1 + mm2 a1 + m 2 a1 )g cos x1
gA =
.
m 2 2 g cos x2

74

7 Dynamics
4

x 10

joint 1 torque

4

1.5

1

1

0.5

0.5

[Nm]

[Nm]

1.5

0
−0.5

x 10

joint 2 torque

0
−0.5

−1
0

0.2

0.4

−1
0

0.6

0.2

[s]

0.4

0.6

[s]

Fig. S7.3. Time history of joint torques for the ﬁrst trajectory of Example 7.2
4

x 10

joint 1 torque

4

1.5

1

1

0.5

0.5

[Nm]

[Nm]

1.5

0
−0.5

x 10

joint 2 torque

0
−0.5

−1
0

0.2

0.4

−1
0

0.6

0.2

[s]

0.4

0.6

[s]

Fig. S7.4. Time history of joint torques for the second trajectory of Example 7.2
4

x 10

joint 1 torque

4

1.5

1

1

0.5

0.5

[Nm]

[Nm]

1.5

0
−0.5
−1
0

x 10

joint 2 torque

0
−0.5

0.2

0.4
[s]

0.6

−1
0

0.2

0.4

0.6

[s]

Fig. S7.5. Time history of joint torques for the third trajectory of Example 7.2

Solution to Problem 7.7
The various terms of the dynamic model are computed by using the minimum parameterization in the solution to Problem 7.4. The tip forces can be
incorporated in the model as in (7.42), i.e., via the Jacobian transpose.
The ﬁles with the solution can be found in Folder 7 7.
The resulting torques for the three trajectories of Example 7.2 are illustrated in Figs. S7.3, S7.4, S7.5, respectively.

7 Dynamics

75

Solution to Problem 7.8
Start by imposing the initial conditions for the velocities and accelerations:
p̈00 − g 00 = [ g

0 0 ]T

ω 00 = ω̇00 = 0.

All quantities shall be referred to the current link frame. With the choice of
the frames as in Fig. S7.2, the following vectors are obtained:
⎡
⎡ ⎤
⎡
⎤
⎡ ⎤
⎤
0
0
C2
a2
r11,C1 = ⎣ C1 ⎦ r10,1 = ⎣ d1 ⎦ r22,C2 = ⎣ 0 ⎦ r 21,2 = ⎣ 0 ⎦
0
0
0
0
where C1 and C2 are both negative quantities. The rotation matrices needed
for vector transformation from one frame to another are:
⎡
⎤
⎡
⎤
−1 0 0
c2 −s2 0
R12 = ⎣ s2 c2 0 ⎦ .
R01 = ⎣ 0 0 1 ⎦
0 1 0
0
0
1
Further it is assumed that the axes of rotations of the rotors coincide with
T
the respective joint axes, i.e., z i−1
mi = z 0 = [ 0 0 1 ] for i = 1, 2.
According to (7.107)–(7.114), the Newton-Euler algorithm requires the
execution of the following steps.
• Forward recursion: Link 1
ω11 = 0
ω̇11 = 0
⎡

⎤
−g
⎥
⎢
p̈11 = ⎣ d¨1 ⎦

p̈1C1

0
⎤
⎡
−g
⎥
⎢
= ⎣ d¨1 ⎦
0
⎡

0

⎢
ω̇0m1 = ⎣

0

⎤
⎥
⎦.

kr1 d¨1
•

Forward recursion: Link 2
⎡ ⎤
0
⎢ ⎥
2
ω2 = ⎣ 0 ⎦
ϑ̇2

76

7 Dynamics

⎡

0

⎤

⎢ ⎥
ω̇22 = ⎣ 0 ⎦
ϑ̈2
⎤
s2 d¨1 − a2 ϑ̇22 − gc2
⎥
⎢
p̈22 = ⎣ c2 d¨1 + a2 ϑ̈2 + gs2 ⎦
⎡

0
⎤
s2 d¨1 − (C2 + a2 )ϑ̇22 − gc2
⎥
⎢
= ⎣ c2 d¨1 + (C + a2 )ϑ̈2 + gs2 ⎦
⎡

p̈2C2

2

0
⎡

⎤

0

⎢
ω̇1m2 = ⎣

⎥
⎦.

0
kr2 ϑ̈2

•

Backward recursion: Link 2
⎤
⎡
m2 s2 d¨1 − m2 (C2 + a2 )ϑ̇22 − m2 gc2
⎥
⎢
f 2 = ⎣ m2 c2 d¨1 + m2 (C + a2 )ϑ̈2 + m2 gs2 ⎦
2

2

0
⎡

⎤

∗

⎢
μ22 = ⎣

⎥
⎦

∗
m2 (C2 +a2 )c2 d¨1 + I¯2zz +m2 (C2 +a2 )2 ϑ̈2 +m2 (C2 +a2 )gs2

2
Im2 ϑ̈2
τ2 = m2 (C2 + a2 )c2 d¨1 + I¯2zz + m2 (C2 + a2 )2 + kr2
+m2 (C2 + a2 )gs2 ,

where the components marked by the symbol ‘∗’ have not been computed,
since they are not related to the joint torque τ2 .
•

Backward recursion: Link 1
⎤
⎡
−m2 (C2 + a2 )s2 ϑ̈2 − m2 (C2 + a2 )c2 ϑ̇22 − (m1 + m2 )g
⎥
⎢
f 1 = ⎣ (m1 + m2 )d¨1 + m2 (C + a2 )c2 ϑ̈2 − m2 (C + a2 )s2 ϑ̇2 ⎦
1

2

2

2

0
2
Im1 )d¨1 + m2 (C2 + a2 )c2 ϑ̈2 − m2 (C2 + a2 )s2 ϑ̇22 .
τ1 = (m1 + m2 + kr1

In view of the following equalities:
m1 = m

1

+ m m2

m2 = m

2

 2 =  C 2 + a2

I¯2zz = I 2 ,

7 Dynamics

77

the above dynamic model coincides with the model derived in (S7.4), (S7.5),
with Lagrange formulation.
Solution to Problem 7.9
In order to diﬀerentiate matrix B A in (7.130) with respect to time, it is worth
recalling the formula for the derivative of the inverse of a matrix A
d −1
A (t) = −A−1 (t)Ȧ(t)A−1 (t).
dt
Therefore, the time derivative of B A can be expressed as


T
Ḃ A = −B A J̇ A B −1 J TA − J A B −1 ḂB −1 J TA + J A B −1 J̇ A B A .
As regards C A , in view of (3.62), the relation in (7.131) can be written as
C A J A q̇ = B A J A B −1 C q̇ − B A J̇ A q̇,
and using a right pseudo-inverse of J A , e.g., the one in (7.138), leads to
C A = B A J A B −1 C J̄ A − B A J̇ A J̄ A .
Hence, the matrix Ḃ A − 2C A can be expressed as
T

T

T

Ḃ A − 2C A = J̄ A (Ḃ − 2C)J̄ A + B A J̇ A J̄ A − J̄ A J̇ A B A .
From the skew-symmetry of Ḃ − 2C it follows that the ﬁrst term represents
a skew-symmetric matrix. Also, by observing that the second term on the
right-hand side is the transpose of the third term, the matrix resulting from
the diﬀerence of those terms is skew-symmetric too. It can be concluded that
the whole matrix Ḃ A − 2C A is skew-symmetric.
Solution to Problem 7.10
For a given (r × 1) vector of end-eﬀector forces γ A , the problem can be formulated as that to ﬁnd the (n × 1) vector τ that satisﬁes the linear equation
T

J̄ A τ = γ A ,

(S7.6)

where J̄ A is given in (7.138), and minimize the quadratic cost functional
g(τ ) =

1
(τ − τ a )T B −1 (τ − τ a ),
2

where τ a is an (n × 1) vector of arbitrary torques.

78

7 Dynamics

This problem can be solved with the method of Lagrangian multipliers.
Consider the modiﬁed cost functional
1
T
g(τ , λ) = (τ − τ a )T B −1 (τ − τ a ) + λT (γ A − J̄ A τ )
2
where λ is an (r × 1) vector of Lagrangian multipliers. The solution has to
satisfy the necessary conditions:
 T

T
∂g
∂g
=0
= 0.
∂τ
∂λ
From the ﬁrst condition, it is
τ = B J̄ A λ + τ a ,

(S7.7)

while the second condition gives the constraint (S7.6). Substituting (S7.7) in
(S7.6) yields
T
T
λ = (J̄ A B J̄ A )−1 (γ A − J̄ A τ a ).
Then, substituting λ back in (S7.7) gives


T
T
T
τ = B J̄ A (J̄ A B J̄ A )−1 γ A + I − B J̄ A (J̄ A B J̄ A )−1 J̄ A τ a .
In view of (7.138) and (7.130), it is
J̄ A = B −1 J TA (J A B −1 J TA )−1 ,
and thus the above expression can be simpliﬁed into
T

τ = J TA (q)γ A + I − J TA (q)J̄ A (q) τ a
T

which is the sought solution. Notice that J TA is a right pseudo-inverse of J̄ A
weighted by B −1 , or else J̄ A is a right pseudo-inverse of J A weighted by B.
Solution to Problem 7.11
For a nonredundant manipulator, the core of the dynamic manipulability ellipsoid is given as in (7.149) which can be written as
J −T (q)B T (q)B(q)J −1 (q) = (J (q)B −1 (q)B −T (q)J T (q))−1 .
By analogy to the velocity manipulability ellipsoid (3.123), the dynamic manipulability measure can be deﬁned as

w (q) = det J (q)B −1 (q)B −T (q)J T (q) .
Since J and B are square matrices and det(B) > 0 for all q, then
w (q) =
with w(q) as in (3.56).

|det J(q) |
w(q)
=
det B(q)
det B(q)

8
Motion Control

Solution to Problem 8.1
The transfer function of the forward path of the scheme in Fig. 5.10 is
P (s) = CP (s)M (s) =

km KP (1 + sTP )
,
s2 (1 + sTm )

being CP (s) the transfer function of he PI position controller and
M (s) =

km
.
s(1 + sTm )

The transfer function of the return path is
H(s) = kT P .
It follows that the closed-loop input/output transfer function is
Θm (s)
P (s)
=
=
Θr (s)
1 + P (s)H(s)

1
kT P
,
s2 (1 + sTm )
1+
km KP kT P (1 + sTP )

while the closed-loop disturbance/output transfer function is
Θm (s)
M (s)
=−
=−
D(s)
1 + P (s)H(s)

sRa
kt KP kT P (1 + sTP )
.
s2 (1 + sTm )
1+
km KP kT P (1 + sTP )

Solution to Problem 8.2
Operating by the rules for block reduction on the scheme in Fig. 5.11, it
is worth moving the input to the block kT V onto Θm and subtracting the

80

8 Motion Control

Fig. S8.1. Reduction on the block scheme of position and velocity feedback control

output to Θr . Then, the reduced block scheme of the system becomes that in
Fig. S8.1.
The transfer function of the forward path is then
P (s) = KP CV (s)M (s) =

km KP KV (1 + sTV )
,
s2 (1 + sTm )

being CV (s) the transfer function of the PI velocity controller and M (s) the
transfer function of the actuator. The transfer function of the return path is


kT V
.
H(s) = kT P 1 + s
KP kT P
It follows that the closed-loop input/output transfer function with TV = Tm
is
1
P (s)
Θm (s)
kT P
=
=
.
Θr (s)
1 + P (s)H(s)
skT V
s2
1+
+
KP kT P
km KP kT P KV
The closed-loop disturbance/output transfer function is
M (s)
Θ(s)
=−
=−
D(s)
1 + P (s)H(s)

sRa
kt KP kT P KV (1 + sTm )
.
skT V
s2
1+
+
KP kT P
km KP kT P KV

Solution to Problem 8.3
First, it is worth observing that the block scheme of Fig. 8.9 has been obtained
from the scheme of Fig. 8.6 operating by the usual rules for block reduction.
In particular, the input to the block kT A has been moved onto Θ̇m , while the
output to the same block has been moved onto the output of CA (s). Likewise,
the input to the block kT V has been moved onto Θm , while the output to the

8 Motion Control

81

Fig. S8.2. Reduction on the block scheme of position, velocity and acceleration
feedback control

same block has been moved onto the input to KP ; then, the resulting two
blocks in parallel have been summed giving (kT P + skT V /KP ). Solving the
inmost feedback loop gives the transfer function
1
kt
km
sRa I
kv
=
=
,
sM (s) =
sRa I
kt kv
1 + sTm
1+
1+
kv kt
sRa I
and thus the block scheme of the system becomes that in Fig. S8.2.
Solving the inner feedback loop gives the transfer function
km
1 + sTm
G (s) =
km kT A KA (1 + sTA )
1+
(1 + sTm )


=

km


⎞,
TA
sTm 1 + km KA kT A
⎜
Tm ⎟
⎟
(1 + km KA kT A ) ⎜
1
+
⎝
⎠
(1 + km KA kT A )
⎛

and thus the block scheme of the system becomes that in Fig. S8.3.
The transfer function of the forward path is then
P (s) =

KP KV KA (1 + sTA ) 
KP KV CA (s)G (s)
=
G (s).
s
s2

The transfer function of the return path is


skT V
H(s) = kT P 1 +
.
KP kT P

82

8 Motion Control

Fig. S8.3. Further reduction on the block scheme of position, velocity and acceleration feedback control

It follows that the closed-loop input/output transfer function with TA = Tm
is
1
P (s)
Θm (s)
kT P
=
=
,
Θr (s)
1 + P (s)H(s)
skT V
s2 (1 + km KA kT A )
1+
+
KP kT P
km KP kT P KV KA
while the closed-loop disturbance/output transfer function is
G (s)
Θm (s)
s
=−
=−
D(s)
1 + P (s)H(s)

sRa
kt KP kT P KV KA (1 + sTA )
.
skT V
s2 (1 + km KA kT A )
1+
+
KP kT P
km KP kT P KV KA

Solution to Problem 8.4
With the given data, the condition Fm
kv kt /Ra is veriﬁed and km =
2 rad/(V · s), Tm = 7.2 s, kT P = 1. The closed-loop input/output transfer
function is
(1 + sTP )
.
W (s) =
s2 (1 + sTm )
1 + sTP +
km KP kT P
Choosing TP = 10Tm = 72 s leads to the root locus in Fig. S8.4 as a function
of the gain K  = 20KP .
If a damping ratio ζ = 0.4 is desired, the resulting controller gain is
KP = 0.0013. For such value, two complex poles (−0.062, ±j0.142) and a real
pole (−0.015) are obtained. Hence, the above transfer function can be written
as
(1 + 72s)
.
W (s) =
(1 + 5.161s + 41.62s2)(1 + 66.66s)

8 Motion Control

83

0.2

image axis

0.1
0
−0.1
−0.2
−0.2

−0.1

0
0.1
real axis

0.2

Fig. S8.4. Root locus for the position feedback control scheme

Correspondingly, the disturbance rejection factor is XR = 0.0013, while an
estimate of the output recovery time is TR = 72 s. These values reveal the
poor disturbance rejection performance of the closed-loop system under simple
position feedback control.
Solution to Problem 8.5
With the given data, the condition Fm
kv kt /Ra is veriﬁed and km =
2 rad/(V· s), Tm = 7.2 s, kT P = 1, kT V = 1. Choosing TV = Tm yields the
closed-loop input/output transfer function
W (s) =

1
.
s
s2
1+
+
KP
2KP KV

With the given speciﬁcations for ζ and ωn , the controller gains can be determined according to (8.30) and (8.31), giving KP = 25 and KV = 8, and thus
the above transfer function becomes
W (s) =

1
1 + 0.04s + 0.0025s2

which has two complex poles (−8, ±j18.33). Correspondingly, the disturbance
rejection factor is XR = 200, while an estimate of the output recovery time is
TR = 7.2 s. These values reveal the good disturbance rejection performance of
the closed-loop system under position and velocity feedback control; however,
it is not possible to further decrease the output recovery time which is limited
by the time constant Tm of the drive system.

84

8 Motion Control

Solution to Problem 8.6
With the given data, the condition Fm
kv kt /Ra is veriﬁed and km =
2 rad/(V · s), Tm = 7.2 s, kT P = 1, kT V = 1, kT A = 1. Choosing TA = Tm
yields the closed-loop input/output transfer function
W (s) =

1
.
s
s2 (1 + 2KA )
1+
+
KP
2KP KV KA

With the given speciﬁcations for ζ, ωn and XR , the controller gains can be
determined according to (8.39), (8.40), (8.41) giving KP = 25, KV = 32,
KA = 0.5, and thus the above transfer function becomes
W (s) =

1
1 + 0.04s + 0.0025s2

which is the same transfer function of the previous problem. Remarkably,
in the case of position, velocity and acceleration feedback control, the disturbance rejection performance is improved in terms of XR , although the
output recovery time TR is still the same as in the previous case. Finally,
in order to reconstruct acceleration from velocity measurement, a ﬁrst-order
ﬁlter as in Fig. 8.11 can be used. With the given ζ and ωn , the closed-loop
system bandwidth is ω3 = 26.84 rad/s, and then the ﬁlter bandwidth shall
be chosen wide enough with respect to the drive system bandwidth, e.g.,
ω3f = kf = 400 rad/s.
Solution to Problem 8.7
As for the block scheme in Fig. 8.12, where the saturation block can be neglected, it is worth moving both the output of the block Tm /km and the
output of the block 1/km onto the input to the block kP (1 + sTP )/s; also the
quantities Θ̇d and Θ̈d can be generated from Θd as sΘd and s2 Θd , respectively.
Therefore, the reduced block scheme becomes that in Fig. S8.5.
The relationship between the desired input and the new reference input
is then



 2
s
s
s
T
m
+
Θd (s)
Θr (s) = kT P +
km
km KP (1 + sTP )


s2 (1 + sTm )
= kT P +
Θd (s).
km KP (1 + sTP )
As for the block scheme in Fig. 8.13, where the saturation block can be
neglected, it is worth moving both the output of the block 1/km KV and
the output of the block kT V onto the input to the block KP ; as above, the
quantities Θ̇d and Θ̈d can be generated from Θd as sΘd and s2 Θd , respectively.
Therefore, the reduced block scheme becomes that in Fig. S8.6.

8 Motion Control

85

Fig. S8.5. Reduction on the block scheme of position feedback control with decentralized feedforward compensation

Fig. S8.6. Reduction on the block scheme of position and velocity feedback control
with decentralized feedforward compensation

The relationship between the desired input and the new reference input
is then


skT V
s2

Θr (s) = kT P +
+
Θd (s).
KP
km KP KV
As for the block scheme in Fig. 8.14, where the saturation block can be
neglected, it is worth moving both the output of the block (kT A + 1/km KA )
and the output of the block kT V onto the input to the block KP ; again, the
quantities Θ̇d and Θ̈d can be generated from Θd as sΘd and s2 Θd , respectively.
Therefore, the reduced block scheme becomes that in Fig. S8.7.

86

8 Motion Control

Fig. S8.7. Reduction on the block scheme of position, velocity and acceleration
feedback control with decentralized feedforward compensation

The relationship between the desired input and the new reference input
is then


skT V
(1 + km KA kT A )s2
Θr (s) = kT P +
+
Θd (s).
KP
km KP KV KA
Solution to Problem 8.8
The block scheme in Fig. 8.15 is clearly equivalent to that in Fig. 8.12, since
the block KP (1 + sTP )/s can be split into two blocks in parallel, i.e., KP TP
and KP /s.
Concerning the block scheme in Fig. 8.28, it is worth considering the input
U and the output Y of the block KV (1 + sTV )/s. By neglecting the eﬀect of
the saturation block and setting E = kT P (Θd − Θm ), the input U is given by
U = KP E + kT V (Θ̇d − Θ̇m ) +

= KP 1 +

1
Θ̈d
km KV


1
kT V
Θ̈d .
s E+
KP kT P
km KV

Then the output can be expressed (TV = Tm ) as




KV (1 + sTV )
1
kT V
Θ̈d
KP 1 +
Y =
s E+
s
KP kT P
km KV

= KP KV

1
+ TV
s




1
kT V
1
+ Tm Θ̈d
s E+
KP kT P
km s

KV TV kT V
KP KV
1
Tm
Θ̇d +
Θ̈d
+
+
s E+
s
kT P
km
km


1+


KV kT V
= KP KV TV +
kT P

8 Motion Control

87

corresponding to the block scheme in Fig. 8.16.
In a similar way, concerning the block scheme in Fig. 8.29, it is worth
considering the input U and the output Y of the block KA (1 + sTA )/s. By
neglecting the eﬀect of the saturation blocks and setting E = kT P (Θd − Θm ),
the input U is given by


 
1
U = KV KP E + kT V (Θ̇d − Θ̇m ) + kT A +
Θ̈d − kT A Θ̈m
km KA


KV kT V
kT A 2
1
Θ̈d .
= KP KV +
s+
s E+
kT P
kT P
km KA
Then the output can be expressed (TA = Tm ) as


 

1
KV kT V
kT A 2
1
Θ̈d
+ TA
s+
s E+
Y = KA
KP KV +
s
kT P
kT P
km KA

=

KV kT V KA
KP KV KA
+
kT P
s



KV kT V KA TA + KA kT A
KA TA kT A 2
+
s E
s+
kT P
kT P
1
Tm
Θ̇d +
Θ̈d
+
km
km

KP KV KA TA +

corresponding to the block scheme in Fig. 8.17.
Solution to Problem 8.9
From (8.75), the following matrix inequality can be written:
Bm I ≤ B −1 ≤ BM I
where all three matrices are positive deﬁnite. Setting
% =
B

2
I,
BM + Bm

which is positive deﬁnite, allows writing the matrix inequality
2Bm
2BM
% ≤
I ≤ B −1 B
I.
BM + Bm
BM + Bm
Subtracting the identity matrix from each term gives
% − I ≤ αI
−αI ≤ B −1 B
where
α=

BM − Bm
BM + Bm

88

8 Motion Control
q_d=[pi/4 -pi/2]

time

q_d=[-pi -3pi/4]

K_d

arm
q_d

+
+

+
K_p

-

dq

inv(B(q))(tau-tau')

q

gravity
compensation
g(q)

plot

Fig. S8.8. Simulink block diagram of joint space PD control with gravity compensation
joint 1 pos

joint 2 pos

0.8

−1.55

0.75

−1.6

[rad]

[rad]

−1.5

0.7
0.65
0.6
0

−1.65
−1.7

0.5

1

1.5

2

2.5

−1.75
0

[s]

0.5

1

1.5

2

2.5

[s]

Fig. S8.9. Time history of the joint positions with joint space PD control with
gravity compensation for the ﬁrst posture

and 0 < α < 1. The matrix in the middle is lower bounded by a negative
deﬁnite diagonal matrix and upper bounded by a positive deﬁnite diagonal
matrix, where these two matrices have the same norm. Thus, it follows that
% − I ≤ α.
B −1 B

8 Motion Control
joint 2 pos

−3.1

−2.3

−3.15

−2.35

−3.2

−2.4

[rad]

[rad]

joint 1 pos

−3.25
−3.3
−3.35
0

89

−2.45
−2.5

0.5

1

1.5

2

2.5

0

[s]

0.5

1

1.5

2

2.5

[s]

Fig. S8.10. Time history of the joint positions with joint space PD control with
gravity compensation for the second posture

Solution to Problem 8.10
The control scheme in Fig. 8.20 is used with the same matrix gains as for case
F in Sect. 8.7, i.e.,
K P = 3750I
K D = 750I.
The initial arm postures for the two cases are chosen as q − [ 0.1 0.1 ]T . The
various terms of the dynamic model are computed by using the minimum
parameterization in the solution to Problem 7.4.
The resulting Simulink block diagram is shown in Fig. S8.8, where both
desired postures can be assigned. The arm is simulated as a continuous-time
system using a variable-step integration method with a maximum step size of
1 ms. All the blocks of the controller are simulated as discrete-time subsystems
with the given sampling time of 1 ms.
The with the solution can be found in Folder 8 10.
The resulting joint positions for the two postures are shown in Figs. S8.9
and S8.10, respectively. The dashed line indicates the desired joint position,
while the solid line indicates the actual joint position. It can be seen that both
postures are reached at steady state.
Solution to Problem 8.11
With reference to the dynamic model in the solution to Problem 7.4, the
presence of a payload alters the dynamic parameters of augmented Link 2.
Since the payload is a mass mL concentrated at the tip, the only parameter
that varies is the mass of the augmented link which becomes
m2 = m2 + mL ;
in fact, both the inertia ﬁrst moment m2 C2 and inertia moment I¯2 remain
the same because they are evaluated with respect to a frame located at the
tip. Hence, only parameters π1 and π3 in (S7.2) have to be recomputed with
the modiﬁed m2 .

90

8 Motion Control
K_p = 5
K_v = 10

time

K_p = 6.25
K_v = 32
computed torque
tau_d

[T,ddq_d]

k_fa

[T,dq_d]

k_fv

[T,q_d]

k_fp

+
-

K_p

+
+
+
-

voltage-controlled
arm
K_v

PI

+
+

M

q
dq

k_tv

k_tp
plot

Fig. S8.11. Simulink block diagram of joint space computed torque control with
feedforward compensation of the diagonal terms of the inertia matrix and of gravitational terms, and independent joint control with position and velocity feedback

The desired trajectories for the two joints are generated via trapezoidal velocity proﬁles with maximum velocities q̇c1 = 3π/4 rad/s and q̇c2 = π/3 rad/s,
respectively.
The control scheme in Fig. 8.19 is used with feedforward compensation
of the diagonal terms of the inertia matrix and of gravitational terms. As for
the decentralized controller, the independent joint control with position and
velocity feedback in Fig. 5.11 is adopted.
Initially, the same gains for each joint servo are chosen as for case A in
Sect. 8.7, i.e., (kT P = kT V = 1)
KP = 5

KV = 10,

corresponding to ωn = 5 rad/s and ζ = 0.5, and thus to a closed-loop bandwidth ω3 = 6.36 rad/s. This gives a disturbance rejection factor XR = 50.
Then, by observing that the required trajectory is faster than the corresponding trajectory in Sect. 8.7, a larger bandwidth is imposed. Setting
ωn = 10 rad/s and ζ = 0.8 gives ω3 = 10.78 rad/s. From (8.30) and (8.31), the
gains become
KV = 32
KP = 6.25

8 Motion Control
joint torques

4000

4000

2000

2000
[Nm]

[Nm]

joint torques

0
−2000
−4000
0

0
−2000

0.5

1

1.5

2

2.5

−4000
0

0.5

1

[s]
joint pos errors

2

2.5

2

2.5

joint pos errors
0.05

[rad]

[rad]

1.5
[s]

0.05

0

−0.05
0

91

0.5

1

1.5
[s]

2

2.5

0

−0.05
0

0.5

1

1.5
[s]

Fig. S8.12. Time history of the joint torques and position errors with joint space
computed torque control with feedforward compensation of the diagonal terms of
the inertia matrix and of gravitational terms, and independent joint control with
position and velocity feedback; left—ﬁrst choice of servo gains, right—second choice
of servo gains

and, from (8.33), it is XR = 200, i.e., a disturbance rejection factor which is
four times as much as the previous one.
The resulting Simulink block diagram is shown in Fig. S8.11, where both
choices of servo gains can be executed. The arm is voltage-controlled as in
the scheme of Fig. 8.3 and is simulated as a continuous-time system using
a variable-step integration method with a maximum step size of 1 ms. All
the blocks of the controller are simulated as discrete-time subsystems with
the given sampling time of 1 ms; in particular, the PI block is discretized
assuming a zero-order hold on the inputs.
The ﬁles with the solution can be found in Folder 8 11.
The resulting joint torques and position errors are shown in Fig. S8.12.
The solid line refers to Joint 1, while the dashed line refers to Joint 2. It can
be seen that the tracking performance with the second choice of the servo
gains is considerably improved, at the expense of a sharper behaviour of joint
torques, though.

92

8 Motion Control
noncompensated
load mass

time

compensated
load mass

[T,ddq_d]

+
-

[T,dq_d]

+

[T,q_d]

+

inertia

arm

+

K_d

B(q)u

+
+

+

q

nonlinear
compensation

K_p

-

dq

inv(B(q))(tau-tau')

n(q,dq)

e

plot

Fig. S8.13. Simulink block diagram of joint space inverse dynamics control
joint pos errors

joint pos errors

−4

0.02

5

x 10

[rad]

[rad]

0.01
0

0

−0.01
−0.02
0

0.5

1

1.5
[s]

2

2.5

−5
0

0.5

1

1.5

2

2.5

[s]

Fig. S8.14. Time history of the joint position errors with joint space inverse dynamics control; left—noncompensated load mass, right—compensated load mass

Solution to Problem 8.12
The modiﬁcation of the dynamic model to account for the load mass and
the generation of joint trajectories are accomplished as in the solution to
Problem 8.11.
The control scheme in Fig. 8.22 is used where the matrix gains are chosen
as
K D = 16I
K P = 100I
corresponding to an error dynamics for both joints characterized by ωni =
10 rad/s and ζi = 0.8 for i = 1, 2.
The resulting Simulink block diagram is shown in Fig. S8.13, where both
the noncompensated load mass case and the compensated load mass case can

8 Motion Control

93

time

variables
initialization
robustness
UV

constant
inertia

+
[T,ddq_d]

tau
arm

+
-

[T,dq_d]

+

[T,q_d]

+
-

B_h

+

K_d

dq

+
+

inv(B(q))(tau-tau')
q

+
friction and gravity
compensation
K_p
F_vdq+g(q)
e

plot

Fig. S8.15. Simulink block diagram of joint space robust control
joint torques

joint pos errors

−3

4000

1.5

x 10

1
2000
[rad]

[Nm]

0.5
0

0
−0.5

−2000
−1
−4000
0

0.5

1

1.5
[s]

2

2.5

−1.5
0

0.5

1

1.5

2

2.5

[s]

Fig. S8.16. Time history of the joint torques and position errors with joint space
robust control

be executed. The arm is torque-controlled as in the scheme of Fig. 8.4 and
is simulated as a continuous-time system using a variable-step integration
method with a maximum step size of 0.1 ms. All the blocks of the controller
are simulated as discrete-time subsystems with the given sampling time of
1 ms.
The ﬁles with the solution can be found in Folder 8 12.
The resulting joint position errors in the two cases are shown in Fig. S8.14.
The solid line refers to Joint 1, while the dashed line refers to Joint 2. It
can be seen that the tracking performance is excellent when load mass is
compensated, while it is degraded both during transient and at steady state

94

8 Motion Control
time

variables
initialization

adaptive
control

[T,ddq_d]
+

+

[T,dq_d]

ddq_r

arm

+

Lam

Y( . )pi_h

+
+

[T,q_d]

+
+
-

q

dq_r

+

Lam

dq

inv(B(q))(tau-tau')

+
-

K_d
sig

e

plot

Fig. S8.17. Simulink block diagram of joint space adaptive control
joint pos errors

−3

1

x 10

load mass estimate
20
15
[kg]

[rad]

0.5
0
−0.5
−1
0

10
5

0.5

1

1.5

2

2.5

0
0

0.5

[s]

1

1.5

2

2.5

[s]

Fig. S8.18. Time history of the joint position errors and of the load mass estimate
with joint space adaptive control

when the load mass is not compensated. Also, in the case of noncompensated
load mass, steady-state errors occur.
Solution to Problem 8.13
The modiﬁcation of the dynamic model to account for the load mass and
the generation of joint trajectories are accomplished as in the solution to
Problem 8.11.
% = B̄)
The control scheme in Fig. 8.23 is used with constant inertia (B
and compensation of friction and gravity (%
n = F v q̇ + g). The gains deﬁning
the error dynamics are chosen as in the solution to Problem 8.12, i.e.,
K P = 100I

K D = 16I;

8 Motion Control

95

furthermore, the matrix P in (8.81) is chosen as
P = I,
and the gain ρ and the thickness of the boundary layer in (8.89) are respectively chosen as
ρ = 70
= 0.001.
The resulting Simulink block diagram is shown in Fig. S8.15. The arm
is simulated as a continuous-time system using a variable step integration
method with a maximum step size of 1 ms. All the blocks of the controller are
simulated as discrete-time subsystems with the given sampling time of 1 ms.
The ﬁles with the solution can be found in Folder 8 13.
The resulting joint torques and position errors are shown in Fig. S8.16.
The solid line refers to Joint 1, while the dashed line refers to Joint 2. It can be
seen that, in spite of the imperfect dynamic model compensation, the tracking
performance is satisfactory, thanks to the use of the robustness action.
Solution to Problem 8.14
A minimum parameterization of the dynamic model is obtained as in the solution to Problem 7.4, with suitable modiﬁcation of augmented Link 2 parameters as in the solution to Problem 8.11. The generation of joint trajectories
is also accomplished as in the solution to Problem 8.11.
The control scheme in Fig. 8.26 is used. In terms of the new reference
velocity and acceleration vectors in (8.92), the regressor in (S7.3) becomes

= a1 ϑ̈r1 + gc1
y11

y12
= ϑ̈r1

y13
= (2a1 c2 + a2 )ϑ̈r1 + (a1 c2 + a2 )ϑ̈r2 − a1 s2 ϑ̇r1 ϑ̇2 − a1 s2 ϑ̇1 ϑ̇r2 − a1 s2 ϑ̇2 ϑ̇r2
+gc12

y14
= ϑ̈r1 + ϑ̈r2

y15
= ϑ̈r1 + kr2 ϑ̈r2

y21
=0

=0
y22

y23
= (a1 c2 + a2 )ϑ̈r1 + a2 ϑ̈r2 + a1 s2 ϑ̇1 ϑ̇r1 + gc12

y24 = ϑ̈r1 + ϑ̈r2

2
y25
= kr2 ϑ̈r1 + kr2
ϑ̈r2 .

In view of the presence of a concentrated tip load mass, the only variations
occurring on the parameters in (S7.2) are:
Δπ1 = a1 mL

Δπ3 = a2 mL .

96

8 Motion Control

Hence, the control law in (8.98) can be formally written as
u = Y  (q, q̇, q̇r , q̈ r )π + F v q̇ r + mL y L + K D σ
where Y  is computed as above and


 
+ a2 y13
a1 y11
yL =
,


a1 y21
+ a2 y23
being π = mL the only unknown parameter.
The gains are chosen as
Λ = 10I

K D = 10000I

kπ = 0.005;

the initial estimate of π is set to 0, and the true value of mL is utilized only
to update the simulated arm model.
The resulting Simulink block diagram is shown in Fig. S8.17. The arm
is simulated as a continuous-time system using a variable-step integration
method with a maximum step size of 1 ms. All the blocks of the controller are
simulated as discrete-time subsystems with the given sampling time of 1 ms.
The ﬁles with the solution can be found in Folder 8 14.
The resulting joint position errors and load mass estimate are shown in
Fig. S8.18. For the joint position errors, the solid line refers to Joint 1, while
the dashed line refers to Joint 2. It can be seen that the tracking performance
is satisfactory, thanks to the adaptive action on the load mass; in this case,
the parameter estimate converges to the true value.
Solution to Problem 8.15
The modiﬁcation of the dynamic model to account for the load mass is accomplished as in the solution to Problem 8.11.
The control scheme in Fig. 8.29 is used with the same matrix gains as for
case K in Sect. 8.7, i.e.,
K P = 16250I

K D = 3250I.

The initial tip positions for the two postures are chosen as p − [ 0.1 0.1 ]T .
The resulting Simulink block diagram is shown in Fig. S8.19, where both
desired positions can be assigned. The arm is simulated as a continuous-time
system using a vaiable-step integration method with a maximum step size of
1 ms. All the blocks of the controller are simulated as discrete-time subsystems
with the given sampling time of 1 ms.
The ﬁles with the solution can be found in Folder 8 15.
The resulting components of tip position for the two postures are shown
in Figs. S8.20 and S8.21, respectively. The dashed line indicates the desired
tip coordinate, while the solid line indicates the actual tip coordinate. It can
be seen that both postures are reached at steady state.

8 Motion Control
p_d=[0.5 0.5]

97
time

p_d=[0.6 -0.2]

J_a(q)
Jacobian
transpose
K_d

-

arm

+
p_d

tr(J_a(q))

+

+
K_p

p

dq

+
inv(B(q))(tau-tau')

q

gravity
compensation

direct
kinematics

sinusoidal
functions

k(q)

cs(q)

g(q)

plot

Fig. S8.19. Simulink block diagram of operational space PD control with gravity
compensation
y−pos
0.55

0.5

0.5
[m]

[m]

x−pos
0.55

0.45
0.4
0.35
0

0.45
0.4

0.5

1

1.5

2

2.5

0.35
0

0.5

[s]

1

1.5

2

2.5

[s]

Fig. S8.20. Time history of the tip position components with operational space PD
control with gravity compensation for the ﬁrst posture

Solution to Problem 8.16
The modiﬁcation of the dynamic model to account for the load mass is accomplished as in the solution to Problem 8.11. The desired tip trajectory is
generated in terms of the path coordinate s via a trapezoidal velocity proﬁle
with maximum velocity ṡc = 1.5.
The control scheme in Fig. 8.30 is used with the matrix gains:
K P = 100I

K D = 16I.

98

8 Motion Control
y−pos
−0.15

0.6

−0.2
[m]

[m]

x−pos
0.65

0.55
0.5

−0.25
−0.3

0.45
0

0.5

1

1.5

2

2.5

0

0.5

1

[s]

1.5

2

2.5

[s]

Fig. S8.21. Time history of the tip position components with operational space PD
control with gravity compensation for the second posture
noncompensated
load mass

time

compensated
load mass

[T,ddp_d]
[T,dp_d]

+
-

+

Jacobian
inverse

+

K_d

+

inertia
inv(J_a(q))

arm

B(q)u

+
+

[T,p_d]

+
-

dq
inv(B(q))(tau-tau')

q

K_p
nonlinear
compensation
e

Jacobian
derivative

direct
kinematics

sinusoidal
functions

k(q)

cs(q)

n(q,dq)

dJ_a(q,dq)

analytical
Jacobian
J_a(q)
plot

Fig. S8.22. Simulink block diagram of operational space inverse dynamics control

The resulting Simulink block diagram is shown in Fig. S8.22, where both
the noncompensated load mass case and the compensated load mass case
can be executed. The arm is simulated as a continuous-time system using a
variable-step integration method with a maximum step size of 1 ms. All the
blocks of the controller are simulated as discrete-time subsystems with the
given sampling time of 1 ms.
The ﬁles with the solution can be found in Folder 8 16.

8 Motion Control
tip pos errors

−3

6

x 10

tip pos errors

−4

4

99

x 10

4
2
[m]

[m]

2
0

0

−2
−2
−4
−6
0

0.5

1

1.5
[s]

2

2.5

−4
0

0.5

1

1.5

2

2.5

[s]

Fig. S8.23. Time history of the components of tip position error with operational space inverse dynamics control; left—noncompensated load mass, right—
compensated load mass

The resulting components of tip position error in the two cases are shown
in Fig. S8.23. The solid line refers to x component, while the dashed line refers
to y component. It can be seen that the performance is excellent when the
load mass is compensated, while it is degraded both during transient and at
steady state when the load mass is not compensated.

9
Force Control

Solution to Problem 9.1
Computing the time derivative of odd,e = RTd (oe − od ) gives
T

ȯdd,e = RTd (ȯe − ȯd ) − Ṙd (oe − od ).

(S9.1)

In view of (3.10), (3.7) and (3.11) the following equality holds
T

Ṙd = −RTd S(ω d ) = −S(RTd ω d )RTd = −S(ω dd )RTd

(S9.2)

which, replaced in (S9.1), gives (9.10).
To derive (9.11), consider the equality
d

T

Ṙe = Ṙd Re + RTd Ṙe = −S(ω dd )Red + S(ω de )Red = S(ω dd,e )Red ,
where (S9.2), (3.10), and (3.11) have been used. It follows that (3.64) can be
rewritten as
ωdd,e = T (φd,e )φ̇d,e
being φd,e the vector of Euler angles extracted from Rde and ω dd,e the angular
d

velocity corresponding to Ṙe . Hence, expression (9.11) follows.
Solution to Problem 9.2
Pre-multiplying by K both sides of the equality
dxr,e = dxr,d − dxe,d ,
and using (9.21) and (9.20) gives
he = Kdxr,d − KK −1
P he

102

9 Force Control

and thus

(I 6 + KK −1
P )he = Kdxr,d ,

from which expression (9.22) follows. Equality (9.23) is obtained replacing (9.22) into (9.20).
Solution to Problem 9.3
Similarly to Example 9.2, all the quantities can be referred to a base frame and
control law with force measurement (9.30), (9.31) can be adopted. To compute
the model of the contact force, in view of the geometry of the environment, it
is useful to consider also a rotated base frame with axes parallel to the axes
xc and yc of Fig. 9.16 and with the same origin of the base frame of axes x0
and y0 . The corresponding rotation matrix is
√ 
 √
1/√2 −1/√ 2
Rc =
.
1/ 2 1/ 2
In the rotated base frame, the environment stiﬀness matrix has the simple
expression
K c = diag{0, ky },
corresponding to the absence of interaction forces along the direction of axis
xc (f ce = [ 0 fyc ]T ). The elastic force in the rotated base frame has the
expression
f ce = K c (oce − ocr ),
being oce the end-eﬀector position and ocr the equilibrium position of the plane
in the rotated base frame. The above expression can be rewritten in the base
frame in the form
f e = K(oe − or ),


with
K = Rc K

c

RTc

= ky

1/2 −1/2
−1/2 1/2



and or = [ 1 0 ]T .
The impedace parameters can be set similarly to Example 9.2, on the
basis of the equations
mdx ẍce + kDx ẋce + kP x xce = kP x xcd
mdy ÿec

+ kDy ẏec + (kP y + ky )yec = ky yrc + kP y ydc

referred to the rotated base frame, where the diagonal matrices
M cd = diag{mdx , mdy }

K cD = diag{kDx , kDy }

K cP = diag{kP x , kP y }

deﬁne an active impedance in the rotated base frame. The corresponding
impedance matrices referred to the base frame are
M d = Rc M cd RTc

K D = Rc M cd RTc

K P = Rc M cd RTc .

9 Force Control
variables
initialization

103
time

f_e

operational space
decoupled arm

[T,ddo_d]

environment
K(o_e−o_r)

u = ddo_e
[T,do_d]
K_d

f_e
do_e

inv(M_d)

o_e

[T,o_d]
K_p

e

plot

Fig. S9.1. SIMULINK block diagram of impedance control.

Notice that, diﬀerently from Example 9.2, the interaction occurs along yc .
Therefore, for the unconstrained motion along xc , the behaviour is determined
by
kDx
kP x
ζx = √
,
ωnx =
mdx
2 mdx kP x
while, for the constrained motion along axis yc , the behaviour is determined
by
&
kP y + ky
kDy
ωny =
ζy = '
.
mdy
2 mdy (kP y + ky )
With the choice
mdx = mdy = 100

kDx = kDy = 1600

kP x = kP y = 5000,

and the given value of environment stiﬀness ky , the dynamics is characterized
by
ωnx = 7.07 rad/s
ζx = 1.13
and
ωny = 10 rad/s

ζy = 0.8.

The desired tip trajectory is generated in terms of the path coordinate s
via a trapezoidal velocity proﬁle with maximum velocity ṡc = 0.5.
The resulting Simulink block diagram is shown in Fig. S9.1. The arm is
simulated as a continuous-time system using the a variable-step integration
method with a minimum step size of 1 ms. All the blocks of the controller are
simulated as discrete-time subsystems with the given sampling time of 1 ms.

104

9 Force Control
pos error

force

0.06

500
x

0.04

x
[N]

[m]

0.02
0

0
y

−0.02

y

−0.04
−0.06
0

0.5

1

1.5

2

−500
0

2.5

0.5

1

[s]

1.5

2

2.5

2

2.5

[s]

pos error

force

0.06

500

0.04
xc

0

[N]

[m]

0.02

xc

0

yc

−0.02
yc

−0.04
−0.06
0

0.5

1

1.5

2

2.5

−500
0

[s]

0.5

1

1.5
[s]

Fig. S9.2. Time history of the tip position error and of the contact force with
impedance control. Top: components in the base frame. Bottom: components in the
rotated base frame.

The ﬁles with the solution can be found in Folder 9 3.
The resulting tip position error and contact force are shown in Fig. S9.2,
both referred to the base frame (top) and to the rotated base frame (bottom).
It can be veriﬁed that the position error keeps small during the task execution,
ensuring a bounded contact force. In particular, it can be observed that the
position error is null along the unconstrained motion direction (axis xc ), where
the force is null; on the other hand, the force response along the constrained
motion direction (axis yc ) remains bounded and converges to a constant value
which depends on the plane stiﬀness and undeformed position as well as on
the active compliance.
Solution to Problem 9.4
From the block scheme in Fig. 9.8, at the equilibrium, it is ẋ ≡ 0, ẍ ≡ 0.
Then, the input to the block K P must be null, which implies
xd + xF − xe = 0.
In view of (9.21) and (9.41), it follows that
xd + C F K(xr − xe ) + f d − xe = 0,

9 Force Control

105

and thus the equilibrium location satisﬁes the equation
xe = xd + C F K(xr − xe ) + f d .

Solution to Problem 9.5
The norm of vector
 = he − S f λ
with weighting matrix W is deﬁned as
√
 W = T W .

(S9.3)

The vector λ which minimizes the norm (S9.3) can computed also as the
solution of a minimization problem for the quadratic cost functional
g() =

1 T
 W .
2

The solution has to satisfy the necessary condition


∂g
∂λ

T
=0

which gives
−S Tf W (he − S f λ) = 0,
and thus

λ = (S Tf W S f )−1 S Tf W he .

Solution to Problem 9.6
Taking into account that CK = KC = I 6 , Equation (9.70) can be rewritten
in the form
K  = S f (S Tf CS f )−1 S Tf CK
which, considering the deﬁnition (9.55) of weighted pseudo-inverse with weight
C, can be rewritten as
K  = S f S †f K = P f K.

Solution to Problem 9.7
Assuming that the screwdriver can move along the slot of the screw, it is worth
choosing the constraint frame attached to the screwdriver as in Fig. S9.3.
Natural constraints can be determined ﬁrst. Motion constraints describe
the impossibility to generate arbitrary linear velocities along axes yc , zc and

106

9 Force Control

Fig. S9.3. Driving a screw in a hole.
Table S9.1. Natural and artiﬁcial constraints for the task of Fig. S9.3.
Natural
Constraints
ṗcy
ṗcz
ωxc
ωyc
fxc
μcz

Artiﬁcial
Constraints
fyc
fzc
μcx
μcy
ṗcx
ωzc

angular velocities about axes xc , yc . Force constraints describe the impossibility to exert arbitrary forces along axis xc and moment about axis zc .
As a consequence, artiﬁcial constraints regard the variables not subject to
natural constraints. Hence, with reference to natural velocity constraints along
axes yc , zc and about xc , yc , it is possible to specify artiﬁcial constraints for
forces along yc , zc and moments about xc , yc . Also, with reference to natural
generalized force constraints along axis yc and about axis zc , it is possible to
specify artiﬁcial constraints for linear velocity along xc and angular velocity
about zc . The set of constraints is summarized in Table S9.1. Notice that, in
the case of a frictionless groove, once ωzc is speciﬁed, then ṗcz is determined
according to the pitch of the screw.
Solution to Problem 9.8
The control input (9.77) referred to the constraint frame, in view of the expressions of S cf , S cv and C  c in Example 9.4, can be computed as
αcx = αν
αcy = c2,2 fλ .

9 Force Control

107

Taking into account (9.81), the control input αcy can be expressed as
αcy = −c2,2 kDλ λ̇ + kP λ (λd − λ),
where a constant desired force λd was considered. In view of the equalities
ȯcy = c2,2 λ̇ and λ = fyc , the equation can be rewritten in the form
αcy = −kDλ ȯcy + kP λ (λd − fyc ),
showing that the the control action in the force controlled subspace consists
of a proportional force control with inner velocity loop.
Solution to Problem 9.9
The computation of S c†
f using the compliance matrix


c1,2
c2,2

c
C = 1,1
c1,2
c



as a weighting matrix gives
cT c c −1 cT c
S c†
S f C = [ c−1
2,2 c1,2
f = (S f C S f )

1].

On the other hand, computing S †f in the base frame gives
S †f

=

(S Tf CS f )−1 S Tf C

1
= √
2



c1,2 − c2,2
c2,2

c1,2 + c2,2
c2,2


,

T
where C = Rc C c RTc . Notice that S †f = S c†
f Rc and the same transformation

rule holds for S †v . Also, if f ce = [ fxc

T

fyc ] , then

†
c
λ = S c†
f f e = Sf f e =

c1,2 c
f + fyc ,
c2,2 x

independently from the frame to which S †f and f e are referred. On the other
T

hand, if the contact force f ce belongs to subspace R(S cf ), i.e., f ce = [ 0 fyc ] ,
then it is λ = fyc independently from the particular weighting matrix.
Analogously, the computation of S c†
v using the stiﬀness matrix


k
k1,2
K c = 1,1
k1,2 k2,2
as a weighting matrix gives
cT
c c −1 cT
S c†
Sv K c = [ 1
v = (S v K S v )

−1
k1,1
k1,2 ]

108

9 Force Control

in the constraint frame, and
S †v

1
=√
2



k1,1 − k1,2
k1,1

k1,1 + k1,2
k1,1



in the base frame.
Solution to Problem 9.10
In view of the results of Example 9.4, the hybrid control law can be designed
by choosing the control inputs αν and fλ according to (9.80) and (9.84),
respectively, with suitable control gains.
In detail, the control gains in (9.80) can be set to
kP ν = 16

kIν = 100,

corresponding to ωnν = 10 rad/s and ζν = 0.8 for the dynamics of the velocity
error νd − ν.
The same dynamics can be imposed to the force λ with the choice
kDλ = 16

kP λ = 100,

in the case that the stiﬀness of the environment is known.
In the case that only an estimate of the stiﬀness of the environment is
available, the quantity Lf = lf has the value
lf =
and thus

ĉ2,2
c2,2

˙
c
λ̂ = ĉ−1
2,2 ȯy .

3
Assuming, for example, ĉ−1
2,2 = 4 · 10 N/m, then lf = 1.25, corresponding
to ωnλ = 11.18 rad/s and ζλ = 0.71 for the dynamics of the force.
The initial tip position is chosen on the plane as oe (0) = [ 1 0 ]T . The
desired velocity along axis xc is set as in Problem 9.3, while the desired force
along axis yc is set to −50 N.
The resulting Simulink block diagram is shown in Fig. S9.4. The arm
is simulated as a continuous-time system using a variable-step integration
method with a minimum step size of 1 ms. All the blocks of the controller are
simulated as discrete-time subsystems with the given sampling time of 1 ms.
The ﬁles with the solution can be found in Folder 9 10.
The resulting tip velocity error along xc and contact force along yc are
shown in Fig. S9.5. It can be veriﬁed that the velocity error keeps small during
task execution; also, the contact force reaches the desired value at steady state.
In Fig. S9.6, the continuous line represents the end-eﬀector path in the base
frame, while the dashed line corresponds to the plane at rest. It can be veriﬁed
that the plane complies during the interaction.

9 Force Control
lam

variables
initialization

K_Dl

time

pS_f

trR_c

G_f

trR_c

C’S_f

operational space
decoupled arm

K_Pl
h_d

109

pS_f

environment
o_e

f_e

K’(o_e−o_r)

u = ddo_e

do_e

R_c
[T,a_d]

pS_v

[T,v_d]

pS_v

o_e
K_Pnu

K Ts

S_v

K_Inu

z−1

pS_v

trR_c

e_nu

plot

Fig. S9.4. SIMULINK block diagram of hybrid force/motion control.
vel error

−3

2

x 10

force
0
−10

1
[N]

[m/s]

−20
0

−30
−40

−1
−50
−2
0

0.5

1

1.5
[s]

2

2.5

−60
0

0.5

1

1.5

2

2.5

[s]

Fig. S9.5. Time history of the tip velocity error and of the contact force with hybrid
force/motion control.

Solution to Problem 9.11
In view of equality q̇ = J ρ (r)ṙ and (9.63) the following equality holds
v e = J(q)q̇ = J(q)J ρ (r)ṙ = S v (q)ṙ.
Hence, pre-multiplying both sides of the above equation by S †v and using (9.59)
gives
ν = ṙ.
Also, it is ν̇ = r̈. Therefore, control law (9.95) yields the closed-loop equation
r̈ − r̈ d + K Dr (ṙ d − ṙ) + K P r (r d − r) = 0,
showing that tracking of a desired position rd (t) is achieved.

110

9 Force Control
0.25
0.2

[m]

0.15
0.1
0.05
0
1

1.1
[m]

1.2

Fig. S9.6. Tip’s path in the base frame (continous line) and undeformed contact
plane (dashed line).

10
Visual servoing

Solution to Problem 10.1
The principal moments of the region are the eigenvalues of matrix I which
are


1
λ=
μ2,0 + μ0,2 + 4μ21,1 + (μ0,2 − μ2,0 )2
2


1

λ =
μ2,0 + μ0,2 − 4μ21,1 + (μ0,2 − μ2,0 )2
2
with λ ≥ λ > 0. The principal axis (i.e., the eigenvector of I) corresponding
to λ is aligned to vector


μ1,1
aλ =
.
λ − μ2,0
Therefore
tα =



1 
μ0,2 − μ2,0 + 4μ21,1 + (μ0,2 − μ2,0 )2 ,
2μ1,1

where tα = tan α. Considering that
t2α =

2tα
2μ1,1
=
,
1 − t2α
μ2,0 − μ0,2

Equation (10.2) follows.
Solution to Problem 10.2
The singular value decomposition of the (12 × 9) matrix A is given by
A = U ΣV T ,

(S10.1)

112

10 Visual servoing

where U is a (12 × 12) orthogonal matrix, V is a (9 × 9) orthogonal matrix
and


D 0
Σ=
D = diag{σ1 , σ2 , . . . , σ8 }
O 0
whith σ1 ≥ σ2 ≥ . . . ≥ σ8 > 0 being the non-null singular values of matrix A,
which is of rank 8. Post-multiplying by V both sides of Eq. (S10.1) gives
AV = U Σ
and thus Av i = σi ui , for i = 1, . . . , 8 and Av 9 = 0, being v i the i-th
column of V , known as right eigenvector, and ui the i-th column of U , known
as left eigenvector, corresponding to σi . In sum, the right eigenvector v 9 ,
corresponding to the null singular value of A, is a non-null solution to (10.10).
Solution to Problem 10.3
T
T cT
Notice that equality Tr(RcT
o U ΣV ) = Tr(V Ro U Σ) derives from property Tr(AB) = Tr(BA), for square matrices A and B. Also, since V T RcT
o U
is an an orthogonal matrix, all its elements have absolute value lower than or
equal to 1.
The problem of minimizing the Frobenius norm is equivalent to that of
minimizing the quantity




Rco − Q 2F = Tr (Rco − Q)T (Rco − Q) = 3 + Tr QT Q − 2Tr RcT
o Q ,



which is minimum when Tr RcT
o Q is maximum. Consider the singular value
decomposition
Q = U ΣV T
where Σ = diag{σ1 , σ2 , σ3 }, σi > 0. The following equality holds
3



 
T
= Tr V T RcT
Tr RcT
zi,i σi
o U ΣV
o UΣ =
i=1

where zi,i , i = 1, 2, 3 are the diagonal elements of the orthogonal matrix
V T RcT
o U , whose absolute values are lower than or equal to 1. Thus
3
 

T
≤
Tr RcT
U
ΣV
σi
o
i=1



and the equality holds when zi,i = 1. Hence the maximum of Tr RcT
o Q is
achieved by choosing Rc as in (10.12), which ensures that V T RcT
o U = I.

10 Visual servoing

113
time

variables
initialization

es

s

Jacobian
inverse

K_s

K Ts
z−1

inv(J_as)vs

xh

s(xh)
plot
sh

Fig. S10.1. Simulink block diagram of the pose estimation algorithm based on the
inverse of image Jacobian. Case of two feature points.
image plane

error norm
0.3

0.4

0.2
0.3
0.1
0.2

0
−0.1

0.1
−0.2
0
0

0.01

0.02
[s]

0.03

0.04

−0.2 −0.1

0

0.1 0.2

Fig. S10.2. Time history of the norm of the estimation error and corresponding
paths of the feature points projections on image plane

Solution to Problem 10.4
In view of the results of Example 10.3, the Jacobian J As , based on the feature
vector of two points, is a square (4 × 4) matrix. Hence, the pose estimation
algorithm based on the inverse of image Jacobian can be adopted.
According to (10.40) and (10.41), the following equality holds
%cc,o = Γ (occ,o )L−1
%̇ c,o = v
x
s K s es ,
where Ls is the (4 × 4) interaction matrix of points P1 and P2 , obtained from
the (4 × 6) interaction matrix of a set of two points by removing the fourth
and ﬁfth column. Also, Γ (occ,o ) is the (4 × 4) matrix


Γ (occ,o )

−I
=
0T


s3 (occ,o )
,
−1

114

10 Visual servoing
pos

orien

0.8

0
z
−0.5

0.4

[rad]

[m]

0.6

0.2

y

0

x

−0.2
0

0.01

0.02
[s]

0.03

−1

0.04

−1.5
0

0.01

0.02
[s]

0.03

0.04

Fig. S10.3. Time history of camera pose estimate
time
variables
initialization

es

s

K_s

Jacobian
inverse
inv(J_asl)vs

K Ts
z−1

xh

sl(xh)
plot
sh

Fig. S10.4. Simulink block diagram of the pose estimation algorithm based on the
inverse of image Jacobian. Case of two feature points.

being s3 (·) is the third column of matrix S(·).
Matrix gain K s has been chosen as K s = 160I 4 . The initial estimate has
been set to x̂c,o (0) = [ 0 0 0.5 0 ]. The resulting Simulink block diagram
is shown in Fig. S10.1. The Euler numerical integration method has been
adopted, with sampling time of 1 ms.
The ﬁles with the solution can be found in Folder 10 4.
The results in Fig. S10.2 show that the norm of the estimation error of
the feature parameters es tends to zero asymptotically with convergence of
exponential type; moreover, due to the fact that matrix gain K s was chosen
diagonal with equal elements, the paths of the projections of the feature points
on the image plane (between the initial positions marked with crosses and the
ﬁnal positions marked with circles) are line segments.
% c,o for
The corresponding time histories of the components of vector x
position and orientation are reported in Fig. S10.3. It can be veriﬁed that, with
the chosen value of K s , the algorithm converges in about 0.03 s, corresponding

10 Visual servoing
error norm

115

image plane
0.3

1.2

0.2

1

0.1

0.8

0

0.6

−0.1

0.4

−0.2

0.2
0
0

0.01

0.02
[s]

0.03

0.04

−0.2 −0.1

0

0.1 0.2

Fig. S10.5. Time history of the norm of the estimation error and corresponding
paths of the feature points projections on image plane
pos

orien

0.8

0
z
−0.5

0.4

[rad]

[m]

0.6

0.2

y

0

x

−0.2
0

0.01

0.02
[s]

0.03

−1

0.04

−1.5
0

0.01

0.02
[s]

0.03

0.04

Fig. S10.6. Time history of camera pose estimate

to 30 iterations. The ﬁnal value, namely, the estimated relative pose of the
T
object with respect to the camera, is x̂c,o  [ −0.1 0.1 0.6 −π/3 ] .
Solution to Problem 10.5
The solution is similar to that of Problem 10.4, but the feature parameters
are deﬁned according to (10.28), and the interaction matrix of a line segment
is used.
Matrix gain K s has been chosen as K s = 160I 4 . The initial estimate has
been set to x̂c,o = [ 0 0 0.5 0 ]. The feature vector of the line segment
corresponding to that of Problem 10.4 is
T

s = [ −0.1250 0.0945 0.1668 −1.0470 ] .
The resulting Simulink block diagram is shown in Fig. S10.4. The Euler
numerical integration method has been adopted, with sampling time of 1 ms.
The ﬁles with the solution can be found in Folder 10 5.
The results in Figs. S10.5 and S10.6 are quite similar to those of Problem 10.4. Notice that, however, the paths of the projections of the feature
points on the image plane are not perfectly linear, due to the diﬀerent choice

116

10 Visual servoing

of feature vector. This is reﬂected also by a diﬀerent time history of the z
component of the camera position during the transient. Obviously, the same
ﬁnal value is achieved for the estimated relative pose of the object with respect
T
to the camera, namely x̂c,o  [ −0.1 0.1 0.6 −π/3 ] .
Solution to Problem 10.6
In view of the equalities pi = p̄i + p̄ and pi = p̄i + p̄ , function (10.55) can
be rewritten as
g(o) =

n


pi − o − Rpi

2

= n p̄ − o − Rp̄

2

+

i=1

n


p̄i − Rp̄i

2

i=1

+2

n


(p̄ − o − Rp̄ ) (p̄i − Rp̄i )
T

i=1

= n p̄ − o − Rp̄

2

+

n


p̄i − Rp̄i 2 ,

i=1

(n

(n


considering that
i=1 p̄i =
i=1 p̄i = 0. Hence, with the choice of o as
in (10.56), function (10.55) is minimum. The same result can be obtained
computing the value of o which satisﬁes the necessary condition (∂g/∂o)T =
0.

Solution to Problem 10.7
Function (10.57) can be rewritten as
n


p̄i −

i=1

Rp̄i 2

=

n


p̄i

2

−2

i=1

n

i=1

p̄Ti RT p̄i

+

n


Rp̄i

2

i=1

(n

which is minimum when the quantity i=1 p̄Ti RT p̄i is maximum. This quantity, in view of the property aT b = Tr(ba), can be rewritten as
n


p̄Ti RT p̄i =

i=1

n




Tr RT p̄i p̄Ti = Tr(RT K),

i=1

(n

with K = i=1 p̄i p̄i T . Hence, the matrix R which minimizes function (10.57)
is the matrix which maximizes the trace of RT K.
Solution to Problem 10.8
The control scheme in Fig. 10.14 is adopted, with the same matrix gains used
in control scheme A of Sect. 10.9, namely,
K P = diag{500, 500, 10, 10}

K D = diag{500, 500, 10, 10}.

10 Visual servoing

117
time

variables
initialization

J_Aq(q)
Jacobian
transpose

K_d

SCARA arm

dq

tr(J_Aq(q))
inv(B(q))(tau−tau’)

q

sinusoidal
functions
cs(q)

cs x_co

x_do
xtilde

K_p
gravity
g(q)

estimate

features

s

plot

Fig. S10.7. Simulink block diagram of the position-based visual servoing scheme
of PD type with gravity compensation

The initial pose of the camera frame is xc (0) = [ 1 1 0.5 π/4 ]T and the
desired pose of the object frame with respect to the camera frame has been set
to xd,o = [ −0.1 0.1 0.6 −π/3 ]T . The various terms of the dynamic model
of the SCARA manipulator are computed as in the solution to Problem 7.3.
The pose estimation algorithm is based on the measurements of the features of the line segment P1 P2 , as in the solution to Problem 10.5, using the
same matrix gain, namely, K s = 160I 4 . The initial pose estimate is chosen
as x̂c,o (0) = xd,o .
The resulting Simulink block diagram is shown in Fig. S10.7. The arm
is simulated as a continuous-time system using a variable-step integration
method with a maximum step size of 1 ms. All the blocks of the controller are
simulated as discrete-time subsystems with the given sampling time of 4 ms,
while the sampling time of the estimation algorithm has been set to 1 ms.
The ﬁles with the solution can be found in Folder 10 8.
The results of Figs. S10.8 and S10.9 (right) are very similar to those of
Figs. 10.18 and 10.19 (right), as expected. In fact, the two control schemes differ only for the estimation algorithms which, in both cases, have fast dynamics
with negligible eﬀects on the control loop dynamics.
Notice that, in Fig. S10.9 (left), the time history of the feature parameters
of the line segment P1 P2 used by the estimation algorithm in Fig. S10.7 is
reported.

118

10 Visual servoing
y−pos
1.4

1.3

1.3
[m]

[m]

x−pos
1.4

1.2
1.1

1.2
1.1

1
0

2

4
[s]

6

1
0

8

2

4
[s]

z−pos

8

6

8

alpha
2

0.5
0.4

1.5
[rad]

[m]

6

0.3

1

0.2
0.1
0

2

4
[s]

6

0.5
0

8

2

4
[s]

Fig. S10.8. Time history of camera frame position and orientation with positionbased visual servoing scheme of PD type with gravity compensation
param

image plane
0.3

0.2

0.2

0
0.1

−0.2
−0.4

0

−0.6

−0.1

−0.8
−0.2

−1
0

2

4
[s]

6

8

−0.2 −0.1

0

0.1 0.2

Fig. S10.9. Time history of feature parameters and corresponding path of feature
points projections on image plane with position-based visual servoing scheme of PD
type with gravity compensation

Solution to Problem 10.9
The control scheme in Fig. 10.17 is adopted, with the same matrix gain used
in control scheme B of Sect. 10.9, namely, K = I 4 .
The initial pose of the camera frame is xc (0) = [ 1 1 0.5 π/4 ]T , and
the desired pose of the object frame with respect to the camera frame has
been set to xd,o = [ −0.1 0.1 0.6 −π/3 ]T .

10 Visual servoing

119
time

variables
initialization

Jacobian
inverse

x_do
xtilde

K
inv(J_Aq(q))

controlled
manipulator
q
1
s

estimate

sinusoidal
functions
cs(q)

cs x_co

features

s

plot

Fig. S10.10. Simulink block diagram of the resolved-velocity position-based visual
servoing scheme

The pose estimation algorithm is based on the measurements of the features of the line segment P1 P2 , as in the solution to Problem 10.5, using the
same matrix gain, namely, K s = 160I 4 . The initial pose estimate is chosen
as x̂c,o (0) = xd,o .
The resulting Simulink block diagram is shown in Fig. S10.10. Notice
that the dynamics of the controlled robot manipulator is approximated by a
simple integrator. All the blocks of the controller are simulated as discretetime subsystems with the given sampling time of 4 ms, while the sampling
time of the estimation algorithm has been set to 1 ms.
The ﬁles with the solution can be found in Folder 10 9.
The results of Figs. S10.11 and S10.12 (right) are very similar to those
of Figs 10.20 and 10.21 (right), as expected (see also the solution to Problem 10.8).
Solution to Problem 10.10
The control scheme in Fig. 10.16 is adopted with the matrix gains
K P s = diag{300, 300, 150, 50}

K Ds = diag{330, 330, 165, 40},

which guarantee a closed-loop dynamics similar to that of control scheme C in
Sect. 10.9. Notice that, in this case, the dynamics of the closed-loop depends
on the type of feature parameters extracted from visual measurements.
The various terms of the dynamic model of the SCARA manipulator are
computed as in the solution to Problem 7.3.
The initial pose of the camera frame is xc (0) = [ 1 1 0.5 π/4 ]T , and
the desired value of the feature parameters of the line segment P1 P2 has

120

10 Visual servoing
y−pos
1.4

1.3

1.3
[m]

[m]

x−pos
1.4

1.2
1.1

1.2
1.1

1
0

2

4
[s]

6

1
0

8

2

4
[s]

z−pos

8

6

8

alpha
2

0.5
0.4

1.5
[rad]

[m]

6

0.3

1

0.2
0.1
0

2

4
[s]

6

0.5
0

8

2

4
[s]

Fig. S10.11. Time history of camera frame position and orientation with resolvedvelocity position-based visual servoing
param

image plane
0.3

0.2

0.2

0
0.1

−0.2
−0.4

0

−0.6

−0.1

−0.8
−0.2

−1
0

2

4
[s]

6

8

−0.2 −0.1

0

0.1 0.2

Fig. S10.12. Time history of feature parameters and corresponding path of feature points projections on image plane with resolved-velocity position-based visual
servoing

been set to sd = [ −0.1250 0.0945 0.1667 −1.0472 ]T , corresponding to
the desired pose xd,o .
% s = Ls (s, zd ),
Matrix Ls (s, z c ) has been approximated with matrix L
where zd is the third component of vector xd,o .
The resulting Simulink block diagram is shown in Fig. S10.13. The arm
is simulated as a continuous-time system using a variable-step integration
method with a maximum step size of 1 ms. All the blocks of the controller are

10 Visual servoing

121
time

variables
initialization
J_L(s,zc,q)
Jacobian
transpose

K_d

SCARA arm
tr(J_L(s,zc,q))
inv(B(q))(tau−tau’)

x_do

dq

sinusoidal
functions

q

cs(q)

cs x_co

features
K_p
gravity
g(q)

features

s

plot

Fig. S10.13. Simulink block diagram of the image-based visual servoing scheme
of PD type with gravity compensation

simulated as discrete-time subsystems with the given sampling time of 4 ms,
while the sampling time of the estimation algorithm has been set to 1 ms.
The ﬁles with the solution can be found in Folder 10 10.
The results are shown in Figs. S10.14 and S10.15. With respect to the
results of Figs 10.22 and 10.23, it can be observed that the phenomenon of
camera retreat is mitigated.
Solution to Problem 10.11
The control scheme in Fig. 10.17 is adopted with the same matrix gain used
in control scheme C of Sect. 10.9, namely, K s = I 4 .
The initial pose of the camera frame is xc (0) = [ 1 1 0.5 π/4 ]T , and
the desired value of the feature parameters of the line segment P1 P2 has
been set to sd = [ −0.1250 0.0945 0.1667 −1.0472 ]T , corresponding to
the desired pose xd,o .
% s = Ls (s, zd ),
Matrix Ls (s, z c ) has been approximated with matrix L
where zd is the third component of vector xd,o .
The resulting Simulink block diagram is shown in Fig. S10.16. Notice
that the dynamics of the controlled robot manipulator is approximated by a
simple integrator. All the blocks of the controller are simulated as discretetime subsystems with the given sampling time of 4 ms, while the sampling
time of the estimation algorithm has been set to 1 ms.
The ﬁles with the solution can be found in Folder 10 11.

122

10 Visual servoing
y−pos
1.4

1.3

1.3
[m]

[m]

x−pos
1.4

1.2
1.1

1.2
1.1

1
0

2

4
[s]

6

1
0

8

2

4
[s]

z−pos

8

6

8

alpha
2

0.5
0.4

1.5
[rad]

[m]

6

0.3

1

0.2
0.1
0

2

4
[s]

6

0.5
0

8

2

4
[s]

Fig. S10.14. Time history of camera frame position and orientation with imagebased visual servoing scheme of PD type with gravity compensation
param

image plane
0.3

0.2

0.2

0
0.1

−0.2
−0.4

0

−0.6

−0.1

−0.8
−0.2

−1
0

2

4
[s]

6

8

−0.2 −0.1

0

0.1 0.2

Fig. S10.15. Time history of feature parameters and corresponding path of feature
points projections on image plane with image-based visual servoing scheme of PD
type with gravity compensation

The results of Figs. S10.17 and S10.18 (right), with the given choice of
the control gains, are quite similar to those of Figs 10.22 and 10.23 (right).
However, it can be noticed the absence of the phenomenon of camera retreat.
Solution to Problem 10.12
The distance dc can be computed as
dc = ncT r cc = zc ncT )
sp ,

10 Visual servoing

123
time

variables
initialization

x_do

Jacobian
inverse

features
K

controlled
manipulator

sinusoidal
functions

1
s

cs(q)

inv(J_L(s,zc,q))

q

cs x_co

features

s

plot

Fig. S10.16. Simulink block diagram of the resolved-velocity image-based visual
servoing scheme

and thus
zc =

dc
.
cT
n )
sp

Analogously, the following equality can be derived
zd =

dd
.
ndT )
sp,d

Hence, expression (10.94) follows directly.
In order to derive (10.95), consider that the distance dc can be computed
also as
dc = dd + ncT occ,d ,
and thus

occ,d
dc
= 1 + ncT
.
dd
dd

Moreover, the following equality holds





1
1
det(H) = det Rcd + occ,d ndT = det
I + occ,d ncT Rcd
dd
dd


1
= det I + occ,d ncT .
dd
By direct computation, it is possible to verify the identity


occ,d
1
det I + occ,d ncT = 1 + ncT
,
dd
dd

124

10 Visual servoing
y−pos
1.4

1.3

1.3
[m]

[m]

x−pos
1.4

1.2
1.1

1.2
1.1

1
0

2

4
[s]

6

1
0

8

2

4
[s]

z−pos

8

6

8

alpha
2

0.5
0.4

1.5
[rad]

[m]

6

0.3

1

0.2
0.1
0

2

4
[s]

6

0.5
0

8

2

4
[s]

Fig. S10.17. Time history of camera frame position and orientation with resolvedvelocity image-based visual servoing
param

image plane
0.3

0.2

0.2

0
0.1

−0.2
−0.4

0

−0.6

−0.1

−0.8
−0.2

−1
0

2

4
[s]

6

8

−0.2 −0.1

0

0.1 0.2

Fig. S10.18. Time history of feature parameters and corresponding path of feature
points projections on image plane with resolved-velocity image-based visual servoing

and thus

dc
= det(H).
dd

10 Visual servoing

Solution to Problem 10.13
Notice that
⎡
∂ep (r cc )
∂rcc

⎢
⎢
zc ⎢
⎢
=
z d ρz ⎢
⎢
⎣
⎡

0
S(r cc ) = zc ⎣ 1
−Y

1
zc

xc
zc2
yc
1
0
−
zc
zc2
1
0
0
−
zc
⎤
−1 Y
0 −X ⎦ .
X
0

−

0

⎤
⎡
⎤
⎥
−1 0
X
⎥
⎥
⎥
⎥= 1 ⎢
⎥ zd ρz ⎣ 0 −1 Y ⎦
⎥
0
0 −1
⎦

Hence, in view of (10.26) and (10.96), the following equalities hold
⎡
⎤
−1 0
X
c
∂ep (r c )
1 ⎢
⎥
Jp =
=
⎣ 0 −1 Y ⎦
c
∂rc
z d ρz
0
0 −1
⎡
⎤
XY
−1 − X 2 Y
c
∂ep (r c )
⎢
⎥
S(r cc ) = ⎣ 1 + Y 2
−XY
−X ⎦ .
Jo = −
∂r cc
−Y
X
0

125

11
Mobile Robots

Solution to Problem 11.1
Denote by (x, y) the Cartesian coordinates of a representative point on the
tricycle (e.g., the midpoint of the rear wheel axle), by φ the steering angle
of the front wheel with respect to the vehicle, and by θ0 the orientation of
the vehicle with respect to the x axis. Once the conﬁguration of the tricycle
and of the ﬁrst i − 1 trailers are given, the conﬁguration of the i-th trailer
is completely described by the its orientation θi with respect to the x axis.
The conﬁguration vector for the complete vehicle is therefore obtained as q =
[ x y φ θ0 θ1 . . . θN ]T , and takes values in IR2 × SO(2) × . . . × SO(2)
(with SO(2) appearing N + 2 times). Another possibility is to replace the
absolute angle θi with the relative angle θi − θi−1 to describe the orientation
of each trailer.
Solution to Problem 11.2
Add the kinematic constraints side-by-side to obtain
3  q̇3 + r(q̇4 + q̇5 + q̇6 ) = 0
that can be integrated as
r
(q4 + q5 + q6 ) + c,
3
where c is an integration constant. The kinematic constraints are then partially integrable. In particular, the orientation q3 of the robot is a linear function of the wheel rotation angles q4 , q5 , q6 .
q3 = −

Solution to Problem 11.3
Consider a single Pfaﬃan constraint
aT (q)q̇ =

n

j=1

aj (q)q̇j = 0.

128

11 Mobile Robots

The corresponding kinematic model is
q̇ =

n−1


g j (q)uj ,

j=1

where {g 1 (q), . . . , g n−1 (q)} is a basis of N (aT (q)). Without loss of generality,
assume that a1 (q) = 0, and consider the following choice of input vector ﬁelds
⎡
⎡
⎡
⎤
⎤
⎤
−γ(q)a2 (q)
−γ(q)a3 (q)
−γ(q)an (q)
⎢ γ(q)a1 (q) ⎥
⎢
⎢
⎥
⎥
0
0
⎢
⎢
⎢
⎥
⎥
⎥
⎢
⎢ γ(q)a1 (q) ⎥
⎢
⎥
⎥
0
0
⎢
⎢
⎢
⎥
⎥
⎥
⎢
⎢
⎢
⎥
⎥
⎥
0
0
0
g1 = ⎢
⎥
⎥ g2 = ⎢
⎥ . . . g n−1 = ⎢
⎢
⎢
⎢
⎥
⎥
⎥
..
..
..
⎢
⎢
⎢
⎥
⎥
⎥
.
.
.
⎢
⎢
⎢
⎥
⎥
⎥
⎣
⎣
⎣
⎦
⎦
⎦
0
0
0
γ(q)a1 (q)
0
0
where γ(q) = 0. The distribution Δ = span{g 1 , . . . , g n−1 } is involutive if any
Lie bracket [g i , g j ] can be written as a linear combination of g 1 , . . . , g n−1 .
For example, consider [g 1 , g 2 ]. A simple computation gives


⎡
⎤
∂(γa3 ) ∂(γa2 )
∂(γa3 )
∂(γa2 )
−
γa1 ⎥
⎢ ∂q1 γa3 − ∂q1 γa2 +
∂q2
∂q3
⎢
⎥
⎢
⎥
∂(γa1 )
∂(γa1 )
⎢
⎥
γa1 −
γa3
⎢
⎥
∂q3
∂q1
⎢
⎥
⎢
⎥
∂(γa1 )
∂(γa1 )
[g 1 , g 2 ] = ⎢
⎥
γa2 −
γa1
⎢
⎥
∂q1
∂q2
⎢
⎥
⎢
⎥
0
⎢
⎥
⎢
⎥
..
⎣
⎦
.
0
Using the integrability condition expressed by (11.9), one easily veriﬁes that
[g 1 , g 2 ] =

∂(γa3 )
∂(γa2 )
∂(γa1 ) a3 g 1 + a2 g 2
g1 −
g2 +
,
∂q1
∂q1
∂q1
a1

i.e., [g 1 , g 2 ] is a linear combination of g 1 , g 2 .
A similar construction can be repeated for any Lie bracket [g i , g j ]; hence,
the distribution Δ is involutive under the integrability condition (11.9).
Solution to Problem 11.4
Consider a set of k Pfaﬃan constraints
AT q̇ = 0

11 Mobile Robots

129

with a constant constraint matrix AT . Then, the associated kinematic model
q̇ =

m


g j uj = G u

m=n−k

(S11.1)

j=1

has constant input vector ﬁelds g 1 , . . . , g m . Its accessibility distribution ΔA
is clearly involutive, because [g i , g j ] = 0 for any i, j. The controllability
condition (11.11) is hence violated, and in particular
dim ΔA (q) = m.
Therefore, the above set of Pfaﬃan constraints is completely integrable, i.e.,
holonomic. This obvious conclusion could have also been reached by noting
that (S11.1) is in fact a driftless linear system, and using directly the controllability rank condition (D.15).
Solution to Problem 11.5
Integration of the kinematic model (11.13) under the given input sequence
starting from the initial conﬁguration q 0 = [ x0 y0 θ0 ]T provides
x(4ε) = x0 + ε cos θ0 − ε cos (θ0 + ε)
y(4ε) = y0 + ε sin θ0 − ε sin (θ0 + ε)
θ(4ε) = θ0 .
The displacement is therefore
⎡

⎤
ε cos θ0 − ε cos (θ0 + ε)
q(4ε) − q 0 = ⎣ ε sin θ0 − ε sin (θ0 + ε) ⎦ = ε2 t,
0

with

⎡

⎤
cos θ0 − cos (θ0 + ε)
⎢
⎥
ε
⎢
⎥
⎢
⎥
sin
θ
−
sin
(θ
+
ε)
t=⎢
0
0
⎥
⎣
⎦
ε
0

— compare with the expression of the displacement for a generic two-input
system as given in Appendix D.
Finally, one obtains
*
⎤
⎡
*
⎤
⎡
− dcos θ *
dθ θ=θ0 ⎥
sin θ0
⎢
*
⎥
⎢
lim t = ⎢ − dsin θ **
⎥ = ⎣ −cos θ0 ⎦ ,
ε→0
⎣
dθ θ=θ0 ⎦
0
0

130

11 Mobile Robots

that coincides with the value at q 0 of the Lie Bracket of the two input vector
ﬁelds (see Sect. 11.2.1).
Solution to Problem 11.6
With reference to Figs. 1.13 and 11.3, denote by pR = [ xR yR ]T , pL =
[ xL yL ]T and pM = [ xM yM ]T respectively the position vector of the
right wheel centre, of the left wheel centre and of the midpoint between the
two. Since
ẋi = r ωi cos θ
ẏi = r ωi sin θ,
where i = R, L, one obtains
ωR + ωL
ẋR + ẋL
= r cos θ
2
2
ωR + ωL
ẏR + ẏL
= r sin θ
=
2
2

ẋM =
ẏM
and therefore
v=


2 = r (ωR + ωL ) .
ẋ2M + ẏM
2

The expression of the angular velocity of the robot can be obtained by
applying the general velocity composition rule (B.4) and making use of the
skew-symmetric operator S(·):
ṗR = ṗL + S(ω)(pR − pL ),
where



0 −ω
S(ω) =
ω 0

(S11.2)



because the motion of the robot is planar. Using the expressions of ṗR , ṗL
and the fact that


d sin θ
pR = pL +
,
−d cos θ
Equation (S11.2) becomes
 
 



r ωL cos θ
0 −ω
d sin θ
r ωR cos θ
=
+
.
ω 0
−d cos θ
r ωR sin θ
r ωL sin θ
Squaring and adding side-by-side these two equations leads to
ω2 =

r2 (ωR − ωL )2
.
d2

11 Mobile Robots

131

Using again one of the two equations, it can be concluded that the only
solution is
r(ωR − ωL )
.
ω=
d
Solution to Problem 11.7
With reference to Fig. 11.4, denote by (xC , yC ) the Cartesian coordinates of
the instantaneous centre of rotation C in the world reference frame, and by

) its coordinates in a moving frame having the origin at the centre of
(xC , yC
the rear wheel and the x axis aligned with the bicycle body. Simple geometry
gives

  
0
xc
=
yc
/tan φ
and thus



xc
yc


=

 
  

x
x
x −  sin θ/tan φ
+ R(θ) c =
,
yc
y
y +  cos θ/tan φ


where
R(θ) =

cos θ −sin θ
sin θ cos θ



is the (planar) rotation matrix of the moving frame with respect to the world
frame.
Since the velocity vector of a generic point P on the robot body is tangent
to the circle centred at C and passing through P , the modulus of the angular
velocity of the bicycle is obtained as
ωbody = vP /RP ,

(S11.3)

where vP is the modulus of the velocity of P , and RP is its instantaneous
radius of rotation (i.e., its radius of curvature):
'
RP = (xP − xC )2 + (yP − yC )2 .
The resulting ωbody is obviously the same for any choice of P . In particular,
by choosing P as the centre of the rear wheel, one has RP = /tan φ, and thus
ωbody = θ̇ = v tan φ/,
consistently with the evolution of θ predicted by the kinematic model (11.19).
Finally, plugging this expression of ω in (S11.3) yields
vP = Rp v tan φ/.

132

11 Mobile Robots

Solution to Problem 11.8
With the conﬁguration vector q = [ x y φ θ0 θ1 . . . θN ]T deﬁned in the
solution to Problem 11.1, the N + 2 kinematic constraints are
ẋf sin (θ0 + φ) − ẏf cos (θ0 + φ) = 0
ẋ sin θ0 − ẏ cos θ0 = 0
ẋi sin θi − ẏi cos θi = 0

i = 1, . . . , N ,

where (xf , yf ) and (xi , yi ) are the Cartesian coordinates of the centre of the
tricycle front wheel and of the i-th trailer wheel axle midpoint, respectively
(compare with (11.15), (11.16)). Denote by  the distance between the front
wheel and the rear axle of the tricycle, and by i the hinge-to-hinge length of
the i-th trailer. It is
xf = x +  cos θ0
yf = y +  sin θ0
and
xi = x −

i


j cos θj

j=1

yi = y −

i


j sin θj ,

j=1

so that the kinematic constraints become
ẋ sin (θ0 + φ) − ẏ cos (θ0 + φ) − θ̇0  cos φ = 0
ẋ sin θ0 − ẏ cos θ0 = 0
ẋ sin θi − ẏ cos θi +

i


θ̇j j cos (θi − θj ) = 0

i = 1, . . . , N .

j=1

The null space of the constraint matrix is spanned by the two columns of
⎡
⎤
cos θ0
0
⎢
sin θ0
0⎥
⎢
⎥
⎢
0
1⎥
⎢
⎥
1
⎢
tan φ
0⎥
⎢
⎥
⎢
− 11 sin (θ1 − θ0 )
0⎥
⎢
⎥
⎢
⎥
0⎥
− 12 cos (θ1 − θ0 ) sin (θ2 − θ1 )
⎢
G(q) = ⎢
⎥ = [ g 1 (q) g 2 (q) ]
..
.. ⎥
⎢
⎢
.
.⎥


⎢
⎥
⎢ − 1 +i−1 cos (θ − θ ) sin (θ − θ ) 0 ⎥
j
j−1
i
i−1
⎢
⎥
j=1
i
⎢
⎥
⎢
..
.. ⎥
⎢
.
.⎥
+

⎣
⎦
N
−1
− 1N
j=1 cos (θj − θj−1 ) sin (θN − θN −1 ) 0

11 Mobile Robots

133

(compare with (11.19)). The kinematic control system is then
q̇ = g 1 (q) v + g 2 (q) ω,
where v and ω are respectively the driving velocity of the rear wheels and the
steering velocity of the tricycle.
Solution to Problem 11.9
According to (11.14), the velocity inputs of the equivalent unicycle model are
given by
r (ωR + ωL )
2
r (ωR − ωL )
ω=
,
d
v=

where r is the wheel radius and d is the distance between the wheel centres.
Solving for ωR in the ﬁrst and replacing the obtained expression in the second
yields
2v − d ω
2r
2v + d ω
.
ωR =
2r
ωL =

The resulting constraints on v, ω are then
*
*
* 2v(t) − d ω(t) *
* ≤ ωRL
*
*
*
2r
*
*
* 2v(t) + d ω(t) *
* ≤ ωRL ,
*
*
*
2r
that correspond to a rhombus-shaped admissible region in the v, ω plane.
In particular, the maximum achievable values for these velocities are easily
computed as the intercepts of the sides of the rhombus with the v, ω axes,
i.e., ±r ωRL for the driving velocity and ±2 r ωRL /d for the steering velocity.
Solution to Problem 11.10
The geometric version of the (2,4) chained form is
z1 = v)1
z2 = v)2
z3 = z2 )
v1
z4 = z3 )
v1 .

134

11 Mobile Robots

Choose the geometric inputs v)1 , v)2 in the parameterized form (11.49), with
Δ = z1,f − z1,i and s ∈ [si , sf ] = [0, |Δ|]. Integrating in a recursive fashion the
second, third and fourth equations and setting z2 (sf ) = z2,f , z3 (sf ) = z3,f
and z4 (sf ) = z4,f leads to a linear system in the form (11.50) where
⎡
⎤
|Δ|3
Δ2
⎡
⎤
|Δ|
2
3
z2,f − z2,i
⎢
⎥
2
3
4 ⎥
⎢
Δ
⎦.
D = ⎢ sgn(Δ) Δ
z3,f − z3,i − z2,i Δ
⎥ d=⎣
sgn(Δ) Δ
2
6
12 ⎦
⎣
z4,f − z4,i − z3,i Δ − z2,i |Δ|
|Δ|3
|Δ|5
Δ4
6
24
60
Solution to Problem 11.11
It is suﬃcient to modify (11.48) as follows:
v)1 = Δ = z1,f − z1,i ,
with the second input (11.49) unchanged. With this choice, z1 will reach its
ﬁnal desired value exactly at sf = 1. The coeﬃcients c0 , . . . , cn−2 in (11.49)
are computed by solving a linear system of equations that is still in the general
form (11.50). For example, in the case of the (2, 3) chained form, one obtains




1
1/2
z2,f − z2,i
.
D=
d=
z3,f − z3,i − z2,i Δ
Δ/2 Δ/6
Solution to Problem 11.12
As shown in the solution to Problem 11.11, the path planning scheme based
on parameterized inputs (scheme A in the following) must be modiﬁed by
letting
v)1 = Δ = z1,f − z1,i
to obtain sf = 1 and allow a comparison with the scheme that uses interpolating polynomials of diﬀerent degree (scheme B in the following). With scheme
A one has then
z1 (s) = z1,i + (z1,f − z1,i )s
that coincides with the expression of z1 (s) generated by scheme B. Moreover,
being
v)2 = c0 + c1 s
for a (2,3) chained form, it is
z2 (s) = z2,i + c0 s + c1 s2 /2
and

,

s

(c0 σ + c1 σ 2 /2)dσ = z3,i + Δ (c0 s2 /2 + c1 s3 /6).

z3 (s) = z3,i + Δ
0

11 Mobile Robots

135

tilde v(s)
[m]

4

[m]

1

2
0
0

0.5

0.2

0.4
0.6
tilde omega(s)

0.8

1

0.8

1

[rad]

10

0
0

0.5

1
[m]

1.5

2

5
0
0

0.2

0.4

0.6

Fig. S11.1. Path planning via Cartesian polynomials; left: Cartesian path of the
unicycle for the required parking manoeuvre, right: geometric inputs along the path

Hence, z3 (s) with scheme A is a cubic polynomial in s, as is with scheme B.
However, there is a unique cubic polynomial that meets the two boundary
conditions on z3 and the two boundary conditions on z2
z3 (0)
= z2i
z1 (0)

z3 (1)
= z2f .
z1 (1)

Hence, the expressions of z3 (s) generated by the two schemes coincide. Since
z1 and z3 are ﬂat outputs for a (2,3) chained form (see the end of Sect. 11.5.2),
a unique z2 (s) is associated to z1 (s) and z3 (s). One can then conclude that
the paths in the z space (and the associated geometric inputs) generated by
the two schemes are exactly the same.
Solution to Problem 11.13
The ﬁles with the solution can be found in Folder 11 13. For the required
manoeuvre, the coeﬃcients of the interpolating polynomials
x(s) = s3 xf − (s − 1)3 xi + αx s2 (s − 1) + βx s(s − 1)2
y(s) = s3 yf − (s − 1)3 yi + αy s2 (s − 1) + βy s(s − 1)2
are found to be
αx = −6

βx = 1

αy = −2

βy = 0

where ki = kf = 1. The corresponding unicycle path is shown in Fig. S11.1.
Using the ﬂatness property of the unicycle, expressed by (11.45), (11.46)
and (11.47), File s11 13.m also generates the geometric inputs )
v (s), ω
) (s)
along the path, as shown in Fig. S11.1.
There are several ways to ﬁnd a feasible timing law s = s(t) over the
planned path. For example, one may simply let s = t, so that v(t) and ω(t)

136

11 Mobile Robots

coincide with v)(s) and ω
) (s), respectively. However, both the inputs violate the
given bounds; in particular, the maximum value is attained by ω at 10 rad/sec.
A uniform scaling procedure with T = 10 can then be used to slow down the
trajectory and recover the feasibility of the inputs (see (11.54), (11.55)).
Solution to Problem 11.14
The (2,3) chained form is
ż1 = v1
ż2 = v2
ż3 = z2 v1 .
Assume that a desired state trajectory (z1d (t), z2d (t), z3d (t)) is given. For the
desired trajectory to be feasible, it must satisfy the equations
ż1d = v1d
ż2d = v2d
ż3d = z2d v1d
for some choice of the reference inputs (v1d (t), v2d (t)). Deﬁning the state errors
ei = zid − zi , for i = 1, 2, 3, and the input errors ui = vid − vi , for i = 1, 2,
one has the following error dynamics
ė1 = u1
ė2 = u2
ė3 = z2d v1d − z2 v1 = v1d e2 + z2d u1 − e2 u1 .
Linearization around the reference trajectory yields the time-varying system
⎡ ⎤ ⎡
⎤⎡ ⎤ ⎡
⎤
ė1
e1
0 0 0
1 0  
u
ė = ⎣ ė2 ⎦ = ⎣ 0 0 0 ⎦ ⎣ e2 ⎦ + ⎣ 0 1 ⎦ 1 = A(t)e + B(t)u.
u2
ė3
e3
0 v1d 0
z2d 0
With the linear time-varying feedback
u1 = −k1 e1
u2 = −k2 e2 −

k3
e3
v1d

the closed-loop linearized error dynamics becomes
⎡
⎤
−k1
0
0
⎢
⎥
3 ⎦ e.
ė = ⎣ 0
−k2 − vk1d
−k1 z2d v1d 0

11 Mobile Robots

137

The characteristic polynomial of this system is constant:
p(λ) = (λ + k1 )(λ2 + k2 λ + k3 ).
The eigenvalues are thus constant, and have negative real part provided that
k1 , k2 and k3 are positive. As discussed in Sect. 11.6.1, this does not guarantee
stability in view of the time-varying nature of the linearized error dynamics,
except for the case when v1d and z2d are constant1 . In this case, the linearized
error system becomes time-invariant, and therefore it is asymptotically stable
with the above choice of gains. Hence, the original error system is also asymptotically stable at the origin, although this result is not guaranteed to hold
globally.
Solution to Problem 11.15
With reference to the kinematic model (11.19) and Fig. 11.4, consider the
following outputs:
y1 = x +  cos θ + b cos (θ + φ)
y2 = y +  sin θ + b sin (θ + φ)
with b = 0. They represent the Cartesian coordinates of a point P on the line
passing through the centre of the front wheel and oriented as the wheel itself,
located at a distance |b| from the contact point of the wheel with the ground.
The time derivatives of y1 and y2 are
  
 
cos θ − tan φ(sin θ + b sin (θ + φ)/) −b sin (θ + φ)
ẏ1
v
=
sin θ + tan φ(cos θ + b cos (θ + φ)/) b cos (θ + φ)
ω
ẏ2
 
v
= T (θ, φ)
.
ω
Matrix T (θ, φ) has determinant b/cos φ, and is therefore always invertible
under the assumption that b = 0 and |φ(t)| ≤ π/2. It is then suﬃcient to use
the following input transformation
 
 
u
v
= T −1 (θ, φ) 1
u2
ω
to put the equations of the bicycle in the input-output linearized form:
ẏ1 = u1
ẏ2 = u2
θ̇ = sin φ(cos (θ + φ)u1 + sin (θ + φ)u2 )/
φ̇ = −(cos (θ + φ)sin φ/ + sin (θ + φ)/b)u1
−(sin (θ + φ)sin φ/ − cos (θ + φ)/b)u2 .
1

One may verify that, if the (2,3) chained form represents a unicycle under the
transformation (11.23), (11.24), constant values of v1d and z2d correspond to
reference trajectories over which ωd is constant and vd increases linearly over
time (as in an Archimedean spiral).

138

11 Mobile Robots
variables
initialization

inputs

x

v
configuration

y

inputs
omega

Cartesian regulator

configuration

theta

unicycle
plot

Fig. S11.2. Simulink block diagram of the unicycle with the modiﬁed cartesian
regulator

At this point, a simple linear controller such as
u1 = ẏ1d + k1 (y1d − y1 )
u2 = ẏ2d + k2 (y2d − y2 ),
with k1 > 0, k2 > 0, guarantees exponential convergence to zero of the Cartesian tracking error, with decoupled dynamics on its two components. Note
that the orientation and the steering angle, whose evolutions are governed by
the third and fourth equation, are not controlled.
Solution to Problem 11.16
First of all, note that the Cartesian regulator (11.78), (11.79) may be equivalently written as
v = k1 nT ep
ω = k2 γ,

(S11.4)
(S11.5)

where n = [ cos θ sin θ ]T is the unit vector aligned with the sagittal axis,
ep = [ −x −y ]T is the Cartesian error and γ = Atan2(y, x) − θ + π is
the pointing error, i.e., the angle between n and ep (see also Fig. 11.18).
Expression (S11.5) shows that ω induces a rotation until n and ep align, i.e.,
until the unicycle points to the origin; as a consequence, the latter will always
be reached in forward motion.
Now, redeﬁne the steering velocity (S11.5) as follows:
if nT ep > 0
k2 γ
ω=
(S11.6)
k2 (γ − sign(γ)π) if nT ep ≤ 0.
With this modiﬁcation, the unicycle is forced to align with ep if the pointing
error is acute, and with −ep otherwise. In the second case, the origin will be
reached in backward motion.
Unicycle control with the modiﬁed position regulator (S11.4), (S11.6) is
implemented by the Simulink block diagram shown in Fig. S11.2. The ﬁles

11 Mobile Robots
driving velocity

2
[m/s]

0

1
[m]

139

-2
0

2

0
-0.5
-1
-1.5
0

2

0

[rad/s]

-1
-2
-2

-1

0
[m]

1

2

-1
4

6
[s]
steering velocity

4

6

8

10

8

10

[s]

Fig. S11.3. Regulation to the origin of the Cartesian position of the unicycle with
the modiﬁed controller (S11.4), (S11.6); left: Cartesian motion of the unicycle, right:
time evolution of the velocity inputs v and ω

with the solution can be found in Folder 11 16. The unicycle is simulated
as a continuous-time system using a variable-step integration method, with
a maximum step size of 0.01 s. The controller gains can be chosen in the
initialization ﬁle.
When the initial pointing error is an acute angle, the modiﬁed and the
original position regulators produce exactly the same trajectories. For example, this is the case of the simulation in Fig. 11.19 (left). However, in the case
of Fig. 11.19 (right), the initial pointing error is obtuse, and thus the modiﬁed position regulator leads the unicycle to the origin in backward motion, as
shown in Fig. S11.3.
Solution to Problem 11.17
Assume that the velocity inputs are constant within each sampling interval:
v(t) = vk

ω(t) = ωk

t ∈ [tk , tk+1 ].

Integration of the kinematic equations (11.13) of the unicycle readily provides
θ(t) = θk + ωk (t − tk )
and
,
x(t) = xk + vk

t−tk

cos (θk + ωk τ )dτ = xk +
0

vk
(sin (θk + ωk (t − tk )) − sin θk )
ωk

vk
= xk +
(sin θ(t) − sin θk )
ωk
, t−tk
vk
y(t) = yk + vk
sin (θk + ωk τ )dτ = yk −
(cos (θk + ωk (t − tk )) − cos θk )
ω
k
0
vk
= yk −
(cos θ(t) − cos θk ).
ωk

140

11 Mobile Robots
variables
initialization

inputs

x

v
polar configuration

inputs

y
omega

configuration

theta

posture regulator

unicycle

delta

x

x_k

v
omega

gamma

y

y_k

x
y

rho

theta

conversion to
polar coordinates

theta_k

theta

plot

odometric localization
with Runge−Kutta

Fig. S11.4. Simulink block diagram of the unicycle with the posture regulator and
Runge–Kutta odometric localization

Evaluating the above expressions at t = tk+1 = tk + Ts leads to the odometric prediction (11.85). Alternative proofs can be devised using geometric
arguments (the unicycle travels along an arc of circle during each sampling
interval) or resorting to a chained-form transformation.
Solution to Problem 11.18
Control of the unicycle with the posture regulator (11.81), (11.82) and Runge–
Kutta odometric localization (11.84) is implemented by the Simulink block
diagram shown in Fig. S11.4. The ﬁles with the solution can be found in
Folder 11 18. The unicycle is simulated as a continuous-time system using a
variable-step integration method, with a maximum step size of 0.01 s. The
controller gains and the duration Ts of the sampling interval for odometric
localization can be chosen in the initialization ﬁle.
The results obtained with Ts = 0.01 s and Ts = 0.1 s are shown in
Fig. S11.5 and Fig. S11.6, respectively. The initial conﬁguration of the unicycle
is the same of Fig. 11.20 (right). Notice how for Ts = 0.1 s the low accuracy of
the odometric localization does not hinder the convergence of the unicycle to
the destination. However, a further increase of Ts will ultimately destabilize
the controlled unicycle.

11 Mobile Robots
driving velocity

2
[m/s]

2

1
[m]

141

0
−2
0

0
−1

2

4
6
[s]
steering velocity

8

10

8

10

[rad/s]

10

−2
−2

−1

0
[m]

1

0
−10
0

2

2

4

6
[s]

Fig. S11.5. Regulation to the origin of the posture of the unicycle with the controller (11.81), (11.82) and odometric localization (11.84) with Ts = 0.01 s; left:
Cartesian motion of the unicycle (continuous) and odometric estimate (dots), right:
time evolution of the velocity inputs v and ω

driving velocity

2
[m/s]

2

[m]

1

0
-2
0

0

[rad/s]

-1
-2
-2

-1

0
[m]

1

2

2

4
6
[s]
steering velocity

8

10

8

10

10
0
-10
0

2

4

6
[s]

Fig. S11.6. Regulation to the origin of the posture of the unicycle with the controller (11.81), (11.82) and odometric localization (11.84) with Ts = 0.1 s; left:
Cartesian motion of the unicycle (continuous) and odometric estimate (dots), right:
time evolution of the velocity inputs v and ω

12
Motion Planning

Solution to Problem 12.1
The conﬁguration of the mobile manipulator is q = [ x y θ0 θ1 . . . θ6 ]T ,
where (x, y) are the Cartesian coordinates of the contact point of the wheel
with the ground (equivalently, of the wheel centre), θ0 is the orientation of the
unicycle with respect to the x axis, and θ1 , . . . , θ6 are the manipulator joint
variables. The conﬁguration space is therefore C = IR2 × SO(2) × . . . × SO(2)
(with SO(2) appearing 7 times), and has dimension 9.
Solution to Problem 12.2
Assume that the conﬁguration q takes values in the subset Q of Fig. 12.1 —
this can be obtained by computing the joint variables q1 and q2 modulo 2π).
Given two conﬁgurations q A = (q1,A , q2,A ), q B = (q1,B , q2,B ) in Q, deﬁne
Δ1 = min(|q1,A − q1,B |, 2π − |q1,A − q1,B |)
Δ2 = min(|q2,A − q2,B |, 2π − |q2,A − q2,B |)
and let
d3 (q A , qB ) =


Δ21 + Δ22 .

This deﬁnition of conﬁguration space distance clearly satisﬁes the requirement
of the problem.
It can be easily shown that d3 (q A , q B ) coincides with d2 (q A , q B ) given by
eq. (12.2)
√ whenever the Euclidean distance between q A and q B is not larger
than 2 π.
Solution to Problem 12.3
A possible solution is shown in Fig. S12.1. In the ﬁrst scene, the robot is
triangular and there is a single square obstacle, while in the second the robot

144

y

12 Motion Planning

B

O

q2

x

B

y
x

O
O

q1

CO

q1

CO

q2

Fig. S12.1. Two diﬀerent scenes (above/below) that result in the same C-obstacle
region. For each scene, left: the robot B, the obstacle O and the growing procedure
for building C-obstacles, right: the conﬁguration space C and the C-obstacle CO

is square and the obstacle is a pentagon. Under the assumption that the robots
can freely translate without changing their orientation, the C-obstacle region
is exactly the same for the two scenes.
Solution to Problem 12.4
The problem can be solved by visual inspection of Fig. 12.4. For example,
the following conﬁgurations of the 2R manipulator lie in the three disjoint
components of Cfree :






π/4
4.12
4.12
qb =
qc =
[rad].
qa =
π/2
π/6
2.88
The manipulator posture and the position in C for each of these conﬁgurations
is shown in Fig. S12.2.
Solution to Problem 12.5
Consider the lists V = {V1 , . . . , Vv } and S = {S1 , . . . , Ss } of all the vertices
and sides of the given limited polygonal subset K of IR2 . A rough sketch of

12 Motion Planning

145

Fig. S12.2. Three conﬁgurations of the 2R manipulator that lie in disjoint components of Cfree : q a (top), q b (centre) and q c (bottom). For each of them, left: the
corresponding manipulator posture, right: the position in C

a naive algorithm for computing the generalized Voronoi diagram of K is the
following.
1. Build all the equidistance curves as follows:
1a. For each vertex-vertex pair (Vi , Vj ), derive the equation of the line LVi Vj
passing through the midpoint of the segment Vi Vj and orthogonal to
the segment itself.
1b. For each vertex-side pair (Vi , Sj ), denote by LSj the line containing Sj ,
and derive the equation of the parabola PVi Sj having Vi as focus and
LSj as directrix.
1c. For each side-side pair (Si , Sj ), denote by LSi and LSj the lines containing Si and Sj , respectively, and derive the equation of the bisectrix
LSi Sj of the angle formed by LSi and LSj which contains Si and Sj .

146

12 Motion Planning

2. For each possible pair of equidistance curves, compute the intersection
points — at most one in the case of a line/line pair, at most two for a
line/parabola or parabola/parabola pair. Each of these points is characterized by the two distances from the features (vertex/vertex, line/line or
line/vertex) that generate the two intersecting equidistance curves.
3. Discard all intersection points that do not belong to K.
4. Discard all intersection points for which the two characteristic distances
are not equal.
5. For each remaining intersection point, compute its clearance, and discard
all intersection points for which the clearance is not equal to the characteristic distance.
6. All the remaining intersection points are nodes of the generalized Voronoi
diagram. Its arcs are the portions of the equidistance curves that are enclosed by these nodes.
Solution to Problem 12.6
Refer to the example shown in Fig. 12.7, and assume that the goal conﬁguration q g is moved somewhere in cell c6 , so that cs = c3 and cg = c6 . The
shortest channel joining cs to cg is clearly {c3 , c2 , c6 }. The path extraction
procedure described in Sect. 12.4.1 would produce a broken line with the following 4 vertices: q s , the midpoint of the common boundary between c3 and
c2 , the midpoint of the common boundary between c2 and c6 , and ﬁnally q g .
Hence, the second of the 3 edges forming the path would lie completely on the
boundary of cell c2 , and in particular would go through the leftmost vertex
of the obstacle.
To solve this problem, the path extraction procedure can be modiﬁed
by including additional vertices in the path. For example, in addition to q s ,
q g and the midpoints of the common boundaries in the channel, one may
include the centroids (or a generic internal point) of all the cells crossed by
the channel, with the exception of cs and cg . In the above case, the resulting
free path would be a broken line with 5 vertices: q s , the midpoint of the
common boundary between c3 and c2 , the centroid of cell c2 , the midpoint of
the common boundary between c2 and c6 , and ﬁnally q g .
Solution to Problem 12.7
The ﬁles with the solution can be found in Folder 12 7; run s12 7.m to execute the program. The robot is a planar manipulator whose ﬁrst two joints
are prismatic (so that the base can move arbitrarily) followed by an arbitrary
number of revolute joints. An admissible range can be speciﬁed for each joint
variable; in particular, a ﬁxed-base manipulator is speciﬁed by setting to zero
the minimum and maximum values of the prismatic joint variables. The obstacles may be line segments, circles or polygons. The user must also input
the start and goal conﬁgurations, the maximum number of conﬁgurations in

12 Motion Planning

147

configuration space

workspace
3
1.5
2
1
1

0.5
0

0

-0.5

-1

-1

-2

-1.5
-3
-1.5

-1

-0.5

0

0.5

1

1.5

2

-3

-2

-1

0

1

2

3

Fig. S12.3. Motion planning for a 2R planar manipulator via the PRM method;
left: stroboscopic motion of the robot in the workspace, right: the PRM and the
solution path (thick gray) in the conﬁguration space

the roadmap, the maximum number of neighbours to which a new sample can
be connected, and the maximum distance between neighbours.
The solution of a speciﬁc planning problem for a 2R ﬁxed-base manipulator is reported in Fig. S12.3. The start and goal conﬁgurations are
q s = [−π/4 3π/4]T and q g = [5π/8 − π/2]T [rad,rad], respectively. Note
that the conﬁguration space, shown as a square in the ﬁgure, is correctly
treated as a two-dimensional torus in the program. In particular, the conﬁguration space distance is a generalization of the deﬁnition proposed in the
solution to Problem 12.2.
Solution to Problem 12.8
The ﬁles with the solution can be found in Folder 12 8; run s12 8.m to execute
the program. The obstacles may be line segments, circles or polygons. The user
must also input the start and goal conﬁgurations, the motion primitives (the
discrete set of admissible constant values of the driving and steering velocities
within the time interval Δ), the value of Δ and the maximum number of
conﬁgurations in the roadmap.
Since the unicycle cannot reach arbitrary conﬁgurations in Cfree with this
planner, the program terminates whenever a conﬁguration is reached that is
suﬃciently close to the desired goal; to this end, a user-selectable distance
threshold is used. Moreover, to bias the search towards the goal, q rand may
be set to q goal — rather than to a random conﬁguration — with a probability
that can be chosen arbitrarily.
The solution of a speciﬁc problem is reported in Fig. S12.4. The start and
goal conﬁgurations are q s = [−3 0 0]T and q g = [3 − 3.5 0]T [m,m,rad],
respectively. Note that the unicycle does not exactly reach q g , shown in gray

148

12 Motion Planning
workspace

configuration space

5
4
3
2
2

1
0

T 0

-1

-2

-2

-5

5

-3
0

-4
-5
-5

0

0
5 -5

5

x

y

Fig. S12.4. Motion planning for a unicycle via the RRT method; left: stroboscopic
motion of the robot and projection of the RRT on the workspace, right: the RRT
and the solution path (thick gray) in the conﬁguration space

in the stroboscopic plot. The conﬁguration space, shown as a parallelepiped
in the ﬁgure, is correctly treated as IR2 × SO(2) in the program. In particular,
the conﬁguration space distance is an adaptation of the deﬁnition proposed
in the solution to Problem 12.2.
Solution to Problem 12.9
Let

⎧
⎨ 1 ka e(q)
Ua (q) = 2
⎩
kb e(q)

2

if

e(q) ≤ ρ

if

e(q) > ρ.

Continuity of the attractive force at the transition radius ρ is guaranteed by
imposing
e(q)
ka e(q) = kb
for e(q) = ρ,
e(q)
i.e., kb = ρ ka .
Solution to Problem 12.10
Consider Fig. S12.5, and in particular the half-line that originates at q g and
passes through the centre of the obstacle. Visualize a point that travels along
this half-line, coming from the point at inﬁnity, where the total potential is
inﬁnity — because of the attractive potential. The total potential goes back
to inﬁnity when the point reaches the boundary of the obstacle — because of
the repulsive potential. Therefore, there is a point on the line where U has a
local minimum; call this point P .

12 Motion Planning

149

R/

h
\

r

4

?

Fig. S12.5. The emergence of local minima due to the superposition of attractive
and repulsive potentials

Now consider points A and B that are located along the same equipotential contour of the repulsive potential Ur as P . Since the attractive potential
is higher at A and B than at C, P is a local minimum also in the direction of
the line AB. In formulæ:
*
*
∂U **
∂U **
=0
= 0,
∂x *P
∂y *P
which imply that P is a stationary point for U , i.e., the gradient of U is zero
at P . To show that P is indeed a local minimum, one should consider the
Hessian matrix of U
⎡ 2
⎤
∂ U ∂U
⎢ ∂x2 ∂x∂y ⎥
⎢
⎥
⎢
⎥
⎣ ∂U ∂ 2 U ⎦
∂y∂x ∂y 2
and prove that it is positive deﬁnite in P . This can be veriﬁed by using the
analytic expression of U . Note that the elements of the diagonal are certainly
positive in P in view of the above arguments.
Solution to Problem 12.11
Denote by (x, y) the Cartesian coordinates of the robot, by (xi , yi ) the Cartesian coordinates of the centre Ci of the i-th circular obstacle, and by ρi its
radius. The attractive potential is
Ua (x, y) =

1
ka (x2 + y 2 ),
2

150

12 Motion Planning

while the repulsive potentials (γ = 2) are
⎧
2

1
1
⎪
⎨ kr,i
−
if ηi (x, y) ≤ η0,i
2
ηi (x, y) η0,i
Ur,i (x, y) =
⎪
⎩
0
if ηi (x, y) > η0,i ,
'
for i = 1, 2, 3, with ηi (x, y) = (x − xi )2 + (y − yi )2 − ρi . The total potential
is
3

Ur,i (x, y),
Ut (x, y) = Ua (x, y) +
i=1

while the total force is
f (x, y) = f a (x, y) +

3


f r,i (x, y)

i=1

 
x
f a (x, y) = −ka
y

with

and

f r,i (x, y) =



⎧
kr,i
1
1
⎪
⎪
−
∇ηi (x, y) if ηi (x, y) ≤ η0,i
⎪
⎨ ηi2 (x, y) ηi (x, y) η0,i
⎪
⎪
⎪
⎩

0

In view of the above expression for ηi (x, y), one has
⎡
x − xi
'
⎢ (x − xi )2 + (y − yi )2
⎢
∇ηi (x, y) = ⎢
y − yi
⎣
'
(x − xi )2 + (y − yi )2

if ηi (x, y) > η0,i .
⎤
⎥
⎥
⎥.
⎦

The actual numerical expressions depend on the choice of the constants ka
and kr,i , for i = 1, 2, 3, as well as of the obstacle range of inﬂuence η0,i .
Assuming that the ranges of inﬂuence of the obstacles do not overlap,
the saddle points of the total potential will be clearly located along the lines
that join the origin of the reference frame (the goal) with the centres of the
circular obstacles, in the ‘shadow zone’ of each obstacle. In particular, the
saddle points are found by solving the equations

 

1
1
kr,i
x
−
=
∇ηi (x, y)
i = 1, 2, 3,
−ka
y
ηi2 (x, y) ηi (x, y) η0,i
each representing the balance between the attractive force and the i-th repulsive force.

12 Motion Planning

151

.1

.n
nn

.d

URERW

n

ODVHUVFDQ
DUHD

Fig. S12.6. On-line planning based on artiﬁcial potential ﬁelds for a circular mobile
robot equipped with a rotating laser range ﬁnder.

Solution to Problem 12.12
In principle, there are two main approaches for on-line motion planning based
on sensor data.
• The ﬁrst approach is to store the data coming from the sensors in a map —
i.e., a representation of the portion of the environment that the robot has
visited so far — and use it to plan in an incremental fashion (deliberative
planning).
• The second approach is to react only to the obstacles that are currently
perceived of the robot, without any form of memory (reactive planning).
The artiﬁcial potential ﬁeld technique naturally lends itself to a reactive
implementation, as brieﬂy discussed below.
Refer to the situation shown in Fig. S12.6. The workspace obstacles O1 ,
O2 and O3 are depicted in black for the portion falling inside the laser scan
area, and in gray outside. Note how the perceived obstacle is larger than
the actual obstacle for O2 , due to the fact that the laser range ﬁnder is a
visibility-based sensor.
Consider ﬁrst the case of a point robot. Since the goal is known, the
attractive potential can be built as in the oﬀ-line case — this is true provided
that the robot is equipped with a localization module. To build the repulsive
potential, it is reasonable to deﬁne the range of inﬂuence η0,i of the obstacles
to be smaller than the maximum measurable range R — if the robot does not
detect the obstacle, it does not react to it. A typical choice is η0,i = R, for all
i. As seen in Sect. 12.6.2, the perceived obstacle region must be decomposed
in convex components to guarantee continuity of the repulsive potential ﬁeld;
in the case of Fig. S12.6, only O2 needs to undergo this procedure. At this
point, the repulsive force produced by each convex component is computed as

152

12 Motion Planning

Fig. S12.7. The assigned occupancy grid, the navigation function and the solution
path from cell (1, 1) to cell (7, 7); left: using 1-adjacency, right: using 2-adjacency

in (12.15), with ηi , ∇ηi directly obtained from the range scan proﬁle — see
the example in Fig. S12.6.
In the case of a robot with a ﬁnite nonzero radius ρ, one simply uses ηi − ρ
in place of ηi in (12.15), and obtains ∇ηi as in the point robot case.
It is easy to realize that this reactive version of the artiﬁcial potential
method behaves exactly as the oﬀ-line version, in the sense that the resulting
motion of the robot is the same. Clearly, this is true as long as the environment is continuously scanned during the motion and the above procedure
for computing the potential ﬁeld can be eﬃciently implemented, so as to be
executed within the duration of the motion control cycle.
Solution to Problem 12.13
The ﬁles with the solution can be found in Folder 12 13. The user is required to
input the binary gridmap representing the conﬁguration space, the start and
goal cells, and the adjacency type (1- or 2-adjacency). The cells are assumed
to be squares with unit side. During the expansion of the wavefront that
originates at q g , the increase of the navigation function between adjacent cells
(cost-to-go) is set equal to the Euclidean distance between the cell centres.
The solution path is then found by following the steepest descent from the
goal cell.
For illustration, Fig. S12.7 shows the results obtained on a (7×7) gridmap,
with (1, 1) as the start cell and (7, 7) as the goal cell. The path found using
2-adjacency is shorter as expected. It is easy to realize that even when the
solution paths are in the same homotopy class — i.e., one can be continuously deformed into the other — the 1-adjacency√path may be longer than
the 2-adjacency path by a factor of as much as 2. On the other hand, 2adjacency produces paths that may graze the C-obstacles; as a consequence,
it is recommended that these are slightly enlarged before planning.

http://www.springer.com/978-1-84628-641-4

Source Exif Data:

File Type                       : PDF
File Type Extension             : pdf
MIME Type                       : application/pdf
PDF Version                     : 1.4
Linearized                      : No
Page Count                      : 159
Producer                        : iText® 5.1.2 ©2000-2011 1T3XT BVBA
Modify Date                     : 2011:11:04 04:23:23+01:00
Create Date                     : 2011:11:04 04:23:23+01:00

EXIF Metadata provided by EXIF.tools

Solution Manual For Robotics Ling Planning And Control

Navigation menu

Versions of this User Manual:

Views

Navigation