Introduction to Robotics
Homework Solutions week 4
2025
Exercise 6
−1 0 0
R = Rot(x̂, π/2)Rot(ẑ, π) = 0 0 −1 .
0 −1 0
In order to express R as e[ω̂]θ we have to evaluate the matrix logarithm. The trace of the above
matrix is −1, and thus we are in the first condition(refer to textbook for algorithm). Either the
first or the second equation can be used to calculate the unit vector ω̂, and the angle of rotation
by definition is θ = π. Therefore
0
1
ω̂ = √ −1
2 1
Exercise 7
(a) From tr(R) = −1 = 1 + 2 cos θ it follows that θ = π.
R = I + 2[ω̂]2
1 − 2(ω22 + ω32 )
2ω1 ω2 2ω1 ω3
= 2ω1 ω2 1 − 2(ω12 + ω32 ) 2ω2 ω3
2ω1 ω3 2ω2 ω3 2 2
1 − 2(ω1 + ω2 )
0 0 1
= 0 −1 0 .
1 0 0
ω̂ = ( √12 , 0, √12 )T or (− √12 , 0, − √12 )T .
(b) From the exponential formula
0 −1 2 −5 4 2
sin θ 1 − cos θ
e[ω̂]θ =I+ 1 0 −2 + 4 −5 2 .
3 9
−2 2 0 2 2 −8
Substituting the above equation into v2 = Rv1 yields
−1 2 sin θ − (1 − cosθ)
1 = 1 − sin θ + 2(1 − cos θ) ⇔ θ = − π .
3 2
0 −2 sin θ − 2(1 − cos θ)
1
Problem 1 Modern robotics, Figure 4.13.
(a) Assign 0 frame as below, not as given. For other frames, here is one way to assign appropriate
frames, but there may be alternatives.
(a) Assigned frames
(b) D-H parameters
Figure 1: Problem 5 solution
(b) Using the DH table, compute transform T0b = T01 T12 T23 T34 T45 T56 T6b and substitute the joint
T
angles θ = 5 90◦ 0◦ −90◦ 90◦ 0◦ .
0 1 0 L2 + L4
0 0 1 L3 + h
T0b =
1 0 0 L1 + 5
0 0 0 1
Python code is provided on the next page if you want to verify your calculation, you are
expected to calculate the result by hand for homework practice.
2
1 import sympy as sp
2
3 # Symbols ( You can do this in 1 line but for readability in pdf its split up )
4 L1 , L2 , L3 , L4 , h , d1 = sp . symbols ( ’ L1 L2 L3 L4 h d1 ’ , real = True )
5 theta2 , theta3 , theta4 = sp . symbols ( ’ theta2 theta3 theta4 ’ , real = True )
6 theta5 , theta6 = sp . symbols ( ’ theta5 theta6 ’ , real = True )
7
8 # DH transform ( Rx -> Tx -> Tz -> Rz )
9 def dh_mod ( alpha , a , d , theta ):
10 Rx = sp . Matrix ([
11 [1 , 0 , 0 , 0] ,
12 [0 , sp . cos ( alpha ) , - sp . sin ( alpha ) , 0] ,
13 [0 , sp . sin ( alpha ) , sp . cos ( alpha ) , 0] ,
14 [0 , 0 , 0 , 1]
15 ])
16
17 Tx = sp . Matrix ([
18 [1 , 0 , 0 , a ] ,
19 [0 , 1 , 0 , 0] ,
20 [0 , 0 , 1 , 0] ,
21 [0 , 0 , 0 , 1]
22 ])
23
24 Tz = sp . Matrix ([
25 [1 , 0 , 0 , 0] ,
26 [0 , 1 , 0 , 0] ,
27 [0 , 0 , 1 , d ] ,
28 [0 , 0 , 0 , 1]
29 ])
30
31 Rz = sp . Matrix ([
32 [ sp . cos ( theta ) , - sp . sin ( theta ) , 0 , 0] ,
33 [ sp . sin ( theta ) , sp . cos ( theta ) , 0 , 0] ,
34 [0 , 0 , 1 , 0] ,
35 [0 , 0 , 0 , 1]
36 ])
37
38 # Combine in the order : Rx -> Tx -> Tz -> Rz
39 return sp . simplify ( Rx * Tx * Tz * Rz )
40
41 # Construct transforms
42 T1 = dh_mod (0 , 0 , L1 + d1 , 0)
43 T2 = dh_mod ( - sp . pi /2 , 0 , h , sp . rad ( theta2 - 90))
44 T3 = dh_mod ( - sp . pi /2 , 0 , 0 , sp . rad ( theta3 ))
45 T4 = dh_mod (0 , L2 , 0 , sp . rad ( theta4 ))
46 T5 = dh_mod (0 , L3 , 0 , sp . rad ( theta5 - 90))
47 T6 = dh_mod ( - sp . pi /2 , 0 , L4 , sp . rad ( theta6 + 90))
48 Tb = dh_mod ( sp . pi /2 , 0 , 0 , 0)
49
50 # End effector pose
51 T0b = sp . simplify ( T1 * T2 * T3 * T4 * T5 * T6 * Tb )
52
53 subs_vals = {
54 d1 : 5 ,
3
55 theta2 : 90 ,
56 theta3 : 0,
57 theta4 : -90 ,
58 theta5 : 90 ,
59 theta6 : 0
60 }
61 # substitute the joint configuration
62 T0b_num = T0b . evalf ( subs = subs_vals )
63
64 # show result
65 sp . pretty_print ( T0b_num )