Lecture 4:

3D Dynamics
27 Feb 2023

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

• Kinetic Energy
• Potential Energy
ABB Robot Arms
Mini Cheetah
 Exercise: 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
θ
 Exercise: Two Axis Gimbal
*Assume origin of all axes aligned

z0
𝑐𝑜𝑠𝜃 0 −𝑠𝑖𝑛𝜃 1 0 0
𝑅01 = 0 1 0 0 𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 y0
𝑠𝑖𝑛𝜃 0 𝑐𝑜𝑠𝜃 0 −𝑠𝑖𝑛𝜑 𝑐𝑜𝑠𝜑 x0
φ
𝑐𝑜𝑠𝜃 𝑠𝑖𝑛𝜑𝑠𝑖𝑛𝜃 −𝑐𝑜𝑠𝜑𝑠𝑖𝑛𝜃 z1
𝑅01 = 0 𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 y1
𝑠𝑖𝑛𝜃 −𝑐𝑜𝑠𝜃𝑠𝑖𝑛𝜑 𝑐𝑜𝑠𝜑𝑐𝑜𝑠𝜃 x1
θ

DCM
 Exercise: Two Axis Gimbal
Euler Rate Matrix 𝜑ሶ z0
𝝎0𝑇 =𝑬
𝜃ሶ x0
y0

𝑬𝟏𝟐 = 𝒆ො 𝑿 𝑹𝑿 𝑻 𝒆ො 𝒀 , z1
φ
y1
x1
𝑇 θ
1 0 0 0 0
𝑹𝑿 𝑻 𝒆ො 𝒀 = 0 𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 1 = 𝑐𝑜𝑠𝜑
0 −𝑠𝑖𝑛𝜑 𝑐𝑜𝑠𝜑 0 𝑠𝑖𝑛𝜑

1 0
𝑬𝟏𝟐 = 0 𝑐𝑜𝑠𝜑
0 𝑠𝑖𝑛𝜑
 Translational Kinetic Energy
Lecture 4:
3D Dynamics

• Kinetic Energy

• Potential Energy • Translation portion straightforward

1
𝑇𝑡 = 𝒎𝒓ሶ 𝑻 𝒓ሶ
2
• With: 𝑑
𝒓ሶ = 𝑫𝑪𝑴 𝑻 𝒓𝟏
𝑑𝑡
 Moment of Inertia (3D)
Inertia about an arbitrary axis:

Eg. Rotation about X-axis:

𝐼𝑥𝑥 𝐼𝑦𝑥 𝐼𝑧𝑥

Total inertia:
𝐼 = 𝐼𝑦𝑥 𝐼𝑦𝑦 𝐼𝑧𝑦
𝐼𝑧𝑥 𝐼𝑦𝑧 𝐼𝑧𝑧

Off diagonals:
 Total Angular Velocity
• Using the inertia: 𝟏 𝟎𝑻 𝟎 𝟎
𝑻𝒓 = 𝝎 𝑰 𝝎
𝟐
• Problem!
• Moment of Inertia wrt to 0 (inertial)
• It will change as a function of orientation as the
body rotates

• Solution?
• Define Inertia in body axis
• Will be constant as the body rotates wrt 0
 Total Angular Velocity
• Using the body inertia:
Constant matrix

𝟏 𝑩𝑻 𝑩 𝑩
𝑻𝒓 = 𝝎 𝑰 𝝎
𝟐
• 𝝎𝑩 is total angular velocity of body in body axes

• Relating inertias: 𝑰𝟎 = 𝑹𝟎𝑩 𝑰𝑩

 Potential Energy
Lecture 4:
3D Dynamics

• Kinetic Energy • Assume all gravity for now

• 𝟎 𝑻 𝟎
Potential Energy
𝑉𝑔 = 𝒎 𝒈 𝒓
• With:
Local gravitational
0 acceleration
𝒈𝟎 = 0
𝑔
 Example: Acrobot
Rotation Matrices:

𝑐𝑜𝑠𝜃1 𝑠𝑖𝑛𝜃1 0 𝑐𝑜𝑠𝜃2 𝑠𝑖𝑛𝜃2 0 2

𝑹𝟏𝟎 = −𝑠𝑖𝑛𝜃1 𝑐𝑜𝑠𝜃1 0 𝑹𝟐𝟏 = −𝑠𝑖𝑛𝜃2 𝑐𝑜𝑠𝜃2 0
0 0 1 0 0 1
θ2
L1
𝑹𝟐𝟎 = 𝑹𝟐𝟏 𝑹𝟏𝟎
y0 1 τ2
θ1
Positions: 𝐿1 0 x0
𝑻 𝑻 2
𝒓𝟎𝟎𝟏 = 𝑹𝟏𝟎 𝒓𝟏𝟎𝟏 = 𝑹𝟏𝟎
0
0
𝐿2
𝑻
𝐿1 𝐿1
𝟐 𝑻 𝟐 𝑻 𝑻 2
𝒓𝟎𝟎𝟐 = 𝑹𝟏𝟎 0 + 𝑹𝟎 𝒓 𝟏𝟐 = 𝑹𝟏
𝟎 0 + 𝑹𝟐𝟎
0
0 0
0
 Example: Acrobot
Angular Velocity:

0
2
𝝎𝟏𝟎𝟏 = 0
𝜃ሶ1 L1
θ2

