Lecture 4:

3D Kinematics
27 Feb 2023

Presented by:
Amir Patel
PhD (Mechatronics) Cape Town
Electrical Engineering Department EEE4119F
University of Cape Town Mechatronics II
 Overview

• Rotation Matrices
• Euler Angles
• 3D Velocity
Disney Acrobats
Homework (Tailbot)
 Homework (Tailbot)

• Generalised coords:
𝑇 mt
𝒒= 𝑥 𝑦 𝜃𝑏 𝜃𝑡 mb Ib

Lt
(x,y) θb
θt

y τin LB

x
 Homework (Tailbot)
• Positions mt
𝑥 mb Ib
𝒓𝒃 = 𝑦
Lt
𝐿𝑏 (x,y) θb
𝑥 − cos 𝜃𝑏 + 𝐿𝑡 cos 𝜃𝑏 + 𝜃𝑡
𝒓𝒕 = 2 θt
𝐿𝑏 LB
𝑦 − sin 𝜃𝑏 + 𝐿𝑡 sin 𝜃𝑏 + 𝜃𝑡 y τin
2
• Velocity x

𝑥ሶ
𝒓ሶ 𝒃 =
𝑦ሶ
𝐿𝑏
𝑥ሶ + sin 𝜃𝑏 𝜃ሶ𝑏 − 𝐿𝑡 sin 𝜃𝑏 + 𝜃𝑡 (𝜃ሶ𝑏 + 𝜃ሶ 𝑡 )
𝒓ሶ 𝒕 = 2
𝐿𝑏
𝑦ሶ − cos 𝜃𝑏 𝜃ሶ𝑏 + 𝐿𝑡 cos 𝜃𝑏 + 𝜃𝑡 (𝜃ሶ 𝑏 + 𝜃ሶ 𝑡 )
2
 Homework (Tailbot)
• Kinetic Energy
1 1
𝑇𝑏 = 𝑚𝑏 𝒓ሶ 𝑏 𝒓ሶ 𝑏 + 𝐼𝑏 𝜃ሶ𝑏2
𝑇
2 2 mt
mb Ib
1
𝑇𝑡 = 𝑚𝑡 𝒓ሶ 𝑇𝑡 𝒓ሶ 𝑡 Lt
2 (x,y) θb
θt

• Potential Energy y τin LB

𝑉𝑏 = 𝑚𝑏 𝑔𝑟𝑏,𝑦 x

𝑉𝑡 = 𝑚𝑡 𝑔𝑟𝑡,𝑦
 Homework (Tailbot)
• Mass matrix 2
𝜕 𝑇
• Some elements: 𝑀𝑖,𝑗 =
𝜕𝑞ሶ 𝑖 𝜕𝑞ሶ 𝑗
𝜕2𝑇
𝑀1,1 = = 𝑚𝑏 + 𝑚𝑡
𝜕𝑥𝜕
ሶ 𝑥ሶ
𝜕2𝑇
𝑀4,2 = = 𝐿𝑡 𝑚𝑡 𝑐𝑜𝑠(𝜃𝑏 + 𝜃𝑡 )
𝜕𝜃ሶ𝑡 𝜕𝑦ሶ
𝜕2𝑇
𝑀4,4 = = 𝑚𝑡 𝐿2𝑡 + 𝐼𝑏
𝜕𝜃ሶ𝑡 𝜕𝜃ሶ𝑡
 Homework (Tailbot)

• C matrix 𝝏𝑻
𝑪 = 𝑴ሶ 𝒒ሶ −
𝝏𝒒

𝝏𝑽
• G matrix 𝑮=
𝝏𝒒
3D Kinematics
 Rotating Axes
Consider vector 𝒑
• In axes 0

• In axes 1 (rotated by 𝜓)
 Rotating Axes

How do we relate 𝒑𝟏 and 𝒑𝟎 ?

𝟎 𝟎 𝟏
𝒑 = 𝑹𝟏 𝒑

𝑝𝑥0 𝑐𝑜𝑠𝜓 𝑠𝑖𝑛𝜓 𝑝𝑥1

0 = −𝑠𝑖𝑛𝜓
Rotation matrix
𝑝𝑦 𝑐𝑜𝑠𝜓 𝑝𝑦1
 Rotation Matrices
Rotation (from 0→1)

Rotation about X

Rotation about Y

Rotation about Z
 Changing axes

𝟏 𝟏 𝟎
𝒑 = 𝑹𝟎 𝒑
𝟎 −𝟏
𝑹𝟏𝟎 = 𝑹𝟏
But 𝑹 is orthogonal:

𝟎 𝑻
𝑹𝟏𝟎 = 𝑹𝟏
 Example: Aircraft Yaw
