TEST BANK FOR Robotics Modelling, Planning and Control 1st Ed By Villani L., Oriolo G., Siciliano B.

2 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
3 Differential Kinematics and Statics . . . . . . . . . . . . . . . . . . . . . . . . 19
4 Trajectory Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
5 Actuators and Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
6 Control Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
7 Dynamics. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
8 Motion Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
9 ForceControl. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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):
Rz(ϕ) =
⎡
⎣
cϕ −sϕ 0
sϕ cϕ 0
0 0 1
⎤
⎦
Rx(ϑ) =
⎡
⎣
1 0 0
0 cϑ −sϑ
0 sϑ cϑ
⎤
⎦
Rz (ϕ) =
⎡
⎣
cψ −sψ 0
sψ cψ 0
0 0 1
⎤
⎦
and taking the products gives
R(φ) =
⎡
⎣
cϕcψ − sϕcϑsψ −cϕsψ − sϕcϑcψ sϕsϑ
sϕcψ + cϕcϑsψ −sϕsψ + cϕcϑcψ −cϕsϑ
sϑsψ sϑcψ cϑ
⎤
⎦.
As for the inverse problem, given a rotation matrix
R =
⎡
⎣
r11 r12 r13
r21 r22 r23
r31 r32 r33
⎤
⎦,
2 2 Kinematics
the set of Euler angles ZXZ is given by
ϕ = Atan2(r13,−r23)
ϑ = Atan2

r2
31 + r2
32, r33

ψ = Atan2(r31, r32)
when ϑ ∈ (0, π). Otherwise, if ϑ ∈ (−π, 0) then the solution is
ϕ = Atan2(−r13, r23)
ϑ = Atan2

−

r2
31 + r2
32, r33

ψ = Atan2(−r31,−r32).
Solution to Problem 2.2
In the case sϑ = 0, the rotation matrix in (2.18) becomes
R(φ) =
⎡
⎣
cϕ+ψ −sϕ+ψ 0
sϕ+ψ cϕ+ψ 0
0 0 1
⎤
⎦
when ϑ = 0. Otherwise, if ϑ = π, then the matrix is
R(φ) =
⎡
⎣
−cϕ−ψ −sϕ−ψ 0
−sϕ−ψ cϕ−ψ 0
0 0 −1
⎤
⎦.
From the elements [1, 2] and [2, 2] it is possible to compute only the sum or
difference of angles ϕ and ψ, i.e.,
ϕ ± ψ = Atan2(−r12, r22)
where the positive sign holds for ϑ = 0 and the negative sign holds for ϑ = π.
Solution to Problem 2.3
In the case cϑ = 0, the rotation matrix in (2.21) becomes
R(φ) =
⎡
⎣
0 sψ−ϕ cψ−ϕ
0 cψ−ϕ −sψ−ϕ
−1 0 0
⎤
⎦
when ϑ = π/2. Otherwise, if ϑ = −π/2, then the matrix is
R(φ) =
⎡
⎣
0 −sψ+ϕ −cψ+ϕ
0 cψ+ϕ −sψ+ϕ
1 0 0
⎤
⎦.
2 Kinematics 3
From the elements [2, 2] and [2, 3] it is possible to compute only the sum or
difference of angles ψ and ϕ, i.e.,
ψ ± ϕ = Atan2(−r23, r22)
where the positive sign holds for ϑ = −π/2 and the negative sign holds for
ϑ = π/2.
Solution to Problem 2.4
The rotation matrix can be obtained as in (2.24)
R(ϑ, r) = Rz(α)Ry(β)Rz(ϑ)Ry(−β)Rz(−α),
where the elementary rotation matrices are given as in (2.6) and (2.7):
Rz(α) =
⎡
⎣
cα −sα 0
sα cα 0
0 0 1
⎤
⎦
Ry(β) =
⎡
⎣
cβ 0 sβ
0 1 0
−sβ 0 cβ
⎤
⎦
Rz(ϑ) =
⎡
⎣
cϑ −sϑ 0
sϑ cϑ 0
0 0 1
⎤
⎦.
Taking the first product gives
Rz(α)Ry(β) =
⎡
⎣
cαcβ −sα cαsβ
sαcβ cα sαsβ
−sβ 0 cβ
⎤
⎦.
The next product gives
Rz(α)Ry(β)Rz(ϑ) =
⎡
⎣
cαcβcϑ − sαsϑ −cαcβsϑ − sαcϑ cαsβ
sαcβcϑ + cαsϑ −sαcβsϑ + cαcϑ sαsβ
−sβcϑ sβsϑ cβ
⎤
⎦.
Then, by observing that
Ry(−β)Rz(−α) = (Rz(α)Ry(β))T ,
the overall rotation matrix is
R(ϑ, r) =
⎡
⎣
(s2
α + c2
αc2
β)cθ + c2
αs2
β sαcαs2
β(1 − cϑ) − cβsϑ
sαcαs2
β(1 − cϑ) + cβsϑ (s2
αc2
β + c2
α)cθ + s2
αs2
β
cαsβcβ(1 − cϑ) − sαsβsϑ sαsβcβ(1 − cϑ) + cαsβsϑ
cαsβcβ(1 − cϑ) + sαsβsϑ
sαsβcβ(1 − cϑ) − cαsβsϑ
s2
βcϑ + c2
β
⎤
⎦.
4 2