M2794.

002700 Introduction to Robotics

Problem Set #4 solution

Problem 1 Modern robotics, Chapter 3, Exercise 3.1.(a)-(g)

(a) The three frames are

Figure 1

(b) The rotation matrices are

   
0 −1 0 1 0 0
Rsa = 0 0 −1 Rsb = 0 0 1
1 0 0 0 −1 0

(c)
 
1 0 0
−1 T
Rsb = Rbs = Rsb = 0 0 −1
0 1 0

(d)
    
0 0 1 1 0 0 0 −1 0
T
Rab = Ras Rsb = Rsa Rsb = −1 0 0 0 0 1 = −1 0 0
0 −1 0 0 −1 0 0 0 −1

(e)
    
0 −1 0 1 0 0 0 0 −1
R1 = Rsa R = Rsa Rsb = 0 0 −1 0 0 1 = 0 1 0 
1 0 0 0 −1 0 1 0 0

R1 corresponds to rotating Rsa by −90◦ about the body-fixed x̂a axis.

    
1 0 0 0 −1 0 0 −1 0
R2 = RRsa = Rsb Rsa = 0 0 −1 0 0 −1 = 1 0 0
0 −1 0 1 0 0 0 0 1

R2 corresponds to rotating Rsa by −90◦ about the world-fixed x̂s axis.

1
 (f) ps = Rsb pb = (1, 3, −2)T .

(g)
    
1 0 0 1 1
′
p = Rsb ps = 0  0 1  2 = 3  =⇒ position displacement
 
0 −1 0 3 −2
    
1 0 0 1 1
p′′ = Rsb
T
ps = 0 0 −1 2 = −3 =⇒ coordinate change
0 1 0 3 2

Problem 2 Modern robotics, Chapter 3, Exercise 3.15.

(a) As a sequence of rotations about the axes of the fixed frame, rotation matrix should be
multiplied on the left. Beginning with R01 = I,
Step 1. R02 = Rot(x̂0 , α)R01 = Rot(x̂0 , α).
Step 2. R03 = Rot(ŷ0 , β)R02 = Rot(ŷ0 , β)Rot(x̂0 , α).
Step 3. R04 = Rot(ẑ0 , γ)R03 = Rot(ẑ0 , γ)Rot(ŷ0 , β)Rot(x̂0 , α).
   
cos γ − sin γ 0 cos β 0 sin β 1 0 0
R04 =  sin γ cos γ 0   0 1 0   0 cos α − sin α  .
0 0 1 − sin β 0 cos β 0 sin α cos α

(b) As this rotation is about the axes of the moving frame, rotation matrix should be mul-
tiplied on the right.

R04 = R03 Rot(ẑ3 , γ)

= Rot(ŷ0 , β)Rot(x̂0 , α)Rot(ẑ3 , γ)
   
cos β 0 sin β 1 0 0 cos γ − sin γ 0
= 0 1 0   0 cos α − sin α   sin γ cos γ 0  .
− sin β 0 cos β 0 sin α cos α 0 0 1

Problem 3 Modern robotics, Chapter 3, Exercise 3.16.(a)-(g)

Figure 2: problem3-(a)

(a)

2
(b)
       
0 −1 0 1 0 0 3 0
Rsa = 0 0 −1 Rsb = 0 0 1 psa = 0 psb = 2
1 0 0 0 −1 0 0 0

   
Rsa psa Rsb psb
Tsa = Tsb =
0 1 0 1

(c)
 
1 0 0
−1 T
Rsb = Rbs = Rsb = 0 0 −1
0 1 0

 
1 0 0 0
T Tp
 
−1 Rsb −Rsb sb
0 0 −1 0 
Tsb = = 
0 1 0 1 0 −2
0 0 0 1

(d)
T T p
  
-1 Rsa −Rsa sa Rsb psb
Tab = Tas Tsb = Tsa Tsb =
0 1 0 1

    
0 0 1 0 1 0 0 0 0 −1 0 0
−1 0 0 3  = −1 0
0 0 1 2  0 3
=
 0 −1 0
 
0 0 −1 0 0   0 0 −1 −2
0 0 0 1 0 0 0 1 0 0 0 1

(e)
 
0 0 −1 1
0 1 0 0
T1 = Tsa T = Tsa Tsb =
1

0 0 0
0 0 0 1

T1 corresponds to rotating Tsa by −90◦ about the body-fixed x̂a axis and traslating it by
2-unit about the body-fixed ŷa .
 
0 −1 0 3
1 0 0 2
T2 = T Tsa = Tsb Tsa = 
0 0 1 0


0 0 0 1

T2 corresponds to rotating Tsa by −90◦ about the world-fixed x̂s and translating it by
2-unit about the world-fixed ŷs .