0 0 y0 1 τ2
θ1
𝝎𝟐𝟎𝟐 = 𝑹𝟐𝟏 0 + 0 0 x0

𝜃ሶ1 𝜃ሶ2
0
𝝎𝟐𝟎𝟐 = 0
𝜃ሶ1 + 𝜃ሶ2
 Example: Quadcopter gimbal

z0

y0
x0

• Euler 3-2-1 for quad body

• Generalised coordinates?
𝑇
𝒒= 𝑥 𝑦 𝑧 𝜑1 𝜃1 𝜓1 𝜑2 𝜃2
 Exercise: Quadcopter gimbal

• Quad Angular Velocity

𝜑ሶ 1
𝝎𝟏𝟎𝟏 = 𝑬′𝑸𝟏 𝜃ሶ1
𝜓ሶ 1 z0

y0
x0

′
𝑬𝑸𝟏 = 𝒆ො 𝑿 , 𝑹𝑿 𝒆ො 𝒀 , 𝑹𝑿 𝑹𝒀 𝒆ො 𝒁
 Exercise: Quadcopter gimbal

• Camera Angular Velocity

𝟐 𝟐 𝟐
𝝎𝟎𝟐 = 𝝎𝟎𝟏 + 𝝎𝟏𝟐

𝜑ሶ 1
𝜃ሶ1
z0
𝝎𝟐𝟎𝟏 = 𝑬′𝑸𝟐
y0
𝜓ሶ 1 x0

𝑬′𝑸𝟐 = 𝑹𝒀 𝑹𝑿 𝒆ො 𝑿 , 𝑹𝒀 𝑹𝑿 𝑹𝑿 𝒆ො 𝒀 , 𝑹𝒀 𝑹𝑿 𝑹𝑿 𝑹𝒀 𝒆ො 𝒁
 Exercise: Puma 560

• Determine Kinetic
Energy of the
gripper

z0

y0
x0

• Roll(Y)→Pitch(X)→Yaw(Z)
 Kinematic Chain
z3
φ3
• What is 𝑹𝟒𝟎 ? z2
x3
y3
z4

φ2 y2 φ4,θ4,ψ4
𝑹𝟒𝟎 = 𝑹𝟒𝟑 𝑹𝟑𝟐 𝑹𝟐𝟏 𝑹𝟏𝟎
x4 y4
x2
• What is 𝑹𝟒𝟑 ? z1
ψ1
y1
x1
z0

y0
x0

𝑐𝑜𝑠𝜓4 𝑠𝑖𝑛𝜓4 0 1 0 0 𝑐𝑜𝑠𝜃4 0 −𝑠𝑖𝑛𝜃4

𝑹𝟒𝟑 = −𝑠𝑖𝑛𝜓4 𝑐𝑜𝑠𝜓4 0 0 𝑐𝑜𝑠𝜑4 𝑠𝑖𝑛𝜑4 0 1 0
0 0 1 0 −𝑠𝑖𝑛𝜑4 𝑐𝑜𝑠𝜑4 𝑠𝑖𝑛𝜃4 0 𝑐𝑜𝑠𝜃4
Z X Y
 Angular Velocity
z3
φ3
x3
• What is 𝝎𝟒
z2 z4
y3
𝟎𝟒 ? φ2 y2 φ4,θ4,ψ4

x4 y4
𝟒 𝟒 𝟒 𝟒 𝟒
𝝎𝟎𝟒 = 𝝎𝟎𝟏 + 𝝎𝟏𝟐 + 𝝎𝟐𝟑 + 𝝎𝟑𝟒 x2
z1
ψ1
y1
x1
z0
𝜑ሶ 4
y0
𝝎𝟒𝟎𝟏 = 𝑬′𝟏 𝜓ሶ 1 , 𝝎𝟒𝟏𝟐 = 𝑬′𝟐 𝜑ሶ 2 , 𝝎𝟒𝟐𝟑 = ′ 0x
𝝎
𝑬𝟑 𝜑ሶ 3 , 𝟑𝟒𝟒
= 𝑬′𝟒 𝜃ሶ4
𝑹𝟒𝟑 𝜓ሶ 4
′ 𝟑 𝟐
𝑬𝟏 = 𝑹𝒁 𝑹𝑿 𝑹𝒀 𝑹𝟐 𝑹𝟏 𝒆ො 𝒁

𝑬′𝟐 = 𝑹𝒁 𝑹𝑿 𝑹𝒀 𝑹𝟑𝟐 𝒆ො 𝑿 𝑬′𝟑 = 𝑹𝒁 𝑹𝑿 𝑹𝒀 𝒆ො 𝑿

 Easier way?

• It would be great if we could just use the final rotation

matrix:
Some magic
𝟒 𝟒
𝑹𝟎 𝝎𝟎𝟒

• We can!
 Skew Symmetric Property

• R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical

Introduction to Robotic Manipulation. CRC Press, 1994.
(Chapter 4.1)
𝑰 𝑻 ሶ𝑰
𝝎𝑩 × = 𝑹𝑩 𝑹𝑩 =𝑺

0 −𝜔3 𝜔2
𝝎𝑩 × = 𝜔3 0 −𝜔1
−𝜔2 𝜔1 0
 The end.

• Next lecture:
• Generalised
forces