ELEC3201 Robotics Problem Sheet:

Forward kinematics
Dr. Matthew Turner

In this problem sheet, we look at the 3D homogenous transformation matrix and associated problems.
3D homogenous transformation matrices depend on the order and type of rotation involved. However, they
all have the general form  
r11 r12 r13 px
A
 r21 r22 r23 py 
BT =  r
 
31 r32 r33 pz 
0 0 0 1
where the rij elements form the 3 × 3 rotation matrix, and the first three elements of the final column form
the position vector of the origin of Frame B w.r.t Frame A. For this problem sheet, you may need to refer to
the lecture slides.
Pleaase forgive any typos etc.!

1 Questions
1. A 3D homogenous transformation matrix is given by
 √ 
1 √0 √0 √2
A
 0 √ 2/2 −√ 2/2 3 2 
BT = 
 
0 2/2 2/2 0 
0 0 0 1

A position vector has the following representation in the B frame:

B
P = [1 10 − 2]0

Find A P .

2. Extract the rotation matrix from the homogenous transformation matrix in the previous question and
determine the equivalent Euler angles of Frame B w.r.t Frame A. You should refer to the lecture notes.

3. Repeat the previous question but this time, find the rotation of frame B w.r.t frame A using a fixed
axis representation.

4. A vector A P is rotated about zA by γ degrees and is subsequently rotated about xA by β degrees.

Give the rotation matrix that accomplishes these rotations in the given order.

1
 5. The transformation matrix A B T represents the transformation from frame B coordinates into frame A
coordinates - it is how much frame B is rotated and displaced from A. The inverse transformation
B T reverses the relationship and describes how much frame A is rotated and displaced w.r.t. frame
A
B. The inverse transformation is given by
B −1
AT = (A
BT )

Show that (A −1 has the general form:

BT )

A RT −A T
 
(A −1 B B R BB,ORG
BT ) =
0 1

Hint: Rotation matrices are orthogonal: RRT = I

6. A mobile robot’s base is related to its position in the room by the transformation matrix 0room T . The
transformation relating the robot’s end-effector to the base is given by E0 T . The transformation relating
an object to the room is obj
room T . Derive a transformation matrix relating the position of the end effector
w.r.t the object.

2 Solutions
1. Recall from the lectures that A  B 
A P B P
P̃ = P̃ =
1 1
This gives  
1
B
 10 
P̃ = 
−2


1
Therefore we have
 √    √ 
1 √0 √0 √2 1 1 +√ 2
A
 0 √2/2 −√ 2/2 3 2   10   9 2 
√ 
P̃ = 
 0
 
 −2 =
2/2 2/2 0  4 2 
0 0 0 1 1 1

From which we have  √ 

1 +√ 2
A
P =  9√2 
4 2

2. In 3D, the rotation matrix is the first three rows/columns of the transformation matrix. Hence
 
1 √0 √0
A
B R = 0 √2/2 − √ 2/2
 
0 2/2 2/2

2
 We thus compare this matrix to the rotation matrix associated with Euler angles (from the lecture
notes):
cos ψ cos θ cos ψ sin θ sin φ − sin ψ cos φ cos ψ sin θcosφ + sin ψ sin φ
" #
A
B RZY 0 X 00 (ψ, θ, ψ) = sin ψ cos θ sin ψ sin θ sin φ + cos ψ cos φ sin ψ sin θ cos φ − cos ψ sin φ
− sin θ cos θ sin φ cos θ cos φ

Looking at the (3,1) element we can see that

− sin θ = 0 ⇒ θ = 0 or π

Assuming θ = 0, from the (3,1) element we then have

√
cos φ = 2/2 ⇒ φ = π/4

Similarly, from the (2,1) element we then also have

sin ψ = 0 ⇒ ψ = 0

3. The solution to this question is effectively identical to the previous question, except we use the fixed-
angle rotation matrix:
cos α cos β cos α sin β sin γ − sin α cos γ cos α sin βcosγ + sin α sin γ
" #
A
B RXY Z (α, β, γ) = sin α cos β sin α sin β sin γ + cos α cos γ sin α sin β cos γ − cos α sin γ
− sin β cos β sin γ cos β cos γ

4. As the rotation is done with respect to a fixed frame (two of the axes of the frame A), the first operation
comes last. Therefore the rotation matrix is

R = RX (β)RZ (γ)

or
    
1 0 0 cos γ − sin γ 0 cos γ − sin γ 0
R = 0 cos β − sin β   sin γ cos γ 0 = cos β sin γ cos β cos γ − sin β 
0 sin β cos β 0 0 1 sin β sin γ sin β cos γ cos β

5. We make several observations:

• Orthogonality of A A −1 =A RT
B R implies that B R B
• Because B A −1 is also a transformation matrix it must have the form
A T = (B T )
 
X Y
0 1

where X ∈ R3×m and Y ∈ R3×1 are unknowns matrices

• Trivially (A −1 A T = I
BT ) B

3
 Therefore
  A   
X Y BR PB,ORG I 0
=
0 1 0 1 0 I

Multiplying the terms out

XA
BR = I
XPB,ORG + Y = 0
(0)A
B R + 1(0) = 0
(0)PB,ORG + 1 = 1

The first equation implies

−1
X = (A
B R) =A
B R
T

The second equation implies

Y = −XPB,ORG = −A
BR
T
PB,ORG

which completes the proof.

6. We need to find a transformation matrix

E
obj T

The rules for cascading transformation matrices are

A
BT =A
C T
C
BT

Hence we have
E
obj T =E
0 T
0
room T
room
obj T

We don’t have room

obj T but we know that

room −1
obj T = (obj
room T )

Therefore the transformation matrix we seek is given by

E −1
obj T =E
0 T
0
room T (obj
room T )