20
Position of nose in aircraft frame: 𝒑𝟏𝒏 =
0

y0

What is the nose position in y1 x1

inertial frame (0)?

𝑐𝑜𝑠𝛼 −𝑠𝑖𝑛𝛼 20 α
𝒑𝟎𝒏 =
𝑠𝑖𝑛𝛼 𝑐𝑜𝑠𝛼 0 x0
 Example: Aircraft Yaw

What about the position of nose relative to 0?

y1 x1

y0 α
B

0 x0

𝑐𝑜𝑠𝛼 −𝑠𝑖𝑛𝛼 20
𝒑𝟎𝒏 = 𝒑𝟎𝑩 +
𝑠𝑖𝑛𝛼 𝑐𝑜𝑠𝛼 0
 Successive Rotations

Any rigid body attitude can be

represented by three rotations. Eg:

Things to take note of:

• Rotation is about the new axes
• Rotation order starts from the right

• Resultant matrix called:

• Direction Cosine Matrix (DCM)
 Euler Angles
Lecture 4:
3D Kinematics

• Rotation Matrices
• Euler angles represent attitude about
• Euler Angles
using three numbers:
• Velocity
𝑇
𝒖= 𝜑 𝜃 𝜓
• Need to specify the order
• Eg. Euler 3-2-1 is
• Rot about Z, then Y and the X
Problems with Euler Angles?
 Translational Velocity
y1 x1
Lecture 9:
3D Kinematics
y0 α
B

0 x0

• Admin • Straightforward once you have the 3D

• Rotation Matrices position
• Euler Angles
𝟎 𝑑 𝟎 𝑐𝑜𝑠𝛼 −𝑠𝑖𝑛𝛼 𝟏
𝒑ሶ 𝑵 = 𝒑𝑩 + 𝒑𝑵
• Velocity 𝑑𝑡 𝑠𝑖𝑛𝛼 𝑐𝑜𝑠𝛼
• What about acceleration?
• The same process!
 Side note on accelerometers

• Total Acceleration relative Earth/inertial:

Gravity
𝑨𝑰𝑻 = 𝑨𝑰𝒔 + 𝑨𝑰𝒈 0
0
𝑔
Total acceleration
Specific acceleration
(everything else)

• What does an accelerometer give output?

𝑩
𝑨𝑩
𝒔 = 𝑨 𝑻 − 𝑨 𝑩
𝒈
 Side note on accelerometers

• What would it output at rest on a level desk?

0
𝑨𝑩
𝒔 =− 0
𝑔
• How would we determine the acceleration
relative to the inertial frame? In the
inertial
0 frame
𝑩
Euler
𝑨𝑩
𝒔 = 𝑹 𝑰 0
angles −𝑔
 Exercise: Aero-pendulum

• If an accelerometer is m
connected to the ψ1
propeller z0 y1
• how can we y0 L2

express the total x0

x1
z2
acceleration of it in L1 y2

inertial using an φ2 x2
accelerometer
 Exercise: Aero-pendulum

• Step 1, determine the F

m
rotation matrix: ψ1
z0 y1
y0 L2

𝑹𝟐𝟎 = 𝑹𝒙 (𝜑2 )𝑹𝒛 (𝜓1 ) x0

x1
z2
L1 y2

φ2 x2

1 0 0 𝑐𝑜𝑠𝜓1 𝑠𝑖𝑛𝜓1 0
𝑹𝟐𝟎 = 0 𝑐𝑜𝑠𝜑2 𝑠𝑖𝑛𝜑2 −𝑠𝑖𝑛𝜓1 𝑐𝑜𝑠𝜓1 0
0 −𝑠𝑖𝑛𝜑2 𝑐𝑜𝑠𝜑2 0 0 1
𝑐𝑜𝑠𝜓1 𝑠𝑖𝑛𝜓1 0
𝑹𝟐𝟎 = −𝑐𝑜𝑠𝜑2 𝑠𝑖𝑛𝜓1 𝑐𝑜𝑠𝜑2 𝑐𝑜𝑠𝜓1 𝑠𝑖𝑛𝜑2
𝑠𝑖𝑛𝜑2 𝑠𝑖𝑛𝜓1 −𝑐𝑜𝑠𝜓1 𝑠𝑖𝑛𝜑2 𝑐𝑜𝑠𝜑2
 Exercise: Aero-pendulum

• We know that: 𝑨𝑩 𝑩 𝑩
𝒔 = 𝑨 𝑻 − 𝑨 𝒈
F

m
ψ1
z0 y1
y0 L2

𝑨𝟎𝑻 = 𝑨𝟎𝒔 + 𝑨𝟎𝒈 x0