3
 (f)
   
ps 
  = Tsb pb  , ps = Rsb pb + psb = (1, 3, −2)T + (0, 2, 0)T = (1, 5, −2)T .
 
   
1 1

(g)
     
1
p′ 
  = Tsb ps  =  5  =⇒ position displacement
   
    −2
1 1 1
        
1 0 0 0 1 1
p′′  p 0 0 −1 0 2 −3
  = T -1  s  = 
       
0 1 0 −2 3 =  0  =⇒ coordinate change
   
  sb  

1 1 0 0 0 1 1 1

Problem 4 Modern robotics, Chapter 3, Exercise 3.20.

Let {a0 } be the frame {a} at t = 0
 
1 0 0 0
0 1 0 0
Tsa0 =  
0 0 1 0
0 0 0 1

Tsb = T rans(zˆs , L + 2rθ)T rans(yˆs , r)Tsa0 Rot(xˆb , θ)

    
1 0 0 0 1 0 0 0 1 0 0 0
0 cos (θ) − sin (θ)
 = 0 cos (θ) − sin (θ)
0 1 0 r  0  r 
=
0
 
0 1 L + r2θ 0 sin (θ) cos (θ) 0  0 sin (θ) cos (θ) L + 2rθ
0 0 0 1 0 0 0 1 0 0 0 1

  
1 0 0 0 1 0 0 −D
0 cos (θ) − sin (θ) r  0 1 0 2r 
Tsc = Tsb T rans(yˆb , 2r)T rans(xˆb , −D) =  
0 sin (θ) cos (θ) L + 2rθ 0

0 1 0 
0 0 0 1 0 0 0 1

 
1 0 0 −D
0 cos (θ) − sin (θ) r + 2r cos (θ) 
= 
0 sin (θ) cos (θ) L + 2rθ + 2r sin (θ)
0 0 0 1

 
1 0 0 0
0 cos (2θ) − sin (2θ) 0 
Tsa = T rans(zˆs , 2rθ)Rot(xˆs , 2θ)Tsa0 =
0 sin (2θ) cos (2θ) 2rθ


0 0 0 1

4
  −1  
1 0 0 0 1 0 0 −D
−1
0 cos (2θ) − sin (2θ) 0  0 cos (θ) − sin (θ) r + 2r cos (θ) 
Tac = Tsa Tsc =   
0 sin (2θ) cos (2θ) 2rθ 0 sin (θ) cos (θ) L + 2rθ + 2r sin (θ)
0 0 0 1 0 0 0 1
  
1 0 0 0 1 0 0 −D
0 cos (2θ) sin (2θ) −2rθ sin (2θ)  0 cos (θ) − sin (θ) r + 2r cos (θ) 
=  
0 − sin (2θ) cos (2θ) −2rθ cos (2θ) 0 sin (θ) cos (θ) L + 2rθ + 2r sin (θ)
0 0 0 1 0 0 0 1
 
1 0 0 −D
0 cos (θ) sin (θ) 2r cos θ + r cos (2θ) + L sin (2θ)
= 
0 − sin (θ) cos (θ) L cos 2θ − r sin (2θ) − 2r sin (θ) 
0 0 0 1

Problem 5 Modern robotics, Chapter 3, Exercise 3.36.

(a) The rotation matrix corresponding to the ZXZ Euler angles can be represented as
R = Rot(ẑ, α)Rot(x̂, β)Rot(ẑ, γ)
   
cos α − sin α 0 1 0 0 cos γ − sin γ 0
=  sin α cos α 0   0 cos β − sin β   sin γ cos γ 0 
0 0 1 0 sin β cos β 0 0 1
 
cos α cos γ − sin α sin γ cos β − cos α sin γ − sin α cos γ cos β sin α sin β
=  sin α cos γ + cos α cos β sin γ − sin α sin γ + cos α cos β cos γ − cos α sin β  .
sin β sin γ sin β cos γ cos β
√2 2
sin β
Since β = tan−1 cos −1 ± r13 +r23 , we consider two possible cases:
β = tan r33
Case 1. 0 ≤ β ≤ π,
q
β = atan2( r13 2 + r 2 , r ),
23 33

α = atan2(r13 , −r23 ),
γ = atan2(r31 , r32 ).
Case 2. −π ≤ β ≤ 0,
q
β = atan2(− 2 + r 2 , r ),
r13 23 33

α = atan2(−r13 , r23 ),
γ = atan2(−r31 , −r32 ).

(b) From the result obtained in (a), we can find two sets of Euler angles:
Case 1. 0 ≤ β ≤ π,
π π
α = π, β = , γ = .
4 4
Case 2. −π ≤ β ≤ 0,
π 3π
α = 0, β = − , γ = − .
4 4