x1
L1
z2
y2

φ2 x2

𝑇
𝑐𝑜𝑠𝜓1 𝑠𝑖𝑛𝜓1 0 𝑎𝑥 0
𝑨𝟎𝑻 = −𝑐𝑜𝑠𝜑2 𝑠𝑖𝑛𝜓1 𝑐𝑜𝑠𝜑2 𝑐𝑜𝑠𝜓1 𝑠𝑖𝑛𝜑2 𝑎𝑦 + 0
𝑠𝑖𝑛𝜑2 𝑠𝑖𝑛𝜓1 −𝑐𝑜𝑠𝜓1 𝑠𝑖𝑛𝜑2 𝑐𝑜𝑠𝜑2 𝑎𝑧 𝑔
 Angular Velocity

• Can only add angular velocity vectors in same

axis!
• Cannot directly add Euler angle rates (𝒖)
ሶ to get
𝝎𝑻𝒐𝒕𝒂𝒍
• Why?

• We want (inertial): 𝝎𝐼𝑇 = 𝑬𝒖ሶ

• Or (Body): 𝝎𝐵𝑇 = 𝑬′ 𝒖ሶ
 Euler Rates Matrix (E)

• Eg: If we use Euler 3-2-1, E is

given by: Unit vectors

𝑻
𝐸321 = 𝑹𝒁 𝑹𝒀 𝑻 𝒆ො 𝑿 , 𝑹𝒁 𝑻 𝒆ො 𝒀 , 𝒆ො 𝒁
• Total angular velocity in Inertial Frame:

𝜑ሶ
𝝎𝐼𝑇 = 𝑬 𝜃ሶ
𝜓ሶ
 Specifically…
𝑇
𝑐𝑜𝑠𝜓 𝑠𝑖𝑛𝜓 0 0 −𝑠𝑖𝑛𝜓
𝑹𝒁 𝑻 𝒆ො 𝒀 = −𝑠𝑖𝑛𝜓 𝑐𝑜𝑠𝜓 0 1 = 𝑐𝑜𝑠𝜓
0 0 1 0 0
𝑇 𝑇
𝑐𝑜𝑠𝜓 𝑠𝑖𝑛𝜓 0 𝑐𝑜𝑠𝜃 0 −𝑠𝑖𝑛𝜃 1
𝑻
𝑹𝒁 𝑹𝒀 𝑻 𝒆ො 𝑿 = −𝑠𝑖𝑛𝜓 𝑐𝑜𝑠𝜓 0 0 1 0 0
0 0 1 𝑠𝑖𝑛𝜃 0 𝑐𝑜𝑠𝜃 0
𝑐𝑜𝑠𝜓𝑐𝑜𝑠𝜃
= 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜃
−𝑠𝑖𝑛𝜃
𝑐𝑜𝑠𝜓𝑐𝑜𝑠𝜃 −𝑠𝑖𝑛𝜓 0
Finally: 𝐸321 = 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜃 𝑐𝑜𝑠𝜓 0
−𝑠𝑖𝑛𝜃 0 1
• What about 𝑬′ ?
 Exercise: Aero-Pendulum

• Determine Euler Rates

F
matrix in link 2. ie. (E’)
m
ψ1
• Remember: z0 y1
y0 L2

x0
x1
z2
L1 y2
𝟐
𝑹𝟎 = 𝑹𝒙 (𝜑2 )𝑹𝒛 (𝜓1 ) φ2 x2

1 0 0 𝑐𝑜𝑠𝜓1 𝑠𝑖𝑛𝜓1 0
𝑹𝟐𝟎 = 0 𝑐𝑜𝑠𝜑2 𝑠𝑖𝑛𝜑2 −𝑠𝑖𝑛𝜓1 𝑐𝑜𝑠𝜓1 0
0 −𝑠𝑖𝑛𝜑2 𝑐𝑜𝑠𝜑2 0 0 1
 Exercise: Aero-Pendulum

• We want angular velocity in F

frame 2 which gives: m

ψ1

2 ′ 𝜓1
ሶ z0 y1

𝝎2 = 𝑬 y0 L2

𝜑ሶ 𝟐 x0
x1
z2
L1 y2

φ2 x2

∴ 𝑬′ = 𝑹𝟐𝟏 𝒆ො 𝒛 𝒆ො 𝒙
 Homework: Two Axis Gimbal

Rotation order:
1. Rotation about X z0
2. Rotation about Y
y0
Task x0
• Derive DCM (𝑹𝟏𝟎 )
• Derive Euler Rate Matrix φ
z1
y1
x1
θ
 The end.

• Next lecture:
• 3D Multibody
Dynamics